Merge pull request #2035 from BrettRD/develop

Adds Jacobians to Lie::interpolate and others

gtsam/base/Lie.h

@@ -326,8 +326,9 @@ T expm(const Vector& x, int K=7) {
 template <typename T>
 T interpolate(const T& X, const T& Y, double t,
               typename MakeOptionalJacobian<T, T>::type Hx = {},
-              typename MakeOptionalJacobian<T, T>::type Hy = {}) {
+              typename MakeOptionalJacobian<T, T>::type Hy = {},
-  if (Hx || Hy) {
+              typename MakeOptionalJacobian<T, double>::type Ht = {}) {
+  if (Hx || Hy || Ht) {
     typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
     const T between =
         traits<T>::Between(X, Y, between_H_x);  // between_H_y = identity
@@ -338,6 +339,7 @@ T interpolate(const T& X, const T& Y, double t,
 
     if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
     if (Hy) *Hy = t * exp_H * log_H;
+    if (Ht) *Ht = delta;
     return result;
   }
   return traits<T>::Compose(

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

@@ -19,6 +19,7 @@
 #include <gtsam/base/testLie.h>
 #include <gtsam/base/lieProxies.h>
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/slam/expressions.h>
 
 
 #include <CppUnitLite/TestHarness.h>
@@ -1167,56 +1168,170 @@ 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));
+  }
+}
+
+TEST(Pose3, expressionWrappers) {
+  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;
+  Values vals;
+  vals.insert(0,X);
+  vals.insert(1,Y);
+  vals.insert(2,t);
+  
+  { // interpolate (templated wrapper applies to all classes)
+    Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT;
+    std::vector<Matrix> Hlist = {{},{},{}};
+    Pose3 expected = interpolate(X, Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT);
+    Pose3 actual = interpolate(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist);
+
+    EXPECT(assert_equal(expected,actual,1e-6));
+    EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6));
+    EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6));
+    EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6));
+  }
+  { // interpolateRt (Pose3 specialisation)
+    Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT;
+    std::vector<Matrix> Hlist = {{},{},{}};
+    Pose3 expected = X.interpolateRt(Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT);
+    Pose3 actual = interpolateRt(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist);
+
+    EXPECT(assert_equal(expected,actual,1e-6));
+    EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6));
+    EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6));
+    EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6));
   }
 }
 

gtsam/slam/expressions.h

@@ -58,6 +58,10 @@ inline Pose3_ transformPoseTo(const Pose3_& p, const Pose3_& q) {
   return Pose3_(p, &Pose3::transformPoseTo, q);
 }
 
+inline Pose3_ interpolateRt(const Pose3_& p, const Pose3_& q, const Double_& t) {
+  return Pose3_(&Pose3::interpolateRt, p, q, t);
+}
+
 inline Point3_ normalize(const Point3_& a){
   Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize;
   return Point3_(f, a);
@@ -195,4 +199,14 @@ gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
       gtsam::traits<T>::Logmap, between(x1, x2));
 }
 
+template <typename T>
+inline Expression<T> interpolate(const Expression<T>& p, const Expression<T>& q,
+                                 const Expression<double>& t){
+  T (*f)(const T&, const T&, double,
+         typename MakeOptionalJacobian<T, T>::type,
+         typename MakeOptionalJacobian<T, T>::type,
+         typename MakeOptionalJacobian<T, double>::type) = &interpolate;
+  return Expression<T>(f, p, q, t);
+}
+
 }  // \namespace gtsam

tests/testLie.cpp

@@ -139,16 +139,19 @@ TEST(Lie, Interpolate) {
   Product y(Point2(6, 7), Pose2(8, 9, 0));
 
   double t;
-  Matrix actH1, numericH1, actH2, numericH2;
+  Matrix actH1, numericH1, actH2, numericH2, actH3, numericH3;
 
   t = 0.0;
-  interpolate<Product>(x, y, t, actH1, actH2);
+  interpolate<Product>(x, y, t, actH1, actH2, actH3);
   numericH1 = numericalDerivative31<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH1, actH1, tol));
   numericH2 = numericalDerivative32<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH2, actH2, tol));
+  numericH3 = numericalDerivative33<Product, Product, Product, double>(
+      interpolate_proxy, x, y, t);
+  EXPECT(assert_equal(numericH3, actH3, tol));
 
   t = 0.5;
   interpolate<Product>(x, y, t, actH1, actH2);
@@ -158,6 +161,9 @@ TEST(Lie, Interpolate) {
   numericH2 = numericalDerivative32<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH2, actH2, tol));
+  numericH3 = numericalDerivative33<Product, Product, Product, double>(
+      interpolate_proxy, x, y, t);
+  EXPECT(assert_equal(numericH3, actH3, tol));
 
   t = 1.0;
   interpolate<Product>(x, y, t, actH1, actH2);
@@ -167,6 +173,9 @@ TEST(Lie, Interpolate) {
   numericH2 = numericalDerivative32<Product, Product, Product, double>(
       interpolate_proxy, x, y, t);
   EXPECT(assert_equal(numericH2, actH2, tol));
+  numericH3 = numericalDerivative33<Product, Product, Product, double>(
+      interpolate_proxy, x, y, t);
+  EXPECT(assert_equal(numericH3, actH3, tol));
 }
 
 //******************************************************************************