Abstract base for pose-graph and bundle-adjustment optimizers. More...

#include <Optimizer.h>

Inheritance diagram for rtabmap::Optimizer:

[legend]

Public Types
enum	Type { kTypeUndef = -1 , kTypeTORO = 0 , kTypeG2O = 1 , kTypeGTSAM = 2 , kTypeCeres = 3 , kTypeCVSBA = 4 }
	Graph-optimizer back-end identifier. More...

Public Member Functions
void	getConnectedGraph (int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut) const
	Extracts the connected component reachable from `fromId`.

virtual Type	type () const =0
	Returns the concrete back-end identifier (one of Type).

virtual void	parseParameters (const ParametersMap &parameters)
	Reads shared knobs from `parameters` and applies them to this instance.

std::map< int, Transform >	optimizeIncremental (int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > intermediateGraphes=0, double finalError=0, int *iterationsDone=0)
	Pose-graph optimization that grows the graph one node at a time.

std::map< int, Transform >	optimize (int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > intermediateGraphes=0, double finalError=0, int *iterationsDone=0)
	Pose-graph optimization (single shot).

virtual std::map< int, Transform >	optimize (int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, cv::Mat &outputCovariance, std::list< std::map< int, Transform > > intermediateGraphes=0, double finalError=0, int *iterationsDone=0)
	Pose-graph optimization with marginal covariance of `rootId`.

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, BAOutliers *outliers=0)
	Bundle adjustment: jointly refine poses and 3D points (back-end-level entry point).

std::map< int, Transform >	optimizeBA (int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, Signature > &signatures, std::map< int, cv::Point3f > &points3DMap, std::map< int, std::map< int, FeatureBA > > &wordReferences, bool rematchFeatures=false, const ParametersMap &registrationParameters=ParametersMap())
	BA wrapper that derives camera models and correspondences from signatures.

std::map< int, Transform >	optimizeBA (int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, Signature > &signatures, bool rematchFeatures=false, const ParametersMap &registrationParameters=ParametersMap())
	BA convenience wrapper: like the overload above but ignores the refined 3D points and observation map.

Transform	optimizeBA (const Link &link, const CameraModel &model, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, BAOutliers *outliers=0)
	Refine a single two-frame link via BA.

void	computeBACorrespondences (const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, Signature > &signatures, std::map< int, cv::Point3f > &points3DMap, std::map< int, std::map< int, FeatureBA > > &wordReferences, bool rematchFeatures=false, bool useLinkTransformAsGuess=false, ParametersMap registrationParameters=ParametersMap())
	Build BA correspondences (3D points + per-frame observations) from signatures.

Getters for the settings shared across back-ends.
int	iterations () const
	Max solver iterations.

bool	isSlam2d () const
	True if optimizing in SE(2) instead of SE(3).

bool	isCovarianceIgnored () const
	If true, all edges share an identity information matrix.

double	epsilon () const
	Convergence threshold on cost decrease.

bool	isRobust () const
	If true, use a robust kernel / switchable factors against bad loop closures.

bool	priorsIgnored () const
	If true, unary priors on poses are dropped.

bool	landmarksIgnored () const
	If true, landmark/marker observations are dropped.

float	gravitySigma () const
	Std-dev (rad) of the gravity prior on roll/pitch; 0 disables it.

Setters mirroring the corresponding getters.
void	setIterations (int iterations)

void	setSlam2d (bool enabled)

void	setCovarianceIgnored (bool enabled)

void	setEpsilon (double epsilon)

void	setRobust (bool enabled)

void	setPriorsIgnored (bool enabled)

void	setLandmarksIgnored (bool enabled)

void	setGravitySigma (float value)

Static Public Member Functions
static bool	isAvailable (Optimizer::Type type)
	Returns whether `type` was compiled in (its third-party dependency was found).

static Optimizer *	create (const ParametersMap &parameters)
	Factory: build an optimizer from a ParametersMap.

static Optimizer *	create (Optimizer::Type type, const ParametersMap &parameters=ParametersMap())
	Factory: build an optimizer of a specific `type`. Caller owns the result.

Protected Member Functions
	Optimizer (int iterations=Parameters::defaultOptimizerIterations(), bool slam2d=Parameters::defaultRegForce3DoF(), bool covarianceIgnored=Parameters::defaultOptimizerVarianceIgnored(), double epsilon=Parameters::defaultOptimizerEpsilon(), bool robust=Parameters::defaultOptimizerRobust(), bool priorsIgnored=Parameters::defaultOptimizerPriorsIgnored(), bool landmarksIgnored=Parameters::defaultOptimizerLandmarksIgnored(), float gravitySigma=Parameters::defaultOptimizerGravitySigma())

	Optimizer (const ParametersMap &parameters)

