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);