i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 17:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing 4.3.1-ros gtsam compatibility (#1783)

Browse Source 
* 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]>
This commit is contained in:
matlabbe
2026-09-30 21:03:04 -07:00
committed by GitHub
co-authored by Frank Dellaert
parent 0f98c63e9d
commit 0156bc22ed
8 changed files with 352 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h
+12 -31
View File
@@ -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
template<typename T> struct traits;
template<> struct traits<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);
}
};
// Use the scalar manifold's dimension, category and chart operations.
template<> struct traits<vertigo::SwitchVariableLinear>
: internal::Manifold<vertigo::SwitchVariableLinear> {};
}