# How can I write a manifold for single euler angles?

98 views

### Rasmus

Dec 3, 2023, 12:16:02 PM12/3/23
to Ceres Solver
I'm trying to write my own manifold subclass to deal with scalar angles.

My optimization problem has as parameters a 6d pose, and then 2 single euler angles, all this describes various connected joints. I came up with this:

```
template <typename T> T normalize_angle(const T &ang) {
if (ang > T(M_PI / 2))
return ang - T(M_PI);
else if (ang < T(-M_PI / 2))
return ang + T(M_PI);
else
return ang;
};

class EulerAngleManifold : public ceres::Manifold {
public:
int AmbientSize() const override { return 1; }

int TangentSize() const override { return 1; }

return true;
}

bool PlusJacobian(const double *x_ptr, double *jacobian_ptr) const override {
*jacobian_ptr = 1;
return true;
}

return true;
}

bool MinusJacobian(const double *x_ptr, double *jacobian_ptr) const override {
*jacobian_ptr = 1;
return true;
}
};
```

But when I set this manifold on the respective parameters, the optimization basically does not even start, the output looks like this

```
0  1.126829e+00    0.00e+00    9.98e+00   0.00e+00   0.00e+00  1.00e+04        0    1.59e-03    2.92e-03
1  9.829076e+04   -9.83e+04    9.98e+00   3.14e+00  -1.36e+05  5.00e+03        1    8.39e-04    3.95e-03
2  9.829140e+04   -9.83e+04    9.98e+00   3.14e+00  -1.36e+05  1.25e+03        1    5.44e-04    4.50e-03
3  9.829524e+04   -9.83e+04    9.98e+00   3.14e+00  -1.36e+05  1.56e+02        1    6.23e-04    5.13e-03
4  9.833099e+04   -9.83e+04    9.98e+00   3.14e+00  -1.36e+05  9.77e+00        1    5.42e-04    5.67e-03
5  9.890223e+04   -9.89e+04    9.98e+00   3.14e+00  -1.37e+05  3.05e-01        1    5.85e-04    6.26e-03
6  1.046268e+05   -1.05e+05    9.98e+00   3.14e+00  -2.48e+05  4.77e-03        1    5.46e-04    6.81e-03
7  1.077072e+05   -1.08e+05    9.98e+00   3.14e+00  -8.30e+06  3.73e-05        1    5.37e-04    7.35e-03
8  1.077835e+05   -1.08e+05    9.98e+00   3.14e+00  -1.05e+09  1.46e-07        1    5.63e-04    7.92e-03
9  1.077841e+05   -1.08e+05    9.98e+00   3.14e+00  -2.68e+11  2.84e-10        1    5.38e-04    8.46e-03
10  1.077841e+05   -1.08e+05    9.98e+00   3.14e+00  -1.37e+14  2.78e-13        1    5.49e-04    9.01e-03
11  1.077841e+05   -1.08e+05    9.98e+00   3.14e+00  -1.40e+17  1.36e-16        1    5.35e-04    9.55e-03
12  1.077841e+05   -1.08e+05    9.98e+00   3.14e+00  -2.88e+20  3.31e-20        1    5.36e-04    1.01e-02
13  1.077841e+05   -1.08e+05    9.98e+00   3.14e+00  -1.18e+24  4.04e-24        1    5.36e-04    1.06e-02
14  1.077841e+05   -1.08e+05    9.98e+00   3.14e+00  -9.65e+27  2.47e-28        1    5.47e-04    1.12e-02
15  1.077841e+05   -1.08e+05    9.98e+00   3.14e+00  -1.58e+32  7.52e-33        1    5.33e-04    1.17e-02
```

When I remove the manifold, the optimization behaves normally, so I must be doing something grossly wrong. The parameters in reality do not actually come close to the 0 crossing in value, so I'm not sure if I could just ignore the wraparound, but I want to understand what the (probably trivial) problem is here.

