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

Added GTSAM BA, updated Ceres to use g2o ba parameters. Renamed g2o's ba related parameters to Optimizer group and used by both gtsam and ceres.

Browse Source
This commit is contained in:
matlabbe
2026-05-31 00:03:18 -07:00
parent 0b0779079f
commit 1ef19d6ac4
18 changed files with 1350 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+5 -4
View File
@@ -472,10 +472,11 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod 3=Eigen");
#endif
RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
RTABMAP_PARAM(g2o, PixelVariance, double, 1.0, "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
RTABMAP_PARAM(g2o, DisparityVariance, double, 1.0, "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8, "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
RTABMAP_PARAM(g2o, Baseline, double, 0.075, "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
RTABMAP_PARAM(Optimizer, Baseline, double, 0.075, "When doing bundle adjustment with RGB-D data (mono camera + depth), set a fake baseline (m) so the BA backend treats depth as stereo disparity. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Set to 0 to keep the problem mono (depth observations are ignored). For real stereo data the baseline in the calibration (Tx) is used directly.");
RTABMAP_PARAM(Optimizer, PixelVariance, double, 1.0, "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0, "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
RTABMAP_PARAM(Optimizer, RobustKernelDelta, double, 8, "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Applies to all BA-capable backends (g2o, GTSAM, Ceres). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
RTABMAP_PARAM(GTSAM, Incremental, bool, false, uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));
corelib/include/rtabmap/core/Rtabmap.h
+1 -1
View File
@@ -678,7 +678,7 @@ public:
* before BA (otherwise reuse existing word-id correspondences).
* @param iterations Solver iterations (0 falls back to @ref Parameters::kOptimizerIterations()).
* @param pixelVariance Pixel reprojection variance used by the cost (0 falls back
* to @ref Parameters::kg2oPixelVariance()).
* to @ref Parameters::kOptimizerPixelVariance()).
* @return True if BA was run and improved poses were stored.
*/
bool globalBundleAdjustment(
corelib/include/rtabmap/core/optimizer/OptimizerCeres.h
+14 -3
View File
@@ -43,13 +43,18 @@ public:
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
OptimizerCeres(const ParametersMap & parameters) :
Optimizer(parameters) {}
Optimizer(iterations, slam2d, covarianceIgnored, epsilon),
pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
baseline_(Parameters::defaultOptimizerBaseline()) {}
OptimizerCeres(const ParametersMap & parameters);
virtual ~OptimizerCeres() {}
virtual Type type() const {return kTypeCeres;}
virtual void parseParameters(const ParametersMap & parameters);
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
@@ -67,6 +72,12 @@ public:
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);
private:
double pixelVariance_;
double disparityVariance_;
double robustKernelDelta_;
double baseline_;
};
} /* namespace rtabmap */
corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h
+13
View File
@@ -58,8 +58,21 @@ public:
double * finalError = 0,
int * iterationsDone = 0);
virtual std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, std::vector<CameraModel> > & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers = 0);
private:
int internalOptimizerType_;
double pixelVariance_;
double disparityVariance_;
double robustKernelDelta_;
double baseline_;
gtsam::ISAM2 * isam2_;
struct ConstraintToFactor {