Fixing 4.3.1-ros gtsam compatibility (#1783)

* Fixing 4.3.1-ros gtsam compatibility

* javobian fix in SwitchVariable

* Fix GTSAM scalar switch traits, Jacobians and attitude API detection

* Separate sigmoid factor correction from GTSAM compatibility

* Correct sigmoid switch Jacobian and add focused factor regression

* combined gtsam tests in same file

* cmake refactor

---------

Co-authored-by: Frank Dellaert <[email protected]>

CMakeLists.txt

@@ -620,14 +620,7 @@ IF(WITH_GTSAM)
     # Force config mode to ignore PCL's findGTSAM.cmake file
     FIND_PACKAGE(GTSAM CONFIG QUIET)
     IF(GTSAM_FOUND)
-      # For issue https://github.com/introlab/rtabmap/pull/1626
+      INCLUDE(${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/CheckGTSAMFeatures.cmake)
-      FIND_FILE(GTSAM_NOISE_MODEL_FACTOR_N_FILE gtsam/nonlinear/NoiseModelFactorN.h
-         PATHS ${GTSAM_INCLUDE_DIR}
-         NO_DEFAULT_PATH)
-      IF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
-         MESSAGE(STATUS "GTSAM with NoiseModelFactorN.h")
-         ADD_DEFINITIONS("-DGTSAM_WITH_NOISE_MODEL_FACTOR_N")
-      ENDIF(GTSAM_NOISE_MODEL_FACTOR_N_FILE)
     ENDIF(GTSAM_FOUND)
 ENDIF(WITH_GTSAM)
 
@@ -1064,6 +1057,7 @@ IF(NOT MSVC)
 ENDIF()
 
 
+
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
   IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))

cmake_modules/CheckGTSAMFeatures.cmake

@@ -0,0 +1,27 @@
+# Detects GTSAM API variations that the version number alone can't tell
+# apart. Included after FIND_PACKAGE(GTSAM) succeeded.
+
+# Pose3AttitudeFactor has been replaced by AttitudeFactor<Pose3> in 4.3, but
+# the 4.3 ROS snapshots share a numeric version (4.3.0) while exposing either
+# API, so probe which one compiles. Older versions only have
+# Pose3AttitudeFactor. The probe links the imported gtsam target, so it
+# inherits GTSAM's usage requirements (including cxx_std_17 for 4.3) and
+# doesn't depend on the C++ standard selected later in the main CMakeLists.txt.
+IF(GTSAM_VERSION VERSION_GREATER_EQUAL "4.3.0")
+   INCLUDE(CheckCXXSourceCompiles)
+   INCLUDE(CMakePushCheckState)
+   CMAKE_PUSH_CHECK_STATE(RESET)
+   SET(CMAKE_REQUIRED_LIBRARIES gtsam)
+   UNSET(RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE CACHE)
+   CHECK_CXX_SOURCE_COMPILES("
+      #include <gtsam/navigation/AttitudeFactor.h>
+      int main() {
+         gtsam::AttitudeFactor<gtsam::Pose3> factor(1, gtsam::Unit3(0,0,1),
+            gtsam::noiseModel::Isotropic::Sigma(2, 1.0));
+         return factor.evaluateError(gtsam::Pose3()).size() != 2;
+      }" RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE)
+   CMAKE_POP_CHECK_STATE()
+   IF(RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE)
+      ADD_DEFINITIONS(-DRTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE)
+   ENDIF()
+ENDIF()

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -504,9 +504,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					gtsam::Unit3 nZ(0,0,1);
 					gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ);
 					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma());
-#if GTSAM_VERSION_NUMERIC <= 40300
+#ifndef RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE
-					// Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here.
-					//       Just replace "<=" above by "<" to use AttitudeFactor<Pose3> below.
 					graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
 #else
 					graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -100,7 +100,8 @@ namespace vertigo {
         // handle derivatives
         if (H1) *H1 = *H1 * w;
         if (H2) *H2 = *H2 * w;
-        if (H3) *H3 = error /* (w*(1.0-w))*/;  // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x
+        // error already includes w; sigmoid's derivative is w*(1-w).
+        if (H3) *H3 = error * (1.0-w);
 
         return error;
       };

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -13,6 +13,7 @@
 // DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
