Added Optimizer doc

2026-10-04 00:57:46 +08:00 · 2026-05-30 21:42:45 -07:00
parent 3ecddfb583
commit 0b0779079f
1 changed files with 183 additions and 30 deletions
@@ -38,6 +38,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

+/**
+ * @class FeatureBA
+ * @brief A single bundle adjustment feature observation: one keypoint seen in one frame.
+ *
+ * Used as the per-frame value in the @c wordReferences map (`<wordId, <frameId, FeatureBA>>`)
+ * passed to @ref Optimizer::optimizeBA(). @ref depth is in meters when available (RGB-D / stereo
+ * with disparity) or 0 if unknown (monocular). @ref descriptor and @ref cameraIndex are optional;
+ * @ref cameraIndex selects which @ref CameraModel of a multi-camera rig the keypoint belongs to.
+ */
 class FeatureBA
 {
 public:
@@ -49,31 +58,72 @@ public:
 	{
 		//UDEBUG("kpt=(%f,%f) depth=%f, camIndex=%d", kpt.pt.x, kpt.pt.y, depth, cameraIndex);
 	}
-	cv::KeyPoint kpt;
-	float depth;
-	cv::Mat descriptor;
-	int cameraIndex;
+	cv::KeyPoint kpt;     ///< 2D image keypoint.
+	float depth;          ///< Depth at @ref kpt in meters, or 0 if unknown (monocular).
+	cv::Mat descriptor;   ///< Optional descriptor for the keypoint (used when re-matching is enabled).
+	int cameraIndex;      ///< Index into the frame's camera model list for multi-camera rigs.
 };

-////////////////////////////////////////////
-// Graph optimizers
-////////////////////////////////////////////
+/**
+ * @class Optimizer
+ * @brief Abstract base for pose-graph and bundle-adjustment optimizers.
+ *
+ * Optimizer is a factory + interface in front of several third-party back-ends
+ * (@ref kTypeTORO "TORO", @ref kTypeG2O "g2o", @ref kTypeGTSAM "GTSAM",
+ * @ref kTypeCeres "Ceres", @ref kTypeCVSBA "cvsba"). Use @ref create() to instantiate one
+ * based on the @c Optimizer/Strategy parameter; use @ref isAvailable() to check whether a
+ * given back-end was compiled in.
+ *
+ * Two families of methods are exposed:
+ * - **Pose-graph optimization** (@ref optimize / @ref optimizeIncremental) — refines poses given
+ *   relative-pose constraints. Subclasses override @ref optimize() with covariance output.
+ * - **Bundle adjustment** (@ref 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 @ref Signature data.
+ *
+ * Common knobs (iterations, robust kernels, 2D-vs-3D, etc.) are configured through
+ * @ref parseParameters() or per-attribute setters and apply to whichever back-end is selected.
+ */
 class RTABMAP_CORE_EXPORT Optimizer
 {
 public:
+	/** @brief Graph-optimizer back-end identifier. */
 	enum Type {
-		kTypeUndef = -1,
-		kTypeTORO = 0,
-		kTypeG2O = 1,
-		kTypeGTSAM = 2,
-		kTypeCeres = 3,
-		kTypeCVSBA = 4
+		kTypeUndef = -1, ///< Unspecified / invalid.
+		kTypeTORO = 0,   ///< TORO (tree-based relaxation).
+		kTypeG2O = 1,    ///< g2o (general graph optimization, supports BA).
+		kTypeGTSAM = 2,  ///< GTSAM (factor graphs, iSAM2-style incremental).
+		kTypeCeres = 3,  ///< Ceres Solver (nonlinear least squares, supports BA).
+		kTypeCVSBA = 4   ///< cvsba (sparse bundle adjustment only).
 	};
+
+	/**
+	 * @brief Returns whether @p type was compiled in (its third-party dependency was found).
+	 *
+	 * @ref kTypeUndef is treated as unavailable. @ref create() falls back through this check
+	 * to pick the first available back-end when the requested one is missing.
+	 */
 	static bool isAvailable(Optimizer::Type type);
+
+	/**
+	 * @brief Factory: build an optimizer from a @ref ParametersMap.
+	 *
+	 * Reads @c Optimizer/Strategy from @p parameters; falls back to the default strategy if
+	 * the chosen back-end isn't compiled in. Caller owns the returned pointer.
+	 */
 	static Optimizer * create(const ParametersMap & parameters);
+
+	/** @brief Factory: build an optimizer of a specific @p type. Caller owns the result. */
 	static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());

