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

Pose3 [Expmap/Logmap]Derivative

release/4.3a0
thduynguyen 2014-12-24 14:01:16 -05:00
parent ea4e9a5ac6
commit ea80e36b24
3 changed files with 74 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
gtsam/geometry/Pose3.cpp
View File

@ -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();

17
gtsam/geometry/Pose3.h
View File

@ -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:

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

@ -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<Vector6, Vector6>(
boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix numericalDerivExpmapInv = numericalDerivative11<Vector, Vector6>(
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));
}
/* ************************************************************************* */