@@ -45,6 +46,7 @@ namespace vertigo {
     }
 
     // Manifold requirements
+    static constexpr int dimension = 1;
 
     /** Returns dimensionality of the tangent space */
     inline size_t dim() const { return 1; }
@@ -61,7 +63,13 @@ namespace vertigo {
     }
 
     /** @return the local coordinates of another object */
-    inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
+    inline gtsam::Vector1 localCoordinates(const SwitchVariableLinear& t2,
+        gtsam::OptionalJacobian<1, 1> H1 = {},
+        gtsam::OptionalJacobian<1, 1> H2 = {}) const {
+      if (H1) *H1 = -gtsam::Matrix11::Identity();
+      if (H2) *H2 = gtsam::Matrix11::Identity();
+      return gtsam::Vector1(t2.value() - value());
+    }
 
     // Group requirements
 
@@ -108,36 +116,9 @@ namespace vertigo {
 }
 
 namespace gtsam {
-// Define Key to be Testable by specializing gtsam::traits
+// Use the scalar manifold's dimension, category and chart operations.
-template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableLinear>
-template<> struct traits<vertigo::SwitchVariableLinear> {
+    : internal::Manifold<vertigo::SwitchVariableLinear> {};
-  static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") {
-    key.print(str);
-  }
-  static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) {
-    return key1.equals(key2, tol);
-  }
-  static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();}
-
-  typedef OptionalJacobian<3, 3> ChartJacobian;
-  typedef gtsam::Vector TangentVector;
-  static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
-#if GTSAM_VERSION_NUMERIC >= 40300
-      ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
-#else
-      ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
-#endif
-    return origin.localCoordinates(other);
-  }
-  static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
-#if GTSAM_VERSION_NUMERIC >= 40300
-      ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
-#else
-      ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
-#endif
-      return g.retract(v);
-    }
-};
 }
 
 

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -13,6 +13,7 @@
 // DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
@@ -45,6 +46,7 @@ namespace vertigo {
     }
 
     // Manifold requirements
+    static constexpr int dimension = 1;
 
     /** Returns dimensionality of the tangent space */
     inline size_t dim() const { return 1; }
@@ -61,7 +63,13 @@ namespace vertigo {
     }
 
     /** @return the local coordinates of another object */
-    inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); }
+    inline gtsam::Vector1 localCoordinates(const SwitchVariableSigmoid& t2,
+        gtsam::OptionalJacobian<1, 1> H1 = {},
+        gtsam::OptionalJacobian<1, 1> H2 = {}) const {
+      if (H1) *H1 = -gtsam::Matrix11::Identity();
+      if (H2) *H2 = gtsam::Matrix11::Identity();
+      return gtsam::Vector1(t2.value() - value());
+    }
 
     // Group requirements
 
@@ -109,36 +117,9 @@ namespace vertigo {
 
 
 namespace gtsam {
-// Define Key to be Testable by specializing gtsam::traits
+// Use the scalar manifold's dimension, category and chart operations.
-template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableSigmoid>
-template<> struct traits<vertigo::SwitchVariableSigmoid> {
+    : internal::Manifold<vertigo::SwitchVariableSigmoid> {};
-  static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") {
-    key.print(str);
-  }
-  static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) {
-    return key1.equals(key2, tol);
-  }
-  static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();}
-
-  typedef OptionalJacobian<3, 3> ChartJacobian;
-  typedef gtsam::Vector TangentVector;
-  static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
-#if GTSAM_VERSION_NUMERIC >= 40300
-	  ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
-#else
-	  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
-#endif
-    return origin.localCoordinates(other);
-  }
-  static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
-#if GTSAM_VERSION_NUMERIC >= 40300
-        ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
-#else
-        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
-#endif
-      return g.retract(v);
-    }
-};
 }
 
 #endif /* SWITCHVARIABLESIGMOID_H_ */

corelib/test/CMakeLists.txt

@@ -68,9 +68,20 @@ set(corelib_test_sources
 	test_sensorcapturethread.cpp        #SensorCaptureThread.h
 )
 
