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

Correct the Jacobian calculation of GravityFactor (#1692)

Browse Source 
* correct the Jacobian calculation of GravityFactor

* reimplement GravityFactor's analytical Jacobian

* adopt some minor changes from the GTSAM mainline

* use GTSAM's native AttitudeFactor instead of GravityFactor

* add missing header file and namespace prefix

* Removed GravityFactor. Fixed build with latest GTSAM version from source. Require c++17 when building with GTSAM>=4.3.0.

* Fixed cmake error when gtsam is not installed

---------

Co-authored-by: matlabbe <[email protected]>
This commit is contained in:
Borong Yuan
2026-04-26 15:33:05 -07:00
committed by GitHub
co-authored by matlabbe
parent aa6d20775f
commit ebba7e4878
10 changed files with 36 additions and 393 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h
+6 -6
View File
@@ -31,9 +31,9 @@ namespace vertigo {
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
gtsam::OptionalMatrixType H1 = OptionalNone,
gtsam::OptionalMatrixType H2 = OptionalNone,
gtsam::OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
@@ -71,9 +71,9 @@ namespace vertigo {
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
gtsam::OptionalMatrixType H1 = OptionalNone,
gtsam::OptionalMatrixType H2 = OptionalNone,
gtsam::OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h
+3 -2
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/nonlinear/NonlinearFactor.h>
namespace vertigo {
@@ -77,8 +78,8 @@ namespace vertigo {
/** between operation */
inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
gtsam::OptionalMatrixType H1=OptionalNone,
gtsam::OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h
+3 -2
View File
@@ -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/nonlinear/NonlinearFactor.h>
namespace vertigo {
@@ -77,8 +78,8 @@ namespace vertigo {
/** between operation */
inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
gtsam::OptionalMatrixType H1=OptionalNone,
gtsam::OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {