From 0ba97cc39ab7c520ac300295ac36a5e1e6adaad9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 6 Jan 2012 20:44:06 +0000 Subject: [PATCH] Mathematica closed form optimization (procrastination?) gone wild --- gtsam/geometry/Rot3M.cpp | 32 +++++++++++++++++++------------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 837501343..5cc637a47 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -243,12 +243,12 @@ Rot3 Rot3::retract(const Vector& omega) const { #else #ifdef SLOW_CAYLEY Matrix Omega = skewSymmetric(omega); - return (*this)*Cayley(-Omega/2); + return (*this)*Cayley<3>(-Omega/2); #else - double x = omega(0), y = omega(1), z = omega(2); - double x2 = x*x, y2 = y*y, z2 = z*z; - double xy = x*y, xz = x*z, yz = y*z; - double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f; + const double x = omega(0), y = omega(1), z = omega(2); + const double x2 = x*x, y2 = y*y, z2 = z*z; + const double xy = x*y, xz = x*z, yz = y*z; + const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f; return (*this)* Rot3( (4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f, (xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f, @@ -263,19 +263,25 @@ Vector Rot3::localCoordinates(const Rot3& T) const { #ifdef CORRECT_ROT3_EXMAP return Logmap(between(T)); #else - // original -// Matrix Omega = Cayley(between(T).matrix()); - // Create a fixed-size matrix Eigen::Matrix3d A(between(T).matrix()); - +#ifdef SLOW_CAYLEY // using templated version of Cayley Matrix Omega = Cayley<3>(A); - - // completely inlined Cayley from template -// Eigen::Matrix3d Omega = (Eigen::Matrix3d::Identity() - A)*(Eigen::Matrix3d::Identity() + A).inverse(); - return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); +#else + // 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 * Vector_(3, x, y, z); +#endif #endif }