Best,
Rasmus

### Andreu Corominas Murtra

Dec 3, 2023, 1:26:23 PM12/3/23

Hi Rasmus ,

just some hints to implement it. Think about the unitary circle in the 2D plane:

- Ambient size = 2

- Tangent size = 1

The Plus and PlusJacobian would be:

virtual bool Plus(const double* _x, const double* _delta, double* _x_plus_delta) const
{
double angle = atan2(_x[1],_x[0]);
_x_plus_delta[0] = cos( angle + _delta[0] );
_x_plus_delta[1] = sin( angle + _delta[0] );
return true;
}

virtual bool PlusJacobian(const double* _x, double* _jacobian) const
{
double angle = atan2(_x[1],_x[0]);
_jacobian[0] = -sin( angle );
_jacobian[1] = cos( angle );
return true;
}

best regards,

```--
------------------------------------
Andreu Corominas Murtra
PhD Engineer
and...@beta-robots.com
+34 635 575 864
www.beta-robots.com
------------------------------------```
--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.

### Dmitriy Korchemkin

Dec 3, 2023, 1:55:24 PM12/3/23
to Ceres Solver
Probably it might be better to compute sin(delta) and cos(delta) and construct sine and cosine of x+delta using formulas with multiplications and additions (in order to avoid using inverse trigonometry for the sake of numerical accuracy and speed)?

### Rasmus

Dec 4, 2023, 2:51:22 AM12/4/23
to Ceres Solver
I got inspired to do this simple implementation by VINS-Mono doing something similar (even though they seem to conflate degrees and radians) in https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/01f9c910d4b5aa65282e6f10b7ab72e12f22d6cc/pose_graph/src/pose_graph.h#L90, but that was with the old *Parametrization API. Can someone explain why it is incorrect to treat angles as just euclidean quantities with a special handling around 0?

### Andreu Corominas Murtra

Dec 4, 2023, 3:26:57 AM12/4/23

Hi,

thanks Dimitry for the comment, probably it may be better as you suggest.

Regarding Rasmus' question, I would say that "Euclidean-like Euler angle approach with special handling at 0" has continuity problems which should be avoided in an optimization NLLS problem. This is one of the reasons to implement such cases with manifolds.

best ,

andreu

### Rasmus

Mar 3, 2024, 3:10:49 AMMar 3
to Ceres Solver
I came up with this implementation now, which at least does not seem to destroy the optimization

```
class EulerAngleManifold : public ceres::Manifold {
public:
int AmbientSize() const override { return 2; }

int TangentSize() const override { return 1; }

virtual bool Plus(const double *_x, const double *_delta,
double *_x_plus_delta) const override {
const double sin_delta = std::sin(_delta[0]);
const double cos_delta = std::cos(_delta[0]);
const double cos_x = _x[0];
const double sin_x = _x[1];

_x_plus_delta[0] = cos_x * cos_delta - sin_x * sin_delta;
_x_plus_delta[1] = sin_x * cos_delta + cos_x * sin_delta;

return true;
}

virtual bool PlusJacobian(const double *_x,
double *_jacobian) const override {
const double angle = atan2(_x[1], _x[0]);

_jacobian[0] = -sin(angle);
_jacobian[1] = cos(angle);
return true;
}

virtual bool Minus(const double *_x, const double *_delta,
double *_x_plus_delta) const override {
const double sin_delta = std::sin(_delta[0]);
const double cos_delta = std::cos(_delta[0]);
const double cos_x = _x[0];
const double sin_x = _x[1];

_x_plus_delta[0] = cos_x * cos_delta + sin_x * sin_delta;
_x_plus_delta[1] = sin_x * cos_delta - cos_x * sin_delta;
return true;
}

virtual bool MinusJacobian(const double *_x,
double *_jacobian) const override {
PlusJacobian(_x, _jacobian);
return true;
}
};
```

Could someone tell me if I'm on the right track?