diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 862ae6f4d..ce6541d21 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -326,8 +326,9 @@ T expm(const Vector& x, int K=7) { template T interpolate(const T& X, const T& Y, double t, typename MakeOptionalJacobian::type Hx = {}, - typename MakeOptionalJacobian::type Hy = {}) { - if (Hx || Hy) { + typename MakeOptionalJacobian::type Hy = {}, + typename MakeOptionalJacobian::type Ht = {}) { + if (Hx || Hy || Ht) { typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; const T between = traits::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::Compose( diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f6d0c43cb..337037676 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/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::type HselfRot, HargRot; + typename MakeJacobian::type HtRot; + typename MakeJacobian::type HselfPoint, HargPoint; + typename MakeJacobian::type HtPoint; + + Rot3 Rint = interpolate(R_, T.R_, t, HselfRot, HargRot, HtRot); + Point3 Pint = interpolate(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(R_, T.R_, t), interpolate(t_, T.t_, t)); + } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index af14ac82c..dd2030567 100644 --- a/gtsam/geometry/Pose3.h +++ b/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 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index b04fb1982..b1df45004 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include @@ -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; - EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5)); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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; - EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5)); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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; - EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT), 1e-5)); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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; - interpolate(X, Y, t, actualJacobianX, actualJacobianY); + Matrix actualJacobianX, actualJacobianY, actualJacobianT; + interpolate(X, Y, t, actualJacobianX, actualJacobianY, actualJacobianT); Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate_rt, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + + Matrix expectedJacobianT = numericalDerivative33(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 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 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)); } } diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 8d34ce681..5a65810aa 100644 --- a/gtsam/slam/expressions.h +++ b/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::TangentVector> logmap( gtsam::traits::Logmap, between(x1, x2)); } +template +inline Expression interpolate(const Expression& p, const Expression& q, + const Expression& t){ + T (*f)(const T&, const T&, double, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) = &interpolate; + return Expression(f, p, q, t); +} + } // \namespace gtsam diff --git a/tests/testLie.cpp b/tests/testLie.cpp index ae934d66a..9272542fb 100644 --- a/tests/testLie.cpp +++ b/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(x, y, t, actH1, actH2); + interpolate(x, y, t, actH1, actH2, actH3); numericH1 = numericalDerivative31( interpolate_proxy, x, y, t); EXPECT(assert_equal(numericH1, actH1, tol)); numericH2 = numericalDerivative32( interpolate_proxy, x, y, t); EXPECT(assert_equal(numericH2, actH2, tol)); + numericH3 = numericalDerivative33( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH3, actH3, tol)); t = 0.5; interpolate(x, y, t, actH1, actH2); @@ -158,6 +161,9 @@ TEST(Lie, Interpolate) { numericH2 = numericalDerivative32( interpolate_proxy, x, y, t); EXPECT(assert_equal(numericH2, actH2, tol)); + numericH3 = numericalDerivative33( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH3, actH3, tol)); t = 1.0; interpolate(x, y, t, actH1, actH2); @@ -167,6 +173,9 @@ TEST(Lie, Interpolate) { numericH2 = numericalDerivative32( interpolate_proxy, x, y, t); EXPECT(assert_equal(numericH2, actH2, tol)); + numericH3 = numericalDerivative33( + interpolate_proxy, x, y, t); + EXPECT(assert_equal(numericH3, actH3, tol)); } //******************************************************************************