Added doc and tests for BayesFilter class

corelib/include/rtabmap/core/BayesFilter.h

@@ -41,45 +41,154 @@ namespace rtabmap {
 class Memory;
 class Signature;
 
+/**
+ * @class BayesFilter
+ * @brief Recursive Bayesian filter for loop-closure hypothesis estimation in RTAB-Map.
+ *
+ * This class implements the prediction and update steps of a Bayes filter used to estimate
+ * the posterior probability over candidate locations (signatures) in working memory. It is
+ * typically called by Rtabmap after likelihood values have been computed from visual
+ * word comparisons.
+ *
+ * The filter operates in two steps on each iteration:
+ * - **Prediction**: builds a transition matrix from the memory graph and multiplies it
+ *   with the previous posterior to obtain the prior.
+ * - **Update**: multiplies the prior by the observation likelihood and normalizes the result.
+ *
+ * The prediction matrix is built from neighbor relationships in @ref Memory, using a
+ * Gaussian-like model configured through @ref Parameters::kBayesPredictionLC(). A virtual
+ * place (negative signature id, see @ref Memory::kIdVirtual) represents the hypothesis
+ * that the current observation comes from a new location.
+ *
+ * Related parameters (see @ref Parameters):
+ * - @ref Parameters::kBayesPredictionLC() — transition probabilities per graph depth level.
+ * - @ref Parameters::kBayesVirtualPlacePriorThr() — prior for the virtual place.
+ * - @ref Parameters::kBayesFullPredictionUpdate() — regenerate the full prediction matrix each iteration.
+ *
+ * @see Memory::getNeighborsId()
+ * @see Rtabmap
+ */
 class RTABMAP_CORE_EXPORT BayesFilter
 {
 public:
+	/**
+	 * @brief Constructs a Bayes filter with default or custom parameters.
+	 * @param parameters Optional parameter map (Bayes group keys). Defaults are used for missing keys.
+	 */
 	BayesFilter(const ParametersMap & parameters = ParametersMap());
 	virtual ~BayesFilter();
+
+	/**
+	 * @brief Updates internal settings from the parameter map.
+	 * @param parameters Map containing Bayes group keys.
+	 */
 	virtual void parseParameters(const ParametersMap & parameters);
+
+	/**
+	 * @brief Runs one Bayes filter iteration (prediction + update).
+	 *
+	 * Given a likelihood map over signature ids, computes and stores the normalized posterior.
+	 * The prediction matrix is generated or updated from @ref Memory using the ids present
+	 * in @p likelihood.
+	 *
+	 * @param memory Working memory instance (must not be null).
+	 * @param likelihood Observation likelihood per signature id (must not be empty).
+	 * @return Reference to the internal posterior map (id → probability). On error (null
+	 *         memory, empty likelihood, or invalid prediction model), returns the unchanged posterior.
+	 */
 	const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+
+	/**
+	 * @brief Clears posterior, prediction matrix and cached neighbor indices.
+	 */
 	void reset();
 
-	//setters
+	/**
+	 * @brief Sets the loop-closure prediction model from a space-separated string.
+	 *
+	 * Format: `{Vp, Lc, l1, l2, l3, ...}` where:
+	 * - **Vp** — virtual place probability. This is the probability to move to a new place (unvisited location).
+	 * - **Lc** — loop closure (depth 0) probability. This is the probability to stay at the same location.
+	 * - **l1, l2, ...** — probabilities for neighbors at increasing graph depth levels. This is the probability to move to a neighbor at the given depth level.
+	 *
+	 * Each value must be in [0, 1]. At least two values are required. Invalid strings are rejected
+	 * and the previous model is kept.
+	 *
+	 * @param prediction Space-separated list of probabilities (same format as @ref Parameters::kBayesPredictionLC()).
+	 */
 	void setPredictionLC(const std::string & prediction);
 
-	//getters
+	/**
+	 * @brief Returns the current posterior probability map.
+	 * @return Map of signature id to normalized posterior probability. This is the probability to be at the given location.
+	 */
 	const std::map<int, float> & getPosterior() const {return _posterior;}
-	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
-	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
-	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
 
+	/**
+	 * @brief Returns the virtual place prior threshold.
+	 * @return Value in [0, 1] used when building the virtual place row of the prediction matrix.
+	 */
+	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
+
+	/**
+	 * @brief Returns the loop-closure prediction model as a vector of values.
+	 * @return Vector in the format `{Vp, Lc, l1, l2, l3, ...}`.
+	 */
+	const std::vector<double> & getPredictionLC() const;
+
+	/**
+	 * @brief Returns the loop-closure prediction model as a space-separated string.
+	 * @return String representation of @ref getPredictionLC().
+	 */
+	std::string getPredictionLCStr() const;
+
+	/**
+	 * @brief Builds or updates the prediction (transition) matrix for the given signature ids.
+	 *
+	 * Rows and columns correspond to @p ids. Neighbor links are queried from @ref Memory to fill
+	 * transition probabilities according to @ref getPredictionLC(). When @p ids match the
+	 * current posterior keys, the cached matrix may be returned without recomputation.
+	 *
+	 * @param memory Working memory instance (must not be null).
+	 * @param ids Ordered list of signature ids (often includes @ref Memory::kIdVirtual as first element).
+	 * @return Square CV_32FC1 matrix of size ids.size() × ids.size().
+	 */
 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
+	/**
+	 * @brief Estimates memory usage of this object and its internal containers.
+	 * @return Approximate memory footprint in bytes.
+	 */
 	unsigned long getMemoryUsed() const;
 
 private:
+	/**
+	 * @brief Incrementally updates the prediction matrix when ids are added or removed.
+	 */
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,
 			const std::vector<int> & oldIds,
 			const std::vector<int> & newIds);
+
+	/**
+	 * @brief Realigns the posterior map with the current set of likelihood ids.
+	 */
 	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+
+	/**
+	 * @brief Normalizes one row of the prediction matrix and applies the virtual place probability.
+	 */
 	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
 
 private:
-	std::map<int, float> _posterior;
-	cv::Mat _prediction;
-	float _virtualPlacePrior;
-	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
-	bool _fullPredictionUpdate;
-	float _totalPredictionLCValues;
-	float _predictionEpsilon;
-	std::map<int, std::map<int, int> > _neighborsIndex;
+	std::map<int, float> _posterior;              ///< Current posterior (signature id → probability).
+	cv::Mat _prediction;                          ///< Cached prediction/transition matrix.
+	float _virtualPlacePrior;                     ///< Prior for virtual place transitions.
+	std::vector<double> _predictionLC;            ///< Model `{Vp, Lc, l1, l2, ...}`.
+	bool _fullPredictionUpdate;                   ///< If true, rebuild the full prediction matrix each time.
+	float _totalPredictionLCValues;               ///< Sum of all values in _predictionLC.
+	float _predictionEpsilon;                     ///< Minimum non-zero probability in the model.
+	std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id.
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -359,8 +359,8 @@ class RTABMAP_CORE_EXPORT Parameters
 	RTABMAP_PARAM(PyDetector, Cuda,           bool, true,   "Use cuda.");
 
     // BayesFilter
-    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
-    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
+    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior. Considering that we are at a new place, this is the prior probability to move again to a new place (unvisited location). The prior probability to move to a previously visited location is 1 - VirtualPlacePriorThr (split equally against all previously visited locations).");
+    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}. Considering we are at a previously visited location, the first value is the probability to move to a new place (unvisited location), the second value is the probability to stay at the same location, the third value is the probability to move to a neighbor or loop closure at the first depth level, the fourth value is the probability to move to a neighbor or loop closure at the second depth level, etc. If the sum of the values is not 1, the difference is normalized against all remaining visited locations. Normally, the sum of these values should be 1.");
     RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, false, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");
 
     // Verify hypotheses

