Change "adjoint" and "adjoint_map" to Adjoint and Adjoint_map, since they correspond to Lie group's Ad operator. Implement the Lie algebra's adjoint operator, aka Lie bracket, and the inverse right-trivialized tangent map of the exponential map using the trapezoidal Lie-Newmark scheme, as detailed in [Kobilarov09siggraph]
Duy-Nguyen Ta
parent
c1f14d60be
commit
96f9ecaf53
|
|
@ -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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Pose3 result = inverse()*p2;
|
||||
if (H1) *H1 = -result.inverse().adjointMap();
|
||||
if (H1) *H1 = -result.inverse().AdjointMap();
|
||||
if (H2) *H2 = I6;
|
||||
return result;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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<Pose3>, 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<Pose3>, T2, T2);
|
||||
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
|
||||
|
|
@ -231,7 +231,7 @@ TEST( Pose3, compose2 )
|
|||
|
||||
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, 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<Pose3>, T1, T2);
|
||||
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
|
||||
|
|
@ -247,7 +247,7 @@ TEST( Pose3, inverse)
|
|||
|
||||
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, 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<Pose3>(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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue