diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2f4fa4da3..daa889c0d 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -100,6 +100,42 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, return result; } +/* ************************************************************************* */ +/// The dual version of Adjoint +Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, + OptionalJacobian<6, 6> H_x) const { + // Ad^T * xi = [ R^T R^T.[-t] . [w + // 0 R^T ] v] + // Declarations and aliases + Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, tv_H_t, tv_H_v, Rtv_H_R, Rtv_H_tv; + Vector6 result; + const Vector3 &w = x.head<3>(), &v = x.tail<3>(); + auto Rv = result.tail<3>(); + + // Calculations + const Vector3 Rw = + R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr); + Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr); + const Vector3 tv = cross(t_, v, tv_H_t, tv_H_v); + const Vector3 Rtv = R_.unrotate(tv, Rtv_H_R, Rtv_H_tv); + result.head<3>() = Rw - Rtv; + + // Jacobians + if (H_this) { + *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R, // -Rtv_H_tv * tv_H_t + /* */ Rv_H_R, /* */ Z_3x3) + .finished(); + } + if (H_x) { + *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // + /* */ Z_3x3, Rv_H_v) + .finished(); + } + + // Return + return result; +} + /* ************************************************************************* */ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 9a8a7e939..cff5fd231 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -156,6 +156,11 @@ public: Vector6 Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this = boost::none, OptionalJacobian<6, 6> H_xib = boost::none) const; + + /// The dual version of Adjoint + Vector6 AdjointTranspose(const Vector6& x, + OptionalJacobian<6, 6> H_this = boost::none, + OptionalJacobian<6, 6> H_x = boost::none) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9fde4c2c1..722087934 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -151,7 +151,11 @@ TEST(Pose3, Adjoint_jacobians) { Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; + [&](const Pose3& T, const Vector6& xi) { + Vector6 res1 = T.AdjointMap() * xi, res2 = T.Adjoint(xi); + EXPECT(assert_equal(res1, res2)); + return res1; + }; gtsam::Vector6 xi = (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); @@ -175,6 +179,41 @@ TEST(Pose3, Adjoint_jacobians) EXPECT(assert_equal(expectedH2, actualH2)); } +/* ************************************************************************* */ +// Check AdjointTranspose and jacobians +TEST(Pose3, AdjointTranspose) +{ + Matrix6 actualH1, actualH2, expectedH1, expectedH2; + std::function AdjointTranspose_proxy = + [&](const Pose3& T, const Vector6& xi) { + Vector6 res1 = T.AdjointMap().transpose() * xi, + res2 = T.AdjointTranspose(xi); + EXPECT(assert_equal(res1, res2)); + return res1; + }; + + gtsam::Vector6 xi = + (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + T.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T2.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T3.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) TEST(Pose3, Adjoint_hat)