-	// Get connected poses and constraints from a set of links
+	/**
+	 * @brief Extracts the connected component reachable from @p fromId.
+	 *
+	 * Walks @p linksIn breadth-first starting at @p fromId and copies every visited pose
+	 * (from @p posesIn) and every traversed link into @p posesOut / @p linksOut. Use this to
+	 * isolate the subgraph that actually affects @p fromId before calling @ref optimize(),
+	 * since the back-ends require a single connected component.
+	 */
 	void getConnectedGraph(
 			int fromId,
 			const std::map<int, Transform> & posesIn,
@@ -84,19 +134,23 @@ public:
 public:
 	virtual ~Optimizer() {}

+	/** @brief Returns the concrete back-end identifier (one of @ref Type). */
 	virtual Type type() const = 0;

-	// getters
-	int iterations() const {return iterations_;}
-	bool isSlam2d() const {return slam2d_;}
-	bool isCovarianceIgnored() const {return covarianceIgnored_;}
-	double epsilon() const {return epsilon_;}
-	bool isRobust() const {return robust_;}
-	bool priorsIgnored() const {return priorsIgnored_;}
-	bool landmarksIgnored() const {return landmarksIgnored_;}
-	float gravitySigma() const {return gravitySigma_;}
+	/// @name Getters for tunables shared across back-ends.
+	/// @{
+	int iterations() const {return iterations_;}             ///< Max solver iterations.
+	bool isSlam2d() const {return slam2d_;}                  ///< True if optimizing in SE(2) instead of SE(3).
+	bool isCovarianceIgnored() const {return covarianceIgnored_;} ///< If true, all edges share an identity information matrix.
+	double epsilon() const {return epsilon_;}                ///< Convergence threshold on cost decrease.
+	bool isRobust() const {return robust_;}                  ///< If true, use a robust kernel / switchable factors against bad loop closures.
+	bool priorsIgnored() const {return priorsIgnored_;}      ///< If true, unary priors on poses are dropped.
+	bool landmarksIgnored() const {return landmarksIgnored_;}///< If true, landmark/marker observations are dropped.
+	float gravitySigma() const {return gravitySigma_;}       ///< Std-dev (rad) of the gravity prior on roll/pitch; 0 disables it.
+	/// @}

-	// setters
+	/// @name Setters mirroring the corresponding getters.
+	/// @{
 	void setIterations(int iterations) {iterations_ = iterations;}
 	void setSlam2d(bool enabled) {slam2d_ = enabled;}
 	void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
@@ -105,9 +159,33 @@ public:
 	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
 	void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
 	void setGravitySigma(float value) {gravitySigma_ = value;}
+	/// @}

+	/**
+	 * @brief Reads shared knobs from @p parameters and applies them to this instance.
+	 *
+	 * Subclasses override to additionally read back-end-specific keys (e.g. @c g2o/Solver,
+	 * @c GTSAM/Optimizer); they should call this base implementation first.
+	 */
 	virtual void parseParameters(const ParametersMap & parameters);

+	/**
+	 * @brief Pose-graph optimization that grows the graph one node at a time.
+	 *
+	 * Inserts poses in @p poses iteration order, propagating odometry edges (@c Neighbor /
+	 * @c NeighborMerged) directly and triggering a call to @ref optimize() whenever a loop
+	 * closure is added. A final full @ref optimize() pass anchors at @p rootId. Useful for
+	 * warm-starting heavily-deformed initial guesses where a single-shot @ref optimize()
+	 * can diverge.
+	 *
+	 * @param rootId  Pose whose absolute transform is held fixed.
+	 * @param poses   Initial poses keyed by id.
+	 * @param constraints  Relative-pose constraints (see @ref Link::Type).
+	 * @param intermediateGraphes  Optional: appended per outer iteration for debug/visualization.
+	 * @param finalError  Optional: written with the solver's final cost.
+	 * @param iterationsDone  Optional: written with the solver's actual iteration count.
+	 * @return Refined poses (same key set as @p poses), or empty on failure.
+	 */
 	std::map<int, Transform> optimizeIncremental(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -116,6 +194,12 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);

+	/**
+	 * @brief Pose-graph optimization (single shot).
+	 *
+	 * Convenience overload that discards the output covariance. See the covariance-returning
+	 * overload below for parameter docs.
+	 */
 	std::map<int, Transform> optimize(
 				int rootId,
 				const std::map<int, Transform> & poses,
@@ -124,7 +208,22 @@ public:
 				double * finalError = 0,
 				int * iterationsDone = 0);

-	// inherited classes should implement one of these methods
+	/**
+	 * @brief Pose-graph optimization with marginal covariance of @p rootId.
+	 *
+	 * This is the primary back-end entry point — concrete subclasses override it. The base
+	 * implementation just emits an error.
+	 *
+	 * @param rootId  Pose to hold fixed during optimization.
+	 * @param poses   Initial pose estimates.
+	 * @param constraints  Relative-pose constraints between pose ids (and optionally to landmarks).
+	 * @param outputCovariance  Output: 6x6 covariance of the last optimized pose w.r.t. @p rootId
+	 *                          (3x3 for 2D mode). Filled only if the back-end supports it.
+	 * @param intermediateGraphes  Optional: appended at each iteration for debug/visualization.
+	 * @param finalError  Optional: written with the solver's final cost.
+	 * @param iterationsDone  Optional: written with the solver's actual iteration count.
+	 * @return Refined poses, or empty on failure.
+	 */
 	virtual std::map<int, Transform> optimize(
 				int rootId,
 				const std::map<int, Transform> & poses,
@@ -133,25 +232,57 @@ public:
 				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 				double * finalError = 0,
 				int * iterationsDone = 0);
+
+	/**
+	 * @brief 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 @ref optimizeBA() overloads ultimately funnel here.
+	 *
+	 * @param rootId  Pose to hold fixed. If negative, ALL poses other than the (positive) lowest id
+	 *                are held fixed.
+	 * @param poses   Initial pose estimates keyed by frame id.
+	 * @param links   Edges used to define the BA problem topology.
+	 * @param models  Camera model(s) per frame; for stereo, @c Tx must be set on the model
+	 *                (= -baseline*fx). Multi-camera rigs have multiple entries per frame.
+	 * @param points3DMap  In/out: world 3D points keyed by word id; refined on return.
+	 * @param wordReferences  Observations: `<wordId, <frameId, FeatureBA>>`. See @ref FeatureBA.
+	 * @param outliers  Optional output: word ids dropped by the back-end's outlier rejection.
+	 * @return Refined poses, or empty on failure.
+	 */
 	virtual std::map<int, Transform> optimizeBA(
-			int rootId, // if negative, all other poses are fixed
+			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
+			const std::map<int, std::vector<CameraModel> > & models,
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
 			std::set<int> * outliers = 0);

+	/**
+	 * @brief BA wrapper that derives camera models and correspondences from signatures.
+	 *
+	 * Builds @c models per frame from each @ref Signature's @ref SensorData (mono or stereo,
+	 * with stereo-baseline encoded in Tx), calls @ref computeBACorrespondences() to populate
+	 * @p points3DMap / @p wordReferences from the signatures' words, then delegates to the
+	 * back-end overload above.
+	 *
+	 * @param rematchFeatures  If true, re-match descriptors across linked frames before
+	 *                         building correspondences (more accurate, more expensive).
+	 * @param registrationParameters  Forwarded to @ref RegistrationVis when re-matching.
+	 */
 	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, // <ID words, IDs frames + keypoint/depth/descriptor>
+			std::map<int, std::map<int, FeatureBA> > & wordReferences,
 			bool rematchFeatures = false,
 			const ParametersMap & registrationParameters = ParametersMap());

