diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c6da8e80b..b0f958efb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -44,7 +44,7 @@ namespace gtsam { // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) // Experimental - unit tests of derivatives based on it do not check out yet - Matrix6 Pose3::adjointMap() const { + Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); Matrix3 A = skewSymmetric(t)*R; @@ -53,6 +53,20 @@ namespace gtsam { return adj; } + /* ************************************************************************* */ + Matrix6 Pose3::adjoint(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) { + return I6 - 0.5*adjoint(xi); + } + /* ************************************************************************* */ void Pose3::print(const string& s) const { cout << s; @@ -210,14 +224,14 @@ namespace gtsam { /* ************************************************************************* */ Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, boost::optional H2) const { - if (H1) *H1 = p2.inverse().adjointMap(); + if (H1) *H1 = p2.inverse().AdjointMap(); if (H2) *H2 = I6; return (*this) * p2; } /* ************************************************************************* */ Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) *H1 = -adjointMap(); + if (H1) *H1 = -AdjointMap(); Rot3 Rt = R_.inverse(); return Pose3(Rt, Rt*(-t_)); } @@ -227,7 +241,7 @@ namespace gtsam { Pose3 Pose3::between(const Pose3& p2, boost::optional H1, boost::optional H2) const { Pose3 result = inverse()*p2; - if (H1) *H1 = -result.inverse().adjointMap(); + if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) *H2 = I6; return result; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 5dbb89013..4a64f48b5 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -145,11 +145,43 @@ namespace gtsam { static Vector6 Logmap(const Pose3& p); /** - * Calculate Adjoint map + * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ - Matrix6 adjointMap() const; /// FIXME Not tested - marked as incorrect - Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect + Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect + + /** + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + */ + Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } /// FIXME Not tested - marked as incorrect + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * [ad(w,v)] = [w^, zero3; v^, w^] + * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, + * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. + * + * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its + * vector representation. + * We have the following relationship: + * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ + * + * We use this to compute the discrete version of the inverse right-trivialized tangent map, + * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. + * + */ + static Matrix6 adjoint(const Vector& xi); + + /** + * 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. + * 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. + */ + static Matrix6 dExpInv_TLN(const Vector& xi); /** * wedge for Pose3: diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index cb63c830a..79a241b58 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -128,15 +128,15 @@ TEST(Pose3, expmap_c_full) TEST(Pose3, Adjoint_full) { Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); - Vector xiprime = T.adjoint(screw::xi); + Vector xiprime = T.Adjoint(screw::xi); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); - Vector xiprime2 = T2.adjoint(screw::xi); + Vector xiprime2 = T2.Adjoint(screw::xi); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); - Vector xiprime3 = T3.adjoint(screw::xi); + Vector xiprime3 = T3.Adjoint(screw::xi); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } @@ -192,7 +192,7 @@ TEST(Pose3, Adjoint_compose_full) const Pose3& T1 = T; Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); Pose3 expected = T1 * Pose3::Expmap(x) * T2; - Vector y = T2.inverse().adjoint(x); + Vector y = T2.inverse().Adjoint(x); Pose3 actual = T1 * T2 * Pose3::Expmap(y); EXPECT(assert_equal(expected, actual, 1e-6)); } @@ -211,7 +211,7 @@ TEST( Pose3, compose ) Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); - EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2); EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4)); @@ -231,7 +231,7 @@ TEST( Pose3, compose2 ) Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); - EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5)); @@ -247,7 +247,7 @@ TEST( Pose3, inverse) Matrix numericalH = numericalDerivative11(testing::inverse, T); EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); - EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3)); + EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3)); } /* ************************************************************************* */ @@ -261,7 +261,7 @@ TEST( Pose3, inverseDerivatives2) Matrix actualDinverse; T.inverse(actualDinverse); EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); - EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3)); + EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3)); } /* ************************************************************************* */ @@ -622,6 +622,24 @@ TEST( Pose3, unicycle ) EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); } +/* ************************************************************************* */ +TEST( Pose3, adjoint) { + Matrix res = Pose3::adjoint(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); + Matrix6 expected; + expected << wh, Z3, vh, wh; + EXPECT(assert_equal(expected,res,1e-5)); +} + +/* ************************************************************************* */ +TEST( Pose3, dExpInv_TLN) { + Matrix res = Pose3::dExpInv_TLN(screw::xi); + Matrix6 expected = eye(6,6) - 0.5*Pose3::adjoint(screw::xi); + EXPECT(assert_equal(expected,res,1e-5)); +} + /* ************************************************************************* */ int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */