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

Fixing gtsam version build issues

Browse Source
This commit is contained in:
matlabbe
2026-06-01 19:40:33 -07:00
parent 36fe9e03a4
commit 4b5e1359bf
1 changed files with 17 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/optimizer/OptimizerGTSAM.cpp
+17 -16
View File
@@ -1143,22 +1143,16 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
// vertex keys stay disjoint from pose keys (max 10 cameras per pose). // vertex keys stay disjoint from pose keys (max 10 cameras per pose).
#define GTSAM_BA_MULTICAM_OFFSET 10 #define GTSAM_BA_MULTICAM_OFFSET 10
// Build a gtsam::Symbol for a 3D point (BA "landmark" in the optimization #ifdef RTABMAP_GTSAM
// sense — NOT the same as Link::kLandmark used in pose-graph land). // Build a gtsam::Symbol for a 3D point. Word ids can be negative,
// wordReferences can mix positive ids (features quantized to a visual // but gtsam symbol cannot.
// vocabulary word) with negative ids (features that weren't matched to
// any vocabulary entry — see Memory.cpp's "Set ID -1 to features not
// used for quantization"). gtsam::Symbol packs the index into 56
// unsigned bits, so a signed int with the high bit set trips
// "Symbol index is too large". Use distinct prefix characters for the
// two cases so the numeric index is always non-negative AND
// positive/negative ids cannot collide on the same Symbol.
static inline gtsam::Symbol point3dSymbol(int id) static inline gtsam::Symbol point3dSymbol(int id)
{ {
return id < 0 return id < 0
? gtsam::Symbol('L', static_cast<std::uint64_t>(-id)) ? gtsam::Symbol('L', static_cast<std::uint64_t>(-id))
: gtsam::Symbol('l', static_cast<std::uint64_t>(id)); : gtsam::Symbol('l', static_cast<std::uint64_t>(id));
} }
#endif
std::map<int, Transform> OptimizerGTSAM::optimizeBA( std::map<int, Transform> OptimizerGTSAM::optimizeBA(
int rootId, int rootId,
@@ -1419,7 +1413,7 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
} }
if(Kshared) if(Kshared)
{ {
smartFactor = boost::make_shared<SmartMono>(monoIsotropicNoise, Kshared); smartFactor = SmartMono::shared_ptr(new SmartMono(monoIsotropicNoise, Kshared));
} }
} }
if(!smartFactor) if(!smartFactor)
@@ -1518,12 +1512,19 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
// the test bounds. On our small problems this is ~3× faster // the test bounds. On our small problems this is ~3× faster
// than the direct Cholesky path. // than the direct Cholesky path.
params.linearSolverType = gtsam::NonlinearOptimizerParams::Iterative; params.linearSolverType = gtsam::NonlinearOptimizerParams::Iterative;
gtsam::PCGSolverParameters::shared_ptr pcg = gtsam::PCGSolverParameters::shared_ptr pcg(new gtsam::PCGSolverParameters());
boost::make_shared<gtsam::PCGSolverParameters>(); gtsam::PreconditionerParameters::shared_ptr preconditioner(
pcg->setPreconditionerParams( new gtsam::BlockJacobiPreconditionerParameters());
boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>()); #if GTSAM_VERSION_NUMERIC >= 40300
// 4.3+: setter removed, fields renamed (epsilon_abs_ -> epsilon_abs).
pcg->preconditioner = preconditioner;
pcg->epsilon_abs = 1e-10;
pcg->epsilon_rel = 1e-10;
#else
pcg->setPreconditionerParams(preconditioner);
pcg->epsilon_abs_ = 1e-10; pcg->epsilon_abs_ = 1e-10;
pcg->epsilon_rel_ = 1e-10; pcg->epsilon_rel_ = 1e-10;
#endif
params.iterativeParams = pcg; params.iterativeParams = pcg;
gtsam::NonlinearOptimizer * optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, params); gtsam::NonlinearOptimizer * optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, params);
UDEBUG("GTSAM BA optimizing (max iterations=%d, robustKernel=%f)...", iterations(), robustKernelDelta_); UDEBUG("GTSAM BA optimizing (max iterations=%d, robustKernel=%f)...", iterations(), robustKernelDelta_);
@@ -1619,7 +1620,7 @@ std::map<int, Transform> OptimizerGTSAM::optimizeBA(
std::map<int, SmartMono::shared_ptr>::const_iterator sit = smartByWord.find(iter->first); std::map<int, SmartMono::shared_ptr>::const_iterator sit = smartByWord.find(iter->first);
if(sit != smartByWord.end()) if(sit != smartByWord.end())
{ {
boost::optional<gtsam::Point3> p = sit->second->point(result); auto p = sit->second->point(result);
if(p) if(p)
{ {
iter->second = cv::Point3f( iter->second = cv::Point3f(