Merge pull request #499 from Eskilade/add_Rot3_RzRyRx_jacobian

Added Jacobians for Rot3::RzRyRx and related named constructors

gtsam/geometry/Rot3.h

@@ -154,12 +154,23 @@ namespace gtsam {
     static Rot3 Rz(double t);
 
     /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
-    static Rot3 RzRyRx(double x, double y, double z);
+    static Rot3 RzRyRx(double x, double y, double z,
+                       OptionalJacobian<3, 1> Hx = boost::none,
+                       OptionalJacobian<3, 1> Hy = boost::none,
+                       OptionalJacobian<3, 1> Hz = boost::none);
 
     /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
-    inline static Rot3 RzRyRx(const Vector& xyz) {
+    inline static Rot3 RzRyRx(const Vector& xyz,
+                              OptionalJacobian<3, 3> H = boost::none) {
       assert(xyz.size() == 3);
-      return RzRyRx(xyz(0), xyz(1), xyz(2));
+      Rot3 out;
+      if (H) {
+        Vector3 Hx, Hy, Hz;
+        out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
+        (*H) << Hx, Hy, Hz;
+      } else
+        out = RzRyRx(xyz(0), xyz(1), xyz(2));
+      return out;
     }
 
     /// Positive yaw is to right (as in aircraft heading). See ypr
@@ -185,7 +196,12 @@ namespace gtsam {
      * Positive pitch is down (decreasing aircraft altitude).
      * Positive roll is to right (decreasing yaw in aircraft).
      */
-    static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
+    static Rot3 Ypr(double y, double p, double r,
+                    OptionalJacobian<3, 1> Hy = boost::none,
+                    OptionalJacobian<3, 1> Hp = boost::none,
+                    OptionalJacobian<3, 1> Hr = boost::none) {
+      return RzRyRx(r, p, y, Hr, Hp, Hy);
+    }
 
     /** Create from Quaternion coefficients */
     static Rot3 Quaternion(double w, double x, double y, double z) {

gtsam/geometry/Rot3M.cpp

@@ -82,7 +82,8 @@ Rot3 Rot3::Rz(double t) {
 
 /* ************************************************************************* */
 // Considerably faster than composing matrices above !
-Rot3 Rot3::RzRyRx(double x, double y, double z) {
+Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
+                  OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) {
   double cx=cos(x),sx=sin(x);
   double cy=cos(y),sy=sin(y);
   double cz=cos(z),sz=sin(z);
@@ -97,6 +98,9 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
   double s_c = sx * cz;
   double c_c = cx * cz;
   double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
+  if (Hx) (*Hx) << 1, 0, 0;
+  if (Hy) (*Hy) << 0, cx, -sx;
+  if (Hz) (*Hz) << -sy, sc_, cc_;
   return Rot3(
       _cc,- c_s + ssc,  s_s + csc,
       _cs,  c_c + sss, -s_c + css,

gtsam/geometry/Rot3Q.cpp

@@ -67,7 +67,20 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
+  Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
+                    OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) {
+    if (Hx) (*Hx) << 1, 0, 0;
+
+    if (Hy or Hz) {
+      const auto cx = cos(x), sx = sin(x);
+      if (Hy) (*Hy) << 0, cx, -sx;
+      if (Hz) {
+        const auto cy = cos(y), sy = sin(y);
+        (*Hz) << -sy, sx * cy, cx * cy;
+      }
+    }
+
+    return Rot3(
         gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
         gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
         gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));

gtsam/geometry/tests/testRot3.cpp

@@ -743,6 +743,57 @@ TEST(Rot3, axisAngle) {
   CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195))
 }
 
+/* ************************************************************************* */
+Rot3 RzRyRx_proxy(double const& a, double const& b, double const& c) {
+  return Rot3::RzRyRx(a, b, c);
+}
+
+TEST(Rot3, RzRyRx_scalars_derivative) {
+  const auto x = 0.1, y = 0.4, z = 0.2;
+  const auto num_x = numericalDerivative31(RzRyRx_proxy, x, y, z);
+  const auto num_y = numericalDerivative32(RzRyRx_proxy, x, y, z);
+  const auto num_z = numericalDerivative33(RzRyRx_proxy, x, y, z);
+
+  Vector3 act_x, act_y, act_z;
+  Rot3::RzRyRx(x, y, z, act_x, act_y, act_z);
+
+  CHECK(assert_equal(num_x, act_x));
+  CHECK(assert_equal(num_y, act_y));
+  CHECK(assert_equal(num_z, act_z));
+}
+
+/* ************************************************************************* */
+Rot3 RzRyRx_proxy(Vector3 const& xyz) { return Rot3::RzRyRx(xyz); }
+
+TEST(Rot3, RzRyRx_vector_derivative) {
+  const auto xyz = Vector3{-0.3, 0.1, 0.7};
+  const auto num = numericalDerivative11(RzRyRx_proxy, xyz);
+
+  Matrix3 act;
+  Rot3::RzRyRx(xyz, act);
+
+  CHECK(assert_equal(num, act));
+}
+
+/* ************************************************************************* */
+Rot3 Ypr_proxy(double const& y, double const& p, double const& r) {
+  return Rot3::Ypr(y, p, r);
+}
+
+TEST(Rot3, Ypr_derivative) {
+  const auto y = 0.7, p = -0.3, r = 0.1;
+  const auto num_y = numericalDerivative31(Ypr_proxy, y, p, r);
+  const auto num_p = numericalDerivative32(Ypr_proxy, y, p, r);
+  const auto num_r = numericalDerivative33(Ypr_proxy, y, p, r);
+
+  Vector3 act_y, act_p, act_r;
+  Rot3::Ypr(y, p, r, act_y, act_p, act_r);
+
+  CHECK(assert_equal(num_y, act_y));
+  CHECK(assert_equal(num_p, act_p));
+  CHECK(assert_equal(num_r, act_r));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;