diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index b5c426597..dc074bb23 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -323,6 +323,32 @@ double Pose2::range(const Pose2& pose, return r; } +/* ************************************************************************* */ +// Compute vectorized Lie algebra generators for SE(2) +static Matrix93 VectorizedGenerators() { + Matrix93 G; + for (size_t j = 0; j < 3; j++) { + const Matrix3 X = Pose2::Hat(Vector::Unit(3, j)); + G.col(j) = Eigen::Map(X.data()); + } + return G; +} + +Vector9 Pose2::vec(OptionalJacobian<9, 3> H) const { + // Vectorize + const Matrix3 M = matrix(); + const Vector9 X = Eigen::Map(M.data()); + + // If requested, calculate H as (I_3 \oplus M) * G. + if (H) { + static const Matrix93 G = VectorizedGenerators(); // static to compute only once + for (size_t i = 0; i < 3; i++) + H->block(i * 3, 0, 3, dimension) = M * G.block(i * 3, 0, 3, dimension); + } + + return X; +} + /* ************************************************************************* * Align finds the angle using a linear method: * a = Pose2::transformFrom(b) = t + R*b diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 93bc6751b..b78d1d3b7 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -328,6 +328,9 @@ public: */ static std::pair rotationInterval() { return {2, 2}; } + /// Return vectorized SE(2) matrix in column order. + Vector9 vec(OptionalJacobian<9, 3> H = {}) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose2& p); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 1a17e61df..2e0463ceb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -534,6 +534,34 @@ Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, Opti return interpolate(*this, other, t, Hx, Hy); } +/* ************************************************************************* */ +// Compute vectorized Lie algebra generators for SE(3) +using Matrix16x6 = Eigen::Matrix; +using Vector16 = Eigen::Matrix; +static Matrix16x6 VectorizedGenerators() { + Matrix16x6 G; + for (size_t j = 0; j < 6; j++) { + const Matrix4 X = Pose3::Hat(Vector::Unit(6, j)); + G.col(j) = Eigen::Map(X.data()); + } + return G; +} + +Vector Pose3::vec(OptionalJacobian<16, 6> H) const { + // Vectorize + const Matrix4 M = matrix(); + const Vector X = Eigen::Map(M.data()); + + // If requested, calculate H as (I_4 \oplus M) * G. + if (H) { + static const Matrix16x6 G = VectorizedGenerators(); // static to compute only once + for (size_t i = 0; i < 4; i++) + H->block(i * 4, 0, 4, dimension) = M * G.block(i * 4, 0, 4, dimension); + } + + return X; +} + /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { // Both Rot3 and Point3 have ostream definitions so we use them. diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 154e24a36..3ef6faca6 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -392,6 +392,9 @@ public: Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {}, OptionalJacobian<6, 6> Hy = {}) const; + /// Return vectorized SE(3) matrix in column order. + Vector vec(OptionalJacobian<16, 6> H = {}) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index f1eca66d8..e13abb048 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -157,6 +157,22 @@ Rot2 Rot2::ClosestTo(const Matrix2& M) { return Rot2::fromCosSin(c, s); } +/* ************************************************************************* */ +Vector4 Rot2::vec(OptionalJacobian<4, 1> H) const { + // Vectorize + const Matrix2 M = matrix(); + const Vector4 X = Eigen::Map(M.data()); + + // If requested, calculate H as (I_3 \oplus M) * G. + if (H) { + static const Matrix41 G = (Matrix41() << 0, 1, -1, 0).finished(); + for (size_t i = 0; i < 2; i++) + H->block(i * 2, 0, 2, dimension) = M * G.block(i * 2, 0, 2, dimension); + } + + return X; +} + /* ************************************************************************* */ } // gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 85b41e72c..d3a335e72 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -223,6 +223,9 @@ namespace gtsam { /** Find closest valid rotation matrix, given a 2x2 matrix */ static Rot2 ClosestTo(const Matrix2& M); + /// Return vectorized SO(2) matrix in column order. + Vector4 vec(OptionalJacobian<4, 1> H = {}) const; + /// @} private: diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 10ba5894e..f5d5a2234 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -146,6 +146,7 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 Point3 Rot3::column(int index) const{ if(index == 3) return r3(); @@ -156,6 +157,7 @@ Point3 Rot3::column(int index) const{ else throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); } +#endif /* ************************************************************************* */ Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 5768399f8..bbdb27818 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -459,9 +459,6 @@ class GTSAM_EXPORT Rot3 : public LieGroup { */ Matrix3 transpose() const; - /// @deprecated, this is base 1, and was just confusing - Point3 column(int index) const; - Point3 r1() const; ///< first column Point3 r2() const; ///< second column Point3 r3() const; ///< third column @@ -530,14 +527,26 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /** * @brief Spherical Linear intERPolation between *this and other * @param t a value between 0 and 1 - * @param other final point of iterpolation geodesic on manifold + * @param other final point of interpolation geodesic on manifold */ Rot3 slerp(double t, const Rot3& other) const; + /// Vee maps from Lie algebra to tangent vector + inline Vector9 vec(OptionalJacobian<9, 3> H = {}) const { return SO3(matrix()).vec(H); } + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); /// @} + /// @name deprecated + /// @{ + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 + /// @deprecated, this is base 1, and was just confusing + Point3 column(int index) const; +#endif + + /// @} private: #if GTSAM_ENABLE_BOOST_SERIALIZATION diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 9bbc8f5ca..ee0e035a4 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -958,6 +958,23 @@ TEST(Pose2, Print) { EXPECT(assert_print_equal(expected2, pose, s)); } +/* ************************************************************************* */ +TEST(Pose2, vec) { + // Test a simple pose + Pose2 pose(Rot2::fromAngle(M_PI / 4), Point2(1, 2)); + + // Test the 'vec' method + Vector9 expected_vec = Eigen::Map(pose.matrix().data()); + Matrix93 actualH; + Vector9 actual_vec = pose.vec(actualH); + EXPECT(assert_equal(expected_vec, actual_vec)); + + // Verify Jacobian with numerical derivatives + std::function f = [](const Pose2& p) { return p.vec(); }; + Matrix93 numericalH = numericalDerivative11(f, pose); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index d14af7ac0..c33985aad 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1401,6 +1401,21 @@ TEST(Pose3, ExpmapChainRule) { EXPECT(assert_equal(expected2, analytic, 1e-5)); // note tolerance } +/* ************************************************************************* */ +TEST(Pose3, vec) { + // Test the 'vec' method + using Vector16 = Eigen::Matrix; + Vector16 expected_vec = Eigen::Map(T.matrix().data()); + Matrix actualH; + Vector16 actual_vec = T.vec(actualH); + EXPECT(assert_equal(expected_vec, actual_vec)); + + // Verify Jacobian with numerical derivatives + std::function f = [](const Pose3& p) { return p.vec(); }; + Matrix numericalH = numericalDerivative11(f, T); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 790ff7a3e..d436fdefb 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -174,6 +174,20 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(expectedH,actualH)); } +/* ************************************************************************* */ +TEST(Rot2, vec) { + // Test the 'vec' method + Vector4 expected_vec = Eigen::Map(R.matrix().data()); + Matrix41 actualH; + Vector4 actual_vec = R.vec(actualH); + EXPECT(assert_equal(expected_vec, actual_vec)); + + // Verify Jacobian with numerical derivatives + std::function f = [](const Rot2& p) { return p.vec(); }; + Matrix41 numericalH = numericalDerivative11(f, R); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + //****************************************************************************** namespace { Rot2 id;