Detailed Description

Abstract base for pose-graph and bundle-adjustment optimizers.

Optimizer is a factory + interface in front of several third-party back-ends (TORO, g2o, GTSAM, Ceres, cvsba). Use create() to instantiate one based on the Optimizer/Strategy parameter; use isAvailable() to check whether a given back-end was compiled in.

Two families of methods are exposed:

Pose-graph optimization (optimize / optimizeIncremental) — refines poses given relative-pose constraints. Subclasses override optimize() with covariance output.
Bundle adjustment (optimizeBA) — jointly refines poses and 3D points using reprojection error. Subclasses override the lowest-level overload; the others are convenience wrappers that fill in models/correspondences from Signature data.

Common knobs (iterations, robust kernels, 2D-vs-3D, etc.) are configured through parseParameters() or per-attribute setters and apply to whichever back-end is selected.

Definition at line 90 of file Optimizer.h.

Member Enumeration Documentation

◆ Type

enum rtabmap::Optimizer::Type

Graph-optimizer back-end identifier.

Enumerator
kTypeUndef	Unspecified / invalid.
kTypeTORO	TORO (tree-based relaxation).
kTypeG2O	g2o (general graph optimization, supports BA).
kTypeGTSAM	GTSAM (factor graphs, iSAM2-style incremental).
kTypeCeres	Ceres Solver (nonlinear least squares, supports BA).
kTypeCVSBA	cvsba (sparse bundle adjustment only).

Definition at line 94 of file Optimizer.h.

Constructor & Destructor Documentation

◆ ~Optimizer()

virtual rtabmap::Optimizer::~Optimizer ( )

inlinevirtual

Definition at line 138 of file Optimizer.h.

Member Function Documentation

◆ isAvailable()

static bool rtabmap::Optimizer::isAvailable ( Optimizer::Type type )

static

Returns whether type was compiled in (its third-party dependency was found).

kTypeUndef is treated as unavailable. create() falls back through this check to pick the first available back-end when the requested one is missing.

◆ create()

static Optimizer * rtabmap::Optimizer::create ( const ParametersMap & parameters )

static

Factory: build an optimizer from a ParametersMap.

Reads Optimizer/Strategy from parameters; falls back to the default strategy if the chosen back-end isn't compiled in. Caller owns the returned pointer.

◆ getConnectedGraph()

void rtabmap::Optimizer::getConnectedGraph	(	int	fromId,
		const std::map< int, Transform > &	posesIn,
		const std::multimap< int, Link > &	linksIn,
		std::map< int, Transform > &	posesOut,
		std::multimap< int, Link > &	linksOut
	)		const

Extracts the connected component reachable from fromId.

Walks linksIn breadth-first starting at fromId and copies every visited pose (from posesIn) and every traversed link into posesOut / linksOut. Use this to isolate the subgraph that actually affects fromId before calling optimize(), since the back-ends require a single connected component.

◆ type()

virtual Type rtabmap::Optimizer::type ( ) const

pure virtual

Returns the concrete back-end identifier (one of Type).

Implemented in rtabmap::OptimizerCeres, rtabmap::OptimizerCVSBA, rtabmap::OptimizerG2O, rtabmap::OptimizerGTSAM, and rtabmap::OptimizerTORO.

◆ iterations()

int rtabmap::Optimizer::iterations ( ) const

inline

Max solver iterations.

Definition at line 145 of file Optimizer.h.

◆ isSlam2d()

bool rtabmap::Optimizer::isSlam2d ( ) const

inline

True if optimizing in SE(2) instead of SE(3).

Definition at line 146 of file Optimizer.h.

◆ isCovarianceIgnored()

bool rtabmap::Optimizer::isCovarianceIgnored ( ) const

inline

If true, all edges share an identity information matrix.

Definition at line 147 of file Optimizer.h.

◆ epsilon()

double rtabmap::Optimizer::epsilon ( ) const

inline

Convergence threshold on cost decrease.

Definition at line 148 of file Optimizer.h.

◆ isRobust()

bool rtabmap::Optimizer::isRobust ( ) const

inline

If true, use a robust kernel / switchable factors against bad loop closures.

Definition at line 149 of file Optimizer.h.

◆ priorsIgnored()

bool rtabmap::Optimizer::priorsIgnored ( ) const

inline

If true, unary priors on poses are dropped.

Definition at line 150 of file Optimizer.h.

◆ landmarksIgnored()

bool rtabmap::Optimizer::landmarksIgnored ( ) const

inline

If true, landmark/marker observations are dropped.

Definition at line 151 of file Optimizer.h.

◆ gravitySigma()

float rtabmap::Optimizer::gravitySigma ( ) const

inline

Std-dev (rad) of the gravity prior on roll/pitch; 0 disables it.

