From 190e87afb9990881cf03a153d8408a154cb14a22 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 2 Mar 2010 17:56:26 +0000 Subject: [PATCH] Correct exmap is indeed correct, but derivatives *still* do not check out. I'm starting to suspect numericalDerivative. --- cpp/Pose3.cpp | 31 +++++---- cpp/Pose3.h | 7 ++ cpp/testPose3.cpp | 161 +++++++++++++++++++++++++++++++++------------- 3 files changed, 144 insertions(+), 55 deletions(-) diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index f09f7b7d8..337b09cc5 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -46,17 +46,24 @@ namespace gtsam { #ifdef SLOW_BUT_CORRECT_EXPMAP - /** Agrawal06iros versions of expmap and logmap*/ - template<> Pose3 expmap(const Vector& d) { - Vector w = vector_range(d, range(0,3)); - Vector u = vector_range(d, range(3,6)); - double t = norm_2(w); - if (t < 1e-5) - return Pose3(Rot3(), expmap (u)); + /** Modified from Murray94book version (which assumes w and v normalized?) */ + template<> Pose3 expmap(const Vector& xi) { + + // get angular velocity omega and translational velocity v from twist xi + Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5)); + + double theta = norm(w); + if (theta < 1e-5) { + static const Rot3 I; + return Pose3(I, v); + } else { - Matrix W = skewSymmetric(w/t); - Matrix A = I3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(expmap (w), expmap (A * u)); + Point3 n(w/theta); // axis unit vector + Rot3 R = rodriguez(n.vector(),theta); + double vn = dot(n,v); // translation parallel to n + Point3 n_cross_v = cross(n,v); // points towards axis + Point3 t = (n_cross_v - R*n_cross_v)/theta + vn*n; + return Pose3(R, t); } } @@ -67,7 +74,7 @@ namespace gtsam { return concatVectors(2, &w, &T); else { Matrix W = skewSymmetric(w/t); - Matrix Ainv = I3 - 0.5*t* W + ((2*sin(t)-t*(1+cos(t)))/2*sin(t)) * (W * W); + Matrix Ainv = I3 - (0.5*t)*W + ((2*sin(t)-t*(1+cos(t)))/(2*sin(t))) * (W * W); Vector u = Ainv*T; return concatVectors(2, &w, &u); } @@ -152,7 +159,7 @@ namespace gtsam { Matrix Dcompose1(const Pose3& p1, const Pose3& p2) { #ifdef SLOW_BUT_CORRECT_EXPMAP // actually does NOT pan out at the moment - return AdjointMap(p2); + return AdjointMap(inverse(p2)); #else const Rot3& R2 = p2.rotation(); const Point3& t2 = p2.translation(); diff --git a/cpp/Pose3.h b/cpp/Pose3.h index 61ba2b585..85fe1261e 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -172,4 +172,11 @@ namespace gtsam { return wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); } + /** + * Calculate Adjoint map + * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) + */ + Matrix AdjointMap(const Pose3& p); + inline Vector Adjoint(const Pose3& p, const Vector& xi) {return AdjointMap(p)*xi; } + } // namespace gtsam diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp index 73b33fdec..cfd6555e0 100644 --- a/cpp/testPose3.cpp +++ b/cpp/testPose3.cpp @@ -3,30 +3,29 @@ * @brief Unit tests for Pose3 class */ +#include #include #include "numericalDerivative.h" #include "Pose3.h" +using namespace std; using namespace gtsam; -Rot3 R = rodriguez(0.3,0,0); -Point3 t(3.5,-8.2,4.2); -Pose3 T(R,t); -Point3 P(0.2,0.7,-2); -Rot3 r1 = rodriguez(-90, 0, 0); -Pose3 pose1(r1, Point3(1, 2, 3)); +static Point3 P(0.2,0.7,-2); +static Rot3 R = rodriguez(0.3,0,0); +static Pose3 T(R,Point3(3.5,-8.2,4.2)); +static Pose3 T2(rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); +static Pose3 T3(rodriguez(-90, 0, 0), Point3(1, 2, 3)); + double error = 1e-8; -#define PI 3.14159265358979323846 - - /* ************************************************************************* */ TEST( Pose3, equals) { - Pose3 pose2 = pose1; - CHECK(pose1.equals(pose2)); + Pose3 pose2 = T3; + CHECK(T3.equals(pose2)); Pose3 origin; - CHECK(!pose1.equals(origin)); + CHECK(!T3.equals(origin)); } /* ************************************************************************* */ @@ -50,13 +49,14 @@ TEST(Pose3, expmap_b) CHECK(assert_equal(expected, p2)); } -/* ************************************************************************* * +#ifdef SLOW_BUT_CORRECT_EXPMAP +/* ************************************************************************* */ // test case for screw motion in the plane namespace screw { double a=0.3, c=cos(a), s=sin(a), w=0.3; - Vector xi = Vector_(6, 0.0, 0.0, w, w, 0.0, 0.0); + Vector xi = Vector_(6, 0.0, 0.0, w, w, 0.0, 1.0); Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); - Point3 expectedT(0.29552, 0.0446635, 0); + Point3 expectedT(0.29552, 0.0446635, 1); Pose3 expected(expectedR, expectedT); } @@ -66,34 +66,114 @@ TEST(Pose3, expmap_c) CHECK(assert_equal(screw::expected, expmap(screw::xi),1e-6)); } +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) TEST(Pose3, Adjoint) { - // assert that T*exp(xi)*T^-1 Pose3 expected = T * expmap(screw::xi) * inverse(T); - // is equal to exp(Ad_T(xi)) Vector xiprime = Adjoint(T, screw::xi); CHECK(assert_equal(expected, expmap(xiprime), 1e-6)); + + Pose3 expected2 = T2 * expmap(screw::xi) * inverse(T2); + Vector xiprime2 = Adjoint(T2, screw::xi); + CHECK(assert_equal(expected2, expmap(xiprime2), 1e-6)); + + Pose3 expected3 = T3 * expmap(screw::xi) * inverse(T3); + Vector xiprime3 = Adjoint(T3, screw::xi); + CHECK(assert_equal(expected3, expmap(xiprime3), 1e-6)); } +/* ************************************************************************* */ +/** Agrawal06iros version */ +using namespace boost::numeric::ublas; +Pose3 Agrawal06iros(const Vector& xi) { + Vector w = vector_range(xi, range(0,3)); + Vector v = vector_range(xi, range(3,6)); + double t = norm_2(w); + if (t < 1e-5) + return Pose3(Rot3(), expmap (v)); + else { + Matrix W = skewSymmetric(w/t); + Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); + return Pose3(expmap (w), expmap (A * v)); + } +} + +/* ************************************************************************* */ +TEST(Pose3, expmaps_galore) +{ + Vector xi; Pose3 actual; + xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); + actual = expmap(xi); + CHECK(assert_equal(expm(xi), actual,1e-6)); + CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); + CHECK(assert_equal(xi, logmap(actual),1e-6)); + + xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); + for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { + Vector txi = xi*theta; + actual = expmap(txi); + CHECK(assert_equal(expm(txi,30), actual,1e-6)); + CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6)); + Vector log = logmap(actual); + CHECK(assert_equal(actual, expmap(log),1e-6)); + CHECK(assert_equal(txi,log,1e-6)); // not true once wraps + } + + // Works with large v as well, but expm needs 10 iterations! + xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); + actual = expmap(xi); + CHECK(assert_equal(expm(xi,10), actual,1e-5)); + CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6)); + CHECK(assert_equal(xi, logmap(actual),1e-6)); +} + +/* ************************************************************************* */ +TEST(Pose3, Adjoint_compose) +{ + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const Pose3& T1 = T; + Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); + Pose3 expected = T1 * expmap(x) * T2; + Vector y = Adjoint(inverse(T2), x); + Pose3 actual = T1 * T2 * expmap(y); + CHECK(assert_equal(expected, actual, 1e-6)); +} +#endif // SLOW_BUT_CORRECT_EXMAP + /* ************************************************************************* */ TEST( Pose3, compose ) { - Rot3 R = rodriguez(0.3,0.2,0.1); - Point3 t(3.5,-8.2,4.2); - Pose3 T(R,t); - - Matrix actual = (T*T).matrix(); - Matrix expected = T.matrix()*T.matrix(); + Matrix actual = (T2*T2).matrix(); + Matrix expected = T2.matrix()*T2.matrix(); CHECK(assert_equal(actual,expected,error)); - Matrix numericalH1 = numericalDerivative21(compose, T, T, 1e-5); - Matrix actualDcompose1 = Dcompose1(T, T); + Matrix numericalH1 = numericalDerivative21(compose, T2, T2, 1e-5); + Matrix actualDcompose1 = Dcompose1(T2, T2); CHECK(assert_equal(numericalH1,actualDcompose1)); - Matrix actualDcompose2 = Dcompose2(T, T); - Matrix numericalH2 = numericalDerivative22(compose, T, T, 1e-5); - CHECK(assert_equal(numericalH2,actualDcompose2));} + Matrix actualDcompose2 = Dcompose2(T2, T2); + Matrix numericalH2 = numericalDerivative22(compose, T2, T2, 1e-5); + CHECK(assert_equal(numericalH2,actualDcompose2)); +} +/* ************************************************************************* */ +TEST( Pose3, compose2 ) +{ + const Pose3& T1 = T; + Matrix actual = (T1*T2).matrix(); + Matrix expected = T1.matrix()*T2.matrix(); + CHECK(assert_equal(actual,expected,error)); + + Matrix numericalH1 = numericalDerivative21(compose, T1, T2, 1e-5); + Matrix actualDcompose1 = Dcompose1(T1, T2); + CHECK(assert_equal(numericalH1,actualDcompose1)); + + Matrix actualDcompose2 = Dcompose2(T1, T2); + Matrix numericalH2 = numericalDerivative22(compose, T1, T2, 1e-5); + CHECK(assert_equal(numericalH2,actualDcompose2)); +} /* ************************************************************************* */ TEST( Pose3, inverse) { @@ -216,7 +296,7 @@ TEST( Pose3, transform_to) /* ************************************************************************* */ TEST( Pose3, transform_from) { - Point3 actual = transform_from(pose1, Point3()); + Point3 actual = transform_from(T3, Point3()); Point3 expected = Point3(1.,2.,3.); CHECK(assert_equal(expected, actual)); } @@ -224,7 +304,7 @@ TEST( Pose3, transform_from) /* ************************************************************************* */ TEST( Pose3, transform_roundtrip) { - Point3 actual = transform_from(pose1, transform_to(pose1, Point3(12., -0.11,7.0))); + Point3 actual = transform_from(T3, transform_to(T3, Point3(12., -0.11,7.0))); Point3 expected(12., -0.11,7.0); CHECK(assert_equal(expected, actual)); } @@ -233,15 +313,15 @@ TEST( Pose3, transform_roundtrip) TEST( Pose3, transformPose_to_origin) { // transform to origin - Pose3 actual = pose1.transform_to(Pose3()); - CHECK(assert_equal(pose1, actual, error)); + Pose3 actual = T3.transform_to(Pose3()); + CHECK(assert_equal(T3, actual, error)); } /* ************************************************************************* */ TEST( Pose3, transformPose_to_itself) { // transform to itself - Pose3 actual = pose1.transform_to(pose1); + Pose3 actual = T3.transform_to(T3); CHECK(assert_equal(Pose3(), actual, error)); } @@ -286,7 +366,7 @@ TEST(Pose3, manifold) { //cout << "manifold" << endl; Pose3 t1 = T; - Pose3 t2 = pose1; + Pose3 t2 = T3; Pose3 origin; Vector d12 = logmap(t1, t2); CHECK(assert_equal(t2, expmap(t1,d12))); @@ -320,23 +400,18 @@ TEST(Pose3, manifold) /* ************************************************************************* */ TEST( Pose3, between ) { - Rot3 R = rodriguez(0.3,0.2,0.1); - Point3 t(3.5,-8.2,4.2); - Pose3 T(R,t); - - Pose3 expected = inverse(T) * pose1; + Pose3 expected = inverse(T2) * T3; Matrix actualDBetween1,actualDBetween2; - Pose3 actual = between(T, pose1, actualDBetween1,actualDBetween2); + Pose3 actual = between(T2, T3, actualDBetween1,actualDBetween2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(between , T, pose1, 1e-5); + Matrix numericalH1 = numericalDerivative21(between , T2, T3, 1e-5); CHECK(assert_equal(numericalH1,actualDBetween1)); // chain rule does not work ?? - Matrix numericalH2 = numericalDerivative22(between , T, pose1, 1e-5); + Matrix numericalH2 = numericalDerivative22(between , T2, T3, 1e-5); CHECK(assert_equal(numericalH2,actualDBetween2)); } /* ************************************************************************* */ int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ -