diff --git a/CMakeLists.txt b/CMakeLists.txt index 8bea0b1c..8b55e49f 100644 --- a/CMakeLists.txt +++ b/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 - 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) + INCLUDE(${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/CheckGTSAMFeatures.cmake) 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)) diff --git a/cmake_modules/CheckGTSAMFeatures.cmake b/cmake_modules/CheckGTSAMFeatures.cmake new file mode 100644 index 00000000..0fc16cd4 --- /dev/null +++ b/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 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 + int main() { + gtsam::AttitudeFactor 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() diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index ae5fbf86..3009301f 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -504,9 +504,7 @@ std::map 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 - // Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here. - // Just replace "<=" above by "<" to use AttitudeFactor below. +#ifndef RTABMAP_GTSAM_HAS_ATTITUDE_FACTOR_TEMPLATE graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas)); #else graph.add(gtsam::AttitudeFactor(iter->first, nZ, model, bGMeas)); diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h index 4d8f7850..96ff2a44 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h +++ b/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; }; diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h index 708a7f53..10c3aaba 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h +++ b/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 +#include #include 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 -template struct traits; -template<> struct traits { - 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); - } -}; +// Use the scalar manifold's dimension, category and chart operations. +template<> struct traits + : internal::Manifold {}; } diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h index a949cc2b..c54e73ff 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h +++ b/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 +#include #include 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 -template struct traits; -template<> struct traits { - 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); - } -}; +// Use the scalar manifold's dimension, category and chart operations. +template<> struct traits + : internal::Manifold {}; } #endif /* SWITCHVARIABLESIGMOID_H_ */ diff --git a/corelib/test/CMakeLists.txt b/corelib/test/CMakeLists.txt index ddec5cb7..25b6b114 100644 --- a/corelib/test/CMakeLists.txt +++ b/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). diff --git a/corelib/test/test_optimizer_gtsam.cpp b/corelib/test/test_optimizer_gtsam.cpp new file mode 100644 index 00000000..121a0671 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h" + +#include +#include + +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::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::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 +void checkSwitch(double value, double other) +{ + static_assert(traits::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 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( + [&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 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 +void checkBetweenLinear(const Pose& first, const Pose& second, double switchValue) +{ + const vertigo::SwitchVariableLinear s(switchValue); + const auto model = noiseModel::Isotropic::Sigma(traits::dimension, 1); + const vertigo::BetweenFactorSwitchableLinear 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 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( + [&](const Pose& p) { return factor.evaluateError(p, second, s); }, first))); + EXPECT_TRUE(near(h2, numericalDerivative11( + [&](const Pose& p) { return factor.evaluateError(first, p, s); }, second))); +#endif + EXPECT_TRUE(near(h3, numericalDerivative11( + [&](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 +void checkBetweenSigmoid(const Pose& second, double switchValue) +{ + const Pose first; + const vertigo::SwitchVariableSigmoid s(switchValue); + const auto model = noiseModel::Isotropic::Sigma(traits::dimension, 1); + const vertigo::BetweenFactorSwitchableSigmoid 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 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(0.4, 0.7); +} + +// Sigmoid switch as a 1-D GTSAM manifold (negative values are valid for it). +TEST(OptimizerGTSAM, SwitchVariableSigmoidManifold) +{ + checkSwitch(-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), 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; +#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( + [&](const Pose3& p) { return gravity.evaluateError(p); }, initial))); + EXPECT_TRUE(near(gravity.evaluateError(Pose3()), Vector2::Zero())); + + NonlinearFactorGraph graph; + graph.add(gravity); + graph.add(PriorFactor(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(1)).norm(), error.norm()*0.01) + << "gravity optimization reduces tilt"; +}