diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index f94c3f58d..b07750068 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -113,13 +113,14 @@ namespace gtsam { /* ************************************************************************* */ Matrix Dtransform_from1(const Pose3& pose, const Point3& p) { -#ifdef NEW_EXMAP - Point3 q = transform_from(pose,p); - Matrix DR = skewSymmetric(-q.x(), -q.y(), -q.z()); +#ifdef SLOW_BUT_CORRECT_EXPMAP + const Matrix R = pose.rotation().matrix(); + Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); + return collect(2,&DR,&R); #else Matrix DR = Drotate1(pose.rotation(), p); -#endif return collect(2,&DR,&I3); +#endif } /* ************************************************************************* */ diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp index 5cbf3e68b..57e497c82 100644 --- a/cpp/testPose3.cpp +++ b/cpp/testPose3.cpp @@ -105,26 +105,26 @@ TEST( Pose3, compose_inverse) /* ************************************************************************* */ TEST( Pose3, Dtransform_from1_a) { - Matrix computed = Dtransform_from1(T, P); + Matrix actualDtransform_from1 = Dtransform_from1(T, P); Matrix numerical = numericalDerivative21(transform_from,T,P); - CHECK(assert_equal(numerical,computed,error)); + CHECK(assert_equal(numerical,actualDtransform_from1,error)); } TEST( Pose3, Dtransform_from1_b) { Pose3 origin; - Matrix computed = Dtransform_from1(origin, P); + Matrix actualDtransform_from1 = Dtransform_from1(origin, P); Matrix numerical = numericalDerivative21(transform_from,origin,P); - CHECK(assert_equal(numerical,computed,error)); + CHECK(assert_equal(numerical,actualDtransform_from1,error)); } TEST( Pose3, Dtransform_from1_c) { Point3 origin; Pose3 T0(R,origin); - Matrix computed = Dtransform_from1(T0, P); + Matrix actualDtransform_from1 = Dtransform_from1(T0, P); Matrix numerical = numericalDerivative21(transform_from,T0,P); - CHECK(assert_equal(numerical,computed,error)); + CHECK(assert_equal(numerical,actualDtransform_from1,error)); } TEST( Pose3, Dtransform_from1_d) @@ -132,19 +132,19 @@ TEST( Pose3, Dtransform_from1_d) Rot3 I; Point3 t0(100,0,0); Pose3 T0(I,t0); - Matrix computed = Dtransform_from1(T0, P); + Matrix actualDtransform_from1 = Dtransform_from1(T0, P); //print(computed, "Dtransform_from1_d computed:"); Matrix numerical = numericalDerivative21(transform_from,T0,P); //print(numerical, "Dtransform_from1_d numerical:"); - CHECK(assert_equal(numerical,computed,error)); + CHECK(assert_equal(numerical,actualDtransform_from1,error)); } /* ************************************************************************* */ TEST( Pose3, Dtransform_from2) { - Matrix computed = Dtransform_from2(T); + Matrix actualDtransform_from2 = Dtransform_from2(T); Matrix numerical = numericalDerivative22(transform_from,T,P); - CHECK(assert_equal(numerical,computed,error)); + CHECK(assert_equal(numerical,actualDtransform_from2,error)); } /* ************************************************************************* */