+# test_optimizer_gtsam.cpp includes the Vertigo GTSAM factors from
+# corelib/src/optimizer directly, so it needs GTSAM itself (rtabmap_core links
+# it PRIVATE).
+IF(GTSAM_FOUND)
+	list(APPEND corelib_test_sources test_optimizer_gtsam.cpp)  #OptimizerGTSAM.h (Vertigo factors, gravity)
+ENDIF(GTSAM_FOUND)
+
 add_executable(test_corelib ${corelib_test_sources})
 target_link_libraries(test_corelib gtest_main rtabmap_core)
 
+IF(GTSAM_FOUND)
+	target_link_libraries(test_corelib gtsam)
+ENDIF(GTSAM_FOUND)
+
 # test_registrationicp.cpp includes corelib/src/icp/libpointmatcher.h directly
 # to test the LaserScan <-> DataPoints conversions, so it needs the library
 # itself (rtabmap_core links it PRIVATE and only re-exports its include dirs).

corelib/test/test_optimizer_gtsam.cpp

@@ -0,0 +1,285 @@
+// Tests for the GTSAM pieces used by rtabmap::OptimizerGTSAM, independent of
+// the optimizer itself:
+//   - Vertigo switch variables (linear and sigmoid): scalar manifold traits,
+//     1x1 Local Jacobians, priors, and constructor/retract clamping.
+//   - Switchable between factors: residuals and Jacobians against the
+//     release's BetweenFactor, the switch derivative against finite
+//     differences, and linearization.
+//   - Gravity (attitude) factor API selected by CMake
+//     (RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE).
+//
+// Background: Optimizer/Robust=true adds, for every loop closure, a switch
+// variable s_ij with a prior, and replaces the loop closure's BetweenFactor
+// by a switchable one whose residual is weighted by s_ij (linear switch) or
+// sigmoid(s_ij) (sigmoid switch). The optimizer can then turn off outlier
+// loop closures by driving their weight to 0. These factors live in
+// corelib/src/optimizer/vertigo/gtsam and depend on GTSAM internals (traits,
+// OptionalJacobian, PriorFactor), which changed across 4.0/4.2/4.3, hence the
+// focused checks below. They are meant to pass on every GTSAM version
+// rtabmap supports.
+
+#include <gtest/gtest.h>
+#include <gtsam/config.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/navigation/AttitudeFactor.h>
+
+#include "../src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h"
+
+#include <cmath>
+#include <sstream>
+
+using namespace gtsam;
+
+namespace {
+
+// Compares matrices (or vectors) with the same tolerance everywhere. A size
+// mismatch is reported separately: a Jacobian of the wrong size is exactly the
+// kind of regression these tests target (e.g., the 3x3 Jacobians the switch
+// traits used to declare for a 1-D variable, which made JacobianFactor throw
+// InvalidMatrixBlock during linearization).
+::testing::AssertionResult near(const Matrix& actual, const Matrix& expected)
+{
+	if(actual.rows() != expected.rows() || actual.cols() != expected.cols())
+	{
+		return ::testing::AssertionFailure() << "dimensions " << actual.rows() << "x" << actual.cols()
+				<< " instead of " << expected.rows() << "x" << expected.cols();
+	}
+	if(!actual.allFinite() || (actual - expected).norm() >= 1e-6)
+	{
+		std::stringstream ss;
+		ss << "Actual:\n" << actual << "\nExpected:\n" << expected;
+		return ::testing::AssertionFailure() << ss.str();
+	}
+	return ::testing::AssertionSuccess();
+}
+
+// Checks that a switch variable behaves as a 1-D manifold for GTSAM:
+//  1. traits<Switch>::dimension is 1 (required by noiseModel::Unit::Create()
+//     and fixed-size code paths in GTSAM >= 4.3).
+//  2. localCoordinates(y) = y - x, with analytical Jacobians -1 (wrt x) and
+//     +1 (wrt y).
+//  3. A PriorFactor on the switch (what OptimizerGTSAM adds for every switch)
+//     gives a scalar residual x - prior and a 1x1 identity Jacobian, matching
+//     a numerical derivative, and can be linearized. Depending on the GTSAM
+//     version/configuration (GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR), the prior
+//     takes its Jacobian from traits<Switch>::Local(), so this is where
+//     incomplete traits show up.
+//  4. On GTSAM >= 4.3, the same with a prior built without a noise model,
+//     which goes through noiseModel::Unit::Create(value) and so needs the
+//     full manifold traits (dimension, structure_category, ManifoldType).
+template<class Switch>
+void checkSwitch(double value, double other)
+{
+	static_assert(traits<Switch>::dimension == 1, "scalar tangent");
+	const Switch x(value), y(other);
+
+	// Chart and its Jacobians
+	Matrix11 h1, h2;
+	EXPECT_TRUE(near(x.localCoordinates(y, h1, h2), Vector1(other - value)));
+	EXPECT_TRUE(near(h1, -Matrix11::Identity()));
+	EXPECT_TRUE(near(h2, Matrix11::Identity()));
+
+	// Prior with an explicit noise model, as created by OptimizerGTSAM
+	const PriorFactor<Switch> prior(1, y, noiseModel::Isotropic::Sigma(1, 1));
+	Matrix h;
+	EXPECT_TRUE(near(prior.evaluateError(x, h), Vector1(value - other)));
+	EXPECT_TRUE(near(h, Matrix11::Identity()));
+	EXPECT_TRUE(near(h, numericalDerivative11<Vector, Switch>(
+		[&prior](const Switch& s) { return prior.evaluateError(s); }, x)));
+	Values values;
+	values.insert(1, x);
+	EXPECT_TRUE(bool(prior.linearize(values))) << "explicit prior linearization";
+
+#if GTSAM_VERSION_NUMERIC >= 40300
+	// Prior with the default (unit) noise model. Default noise was added in
+	// 4.3; 4.2 requires the explicit model above.
+	const PriorFactor<Switch> unitPrior(1, y);
+	EXPECT_TRUE(near(unitPrior.evaluateError(x, h), Vector1(value - other)));
+	EXPECT_TRUE(near(h, Matrix11::Identity()));
+	EXPECT_TRUE(bool(unitPrior.linearize(values))) << "default prior linearization";
+#endif
+}
+
+// Checks BetweenFactorSwitchableLinear, the robust loop closure factor used
+// by OptimizerGTSAM: residual = s * BetweenFactor residual.
+//  - The pose Jacobians must be the regular BetweenFactor Jacobians scaled by
+//    the switch weight. They are compared against the BetweenFactor of the
+//    installed GTSAM rather than against numerical derivatives, because older
+//    GTSAM (without GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR) uses an approximate
+//    Local Jacobian for poses. When the exact one is enabled, they are also
+//    compared against numerical derivatives.
+//  - The switch Jacobian (d residual / d s = raw residual) is always compared
+//    against a numerical derivative.
+//  - The factor, with the two poses and the switch in a Values, must
+//    linearize (which checks that all Jacobian sizes agree with the residual).
+template<class Pose>
+void checkBetweenLinear(const Pose& first, const Pose& second, double switchValue)
+{
+	const vertigo::SwitchVariableLinear s(switchValue);
+	const auto model = noiseModel::Isotropic::Sigma(traits<Pose>::dimension, 1);
+	const vertigo::BetweenFactorSwitchableLinear<Pose> factor(1, 2, 3, Pose(), model);
+	Matrix h1, h2, h3;
+#if GTSAM_VERSION_NUMERIC >= 40300
+	const Vector error = factor.evaluateError(first, second, s, &h1, &h2, &h3);
+#else
+	const Vector error = factor.evaluateError(first, second, s, h1, h2, h3);
+#endif
+	// Same measurement without switch: its residual ratio gives the weight
+	// actually applied, which must also scale the pose Jacobians.
+	Matrix rawH1, rawH2;
+	const BetweenFactor<Pose> raw(1, 2, Pose(), model);
+	const Vector rawError = raw.evaluateError(first, second, rawH1, rawH2);
+	const double weight = error.norm() / rawError.norm();
+	EXPECT_TRUE(near(h1, rawH1 * weight));
+	EXPECT_TRUE(near(h2, rawH2 * weight));
+#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
+	EXPECT_TRUE(near(h1, numericalDerivative11<Vector, Pose>(
+		[&](const Pose& p) { return factor.evaluateError(p, second, s); }, first)));
+	EXPECT_TRUE(near(h2, numericalDerivative11<Vector, Pose>(
+		[&](const Pose& p) { return factor.evaluateError(first, p, s); }, second)));
+#endif
+	EXPECT_TRUE(near(h3, numericalDerivative11<Vector, vertigo::SwitchVariableLinear>(
+		[&](const vertigo::SwitchVariableLinear& x) { return factor.evaluateError(first, second, x); }, s)));
+	EXPECT_TRUE(error.allFinite());
+
+	Values values;
+	values.insert(1, first); values.insert(2, second); values.insert(3, s);
+	EXPECT_TRUE(bool(factor.linearize(values))) << "switchable linearization";
+}
+
+// Checks BetweenFactorSwitchableSigmoid: residual = w * BetweenFactor
+// residual, with w = sigmoid(s) = 1/(1+exp(-s)).
+//  - Residual and pose Jacobians must be the BetweenFactor ones scaled by w.
+//  - The switch Jacobian must be raw residual * dw/ds = raw * w*(1-w), both
+//    analytically and by central finite differences. This catches the bug
+//    where it returned the weighted residual (raw * w), missing the (1-w)
+//    factor.
+//  - The factor must linearize.
+template<class Pose>
+void checkBetweenSigmoid(const Pose& second, double switchValue)
+{
+	const Pose first;
+	const vertigo::SwitchVariableSigmoid s(switchValue);
+	const auto model = noiseModel::Isotropic::Sigma(traits<Pose>::dimension, 1);
+	const vertigo::BetweenFactorSwitchableSigmoid<Pose> factor(1, 2, 3, Pose(), model);
+	Matrix h1, h2, h3;
+#if GTSAM_VERSION_NUMERIC >= 40300
+	const Vector error = factor.evaluateError(first, second, s, &h1, &h2, &h3);
+#else
+	const Vector error = factor.evaluateError(first, second, s, h1, h2, h3);
+#endif
+	Matrix rawH1, rawH2;
+	const BetweenFactor<Pose> raw(1, 2, Pose(), model);
+	const Vector rawError = raw.evaluateError(first, second, rawH1, rawH2);
+	const double weight = 1.0 / (1.0 + std::exp(-switchValue));
+	EXPECT_TRUE(near(error, rawError * weight));
+	EXPECT_TRUE(near(h1, rawH1 * weight));
+	EXPECT_TRUE(near(h2, rawH2 * weight));
+	// Analytical: d(sigmoid)/ds = w*(1-w)
+	EXPECT_TRUE(near(h3, rawError * (weight * (1.0 - weight))));
+	// Numerical: central difference over the switch value. The switch is
+	// re-constructed rather than retracted, as retract() clamps it.
+	const double step = 1e-5;
+	const Vector plus = factor.evaluateError(first, second,
+		vertigo::SwitchVariableSigmoid(switchValue + step));
+	const Vector minus = factor.evaluateError(first, second,
+		vertigo::SwitchVariableSigmoid(switchValue - step));
+	EXPECT_TRUE(near(h3, (plus - minus) / (2.0 * step)));
+
+	Values values;
+	values.insert(1, first); values.insert(2, second); values.insert(3, s);
+	EXPECT_TRUE(bool(factor.linearize(values))) << "sigmoid linearization";
+}
+
+} // namespace
+
+// Linear switch (the one OptimizerGTSAM uses) as a 1-D GTSAM manifold.
+TEST(OptimizerGTSAM, SwitchVariableLinearManifold)
+{
+	checkSwitch<vertigo::SwitchVariableLinear>(0.4, 0.7);
+}
+
+// Sigmoid switch as a 1-D GTSAM manifold (negative values are valid for it).
+TEST(OptimizerGTSAM, SwitchVariableSigmoidManifold)
+{
+	checkSwitch<vertigo::SwitchVariableSigmoid>(-0.4, 0.7);
+}
+
+// Switch bounds: retract() (applied at each optimization step) projects the
+// linear switch to [0,1] and the sigmoid switch to [-10,10]. The constructor
+// doesn't clamp the linear switch, but does clamp the sigmoid one. Moving the
+// traits to internal::Manifold must not change this behavior.
+TEST(OptimizerGTSAM, SwitchVariableClamping)
+{
+	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableLinear(0.4).retract(Vector1(2)).value(), 1.0);
+	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableLinear(0.4).retract(Vector1(-2)).value(), 0.0);
+	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableLinear(2).value(), 2.0);
+	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(0).retract(Vector1(20)).value(), 10.0);
+	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(0).retract(Vector1(-20)).value(), -10.0);
+	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(20).value(), 10.0);
+	EXPECT_DOUBLE_EQ(vertigo::SwitchVariableSigmoid(-20).value(), -10.0);
+}
+
+// Robust loop closure factor with a linear switch, for 2D (Optimizer/Slam2D)
+// and 3D graphs, with a partially-on switch (0.4).
+TEST(OptimizerGTSAM, BetweenFactorSwitchableLinear)
+{
+	checkBetweenLinear(Pose2(), Pose2(1, 2, 0.2), 0.4);
+	checkBetweenLinear(Pose3(), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3)), 0.4);
+}
+
+// Robust loop closure factor with a sigmoid switch, for 2D and 3D graphs.
+// Switch values -2, 0 and 2 give weights ~0.12, 0.5 and ~0.88, staying away
+// from the [-10,10] clamps.
+TEST(OptimizerGTSAM, BetweenFactorSwitchableSigmoid)
+{
+	for(double s : {-2.0, 0.0, 2.0})
+	{
+		SCOPED_TRACE(s);
+		checkBetweenSigmoid(Pose2(1, 2, 0.2), s);
+		checkBetweenSigmoid(Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3)), s);
+	}
+}
+
+// Gravity constraints (Link::kGravity, Optimizer/GravitySigma > 0) are added
+// as a Pose3 attitude factor. Its class changed name in GTSAM 4.3
+// (Pose3AttitudeFactor -> AttitudeFactor<Pose3>), and some ROS 4.3 snapshots
+// report the same version number with either API, so CMake detects which one
+// compiles. This checks the detected API is usable as OptimizerGTSAM uses it:
+//  - Jacobian matches a numerical derivative on a tilted pose.
+//  - Residual is zero when the pose is aligned with gravity.
+//  - Optimizing a tilted pose (with a loose prior to fix the yaw and the
+//    translation, which gravity doesn't observe) removes the tilt.
+TEST(OptimizerGTSAM, GravityFactor)
+{
+#ifdef RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE
+	using GravityFactor = AttitudeFactor<Pose3>;
+#else
+	using GravityFactor = Pose3AttitudeFactor;
+#endif
+	// Reference direction: world z axis; measured in body frame: also z (pose
+	// should be level).
+	const GravityFactor gravity(1, Unit3(0,0,1), noiseModel::Isotropic::Sigma(2, 0.1));
+	const Pose3 initial(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1, 2, 3));
+	Matrix h;
+	const Vector error = gravity.evaluateError(initial, h);
+	EXPECT_TRUE(near(h, numericalDerivative11<Vector, Pose3>(
+		[&](const Pose3& p) { return gravity.evaluateError(p); }, initial)));
+	EXPECT_TRUE(near(gravity.evaluateError(Pose3()), Vector2::Zero()));
+
+	NonlinearFactorGraph graph;
+	graph.add(gravity);
+	graph.add(PriorFactor<Pose3>(1, Pose3(), noiseModel::Isotropic::Sigma(6, 1)));
+	Values values;
+	values.insert(1, initial);
+	const Values result = LevenbergMarquardtOptimizer(graph, values).optimize();
+	EXPECT_LT(gravity.evaluateError(result.at<Pose3>(1)).norm(), error.norm()*0.01)
+		<< "gravity optimization reduces tilt";
+}