+	/** @brief BA convenience wrapper: like the overload above but ignores the
+	 *  refined 3D points and observation map. */
 	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -160,6 +291,13 @@ public:
 			bool rematchFeatures = false,
 			const ParametersMap & registrationParameters = ParametersMap());

+	/**
+	 * @brief Refine a single two-frame link via BA.
+	 *
+	 * Sets the @c from frame at identity and the @c to frame at @c link.transform(), then runs
+	 * BA over the supplied 3D points and observations. Returns the refined relative transform,
+	 * or @c link.transform() unchanged on failure.
+	 */
 	Transform optimizeBA(
 			const Link & link,
 			const CameraModel & model,
@@ -167,12 +305,27 @@ public:
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
 			std::set<int> * outliers = 0);

+	/**
+	 * @brief Build BA correspondences (3D points + per-frame observations) from signatures.
+	 *
+	 * For each link, matches words between the two signatures with @ref RegistrationVis,
+	 * triangulates / lifts depth into world coordinates using the frames' initial poses, and
+	 * populates @p points3DMap (one entry per word id) and @p wordReferences (one entry per
+	 * (word, frame) observation).
+	 *
+	 * @param rematchFeatures  If true, descriptors are re-matched between frames instead of
+	 *                         relying on pre-existing word ids — more robust to feature drift.
+	 * @param useLinkTransformAsGuess  If true, the link's transform seeds the PnP guess
+	 *                                 instead of estimating it from scratch.
+	 * @param registrationParameters  Forwarded to @ref RegistrationVis (estimation type,
+	 *                                inlier counts, NNDR, etc.).
+	 */
 	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, // <ID words, IDs frames + keypoint/depth/descriptor>
+			std::map<int, std::map<int, FeatureBA > > & wordReferences,
 			bool rematchFeatures = false,
 			bool useLinkTransformAsGuess = false,
 			ParametersMap registrationParameters = ParametersMap());