i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 18:47:48 +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/OptimizerGTSAM.cpp
+18 -11
View File
@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include "gtsam/GravityFactor.h"
#include <gtsam/navigation/AttitudeFactor.h>
#include <optimizer/gtsam/XYFactor.h>
#include <optimizer/gtsam/XYZFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
@@ -121,7 +121,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
params.relinearizeThreshold = threshold;
params.relinearizeSkip = skip;
params.evaluateNonlinearError = true;
isam2_ = new ISAM2(params);
isam2_ = new gtsam::ISAM2(params);
addedPoses_.clear();
lastAddedConstraints_.clear();
@@ -392,7 +392,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -400,7 +400,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -431,7 +431,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
@@ -442,7 +442,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
@@ -471,10 +471,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
{
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
gtsam::Rot3 nRbMeas = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation();
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<Pose3> below.
graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
#else
graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));
#endif
lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
}
}
@@ -783,7 +790,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
float x,y,z,roll,pitch,yaw;
std::map<int, Transform> tmpPoses;
const Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
#if GTSAM_VERSION_NUMERIC >= 40200
for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
#else