Merge pull request #867 from borglab/feature/between-factor-cmake-flag

BetweenFactor Jacobian CMake flag
borglab · Oct 21, 2021 · 0a6a6b0 · 0a6a6b0
2 parents 6eb01aa + f9703e9
commit 0a6a6b0
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 15 deletions.
diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake
@@ -14,20 +14,21 @@ if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_UNSTABLE_BUILD_PYTHON       "Enable/Disable Python wrapper for libgtsam_unstable" ON)
     option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
 endif()
-option(BUILD_SHARED_LIBS                 "Build shared gtsam library, instead of static" ON)
-option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
-option(GTSAM_POSE3_EXPMAP                "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
-option(GTSAM_ROT3_EXPMAP                 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
-option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
-option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
-option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" OFF)
-option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
-option(GTSAM_THROW_CHEIRALITY_EXCEPTION  "Throw exception when a triangulated point is behind a camera" ON)
-option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module with pybind11" OFF)
-option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
-option(GTSAM_ALLOW_DEPRECATED_SINCE_V41  "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
-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(BUILD_SHARED_LIBS                    "Build shared gtsam library, instead of static" ON)
+option(GTSAM_USE_QUATERNIONS                "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
+option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
+option(GTSAM_ROT3_EXPMAP                    "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
+option(GTSAM_ENABLE_CONSISTENCY_CHECKS      "Enable/Disable expensive consistency checks"       OFF)
+option(GTSAM_WITH_TBB                       "Use Intel Threaded Building Blocks (TBB) if available" ON)
+option(GTSAM_WITH_EIGEN_MKL                 "Eigen will use Intel MKL if available" OFF)
+option(GTSAM_WITH_EIGEN_MKL_OPENMP          "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
+option(GTSAM_THROW_CHEIRALITY_EXCEPTION     "Throw exception when a triangulated point is behind a camera" ON)
+option(GTSAM_BUILD_PYTHON                   "Enable/Disable building & installation of Python module with pybind11" OFF)
+option(GTSAM_INSTALL_MATLAB_TOOLBOX         "Enable/Disable installation of matlab toolbox"  OFF)
+option(GTSAM_ALLOW_DEPRECATED_SINCE_V41     "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
+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)
 if(NOT MSVC AND NOT XCODE_VERSION)
     option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
 endif()

diff --git a/gtsam/config.h.in b/gtsam/config.h.in
@@ -80,3 +80,6 @@
 
 // Whether to use the system installed Metis instead of the provided one
 #cmakedefine GTSAM_USE_SYSTEM_METIS
+
+// Toggle switch for BetweenFactor jacobian computation
+#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
@@ -103,7 +103,7 @@ namespace gtsam {
       boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
       // manifold equivalent of h(x)-z -> log(z,h(x))
-#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR
+#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
       typename traits<T>::ChartJacobian::Jacobian Hlocal;
       Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
       if (H1) *H1 = Hlocal * (*H1);