Definition at line 152 of file Optimizer.h.

◆ setIterations()

void rtabmap::Optimizer::setIterations ( int iterations )

inline

Definition at line 157 of file Optimizer.h.

◆ setSlam2d()

void rtabmap::Optimizer::setSlam2d ( bool enabled )

inline

Definition at line 158 of file Optimizer.h.

◆ setCovarianceIgnored()

void rtabmap::Optimizer::setCovarianceIgnored ( bool enabled )

inline

Definition at line 159 of file Optimizer.h.

◆ setEpsilon()

void rtabmap::Optimizer::setEpsilon ( double epsilon )

inline

Definition at line 160 of file Optimizer.h.

◆ setRobust()

void rtabmap::Optimizer::setRobust ( bool enabled )

inline

Definition at line 161 of file Optimizer.h.

◆ setPriorsIgnored()

void rtabmap::Optimizer::setPriorsIgnored ( bool enabled )

inline

Definition at line 162 of file Optimizer.h.

◆ setLandmarksIgnored()

void rtabmap::Optimizer::setLandmarksIgnored ( bool enabled )

inline

Definition at line 163 of file Optimizer.h.

◆ setGravitySigma()

void rtabmap::Optimizer::setGravitySigma ( float value )

inline

Definition at line 164 of file Optimizer.h.

◆ parseParameters()

virtual void rtabmap::Optimizer::parseParameters ( const ParametersMap & parameters )

virtual

Reads shared knobs from parameters and applies them to this instance.

Subclasses override to additionally read back-end-specific keys (e.g. g2o/Solver, GTSAM/Optimizer); they should call this base implementation first.

Reimplemented in rtabmap::OptimizerCeres, rtabmap::OptimizerG2O, and rtabmap::OptimizerGTSAM.

◆ optimizeIncremental()

std::map< int, Transform > rtabmap::Optimizer::optimizeIncremental	(	int	rootId,
		const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	constraints,
		std::list< std::map< int, Transform > > *	intermediateGraphes = `0`,
		double *	finalError = `0`,
		int *	iterationsDone = `0`
	)

Pose-graph optimization that grows the graph one node at a time.

Inserts poses in poses iteration order, propagating odometry edges (Neighbor / NeighborMerged) directly and triggering a call to optimize() whenever a loop closure is added. A final full optimize() pass anchors at rootId. Useful for warm-starting heavily-deformed initial guesses where a single-shot optimize() can diverge.

Parameters

rootId	Pose whose absolute transform is held fixed.
poses	Initial poses keyed by id.
constraints	Relative-pose constraints (see Link::Type).
intermediateGraphes	Optional: appended per outer iteration for debug/visualization.
finalError	Optional: written with the solver's final cost.
iterationsDone	Optional: written with the solver's actual iteration count.

Returns: Refined poses (same key set as poses), or empty on failure.

◆ optimize() [1/2]

std::map< int, Transform > rtabmap::Optimizer::optimize	(	int	rootId,
		const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	constraints,
		std::list< std::map< int, Transform > > *	intermediateGraphes = `0`,
		double *	finalError = `0`,
		int *	iterationsDone = `0`
	)

Pose-graph optimization (single shot).

Convenience overload that discards the output covariance. See the covariance-returning overload below for parameter docs.

◆ optimize() [2/2]

virtual std::map< int, Transform > rtabmap::Optimizer::optimize	(	int	rootId,
		const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	constraints,
		cv::Mat &	outputCovariance,
		std::list< std::map< int, Transform > > *	intermediateGraphes = `0`,
		double *	finalError = `0`,
		int *	iterationsDone = `0`
	)

virtual

Pose-graph optimization with marginal covariance of rootId.

This is the primary back-end entry point — concrete subclasses override it. The base implementation just emits an error.

Parameters

rootId	Pose to hold fixed during optimization.
poses	Initial pose estimates.
constraints	Relative-pose constraints between pose ids (and optionally to landmarks).
outputCovariance	Output: 6x6 covariance of the last optimized pose w.r.t. `rootId` (3x3 for 2D mode). Filled only if the back-end supports it.
intermediateGraphes	Optional: appended at each iteration for debug/visualization.
finalError	Optional: written with the solver's final cost.
iterationsDone	Optional: written with the solver's actual iteration count.

Returns: Refined poses, or empty on failure.

Reimplemented in rtabmap::OptimizerCeres, rtabmap::OptimizerG2O, rtabmap::OptimizerGTSAM, and rtabmap::OptimizerTORO.

◆ optimizeBA() [1/3]

virtual std::map< int, Transform > rtabmap::Optimizer::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,
		BAOutliers *	outliers = `0`
	)

virtual

Bundle adjustment: jointly refine poses and 3D points (back-end-level entry point).

