12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
Duy-Nguyen Ta 2013-04-29 21:22:33 +00:00
parent 26f5f93c60
commit 104ad15e91
3 changed files with 103 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/geometry/Pose3.cpp
View File

@ -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<Matrix&> 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<Matrix&> 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<N; ++i) {
ad_i = ad_xi * ad_i;

22
gtsam/geometry/Pose3.h
View File

@ -19,7 +19,7 @@
#pragma once
#ifndef POSE3_DEFAULT_COORDINATES_MODE
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP
#endif
#include <gtsam/base/DerivedValue.h>
@ -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<Matrix&> 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<Matrix&> 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<EFBFBD> 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:

60
gtsam/geometry/tests/testPose3.cpp
View File

@ -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<Vector(const LieVector&)>(
@ -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<Vector(const LieVector&, const LieVector&)>(
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<Vector(const LieVector&, const LieVector&)>(
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));
}
/* ************************************************************************* */