diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9007ce1bd..3507dc7af 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -715,7 +715,41 @@ TEST( Pose3, ExpmapDerivative1) { } /* ************************************************************************* */ -TEST( Pose3, LogmapDerivative1) { +TEST(Pose3, ExpmapDerivative2) { + // Iserles05an (Lie-group Methods) says: + // scalar is easy: d exp(a(t)) / dt = exp(a(t) a'(t) + // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) + // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3) + // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4) + + // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors. + // xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t) + + // Let's verify the above formula. + + auto xi = [](double t) { + Vector6 v; + v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t; + return v; + }; + auto xi_dot = [](double t) { + Vector6 v; + v << 2, cos(t), 8 * t, 2, cos(t), 8 * t; + return v; + }; + + // We define a function T + auto T = [xi](double t) { return Pose3::Expmap(xi(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(T, t); + const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST( Pose3, LogmapDerivative) { Matrix6 actualH; Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; Pose3 p = Pose3::Expmap(w); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a61467b82..a70ff9fc3 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -244,44 +244,74 @@ TEST(Rot3, retract_localCoordinates2) EXPECT(assert_equal(t1, t2.retract(d21))); } /* ************************************************************************* */ -Vector w = Vector3(0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) wrpt w: +namespace exmap_derivative { +static const Vector3 w(0.1, 0.27, -0.2); +} +// Left trivialized Derivative of exp(w) wrpt w: // How does exp(w) change when w changes? // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 // => y = log (exp(-w) * exp(w+dw)) Vector3 testDexpL(const Vector3& dw) { + using exmap_derivative::w; return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); } TEST( Rot3, ExpmapDerivative) { - Matrix actualDexpL = Rot3::ExpmapDerivative(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, + using exmap_derivative::w; + const Matrix actualDexpL = Rot3::ExpmapDerivative(w); + const Matrix expectedDexpL = numericalDerivative11(testDexpL, Vector3::Zero(), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - Matrix actualDexpInvL = Rot3::LogmapDerivative(w); + const Matrix actualDexpInvL = Rot3::LogmapDerivative(w); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); + } + +/* ************************************************************************* */ +TEST( Rot3, ExpmapDerivative2) +{ + const Vector3 thetahat(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + + const Matrix Jactual = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual)); + + const Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual2)); } /* ************************************************************************* */ -Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, ExpmapDerivative2) -{ - Matrix Jexpected = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); +TEST(Rot3, ExpmapDerivative3) { + // Iserles05an (Lie-group Methods) says: + // scalar is easy: d exp(a(t)) / dt = exp(a(t) a'(t) + // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) + // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) + // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) - Matrix Jactual = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual)); + // In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors. + // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) - Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual2)); + // Let's verify the above formula. + + auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; + auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + + // We define a function R + auto R = [w](double t) { return Rot3::Expmap(w(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(R, t); + const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } } /* ************************************************************************* */ TEST( Rot3, jacobianExpmap ) { - Matrix Jexpected = numericalDerivative11(boost::bind( + const Vector3 thetahat(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Expmap, _1, boost::none), thetahat); Matrix3 Jactual; const Rot3 R = Rot3::Expmap(thetahat, Jactual); @@ -291,18 +321,20 @@ TEST( Rot3, jacobianExpmap ) /* ************************************************************************* */ TEST( Rot3, LogmapDerivative ) { - Rot3 R = Rot3::Expmap(thetahat); // some rotation - Matrix Jexpected = numericalDerivative11(boost::bind( + const Vector3 thetahat(0.1, 0, 0.1); + const Rot3 R = Rot3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); + const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ -TEST( Rot3, jacobianLogmap ) +TEST( Rot3, JacobianLogmap ) { - Rot3 R = Rot3::Expmap(thetahat); // some rotation - Matrix Jexpected = numericalDerivative11(boost::bind( + const Vector3 thetahat(0.1, 0, 0.1); + const Rot3 R = Rot3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Logmap, _1, boost::none), R); Matrix3 Jactual; Rot3::Logmap(R, Jactual);