Concrete subclasses (g2o, Ceres, cvsba) override this; the base implementation errors out. The other optimizeBA() overloads ultimately funnel here.

Parameters

rootId	Pose to hold fixed. If negative, ALL poses other than the (positive) lowest id are held fixed.
poses	Initial pose estimates keyed by frame id.
links	Edges used to define the BA problem topology.
models	Camera model(s) per frame; for stereo, `Tx` must be set on the model (= -baseline*fx). Multi-camera rigs have multiple entries per frame.
points3DMap	In/out: world 3D points keyed by word id; refined on return.
wordReferences	Observations: `<wordId, <frameId, FeatureBA>>`. See FeatureBA.
outliers	Optional output: word ids dropped by the back-end's outlier rejection.

Returns: Refined poses, or empty on failure.

Reimplemented in rtabmap::OptimizerCeres, rtabmap::OptimizerCVSBA, rtabmap::OptimizerG2O, and rtabmap::OptimizerGTSAM.

◆ optimizeBA() [2/3]

std::map< int, Transform > rtabmap::Optimizer::optimizeBA	(	int	rootId,
		const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	links,
		const std::map< int, Signature > &	signatures,
		std::map< int, cv::Point3f > &	points3DMap,
		std::map< int, std::map< int, FeatureBA > > &	wordReferences,
		bool	rematchFeatures = `false`,
		const ParametersMap &	registrationParameters = `ParametersMap()`
	)

BA wrapper that derives camera models and correspondences from signatures.

Builds models per frame from each Signature's SensorData (mono or stereo, with stereo-baseline encoded in Tx), calls computeBACorrespondences() to populate points3DMap / wordReferences from the signatures' words, then delegates to the back-end overload above.

Parameters

rematchFeatures	If true, re-match descriptors across linked frames before building correspondences (more accurate, more expensive).
registrationParameters	Forwarded to RegistrationVis when re-matching.

◆ optimizeBA() [3/3]

Transform rtabmap::Optimizer::optimizeBA	(	const Link &	link,
		const CameraModel &	model,
		std::map< int, cv::Point3f > &	points3DMap,
		const std::map< int, std::map< int, FeatureBA > > &	wordReferences,
		BAOutliers *	outliers = `0`
	)

Refine a single two-frame link via BA.

Sets the from frame at identity and the to frame at link.transform(), then runs BA over the supplied 3D points and observations. Returns the refined relative transform, or link.transform() unchanged on failure.

◆ computeBACorrespondences()

void rtabmap::Optimizer::computeBACorrespondences	(	const std::map< int, Transform > &	poses,
		const std::multimap< int, Link > &	links,
		const std::map< int, Signature > &	signatures,
		std::map< int, cv::Point3f > &	points3DMap,
		std::map< int, std::map< int, FeatureBA > > &	wordReferences,
		bool	rematchFeatures = `false`,
		bool	useLinkTransformAsGuess = `false`,
		ParametersMap	registrationParameters = `ParametersMap()`
	)

Build BA correspondences (3D points + per-frame observations) from signatures.

For each link, matches words between the two signatures with RegistrationVis, triangulates / lifts depth into world coordinates using the frames' initial poses, and populates points3DMap (one entry per word id) and wordReferences (one entry per (word, frame) observation).

Parameters

rematchFeatures	If true, descriptors are re-matched between frames instead of relying on pre-existing word ids — more robust to feature drift.
useLinkTransformAsGuess	If true, the link's transform seeds the PnP guess instead of estimating it from scratch.
registrationParameters	Forwarded to RegistrationVis (estimation type, inlier counts, NNDR, etc.).

The documentation for this class was generated from the following file:

corelib/include/rtabmap/core/Optimizer.h

Public Types

Public Member Functions

Static Public Member Functions

Protected Member Functions

Detailed Description

Member Enumeration Documentation

◆ Type

Constructor & Destructor Documentation

◆ ~Optimizer()

Member Function Documentation

◆ isAvailable()

◆ create()

◆ getConnectedGraph()

◆ type()

◆ iterations()

◆ isSlam2d()

◆ isCovarianceIgnored()

◆ epsilon()

◆ isRobust()

◆ priorsIgnored()

◆ landmarksIgnored()

◆ gravitySigma()

◆ setIterations()

◆ setSlam2d()

◆ setCovarianceIgnored()

◆ setEpsilon()

◆ setRobust()

◆ setPriorsIgnored()

◆ setLandmarksIgnored()

◆ setGravitySigma()

◆ parseParameters()

◆ optimizeIncremental()

◆ optimize() [1/2]

◆ optimize() [2/2]

◆ optimizeBA() [1/3]

◆ optimizeBA() [2/3]

◆ optimizeBA() [3/3]

◆ computeBACorrespondences()