added interpolation function from shteren1

borglab · Jul 20, 2021 · a0ca338 · a0ca338
1 parent 16d624d
commit a0ca338
Show file tree

Hide file tree

Showing 2 changed files with 87 additions and 2 deletions.
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
@@ -17,6 +17,7 @@
  * @author Frank Dellaert
  * @author Mike Bosse
  * @author Duy Nguyen Ta
+ * @author shteren1
  */
 
 
@@ -322,8 +323,30 @@ T expm(const Vector& x, int K=7) {
  * Linear interpolation between X and Y by coefficient t in [0, 1].
  */
 template <typename T>
-T interpolate(const T& X, const T& Y, double t) {
-  assert(t >= 0 && t <= 1);
+T interpolate(const T& X, const T& Y, double t,
+              OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hx = boost::none,
+              OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hy = boost::none) {
+  assert(t >= 0.0 && t <= 1.0);
+  if (Hx || Hy) {
+    typedef Eigen::Matrix<double, traits<T>::dimension, traits<T>::dimension> Jacobian;
+    typename traits<T>::TangentVector log_Xinv_Y;
+    Jacobian Hx_tmp, Hy_tmp, H1, H2;
+
+    T Xinv_Y = traits<T>::Between(X, Y, Hx_tmp, Hy_tmp);
+    log_Xinv_Y = traits<T>::Logmap(Xinv_Y, H1);
+    Hx_tmp = H1 * Hx_tmp;
+    Hy_tmp = H1 * Hy_tmp;
+    Xinv_Y = traits<T>::Expmap(t * log_Xinv_Y, H1);
+    Hx_tmp = t * H1 * Hx_tmp;
+    Hy_tmp = t * H1 * Hy_tmp;
+    Xinv_Y = traits<T>::Compose(X, Xinv_Y, H1, H2);
+    Hx_tmp = H1 + H2 * Hx_tmp;
+    Hy_tmp = H2 * Hy_tmp;
+
+    if(Hx) *Hx = Hx_tmp;
+    if(Hy) *Hy = Hy_tmp;
+    return Xinv_Y;
+  }
   return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
 }
 

diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
@@ -1046,6 +1046,68 @@ TEST(Pose3, interpolate) {
   EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
 }
 
+/* ************************************************************************* */
+Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); }
+
+TEST(Pose3, interpolateJacobians) {
+  {
+    Pose3 X = Pose3::identity();
+    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;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 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));
+  }
+  {
+    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;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 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));
+  }
+  {
+    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;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 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));
+  }
+  {
+    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;
+    interpolate(X, Y, t, actualJacobianX, actualJacobianY);
+
+    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));
+  }
+}
+
 /* ************************************************************************* */
 TEST(Pose3, Create) {
   Matrix63 actualH1, actualH2;