corelib/include/rtabmap/core/util2d.h

@@ -728,9 +728,11 @@ void RTABMAP_CORE_EXPORT NMS(
  * using a square covering method and binary search optimization to achieve a desired number of keypoints.
  *
  * @param[in] keypoints    Input vector of keypoints to select from.
- * @param[in] maxKeypoints Desired number of output keypoints. The algorithm attempts to select this many,
- *                         within a tolerance range.
- * @param[in] tolerance    Relative tolerance for the number of output keypoints (e.g., 0.1 allows ±10%).
+ * @param[in] maxKeypoints Desired upper bound on the number of output keypoints. The internal target is
+ *                         first reduced by `round(maxKeypoints * tolerance)` so the result is always
+ *                         less than or equal to this value.
+ * @param[in] tolerance    Relative tolerance applied to the reduced target (e.g., 0.1 allows ±10% of the
+ *                         reduced target, not of `maxKeypoints`).
  * @param[in] cols         Width of the image in pixels.
  * @param[in] rows         Height of the image in pixels.
  * @param[in] indx         Optional vector of indices to use instead of the original keypoints ordering.
@@ -741,7 +743,9 @@ void RTABMAP_CORE_EXPORT NMS(
  *
  * @note The algorithm operates by covering the image with a grid of cells and retaining the most confident
  *       keypoint in each uncovered cell while suppressing nearby keypoints within a computed square radius.
- * @note Uses binary search to find the optimal suppression radius that yields `maxKeypoints` (± `tolerance`).
+ * @note Uses binary search to find the optimal suppression radius so the number of selected keypoints is
+ *       within [effectiveMax * (1 - tolerance), effectiveMax * (1 + tolerance)], where
+ *       effectiveMax = maxKeypoints - round(maxKeypoints * tolerance).
  * @note Works best when `keypoints` are pre-sorted by response strength (e.g., strongest first).
  * @note If the `indx` vector is provided, the returned indices refer to the original list, not just `indx`.
  */