How can I write a manifold for single euler angles?

[Rasmus]

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; }

  bool Plus(const double *theta_radians, const double *delta_theta_radians,
            double *theta_radians_plus_delta) const override {
    *theta_radians_plus_delta =
        normalize_angle(*theta_radians + *delta_theta_radians);

    return true;
  }

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

  bool Minus(const double *theta_radians_x, const double *theta_radians_y,
             double *theta_radians_x_minus_y) const override {
    *theta_radians_x_minus_y =
        normalize_angle(*theta_radians_x - *theta_radians_y);

    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

```

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   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]

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
Business Director & Co-founder
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.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/cd696a9f-9813-49ab-a354-8542c4691c9en%40googlegroups.com.

[Dmitriy Korchemkin]

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]

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]

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

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/2fb563b4-6805-4631-a197-b941bca9b6e2n%40googlegroups.com.

[Rasmus]

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?