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

add jacobian of second argument to adjoint and adjointTranpsose

release/4.3a0
Gerry Chen 2021-11-02 12:04:04 -04:00
parent 7b22090352
commit 508db60f74
No known key found for this signature in database
GPG Key ID: E9845092D3A57286
3 changed files with 31 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Pose3.cpp
View File

@ -116,7 +116,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi) { OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) { if (Hxi) {
Hxi->setZero(); Hxi->setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
@ -127,12 +127,14 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
Hxi->col(i) = Gi * y; Hxi->col(i) = Gi * y;
} }
} }
return adjointMap(xi) * y; const Matrix6& ad_xi = adjointMap(xi);
if (H_y) *H_y = ad_xi;
return ad_xi * y;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi) { OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) { if (Hxi) {
Hxi->setZero(); Hxi->setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
@ -143,7 +145,9 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
Hxi->col(i) = GTi * y; Hxi->col(i) = GTi * y;
} }
} }
return adjointMap(xi).transpose() * y; const Matrix6& adT_xi = adjointMap(xi).transpose();
if (H_y) *H_y = adT_xi;
return adT_xi * y;
} }
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/geometry/Pose3.h
View File

@ -177,13 +177,14 @@ public:
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
* *
*/ */
static Matrix6 adjointMap(const Vector6 &xi); static Matrix6 adjointMap(const Vector6& xi);
/** /**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/ */
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none); OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
// temporary fix for wrappers until case issue is resolved // temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
@ -193,7 +194,8 @@ public:
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/ */
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none); OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
/// Derivative of Expmap /// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi); static Matrix6 ExpmapDerivative(const Vector6& xi);

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

@ -912,16 +912,20 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
} }
TEST( Pose3, adjoint) { TEST( Pose3, adjoint) {
Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi); Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished();
Vector expected = testDerivAdjoint(screwPose3::xi, v);
Matrix actualH; Matrix actualH1, actualH2;
Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH); Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2);
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>( Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5); testDerivAdjoint, screwPose3::xi, v, 1e-5);
Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
testDerivAdjoint, screwPose3::xi, v, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH1,actualH1,1e-5));
EXPECT(assert_equal(numericalH2,actualH2,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -934,14 +938,17 @@ TEST( Pose3, adjointTranspose) {
Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished(); Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished();
Vector expected = testDerivAdjointTranspose(xi, v); Vector expected = testDerivAdjointTranspose(xi, v);
Matrix actualH; Matrix actualH1, actualH2;
Vector actual = Pose3::adjointTranspose(xi, v, actualH); Vector actual = Pose3::adjointTranspose(xi, v, actualH1, actualH2);
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>( Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5);
Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5); testDerivAdjointTranspose, xi, v, 1e-5);
EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(expected,actual,1e-15));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH1,actualH1,1e-5));
EXPECT(assert_equal(numericalH2,actualH2,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */