Merge pull request #1520 from borglab/fan/cmake_rot2_option

borglab · Jul 2, 2023 · 7703b7d · 7703b7d
2 parents 4c0cd22 + d87db0a
commit 7703b7d
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 4 deletions.
diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake
@@ -31,6 +31,7 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V43     "Allow use of methods/functions depr
 option(GTSAM_SUPPORT_NESTED_DISSECTION      "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION         "Use new ImuFactor with integration on tangent space" ON)
 option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
+option(GTSAM_SLOW_BUT_CORRECT_EXPMAP        "Use slower but correct expmap for Pose2"  OFF)
 
 if (GTSAM_FORCE_SHARED_LIB)
     message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB")

diff --git a/gtsam/config.h.in b/gtsam/config.h.in
@@ -83,3 +83,5 @@
 
 // Toggle switch for BetweenFactor jacobian computation
 #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
+
+#cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
@@ -97,7 +97,7 @@ Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
 
 /* ************************************************************************* */
 Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
-#ifdef SLOW_BUT_CORRECT_EXPMAP
+#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
   return Expmap(v, H);
 #else
   if (H) {
@@ -109,7 +109,7 @@ Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
 }
 /* ************************************************************************* */
 Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) {
-#ifdef SLOW_BUT_CORRECT_EXPMAP
+#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
   return Logmap(r, H);
 #else
   if (H) {

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
@@ -166,7 +166,9 @@ class Rot2 {
 
   // Manifold
   gtsam::Rot2 retract(Vector v) const;
+  gtsam::Rot2 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
   Vector localCoordinates(const gtsam::Rot2& p) const;
+  Vector localCoordinates(const gtsam::Rot2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
 
   // Lie Group
   static gtsam::Rot2 Expmap(Vector v);
@@ -397,19 +399,24 @@ class Pose2 {
   static gtsam::Pose2 Identity();
   gtsam::Pose2 inverse() const;
   gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
+  gtsam::Pose2 compose(const gtsam::Pose2& p2, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
   gtsam::Pose2 between(const gtsam::Pose2& p2) const;
+  gtsam::Pose2 between(const gtsam::Pose2& p2, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
 
   // Operator Overloads
   gtsam::Pose2 operator*(const gtsam::Pose2& p2) const;
 
   // Manifold
   gtsam::Pose2 retract(Vector v) const;
+  gtsam::Pose2 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
   Vector localCoordinates(const gtsam::Pose2& p) const;
+  Vector localCoordinates(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2) const;
 
   // Lie Group
   static gtsam::Pose2 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose2& p);
   Vector logmap(const gtsam::Pose2& p);
+  Vector logmap(const gtsam::Pose2& p, Eigen::Ref<Eigen::MatrixXd> H);
   static Matrix ExpmapDerivative(Vector v);
   static Matrix LogmapDerivative(const gtsam::Pose2& v);
   Matrix AdjointMap() const;

diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
@@ -66,7 +66,7 @@ TEST(Pose2, manifold) {
 /* ************************************************************************* */
 TEST(Pose2, retract) {
   Pose2 pose(M_PI/2.0, Point2(1, 2));
-#ifdef SLOW_BUT_CORRECT_EXPMAP
+#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
   Pose2 expected(1.00811, 2.01528, 2.5608);
 #else
   Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
@@ -204,7 +204,7 @@ TEST(Pose2, Adjoint_hat) {
 TEST(Pose2, logmap) {
   Pose2 pose0(M_PI/2.0, Point2(1, 2));
   Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
-#ifdef SLOW_BUT_CORRECT_EXPMAP
+#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
   Vector3 expected(0.00986473, -0.0150896, 0.018);
 #else
   Vector3 expected(0.01, -0.015, 0.018);