From f684becf1ff92212fd6bbf7de2f7d99b08e18d15 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 8 Jan 2010 03:06:03 +0000 Subject: [PATCH] Fixed Dbetween1 --- cpp/Pose3.cpp | 2 +- cpp/testPose3.cpp | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 3c65790f9..2c275da20 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -174,7 +174,7 @@ namespace gtsam { Matrix Dbetween1(const Pose3& p1, const Pose3& p2){ Pose3 invp1 = inverse(p1); - return Dinverse(p1) * Dcompose2(p2,invp1); + return Dcompose2(p2,invp1) * Dinverse(p1); } Matrix Dbetween2(const Pose3& p1, const Pose3& p2){ diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp index 0b8d8d4bf..f775e8bbc 100644 --- a/cpp/testPose3.cpp +++ b/cpp/testPose3.cpp @@ -283,13 +283,17 @@ 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 = pose1 * inverse(T); Pose3 actual = between(T, pose1); CHECK(assert_equal(expected,actual)); Matrix numericalH1 = numericalDerivative21(between , T, pose1, 1e-5); Matrix actualH1 = Dbetween1(T, pose1); -// CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ?? + CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ?? Matrix actualH2 = Dbetween2(T, pose1); Matrix numericalH2 = numericalDerivative22(between , T, pose1, 1e-5);