Merge pull request #520 from Eskilade/add_jacobians_Rot3_ypr

Added Jacobians for Rot3::xyz and related conversions to euler angles

gtsam/geometry/Rot3.cpp

@@ -158,21 +158,73 @@ Point3 Rot3::column(int index) const{
 }
 
 /* ************************************************************************* */
-Vector3 Rot3::xyz() const {
+Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
   Matrix3 I;Vector3 q;
-  boost::tie(I,q)=RQ(matrix());
+  if (H) {
+    Matrix93 mH;
+    const auto m = matrix();
+#ifdef GTSAM_USE_QUATERNIONS
+    SO3{m}.vec(mH);
+#else
+    rot_.vec(mH);
+#endif
+
+    Matrix39 qHm;
+    boost::tie(I, q) = RQ(m, qHm);
+
+    // TODO : Explore whether this expression can be optimized as both
+    // qHm and mH are super-sparse
+    *H = qHm * mH;
+  } else
+    boost::tie(I, q) = RQ(matrix());
   return q;
 }
 
 /* ************************************************************************* */
-Vector3 Rot3::ypr() const {
-  Vector3 q = xyz();
+Vector3 Rot3::ypr(OptionalJacobian<3, 3> H) const {
+  Vector3 q = xyz(H);
+  if (H) H->row(0).swap(H->row(2));
+
   return Vector3(q(2),q(1),q(0));
 }
 
 /* ************************************************************************* */
-Vector3 Rot3::rpy() const {
-  return xyz();
+Vector3 Rot3::rpy(OptionalJacobian<3, 3> H) const { return xyz(H); }
+
+/* ************************************************************************* */
+double Rot3::roll(OptionalJacobian<1, 3> H) const {
+  double r;
+  if (H) {
+    Matrix3 xyzH;
+    r = xyz(xyzH)(0);
+    *H = xyzH.row(0);
+  } else
+    r = xyz()(0);
+  return r;
+}
+
+/* ************************************************************************* */
+double Rot3::pitch(OptionalJacobian<1, 3> H) const {
+  double p;
+  if (H) {
+    Matrix3 xyzH;
+    p = xyz(xyzH)(1);
+    *H = xyzH.row(1);
+  } else
+    p = xyz()(1);
+  return p;
+}
+
+/* ************************************************************************* */
+double Rot3::yaw(OptionalJacobian<1, 3> H) const {
+  double y;
+  if (H) {
+    Matrix3 xyzH;
+    y = xyz(xyzH)(2);
+    *H = xyzH.row(2);
+  } else
+    y = xyz()(2);
+  return y;
 }
 
 /* ************************************************************************* */
@@ -203,21 +255,62 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x)    {
 }
 
 /* ************************************************************************* */
-pair<Matrix3, Vector3> RQ(const Matrix3& A) {
-
-  double x = -atan2(-A(2, 1), A(2, 2));
-  Rot3 Qx = Rot3::Rx(-x);
-  Matrix3 B = A * Qx.matrix();
-
-  double y = -atan2(B(2, 0), B(2, 2));
-  Rot3 Qy = Rot3::Ry(-y);
-  Matrix3 C = B * Qy.matrix();
-
-  double z = -atan2(-C(1, 0), C(1, 1));
-  Rot3 Qz = Rot3::Rz(-z);
-  Matrix3 R = C * Qz.matrix();
+pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
+  const double x = -atan2(-A(2, 1), A(2, 2));
+  const auto Qx = Rot3::Rx(-x).matrix();
+  const Matrix3 B = A * Qx;
+
+  const double y = -atan2(B(2, 0), B(2, 2));
+  const auto Qy = Rot3::Ry(-y).matrix();
+  const Matrix3 C = B * Qy;
+
+  const double z = -atan2(-C(1, 0), C(1, 1));
+  const auto Qz = Rot3::Rz(-z).matrix();
+  const Matrix3 R = C * Qz;
+
+  if (H) {
+    if (std::abs(y - M_PI / 2) < 1e-2)
+      throw std::runtime_error(
+          "Rot3::RQ : Derivative undefined at singularity (gimbal lock)");
+
+    auto atan_d1 = [](double y, double x) { return x / (x * x + y * y); };
+    auto atan_d2 = [](double y, double x) { return -y / (x * x + y * y); };
+
+    const auto sx = -Qx(2, 1), cx = Qx(1, 1);
+    const auto sy = -Qy(0, 2), cy = Qy(0, 0);
+
+    *H = Matrix39::Zero();
+    // First, calculate the derivate of x
+    (*H)(0, 5) = atan_d1(A(2, 1), A(2, 2));
+    (*H)(0, 8) = atan_d2(A(2, 1), A(2, 2));
+
+    // Next, calculate the derivate of y. We have
+    // b20 = a20 and b22 = a21 * sx + a22 * cx
+    (*H)(1, 2) = -atan_d1(B(2, 0), B(2, 2));
+    const auto yHb22 = -atan_d2(B(2, 0), B(2, 2));
+    (*H)(1, 5) = yHb22 * sx;
+    (*H)(1, 8) = yHb22 * cx;
+
+    // Next, calculate the derivate of z. We have
+    // c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy
+    // c22 = a11 * cx - a12 * sx
+    const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
+    const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
+    Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);
+    c10HA[1] = cy;
+    c10HA[4] = sx * sy;
+    c10HA[7] = cx * sy;
+
+    const auto c11Hx = -A(1, 2) * cx - A(1, 1) * sx;
+    Vector9 c11HA = c11Hx * H->row(0);
+    c11HA[4] = cx;
+    c11HA[7] = -sx;
+
+    H->block<1, 9>(2, 0) =
+        atan_d1(C(1, 0), C(1, 1)) * c10HA + atan_d2(C(1, 0), C(1, 1)) * c11HA;
+  }
 
-  Vector xyz = Vector3(x, y, z);
+  const auto xyz = Vector3(x, y, z);
   return make_pair(R, xyz);
 }
 

