From 6d47bd45522d073a3e52235f3409d027a815d01e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Mar 2025 19:45:52 -0500 Subject: [PATCH 1/5] Address review comments --- gtsam/base/Lie.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 827141e58..325ce2d40 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -229,7 +229,7 @@ struct LieGroupTraits: public GetDimensionImpl { /// Both LieGroupTraits and Testable template struct LieGroup: LieGroupTraits, Testable {}; -/// Adds LieAlgebra, Hat, and Vie to LieGroupTraits +/// Adds LieAlgebra, Hat, and Vee to LieGroupTraits template struct MatrixLieGroupTraits: LieGroupTraits { using LieAlgebra = typename Class::LieAlgebra; using TangentVector = typename LieGroupTraits::TangentVector; @@ -351,7 +351,7 @@ template Matrix wedge(const Vector& x); */ template T expm(const Vector& x, int K = 7) { - auto xhat = T::Hat(x); + const Matrix xhat = T::Hat(x); return T(expm(xhat, K)); } From 63257bcf19f7fd8c4a2e57cf21820e7c2b471d60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 10 Mar 2025 17:19:33 -0400 Subject: [PATCH 2/5] Rot2::vec, Rot3::vec, Pose2::vec, Pose3::vec --- gtsam/geometry/Pose2.cpp | 26 ++++++++++++++++++++++++++ gtsam/geometry/Pose2.h | 3 +++ gtsam/geometry/Pose3.cpp | 28 ++++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 3 +++ gtsam/geometry/Rot2.cpp | 16 ++++++++++++++++ gtsam/geometry/Rot2.h | 3 +++ gtsam/geometry/Rot3.cpp | 2 ++ gtsam/geometry/Rot3.h | 17 +++++++++++++---- gtsam/geometry/tests/testPose2.cpp | 17 +++++++++++++++++ gtsam/geometry/tests/testPose3.cpp | 15 +++++++++++++++ gtsam/geometry/tests/testRot2.cpp | 14 ++++++++++++++ 11 files changed, 140 insertions(+), 4 deletions(-) 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; From 60f93850063fe0dc381b2b48ca8cd411d5273620 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 10 Mar 2025 21:30:50 -0400 Subject: [PATCH 3/5] FrobeniusFactors now work for any Matrix Lie group with vec --- gtsam/slam/FrobeniusFactor.h | 86 ++++++++-------- gtsam/slam/slam.i | 12 +-- gtsam/slam/tests/testFrobeniusFactor.cpp | 126 ++++++++++++++++++----- 3 files changed, 149 insertions(+), 75 deletions(-) diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 37efa5f24..af63f2d39 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -33,11 +33,10 @@ namespace gtsam { * model with sigma=1.0. * If not, we we check if the d-dimensional noise model on rotations is * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an - * error. - * If the noise model is a robust error model, we use the sigmas of the + * error. If the noise model is a robust error model, we use the sigmas of the * underlying noise model. * - * If defaultToUnit == false throws an exception on unexepcted input. + * If defaultToUnit == false throws an exception on unexpected input. */ GTSAM_EXPORT SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t n, @@ -47,29 +46,30 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n, * FrobeniusPrior calculates the Frobenius norm between a given matrix and an * element of SO(3) or SO(4). */ -template -class FrobeniusPrior : public NoiseModelFactorN { - inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; - using MatrixNN = typename Rot::MatrixNN; +template +class FrobeniusPrior : public NoiseModelFactorN { + inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime; + inline constexpr static auto Dim = N * N; + using MatrixNN = Eigen::Matrix; Eigen::Matrix vecM_; ///< vectorized matrix to approximate public: // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor1::evaluateError; + using NoiseModelFactor1::evaluateError; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Constructor FrobeniusPrior(Key j, const MatrixNN& M, const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j) { + : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j) { vecM_ << Eigen::Map(M.data(), Dim, 1); } - /// Error is just Frobenius norm between Rot element and vectorized matrix M. - Vector evaluateError(const Rot& R, OptionalMatrixType H) const override { - return R.vec(H) - vecM_; // Jacobian is computed only when needed. + /// Error is just Frobenius norm between T element and vectorized matrix M. + Vector evaluateError(const T& g, OptionalMatrixType H) const override { + return g.vec(H) - vecM_; // Jacobian is computed only when needed. } }; @@ -77,24 +77,24 @@ class FrobeniusPrior : public NoiseModelFactorN { * FrobeniusFactor calculates the Frobenius norm between rotation matrices. * The template argument can be any fixed-size SO. */ -template -class FrobeniusFactor : public NoiseModelFactorN { - inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; +template +class FrobeniusFactor : public NoiseModelFactorN { + inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime; + inline constexpr static auto Dim = N * N; public: // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor2::evaluateError; + using NoiseModelFactor2::evaluateError; /// Constructor FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j1, - j2) {} + : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j1, j2) {} /// Error is just Frobenius norm between rotation matrices. - Vector evaluateError(const Rot& R1, const Rot& R2, + Vector evaluateError(const T& T1, const T& T2, OptionalMatrixType H1, OptionalMatrixType H2) const override { - Vector error = R2.vec(H2) - R1.vec(H1); + Vector error = T2.vec(H2) - T1.vec(H1); if (H1) *H1 = -*H1; return error; } @@ -106,17 +106,18 @@ class FrobeniusFactor : public NoiseModelFactorN { * Logmap of the error). This factor is only defined for fixed-dimension types, * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. */ -template -class FrobeniusBetweenFactor : public NoiseModelFactorN { - Rot R12_; ///< measured rotation between R1 and R2 - Eigen::Matrix - R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 - inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; - +template +class FrobeniusBetweenFactor : public NoiseModelFactorN { + T T12_; ///< measured rotation between T1 and T2 + Eigen::Matrix + T2hat_H_T1_; ///< fixed derivative of T2hat wrpt T1 + inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime; + inline constexpr static auto Dim = N * N; + public: // Provide access to the Matrix& version of evaluateError: - using NoiseModelFactor2::evaluateError; + using NoiseModelFactor2::evaluateError; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -124,12 +125,11 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { /// @{ /// Construct from two keys and measured rotation - FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, + FrobeniusBetweenFactor(Key j1, Key j2, const T& T12, const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN( - ConvertNoiseModel(model, Dim), j1, j2), - R12_(R12), - R2hat_H_R1_(R12.inverse().AdjointMap()) {} + : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j1, j2), + T12_(T12), + T2hat_H_T1_(T12.inverse().AdjointMap()) {} /// @} /// @name Testable @@ -139,10 +139,10 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { void print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name()) + std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(T).name()) << ">(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - traits::Print(R12_, " R12: "); + traits::Print(T12_, " T12: "); this->noiseModel_->print(" noise model: "); } @@ -150,21 +150,21 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { bool equals(const NonlinearFactor &expected, double tol = 1e-9) const override { auto e = dynamic_cast(&expected); - return e != nullptr && NoiseModelFactorN::equals(*e, tol) && - traits::Equals(this->R12_, e->R12_, tol); + return e != nullptr && NoiseModelFactorN::equals(*e, tol) && + traits::Equals(this->T12_, e->T12_, tol); } /// @} /// @name NoiseModelFactorN methods /// @{ - /// Error is Frobenius norm between R1*R12 and R2. - Vector evaluateError(const Rot& R1, const Rot& R2, + /// Error is Frobenius norm between T1*T12 and T2. + Vector evaluateError(const T& T1, const T& T2, OptionalMatrixType H1, OptionalMatrixType H2) const override { - const Rot R2hat = R1.compose(R12_); - Eigen::Matrix vec_H_R2hat; - Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); - if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; + const T T2hat = T1.compose(T12_); + Eigen::Matrix vec_H_T2hat; + Vector error = T2.vec(H2) - T2hat.vec(H1 ? &vec_H_T2hat : nullptr); + if (H1) *H1 = -vec_H_T2hat * T2hat_H_T1_; return error; } /// @} diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 1761a6cc3..bd728e489 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -403,21 +403,21 @@ gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, size_t d); -template +template virtual class FrobeniusFactor : gtsam::NoiseModelFactor { FrobeniusFactor(size_t key1, size_t key2); FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); - gtsam::Vector evaluateError(const T& R1, const T& R2); + gtsam::Vector evaluateError(const T& T1, const T& T2); }; -template +template virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& T12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& T12, gtsam::noiseModel::Base* model); - gtsam::Vector evaluateError(const T& R1, const T& R2); + gtsam::Vector evaluateError(const T& T1, const T& T2); }; #include diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp index 321b54c86..dc27a7702 100644 --- a/gtsam/slam/tests/testFrobeniusFactor.cpp +++ b/gtsam/slam/tests/testFrobeniusFactor.cpp @@ -9,18 +9,20 @@ * -------------------------------------------------------------------------- */ -/** - * testFrobeniusFactor.cpp - * - * @file testFrobeniusFactor.cpp - * @date March 2019 - * @author Frank Dellaert - * @brief Check evaluateError for various Frobenius norm - */ + /** + * testFrobeniusFactor.cpp + * + * @file testFrobeniusFactor.cpp + * @date March 2019 + * @author Frank Dellaert + * @brief Check evaluateError for various Frobenius norms + */ #include #include #include +#include +#include #include #include #include @@ -30,17 +32,16 @@ #include -using namespace std; using namespace gtsam; -//****************************************************************************** +/* ************************************************************************* */ namespace so3 { -SO3 id; -Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished(); -SO3 R1 = SO3::Expmap(v1); -Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished(); -SO3 R2 = SO3::Expmap(v2); -SO3 R12 = R1.between(R2); + SO3 id; + Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished(); + SO3 R1 = SO3::Expmap(v1); + Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished(); + SO3 R2 = SO3::Expmap(v2); + SO3 R12 = R1.between(R2); } // namespace so3 /* ************************************************************************* */ @@ -61,8 +62,8 @@ TEST(FrobeniusPriorSO3, ClosestTo) { // Example top-left of SO(4) matrix not quite on SO(3) manifold Matrix3 M; M << 0.79067393, 0.6051136, -0.0930814, // - 0.4155925, -0.64214347, -0.64324489, // - -0.44948549, 0.47046326, -0.75917576; + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; SO3 expected = SO3::ClosestTo(M); @@ -89,7 +90,7 @@ TEST(FrobeniusPriorSO3, ChordalL2mean) { SO3 expected; // identity Matrix3 M = R1.matrix() + R1.matrix().transpose(); EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6)); - EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6)); + EXPECT(assert_equal(expected, SO3::ChordalMean({ R1, R1.inverse() }), 1e-6)); // manifold optimization gets same result as ChordalMean NonlinearFactorGraph graph; @@ -148,13 +149,13 @@ TEST(FrobeniusBetweenFactorSO3, evaluateError) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } -//****************************************************************************** +/* ************************************************************************* */ namespace so4 { -SO4 id; -Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); -SO4 Q1 = SO4::Expmap(v1); -Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished(); -SO4 Q2 = SO4::Expmap(v2); + SO4 id; + Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); + SO4 Q1 = SO4::Expmap(v1); + Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished(); + SO4 Q2 = SO4::Expmap(v2); } // namespace so4 /* ************************************************************************* */ @@ -174,7 +175,7 @@ TEST(FrobeniusFactorSO4, evaluateError) { /* ************************************************************************* */ TEST(FrobeniusBetweenFactorSO4, evaluateError) { using namespace ::so4; - Matrix4 M{I_4x4}; + Matrix4 M{ I_4x4 }; M.topLeftCorner<3, 3>() = ::so3::R12.matrix(); auto factor = FrobeniusBetweenFactor(1, 2, Q1.between(Q2)); Matrix H1, H2; @@ -188,6 +189,79 @@ TEST(FrobeniusBetweenFactorSO4, evaluateError) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } +/* ************************************************************************* */ +namespace pose2 { + Pose2 id; + Pose2 P1 = Pose2(0.1, 0.2, 0.3); + Pose2 P2 = Pose2(0.4, 0.5, 0.6); +} // namespace pose2 + +/* ************************************************************************* */ +TEST(FrobeniusFactorPose2, evaluateError) { + using namespace ::pose2; + auto factor = FrobeniusFactor(1, 2, noiseModel::Unit::Create(3)); + Vector actual = factor.evaluateError(P1, P2); + Vector expected = P2.vec() - P1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, P1); + values.insert(2, P2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorPose2, evaluateError) { + using namespace ::pose2; + auto factor = FrobeniusBetweenFactor(1, 2, P1.between(P2)); + Matrix H1, H2; + Vector actual = factor.evaluateError(P1, P2, H1, H2); + Vector expected = Vector9::Zero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, P1); + values.insert(2, P2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +namespace pose3 { + Pose3 id; + Pose3 P1 = Pose3(Rot3::Expmap(Vector3(0.1, 0.2, 0.3)), Vector3(0.4, 0.5, 0.6)); + Pose3 P2 = Pose3(Rot3::Expmap(Vector3(0.2, 0.3, 0.4)), Vector3(0.7, 0.8, 0.9)); +} // namespace pose3 + +/* ************************************************************************* */ +TEST(FrobeniusFactorPose3, evaluateError) { + using namespace ::pose3; + auto factor = FrobeniusFactor(1, 2, noiseModel::Unit::Create(12)); + Vector actual = factor.evaluateError(P1, P2); + Vector expected = P2.vec() - P1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, P1); + values.insert(2, P2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorPose3, evaluateError) { + using namespace ::pose3; + auto factor = FrobeniusBetweenFactor(1, 2, P1.between(P2)); + Matrix H1, H2; + Vector actual = factor.evaluateError(P1, P2, H1, H2); + Vector expected(16); + expected.setZero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, P1); + values.insert(2, P2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; From b9260adf09da8df5ca3151777c18240f9b8ef8af Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 10 Mar 2025 21:53:21 -0400 Subject: [PATCH 4/5] Don't use column --- gtsam/geometry/tests/testUnit3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 855e50a42..64a9ed071 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -67,7 +67,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) { TEST(Unit3, rotate) { Rot3 R = Rot3::Yaw(0.5); Unit3 p(1, 0, 0); - Unit3 expected = Unit3(R.column(1)); + Unit3 expected = Unit3(R.matrix().col(0)); Unit3 actual = R * p; EXPECT(assert_equal(expected, actual, 1e-5)); Matrix actualH, expectedH; From 7750dec317e0e3852fb0e1c87d427fc3cde71428 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 10 Mar 2025 22:22:18 -0400 Subject: [PATCH 5/5] One more straggler --- gtsam/geometry/geometry.i | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 2430105e9..e01394bfe 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -395,7 +395,6 @@ class Rot3 { // Standard Interface gtsam::Matrix matrix() const; gtsam::Matrix transpose() const; - gtsam::Point3 column(size_t index) const; gtsam::Vector xyz() const; gtsam::Vector ypr() const; gtsam::Vector rpy() const;