From 104ad15e912183a4c7b11f036ead675d962b3f4c Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Mon, 29 Apr 2013 21:22:33 +0000 Subject: [PATCH] Separate adjointMap, which returns a matrix, and adjoint+adjointTranspose actions on a vector with optional derivatives. Also, change dExpInv_TLN to dExpInv_exp with higher order of approximation --- gtsam/geometry/Pose3.cpp | 34 +++++++++++++++-- gtsam/geometry/Pose3.h | 22 ++++++++--- gtsam/geometry/tests/testPose3.cpp | 60 +++++++++++++++++++++++++++--- 3 files changed, 103 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index a630d8be7..adaa2ce69 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -55,22 +55,50 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix6 Pose3::adjoint(const Vector& xi) { + Matrix6 Pose3::adjointMap(const Vector& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; adj << w_hat, Z3, v_hat, w_hat; + return adj; } /* ************************************************************************* */ - Matrix6 Pose3::dExpInv_TLN(const Vector& xi) { + Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional H) { + if (H) { + *H = zeros(6,6); + for (int i = 0; i<6; ++i) { + Vector dxi = zero(6); dxi(i) = 1.0; + Matrix Gi = adjointMap(dxi); + (*H).col(i) = Gi*y; + } + } + return adjointMap(xi)*y; + } + + /* ************************************************************************* */ + Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional H) { + if (H) { + *H = zeros(6,6); + for (int i = 0; i<6; ++i) { + Vector dxi = zero(6); dxi(i) = 1.0; + Matrix GTi = adjointMap(dxi).transpose(); + (*H).col(i) = GTi*y; + } + } + Matrix adjT = adjointMap(xi).transpose(); + return adjointMap(xi).transpose() * y; + } + + /* ************************************************************************* */ + Matrix6 Pose3::dExpInv_exp(const Vector& 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); static const int N = 5; // order of approximation Matrix res = I6; Matrix6 ad_i = I6; - Matrix6 ad_xi = adjoint(xi); + Matrix6 ad_xi = adjointMap(xi); double fac = 1.0; for (int i = 1 ; i @@ -171,17 +171,29 @@ namespace gtsam { * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * */ - static Matrix6 adjoint(const Vector& xi); + static Matrix6 adjointMap(const Vector& xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector adjoint(const Vector& xi, const Vector& y, boost::optional H = boost::none); + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional H = boost::none); /** * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, - * using the trapezoidal Lie-Newmark (TLN) scheme - * as detailed in [Kobilarov09siggraph] eq. (15) and C_TLN. + * 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. */ - static Matrix6 dExpInv_TLN(const Vector& xi); + static Matrix6 dExpInv_exp(const Vector& xi); /** * wedge for Pose3: diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 94c0ecac6..ed8f850d2 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -627,8 +627,8 @@ TEST( Pose3, unicycle ) } /* ************************************************************************* */ -TEST( Pose3, adjoint) { - Matrix res = Pose3::adjoint(screw::xi); +TEST( Pose3, adjointMap) { + Matrix res = Pose3::adjointMap(screw::xi); Matrix wh = skewSymmetric(screw::xi(0), screw::xi(1), screw::xi(2)); Matrix vh = skewSymmetric(screw::xi(3), screw::xi(4), screw::xi(5)); Matrix Z3 = zeros(3,3); @@ -674,12 +674,16 @@ 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); + Vector testDerivExpmapInv(const LieVector& dxi) { - return Pose3::Logmap(Pose3::Expmap(-screw::xi)*Pose3::Expmap(screw::xi+dxi)); + Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); + return y; } TEST( Pose3, dExpInv_TLN) { - Matrix res = Pose3::dExpInv_TLN(screw::xi); + Matrix res = Pose3::dExpInv_exp(xi); Matrix numericalDerivExpmapInv = numericalDerivative11( boost::function( @@ -688,7 +692,53 @@ TEST( Pose3, dExpInv_TLN) { LieVector(Vector::Zero(6)), 1e-5 ); - EXPECT(assert_equal(numericalDerivExpmapInv,res,1e-1)); + EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); +} + +/* ************************************************************************* */ +Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) { + return Pose3::adjointMap(xi)*v; +} + +TEST( Pose3, adjoint) { + Vector expected = testDerivAdjoint(screw::xi, screw::xi); + + Matrix actualH; + Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); + + Matrix numericalH = numericalDerivative21( + boost::function( + boost::bind(testDerivAdjoint, _1, _2) + ), + LieVector(screw::xi), LieVector(screw::xi), 1e-5 + ); + + EXPECT(assert_equal(expected,actual,1e-5)); + EXPECT(assert_equal(numericalH,actualH,1e-5)); +} + +/* ************************************************************************* */ +Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { + return Pose3::adjointMap(xi).transpose()*v; +} + +TEST( Pose3, adjointTranspose) { + Vector xi = Vector_(6, 0.01, 0.02, 0.03, 1.0, 2.0, 3.0); + Vector v = Vector_(6, 0.04, 0.05, 0.06, 4.0, 5.0, 6.0); + Vector expected = testDerivAdjointTranspose(xi, v); + + Matrix actualH; + Vector actual = Pose3::adjointTranspose(xi, v, actualH); + + Matrix numericalH = numericalDerivative21( + boost::function( + boost::bind(testDerivAdjointTranspose, _1, _2) + ), + LieVector(xi), LieVector(v), 1e-5 + ); + + EXPECT(assert_equal(expected,actual,1e-15)); + EXPECT(assert_equal(numericalH,actualH,1e-5)); } /* ************************************************************************* */