gtsam/geometry/Rot3.h

@@ -441,43 +441,43 @@ namespace gtsam {
      * Use RQ to calculate xyz angle representation
      * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
      */
-    Vector3 xyz() const;
+    Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const;
 
     /**
      * Use RQ to calculate yaw-pitch-roll angle representation
      * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r)
      */
-    Vector3 ypr() const;
+    Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const;
 
     /**
      * Use RQ to calculate roll-pitch-yaw angle representation
      * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r)
      */
-    Vector3 rpy() const;
+    Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const;
 
     /**
      * Accessor to get to component of angle representations
      * NOTE: these are not efficient to get to multiple separate parts,
      * you should instead use xyz() or ypr()
      * TODO: make this more efficient
      */
-    inline double roll() const  { return xyz()(0); }
+    double roll(OptionalJacobian<1, 3> H = boost::none) const;
 
     /**
      * Accessor to get to component of angle representations
      * NOTE: these are not efficient to get to multiple separate parts,
      * you should instead use xyz() or ypr()
      * TODO: make this more efficient
      */
-    inline double pitch() const { return xyz()(1); }
+    double pitch(OptionalJacobian<1, 3> H = boost::none) const;
 
     /**
      * Accessor to get to component of angle representations
      * NOTE: these are not efficient to get to multiple separate parts,
      * you should instead use xyz() or ypr()
      * TODO: make this more efficient
      */
-    inline double yaw() const   { return xyz()(2); }
+    double yaw(OptionalJacobian<1, 3> H = boost::none) const;
 
     /// @}
     /// @name Advanced Interface
@@ -557,7 +557,8 @@ namespace gtsam {
    * @return an upper triangular matrix R
    * @return a vector [thetax, thetay, thetaz] in radians.
    */
-  GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
+  GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
+      const Matrix3& A, OptionalJacobian<3, 9> H = boost::none);
 
   template<>
   struct traits<Rot3> : public internal::LieGroup<Rot3> {};

gtsam/geometry/tests/testRot3.cpp

@@ -794,6 +794,122 @@ TEST(Rot3, Ypr_derivative) {
   CHECK(assert_equal(num_r, act_r));
 }
 
+/* ************************************************************************* */
+Vector3 RQ_proxy(Matrix3 const& R) {
+  const auto RQ_ypr = RQ(R);
+  return RQ_ypr.second;
+}
+
+TEST(Rot3, RQ_derivative) {
+  using VecAndErr = std::pair<Vector3, double>;
+  std::vector<VecAndErr> test_xyz;
+  // Test zeros and a couple of random values
+  test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
+  test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error});
+  test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error});
+  test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error});
+  test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error});
+  test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error});
+  test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error});
+  test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error});
+
+  // Test close to singularity
+  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8});
+  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8});
+  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4});
+  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4});
+
+  for (auto const& vec_err : test_xyz) {
+    auto const& xyz = vec_err.first;
+
+    const auto R = Rot3::RzRyRx(xyz).matrix();
+    const auto num = numericalDerivative11(RQ_proxy, R);
+    Matrix39 calc;
+    RQ(R, calc).second;
+
+    const auto err = vec_err.second;
+    CHECK(assert_equal(num, calc, err));
+  }
+}
+
+/* ************************************************************************* */
+Vector3 xyz_proxy(Rot3 const& R) { return R.xyz(); }
+
+TEST(Rot3, xyz_derivative) {
+  const auto aa = Vector3{-0.6, 0.3, 0.2};
+  const auto R = Rot3::Expmap(aa);
+  const auto num = numericalDerivative11(xyz_proxy, R);
+  Matrix3 calc;
+  R.xyz(calc);
+
+  CHECK(assert_equal(num, calc));
+}
+
+/* ************************************************************************* */
+Vector3 ypr_proxy(Rot3 const& R) { return R.ypr(); }
+
+TEST(Rot3, ypr_derivative) {
+  const auto aa = Vector3{0.1, -0.3, -0.2};
+  const auto R = Rot3::Expmap(aa);
+  const auto num = numericalDerivative11(ypr_proxy, R);
+  Matrix3 calc;
+  R.ypr(calc);
+
+  CHECK(assert_equal(num, calc));
+}
+
+/* ************************************************************************* */
+Vector3 rpy_proxy(Rot3 const& R) { return R.rpy(); }
+
+TEST(Rot3, rpy_derivative) {
+  const auto aa = Vector3{1.2, 0.3, -0.9};
+  const auto R = Rot3::Expmap(aa);
+  const auto num = numericalDerivative11(rpy_proxy, R);
+  Matrix3 calc;
+  R.rpy(calc);
+
+  CHECK(assert_equal(num, calc));
+}
+
+/* ************************************************************************* */
+double roll_proxy(Rot3 const& R) { return R.roll(); }
+
+TEST(Rot3, roll_derivative) {
+  const auto aa = Vector3{0.8, -0.8, 0.8};
+  const auto R = Rot3::Expmap(aa);
+  const auto num = numericalDerivative11(roll_proxy, R);
+  Matrix13 calc;
+  R.roll(calc);
+
+  CHECK(assert_equal(num, calc));
+}
+
+/* ************************************************************************* */
+double pitch_proxy(Rot3 const& R) { return R.pitch(); }
+
+TEST(Rot3, pitch_derivative) {
+  const auto aa = Vector3{0.01, 0.1, 0.0};
+  const auto R = Rot3::Expmap(aa);
+  const auto num = numericalDerivative11(pitch_proxy, R);
+  Matrix13 calc;
+  R.pitch(calc);
+
+  CHECK(assert_equal(num, calc));
+}
+
+/* ************************************************************************* */
+double yaw_proxy(Rot3 const& R) { return R.yaw(); }
+
+TEST(Rot3, yaw_derivative) {
+  const auto aa = Vector3{0.0, 0.1, 0.6};
+  const auto R = Rot3::Expmap(aa);
+  const auto num = numericalDerivative11(yaw_proxy, R);
+  Matrix13 calc;
+  R.yaw(calc);
+
+  CHECK(assert_equal(num, calc));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;