diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 7954ff17e..0976a7c1e 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -44,7 +44,7 @@ namespace gtsam { /* ************************************************************************* */ -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP /** Modified from Murray94book version (which assumes w and v normalized?) */ template<> Pose3 expmap(const Vector& xi) { @@ -144,7 +144,7 @@ namespace gtsam { /* ************************************************************************* */ Matrix Dtransform_from1(const Pose3& pose, const Point3& p) { -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP const Matrix R = pose.rotation().matrix(); Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); return collect(2,&DR,&R); @@ -170,7 +170,7 @@ namespace gtsam { Matrix Dtransform_to1(const Pose3& pose, const Point3& p) { Point3 q = transform_to(pose,p); Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP return collect(2, &DR, &_I3); #else Matrix DT = - pose.rotation().transpose(); // negative because of sub @@ -186,7 +186,7 @@ namespace gtsam { /* ************************************************************************* */ // compose = Pose3(compose(R1,R2),transform_from(p1,t2) Matrix Dcompose1(const Pose3& p1, const Pose3& p2) { -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP return AdjointMap(inverse(p2)); #else const Rot3& R2 = p2.rotation(); @@ -199,7 +199,7 @@ namespace gtsam { } Matrix Dcompose2(const Pose3& p1, const Pose3& p2) { -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP return I6; #else @@ -214,7 +214,7 @@ namespace gtsam { // inverse = Pose3(inverse(R),-unrotate(R,t)); // TODO: combined function will save ! Matrix Dinverse(const Pose3& p) { -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP return - AdjointMap(p); #else const Rot3& R = p.rotation(); diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp index e2974df77..c25a9571d 100644 --- a/cpp/testPose3.cpp +++ b/cpp/testPose3.cpp @@ -34,7 +34,7 @@ TEST( Pose3, expmap_a) fill(v.begin(), v.end(), 0); v(0) = 0.3; CHECK(assert_equal(expmap(id,v), Pose3(R, Point3()))); -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; #else @@ -52,7 +52,7 @@ TEST(Pose3, expmap_b) CHECK(assert_equal(expected, p2)); } -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP /* ************************************************************************* */ // test case for screw motion in the plane @@ -384,7 +384,7 @@ TEST(Pose3, manifold) // Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-) CHECK(assert_equal(d12,-d21)); -#ifndef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP // todo: Frank - Below only works for correct "Agrawal06iros style expmap diff --git a/cpp/testPose3Config.cpp b/cpp/testPose3Config.cpp index b97c45300..07b5e7eee 100644 --- a/cpp/testPose3Config.cpp +++ b/cpp/testPose3Config.cpp @@ -36,17 +36,17 @@ TEST( Pose3Config, pose3Circle ) TEST( Pose3Config, expmap ) { Pose3Config expected; -#ifdef FASTER_BUT_INCORRECT_EXMAP +#ifdef CORRECT_POSE3_EXMAP + expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); + expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); + expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); + expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); +#else // expected is circle shifted to East expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0))); expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0))); expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0))); expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0))); -#else - expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); #endif // Note expmap coordinates are in global coordinates with non-compose expmap