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

fixing build without gtsam

Browse Source
This commit is contained in:
matlabbe
2026-05-31 11:47:19 -07:00
parent 1ef19d6ac4
commit ce832878a0
2 changed files with 60 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/OptimizerGTSAM.cpp
+1 -43
View File
@@ -57,10 +57,10 @@ 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/base/numericalDerivative.h>
#include <gtsam/navigation/AttitudeFactor.h>
#include <optimizer/gtsam/XYFactor.h>
#include <optimizer/gtsam/XYZFactor.h>
#include <optimizer/gtsam/PlanarBodyZFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#ifdef RTABMAP_VERTIGO
@@ -1140,48 +1140,6 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
// vertex keys stay disjoint from pose keys (max 10 cameras per pose).
#define GTSAM_BA_MULTICAM_OFFSET 10
namespace {
// Unary planar constraint mirroring g2o's EdgeSBACamPrior (pinfo(2,2) = 1e9):
// locks the BODY-frame z of the camera vertex to its initial value, leaving
// lateral motion + yaw free. Used when isSlam2d() is true in BA.
class PlanarBodyZFactor : public gtsam::NoiseModelFactor1<gtsam::Pose3>
{
public:
PlanarBodyZFactor(gtsam::Key key,
const gtsam::Pose3 & camera_to_body,
double initial_body_z,
const gtsam::SharedNoiseModel & model)
: gtsam::NoiseModelFactor1<gtsam::Pose3>(model, key),
camera_to_body_(camera_to_body),
initial_body_z_(initial_body_z) {}
gtsam::Vector evaluateError(const gtsam::Pose3 & camPose,
#if GTSAM_VERSION_NUMERIC >= 40300
gtsam::OptionalMatrixType H = OptionalNone) const override
#else
boost::optional<gtsam::Matrix &> H = boost::none) const override
#endif
{
const auto error_fn = [this](const gtsam::Pose3 & p) {
gtsam::Vector1 e;
e(0) = p.compose(camera_to_body_).translation().z() - initial_body_z_;
return e;
};
if(H)
{
*H = gtsam::numericalDerivative11<gtsam::Vector1, gtsam::Pose3>(error_fn, camPose);
}
return error_fn(camPose);
}
private:
gtsam::Pose3 camera_to_body_;
double initial_body_z_;
};
} // namespace
std::map<int, Transform> OptimizerGTSAM::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
corelib/src/optimizer/gtsam/PlanarBodyZFactor.h
+59
View File
@@ -0,0 +1,59 @@
/**
* Author: Mathieu Labbe
*/
/**
* Unary planar constraint mirroring g2o's EdgeSBACamPrior (pinfo(2,2) = 1e9):
* locks the BODY-frame z of the camera vertex to its initial value, leaving
* lateral motion + yaw free. Used when isSlam2d() is true in BA.
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
namespace rtabmap {
class PlanarBodyZFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
gtsam::Pose3 camera_to_body_;
double initial_body_z_;
public:
PlanarBodyZFactor(gtsam::Key poseKey,
const gtsam::Pose3 & camera_to_body,
double initial_body_z,
const gtsam::SharedNoiseModel & model):
gtsam::NoiseModelFactor1<gtsam::Pose3>(model, poseKey),
camera_to_body_(camera_to_body),
initial_body_z_(initial_body_z) {}
gtsam::Vector evaluateError(const gtsam::Pose3 & camPose,
#if GTSAM_VERSION_NUMERIC >= 40300
gtsam::OptionalMatrixType H = OptionalNone) const override
#else
boost::optional<gtsam::Matrix &> H = boost::none) const override
#endif
{
const auto error_fn = [this](const gtsam::Pose3 & p) {
gtsam::Vector1 e;
e(0) = p.compose(camera_to_body_).translation().z() - initial_body_z_;
return e;
};
if(H)
{
*H = gtsam::numericalDerivative11<gtsam::Vector1, gtsam::Pose3>(error_fn, camPose);
}
return error_fn(camPose);
}
}; // PlanarBodyZFactor
} /// namespace rtabmap