adds jacobians to Pose3::InterpolateRt

gtsam/geometry/Pose3.cpp

@@ -171,9 +171,29 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
 }
 
 /* ************************************************************************* */
-Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
+Pose3 Pose3::interpolateRt(const Pose3& T, double t,
+                           OptionalJacobian<6, 6> Hself,
+                           OptionalJacobian<6, 6> Harg,
+                           OptionalJacobian<6, 1> Ht) const {
+  if(Hself || Harg || Ht){
+    typename MakeJacobian<Rot3, Rot3>::type HselfRot, HargRot;
+    typename MakeJacobian<Rot3, double>::type HtRot;
+    typename MakeJacobian<Point3, Point3>::type HselfPoint, HargPoint;
+    typename MakeJacobian<Point3, double>::type HtPoint;
+
+    Rot3 Rint = interpolate<Rot3>(R_, T.R_, t, HselfRot, HargRot, HtRot);
+    Point3 Pint = interpolate<Point3>(t_, T.t_, t, HselfPoint, HargPoint, HtPoint);
+    Pose3 result = Pose3(Rint, Pint);
+
+    if(Hself) *Hself << HselfRot, Z_3x3, Z_3x3, Rint.transpose() * R_.matrix() * HselfPoint;
+    if(Harg) *Harg << HargRot, Z_3x3, Z_3x3, Rint.transpose() * T.R_.matrix() * HargPoint;
+    if(Ht) *Ht << HtRot, Rint.transpose() * HtPoint;
+
+    return result;
+  }
   return Pose3(interpolate<Rot3>(R_, T.R_, t),
                interpolate<Point3>(t_, T.t_, t));
+
 }
 
 /* ************************************************************************* */

gtsam/geometry/Pose3.h

@@ -132,7 +132,10 @@ public:
    * @param T End point of interpolation.
    * @param t A value in [0, 1].
    */
-  Pose3 interpolateRt(const Pose3& T, double t) const;
+  Pose3 interpolateRt(const Pose3& T, double t,
+                      OptionalJacobian<6, 6> Hself = {},
+                      OptionalJacobian<6, 6> Harg = {},
+                      OptionalJacobian<6, 1> Ht = {}) const;
 
   /// @}
   /// @name Lie Group

gtsam/geometry/tests/testPose3.cpp

@@ -1167,56 +1167,137 @@ TEST(Pose3, interpolateJacobians) {
     Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
     double t = 0.5;
     Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
-    Matrix actualJacobianX, actualJacobianY;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
-    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5));
 
     Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
 
     Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
   }
   {
     Pose3 X = Pose3::Identity();
     Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
     double t = 0.3;
     Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0));
-    Matrix actualJacobianX, actualJacobianY;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
-    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5));
 
     Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
 
     Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
   }
   {
     Pose3 X = Pose3::Identity();
     Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
     double t = 0.5;
     Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
-    Matrix actualJacobianX, actualJacobianY;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
-    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5));
 
     Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
 
     Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
   }
   {
     Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
     Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
     double t = 0.3;
     Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
-    Matrix actualJacobianX, actualJacobianY;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
-    interpolate(X, Y, t, actualJacobianX, actualJacobianY);
+    interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
 
     Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
 
     Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
     EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
+  }
+}
+/* ************************************************************************* */
+Pose3 testing_interpolate_rt(const Pose3& t1, const Pose3& t2, double gamma) { return t1.interpolateRt(t2, gamma); }
+
+TEST(Pose3, interpolateRtJacobians) {
+  {
+    Pose3 X = Pose3::Identity();
+    Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
+    double t = 0.5;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
+    X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
+  }
+  {
+    Pose3 X = Pose3::Identity();
+    Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
+    double t = 0.3;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
+    X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
+  }
+  {
+    Pose3 X = Pose3::Identity();
+    Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
+    double t = 0.5;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
+    X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
+  }
+  {
+    Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
+    Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
+    double t = 0.3;
+    Matrix actualJacobianX, actualJacobianY, actualJacobianT;
+    X.interpolateRt(Y, t, actualJacobianX, actualJacobianY, actualJacobianT);
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+
+    Matrix expectedJacobianT = numericalDerivative33<Pose3,Pose3,Pose3,double>(testing_interpolate_rt, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianT,actualJacobianT,1e-6));
   }
 }