From e66fcf8612fcc9a6a20eff1339a4b44a374276b6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 01:27:18 -0800 Subject: [PATCH] New tests --- gtsam/geometry/tests/testPoint3.cpp | 11 ++++ gtsam/geometry/tests/testPose3.cpp | 37 ++++++++++- gtsam/geometry/tests/testRot3.cpp | 99 +++++++++++++++++++++++------ 3 files changed, 125 insertions(+), 22 deletions(-) diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 6811ed122..246d23c9a 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -133,6 +133,17 @@ TEST (Point3, distance) { EXPECT(assert_equal(numH2, H2, 1e-8)); } +/* ************************************************************************* */ +TEST(Point3, cross) { + Matrix aH1, aH2; + boost::function f = + boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); + const Point3 omega(0, 1, 0), theta(4, 6, 8); + omega.cross(theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index fb3907df3..56e1e022c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -715,7 +715,42 @@ 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) + // and dexp[A] is a linear map from 4*4 to 4*4 derivatives 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 3cf3f9387..25ddd16ce 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -244,44 +244,99 @@ 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 theta(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), theta); + + CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); } /* ************************************************************************* */ -Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, ExpmapDerivative2) +TEST( Rot3, ExpmapDerivative3) { - Matrix Jexpected = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + const Vector3 theta(10, 20, 30); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), theta); - Matrix Jactual = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual)); + CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); +} - Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual2)); +/* ************************************************************************* */ +TEST(Rot3, ExpmapDerivative4) { + // 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) + // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) + // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) + + // 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) + + // 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, ExpmapDerivative5) { + 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); }; + + // Same as above, but define R as mapping local coordinates to neighborhood aroud R0. + const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2); + auto R = [R0, w](double t) { return R0.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 +346,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);