diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 77d97219c..e0d3086e9 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -270,23 +270,23 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const return Logmap(between(T)); } else if(mode == Rot3::CAYLEY) { // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); + Matrix3 A = rot_.transpose() * T.matrix(); // Mathematica closed form optimization (procrastination?) gone wild: const double a=A(0,0),b=A(0,1),c=A(0,2); const double d=A(1,0),e=A(1,1),f=A(1,2); const double g=A(2,0),h=A(2,1),i=A(2,2); const double di = d*i, ce = c*e, cd = c*d, fg=f*g; const double M = 1 + e - f*h + i + e*i; - const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = (a * f - cd + f) * K; - const double y = (b * f - ce - c) * K; - const double z = (fg - di - d) * K; - return -2 * Vector3(x, y, z); + const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } else if(mode == Rot3::SLOW_CAYLEY) { // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); + Matrix3 A(between(T).matrix()); // using templated version of Cayley - Eigen::Matrix3d Omega = CayleyFixed<3>(A); + Matrix3 Omega = CayleyFixed<3>(A); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); } else { assert(false); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 08f5a42e9..24a092fbe 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -145,10 +145,15 @@ TEST( Rot3, rodriguez4) } /* ************************************************************************* */ -TEST( Rot3, expmap) +TEST( Rot3, retract) { Vector v = zero(3); - CHECK(assert_equal(R.retract(v), R)); + CHECK(assert_equal(R, R.retract(v))); + + // test Canonical coordinates + Canonical chart; + Vector v2 = chart.apply(R); + CHECK(assert_equal(R, chart.retract(v2))); } /* ************************************************************************* */