From ea4e9a5ac67a7113324741a820618049be3b1574 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Wed, 24 Dec 2014 14:00:33 -0500 Subject: [PATCH 1/7] small code optim --- gtsam/geometry/Pose2.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 699994c3b..8e5d58047 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -137,6 +137,7 @@ Matrix3 Pose2::adjointMap(const Vector& v) { /* ************************************************************************* */ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { double alpha = v[2]; + Matrix3 J; if (fabs(alpha) > 1e-5) { // Chirikjian11book2, pg. 36 /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26 @@ -149,7 +150,7 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { */ double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; - return (Matrix(3,3) << + J = (Matrix(3,3) << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, 0, 0, 1).finished(); @@ -157,30 +158,34 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { else { // Thanks to Krunal: Apply L'Hospital rule to several times to // compute the limits when alpha -> 0 - return (Matrix(3,3) << 1,0,-0.5*v[1], + J = (Matrix(3,3) << 1,0,-0.5*v[1], 0,1, 0.5*v[0], 0,0, 1).finished(); } + + return J; } /* ************************************************************************* */ Matrix3 Pose2::LogmapDerivative(const Vector3& v) { double alpha = v[2]; + Matrix3 J; if (fabs(alpha) > 1e-5) { double alphaInv = 1/alpha; double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); double v1 = v[0], v2 = v[1]; - return (Matrix(3,3) << + J = (Matrix(3,3) << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, 0, 0, 1).finished(); } else { - return (Matrix(3,3) << 1,0, 0.5*v[1], + J = (Matrix(3,3) << 1,0, 0.5*v[1], 0,1, -0.5*v[0], 0,0, 1).finished(); } + return J; } /* ************************************************************************* */ From ea80e36b2408087ea5a4c7feb945eefc7b27022a Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Wed, 24 Dec 2014 14:01:16 -0500 Subject: [PATCH 2/7] Pose3 [Expmap/Logmap]Derivative --- gtsam/geometry/Pose3.cpp | 78 ++++++++++++++++++++++-------- gtsam/geometry/Pose3.h | 17 ++----- gtsam/geometry/tests/testPose3.cpp | 21 ++++---- 3 files changed, 74 insertions(+), 42 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0bc07b753..d66ac8148 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -93,26 +93,6 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, return adjointMap(xi).transpose() * y; } -/* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { - // Bernoulli numbers, from Wikipedia - static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, - 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); - static const int N = 5; // order of approximation - Matrix6 res; - res = I_6x6; - Matrix6 ad_i; - ad_i = I_6x6; - Matrix6 ad_xi = adjointMap(xi); - double fac = 1.0; - for (int i = 1; i < N; ++i) { - ad_i = ad_xi * ad_i; - fac = fac * i; - res = res + B(i) / fac * ad_i; - } - return res; -} - /* ************************************************************************* */ void Pose3::print(const string& s) const { cout << s; @@ -221,6 +201,64 @@ Vector6 Pose3::localCoordinates(const Pose3& T, } } +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::ExpmapDerivative(w); + + Vector3 v(sub(xi, 3, 6)); + Matrix3 V = skewSymmetric(v); + Matrix3 W = skewSymmetric(w); + +#ifdef NUMERICAL_EXPMAP_DERIV + Matrix3 Qj = Z_3x3; + double invFac = 1.0; + Matrix3 Q = Z_3x3; + Matrix3 Wj = I_3x3; + for (size_t j=1; j<10; ++j) { + Qj = Qj*W + Wj*V; + invFac = -invFac/(j+1); + Q = Q + invFac*Qj; + Wj = Wj*W; + } +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi, + phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Matrix3 Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W); +#endif + + Matrix6 J; + J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished(); + return J; +} + +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::LogmapDerivative(w); + + Vector3 v(sub(xi, 3, 6)); + Matrix3 V = skewSymmetric(v); + Matrix3 W = skewSymmetric(w); + + // The closed-form formula in Barfoot14tro eq. (102) + double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi, + phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Matrix3 Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W); + Matrix3 Q2 = -Jw*Q*Jw; + + Matrix6 J; + J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); + return J; +} + /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 805bc6012..15f71344b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,18 +204,11 @@ public: */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); - /** - * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, - * as detailed in [Kobilarov09siggraph] eq. (15) - * The full formula is documented in [Celledoni99cmame] - * Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and - * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003. - * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2 - * Ernst Hairer, et al., Geometric Numerical Integration, - * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. - * See also Iserles05an, pg. 33, formula 2.46 - */ - static Matrix6 dExpInv_exp(const Vector6 &xi); + /// Left-trivialized derivative of the exponential map + static Matrix6 ExpmapDerivative(const Vector6& v); + + /// Left-trivialized inverse derivative of the exponential map + static Matrix6 LogmapDerivative(const Vector6& v); /** * wedge for Pose3: diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 52f721f41..421dea879 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -672,21 +672,22 @@ TEST(Pose3, align_2) { } /* ************************************************************************* */ -/// exp(xi) exp(y) = exp(xi + x) -/// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0).finished(); +/// exp(xi) exp(y) = exp(xi + dxi) +/// Hence, y = log (exp(-xi)*exp(xi+dxi)) +Vector6 xi = (Vector(6) << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0).finished(); -Vector testDerivExpmapInv(const Vector6& dxi) { +Vector6 testExpmapDerivative(const Vector6& xi, const Vector6& dxi) { return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); } -TEST( Pose3, dExpInv_TLN) { - Matrix res = Pose3::dExpInv_exp(xi); +TEST( Pose3, ExpmapDerivative) { + Matrix actualDexpL = Pose3::ExpmapDerivative(xi); + Matrix expectedDexpL = numericalDerivative11( + boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - Matrix numericalDerivExpmapInv = numericalDerivative11( - testDerivExpmapInv, Vector6::Zero(), 1e-5); - - EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); + Matrix actualDexpInvL = Pose3::LogmapDerivative(xi); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } /* ************************************************************************* */ From 5ae9f19de29c3539d7b5bed045837d9ad08947bf Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Wed, 24 Dec 2014 14:08:08 -0500 Subject: [PATCH 3/7] unify duplicated code --- gtsam/geometry/Pose3.cpp | 38 ++++++++++++++++---------------------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index d66ac8148..8732d0aa3 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -202,18 +202,18 @@ Vector6 Pose3::localCoordinates(const Pose3& T, } /* ************************************************************************* */ -Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { +Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi) { Vector3 w(sub(xi, 0, 3)); - Matrix3 Jw = Rot3::ExpmapDerivative(w); - Vector3 v(sub(xi, 3, 6)); Matrix3 V = skewSymmetric(v); Matrix3 W = skewSymmetric(w); + Matrix3 Q; + #ifdef NUMERICAL_EXPMAP_DERIV Matrix3 Qj = Z_3x3; double invFac = 1.0; - Matrix3 Q = Z_3x3; + Q = Z_3x3; Matrix3 Wj = I_3x3; for (size_t j=1; j<10; ++j) { Qj = Qj*W + Wj*V; @@ -226,13 +226,20 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; // Invert the sign of odd-order terms to have the right Jacobian - Matrix3 Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W); #endif - Matrix6 J; - J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished(); + return Q; +} + +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::ExpmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished(); return J; } @@ -240,22 +247,9 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { Vector3 w(sub(xi, 0, 3)); Matrix3 Jw = Rot3::LogmapDerivative(w); - - Vector3 v(sub(xi, 3, 6)); - Matrix3 V = skewSymmetric(v); - Matrix3 W = skewSymmetric(w); - - // The closed-form formula in Barfoot14tro eq. (102) - double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi, - phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Matrix3 Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) - + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W); + Matrix3 Q = computeQforExpmapDerivative(xi); Matrix3 Q2 = -Jw*Q*Jw; - - Matrix6 J; - J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); return J; } From 5d0e8f202fd06193e0fdb859337071799de1374e Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Wed, 24 Dec 2014 14:08:32 -0500 Subject: [PATCH 4/7] unify duplicated code --- gtsam/geometry/Pose3.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 15f71344b..8c3c74069 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,11 +204,16 @@ public: */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); +private: + static Matrix3 computeQforExpmapDerivative(const Vector6& xi); + +public: + /// Left-trivialized derivative of the exponential map - static Matrix6 ExpmapDerivative(const Vector6& v); + static Matrix6 ExpmapDerivative(const Vector6& xi); /// Left-trivialized inverse derivative of the exponential map - static Matrix6 LogmapDerivative(const Vector6& v); + static Matrix6 LogmapDerivative(const Vector6& xi); /** * wedge for Pose3: From 34592f21c431cb650a4791e8c610fc30f59a885a Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 25 Dec 2014 08:53:53 -0500 Subject: [PATCH 5/7] remove unnecessary malloc --- gtsam/geometry/Pose2.cpp | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 8e5d58047..a2dfc9f18 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -150,17 +150,16 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { */ double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; - J = (Matrix(3,3) << - sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, - c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, - 0, 0, 1).finished(); + J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, + c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, + 0, 0, 1; } else { // Thanks to Krunal: Apply L'Hospital rule to several times to // compute the limits when alpha -> 0 - J = (Matrix(3,3) << 1,0,-0.5*v[1], + J << 1,0,-0.5*v[1], 0,1, 0.5*v[0], - 0,0, 1).finished(); + 0,0, 1; } return J; @@ -175,15 +174,14 @@ Matrix3 Pose2::LogmapDerivative(const Vector3& v) { double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); double v1 = v[0], v2 = v[1]; - J = (Matrix(3,3) << - alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, - 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, - 0, 0, 1).finished(); + J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, + 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, + 0, 0, 1; } else { - J = (Matrix(3,3) << 1,0, 0.5*v[1], + J << 1,0, 0.5*v[1], 0,1, -0.5*v[0], - 0,0, 1).finished(); + 0,0, 1; } return J; } From 6feb06bc9135bfc1be0fe86c2589ae2dfa441335 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 25 Dec 2014 08:54:53 -0500 Subject: [PATCH 6/7] less confusing comments --- gtsam/geometry/Pose2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index aa201cfa2..32046cfcb 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -181,10 +181,10 @@ public: return m; } - /// Left-trivialized derivative of the exponential map + /// Derivative of Expmap static Matrix3 ExpmapDerivative(const Vector3& v); - /// Left-trivialized derivative inverse of the exponential map + /// Derivative of Logmap static Matrix3 LogmapDerivative(const Vector3& v); From 65bbb5e12d0625ec3378e799d517817d06a3fa14 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 25 Dec 2014 09:07:21 -0500 Subject: [PATCH 7/7] use static free function and fix comments --- gtsam/geometry/Pose3.cpp | 10 +++++++++- gtsam/geometry/Pose3.h | 7 ++----- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8732d0aa3..f5b840d08 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -202,7 +202,15 @@ Vector6 Pose3::localCoordinates(const Pose3& T, } /* ************************************************************************* */ -Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi) { +/** + * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix + * J(xi) = [J_(w) Z_3x3; + * Q J_(w)] + * where J_(w) is the SO3 Expmap derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is similar to formula 102 in Barfoot14tro) + */ +static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { Vector3 w(sub(xi, 0, 3)); Vector3 v(sub(xi, 3, 6)); Matrix3 V = skewSymmetric(v); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8c3c74069..39d74bd9b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,15 +204,12 @@ public: */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); -private: - static Matrix3 computeQforExpmapDerivative(const Vector6& xi); - public: - /// Left-trivialized derivative of the exponential map + /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); - /// Left-trivialized inverse derivative of the exponential map + /// Derivative of Logmap static Matrix6 LogmapDerivative(const Vector6& xi); /**