Use BFS instead of A* for proximity graph-depth filtering (#1756)

* Use BFS instead of A* for proximity graph-depth filtering

* refactored name of the function, added performance test comparison

---------

Co-authored-by: matlabbe <[email protected]>

corelib/include/rtabmap/core/Graph.h

@@ -490,6 +490,29 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			int to,
 			bool updateNewCosts = false);
 
+/**
+ * @brief Single-source graph depth via BFS.
+ *
+ * Runs one breadth-first search from @p from and returns, for every reached
+ * node, its depth from @p from (i.e., the number of links on the shortest
+ * path, @p from itself having depth 0).
+ *
+ * @note The depth is a number of hops, not a distance: it counts links, and the poses
+ *       of the nodes play no part in it. The path it stands for is thus not the one
+ *       @ref computePath() returns, which minimizes the Euclidean length instead and
+ *       can walk more links to save meters.
+ *
+ * @param links Directed edges (`from` → `to`) keyed by source id.
+ * @param from Start node id.
+ * @param maxDepth If &gt; 0, only nodes with depth ≤ this value are returned (the
+ *                 frontier is not expanded further); `0` explores the whole component.
+ * @return Node id → depth mapping.
+ */
+std::map<int, int> RTABMAP_CORE_EXPORT computePathDepths(
+			const std::multimap<int, int> & links,
+			int from,
+			int maxDepth = 0);
+
 /**
  * @brief Dijkstra shortest path on link constraints.
  *