Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add Jacobian of second argument to adjoint and adjointTranpsose #922

Merged
merged 2 commits into from
Nov 11, 2021
Merged

Add Jacobian of second argument to adjoint and adjointTranpsose #922

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
508db60
add jacobian of second argument to adjoint and adjointTranpsose
gchenfc Nov 2, 2021
d86fc98
update python wrapper
gchenfc Nov 11, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 8 additions & 4 deletions gtsam/geometry/Pose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {

/* ************************************************************************* */
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi) {
OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) {
Hxi->setZero();
for (int i = 0; i < 6; ++i) {
Expand All @@ -127,12 +127,14 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& 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,
OptionalJacobian<6, 6> Hxi) {
OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) {
Hxi->setZero();
for (int i = 0; i < 6; ++i) {
Expand All @@ -143,7 +145,9 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& 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;
}

/* ************************************************************************* */
Expand Down
10 changes: 6 additions & 4 deletions gtsam/geometry/Pose3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -177,13 +177,14 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
* 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
*/
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
OptionalJacobian<6, 6> Hxi = boost::none);
static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);

// temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
Expand All @@ -193,7 +194,8 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
* 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,
OptionalJacobian<6, 6> Hxi = boost::none);
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);

/// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi);
Expand Down
3 changes: 3 additions & 0 deletions gtsam/geometry/geometry.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -473,6 +473,9 @@ class Pose3 {
Vector logmap(const gtsam::Pose3& pose);
Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const;
Vector AdjointTranspose(Vector xi) const;
static Matrix adjointMap(Vector xi);
static Vector adjoint(Vector xi, Vector y);
static Matrix adjointMap_(Vector xi);
static Vector adjoint_(Vector xi, Vector y);
static Vector adjointTranspose(Vector xi, Vector y);
Expand Down
27 changes: 17 additions & 10 deletions gtsam/geometry/tests/testPose3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -912,16 +912,20 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
}

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;
Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH);
Matrix actualH1, actualH2;
Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2);

Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);
Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
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(numericalH,actualH,1e-5));
EXPECT(assert_equal(numericalH1,actualH1,1e-5));
EXPECT(assert_equal(numericalH2,actualH2,1e-5));
}

/* ************************************************************************* */
Expand All @@ -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 expected = testDerivAdjointTranspose(xi, v);

Matrix actualH;
Vector actual = Pose3::adjointTranspose(xi, v, actualH);
Matrix actualH1, actualH2;
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);

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));
}

/* ************************************************************************* */
Expand Down
10 changes: 9 additions & 1 deletion python/gtsam/tests/test_Pose3.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,16 @@ def test_range(self):
self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))

def test_adjoint(self):
"""Test adjoint method."""
"""Test adjoint methods."""
T = Pose3()
xi = np.array([1, 2, 3, 4, 5, 6])
# test calling functions
T.AdjointMap()
T.Adjoint(xi)
T.AdjointTranspose(xi)
Pose3.adjointMap(xi)
Pose3.adjoint(xi, xi)
# test correctness of adjoint(x, y)
expected = np.dot(Pose3.adjointMap_(xi), xi)
actual = Pose3.adjoint_(xi, xi)
np.testing.assert_array_equal(actual, expected)
Expand Down