From 9279ab68ca2f504933dcfdf782f1e930fbe89f64 Mon Sep 17 00:00:00 2001 From: Torjus Iveland Date: Sun, 30 Aug 2026 07:41:13 +0200 Subject: [PATCH] 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 --- corelib/include/rtabmap/core/Graph.h | 23 ++ corelib/src/Graph.cpp | 33 +++ corelib/src/Rtabmap.cpp | 15 +- corelib/test/CMakeLists.txt | 14 ++ corelib/test/perf_graph.cpp | 315 +++++++++++++++++++++++++++ 5 files changed, 394 insertions(+), 6 deletions(-) create mode 100644 corelib/test/perf_graph.cpp diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index a2af6a5a..c0b0a663 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -490,6 +490,29 @@ std::list > 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 > 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 RTABMAP_CORE_EXPORT computePathDepths( + const std::multimap & links, + int from, + int maxDepth = 0); + /** * @brief Dijkstra shortest path on link constraints. * diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 6c2516f6..b93c9798 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -1904,6 +1904,39 @@ std::list > computePath( return path; } +std::map computePathDepths( + const std::multimap & links, + int from, + int maxDepth) +{ + std::map pathDepths; + pathDepths.insert(std::make_pair(from, 0)); + std::list frontier; + frontier.push_back(from); + while(!frontier.empty()) + { + int currentId = frontier.front(); + frontier.pop_front(); + int currentDepth = pathDepths.at(currentId); + if(maxDepth > 0 && currentDepth >= maxDepth) + { + continue; + } + for(std::multimap::const_iterator iter = links.find(currentId); + iter!=links.end() && iter->first == currentId; + ++iter) + { + int nextId = iter->second; + if(pathDepths.find(nextId) == pathDepths.end()) + { + pathDepths.insert(std::make_pair(nextId, currentDepth+1)); + frontier.push_back(nextId); + } + } + } + return pathDepths; +} + // Dijksta std::list computePath( const std::multimap & links, diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 4083ebb8..4096be38 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2746,7 +2746,6 @@ bool Rtabmap::process( std::map nearestPoses; std::map optimizedPosesWithOdomCache; std::multimap links; - std::map * refPoses = &_optimizedPoses; if(_memory->isIncremental() && _proximityMaxGraphDepth>0) { // get bidirectional links @@ -2765,7 +2764,6 @@ bool Rtabmap::process( // mapping mode while being localized on the previous session. optimizedPosesWithOdomCache = _optimizedPoses; optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end()); - refPoses = &optimizedPosesWithOdomCache; for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter) { if(uContains(optimizedPosesWithOdomCache, iter->second.from()) && @@ -2778,18 +2776,23 @@ bool Rtabmap::process( } } } + std::map proximityPathDepths; + if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) + { + proximityPathDepths = graph::computePathDepths(links, signature->id(), _proximityMaxGraphDepth); + } for(std::map::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter) { if(_memory->getStMem().find(iter->first) == _memory->getStMem().end()) { if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) { - std::list > path = graph::computePath(*refPoses, links, signature->id(), iter->first); - UDEBUG("Graph depth to %d = %ld", iter->first, path.size()); - if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth) + std::map::const_iterator depthIter = proximityPathDepths.find(iter->first); + if(depthIter == proximityPathDepths.end()) { - nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + continue; } + nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); } else { diff --git a/corelib/test/CMakeLists.txt b/corelib/test/CMakeLists.txt index 318441dd..b5ca0d4b 100644 --- a/corelib/test/CMakeLists.txt +++ b/corelib/test/CMakeLists.txt @@ -137,6 +137,20 @@ IF(BUILD_PERF_TESTS) set_tests_properties(test_bayesfilter_perf PROPERTIES TIMEOUT ${_perf_timeout} LABELS "performance") + + # Comparison of the two ways of getting the graph depth of every node of a map, which + # is what the RGBD/ProximityMaxGraphDepth filtering needs: one graph::computePath() + # (A*) per candidate against one graph::computePathDepths() (BFS) for all of them, + # over spiral graphs, where the straight line to the goal tells A* nothing: + # bin/test_graph_perf + # bin/test_graph_perf --gtest_filter=*ProximityLinks* + add_executable(test_graph_perf perf_graph.cpp) + target_link_libraries(test_graph_perf gtest_main rtabmap_core) + + add_test(NAME test_graph_perf COMMAND test_graph_perf) + set_tests_properties(test_graph_perf PROPERTIES + TIMEOUT ${_perf_timeout} + LABELS "performance") ENDIF(BUILD_PERF_TESTS) # Rtabmap end-to-end replay of sample DBs (test data fetched by diff --git a/corelib/test/perf_graph.cpp b/corelib/test/perf_graph.cpp new file mode 100644 index 00000000..5cdcdf4c --- /dev/null +++ b/corelib/test/perf_graph.cpp @@ -0,0 +1,315 @@ +// Comparison of the two ways of getting the graph depth of every node of a map, +// which is what Rtabmap::process() needs to reject proximity candidates that are +// too far in the graph (Parameters::kRGBDProximityMaxGraphDepth()): +// +// - one graph::computePath() (A*) per candidate, which is what it did before, each +// search paying for the whole graph again; +// - one graph::computePathDepths() (BFS) for all of them, which is what it does now. +// +// The graph is a spiral walked inward, a pose every 30 cm: the shape a robot draws +// covering a room, and the worst case for the A* heuristic. Two nodes on neighboring +// turns are ~50 cm apart in space but a whole turn apart in the graph, so the straight +// line to the goal says nothing about the path to it and each A* expands nearly the +// whole graph. That is exactly the situation proximity detection is called for. +// +// Its own executable, run by ctest under the "performance" label, so that its seconds +// of benchmarking stay out of the unit test shards: +// ctest -L performance to run them +// ctest -LE performance to skip them +// bin/test_graph_perf --gtest_filter=*Spiral* +// +// Each spiral it builds is written to the temp directory as a g2o file (the path is +// printed with the results), so that the graph a number was measured on can be looked at +// with rtabmap-graphViewer or g2o_viewer, or replayed by another tool. +// +// The times are reported rather than asserted on, as they depend on the machine. What +// is asserted is that both approaches answer the same thing on the spiral, so that the +// numbers below compare two ways of computing the same depths. +#include +#include "TestUtils.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace rtabmap; + +namespace { + +// All the spirals have their turns PITCH apart and a pose every SPACING meters, walked +// from their own radius in to RADIUS_END. +static const float RADIUS_END = 1.0f; +static const float PITCH = 0.5f; +static const float SPACING = 0.3f; + +// An Archimedean spiral r(theta) = radiusStart - pitch*theta/(2*pi), walked from +// radiusStart inward to radiusEnd with one pose every `spacing` meters of arc length, +// linked as a chain in the order it was walked. Ids are 1..n, so the last id is the +// innermost pose: the one a session ends on, and the one the depths are computed from. +struct Spiral +{ + std::map poses; + std::multimap links; // bidirectional, as Rtabmap builds them + std::vector ids; // in the order they were walked + float length = 0.0f; // walked arc length, meters +}; + +Spiral makeSpiral(float radiusStart, float radiusEnd, float pitch, float spacing) +{ + UASSERT(radiusStart > radiusEnd && pitch > 0.0f && spacing > 0.0f); + Spiral spiral; + const float b = pitch/(2.0f*M_PI); // -dr/dtheta + float theta = 0.0f; + float r = radiusStart; + while(r >= radiusEnd) + { + const int id = (int)spiral.ids.size()+1; + // Heading along the tangent, so that the poses are what a robot would have. + const float tangent = theta + M_PI_2 - std::atan2(b, r); + spiral.poses.insert(std::make_pair(id, + Transform(r*std::cos(theta), r*std::sin(theta), 0.0f, 0.0f, 0.0f, tangent))); + spiral.ids.push_back(id); + if(id > 1) + { + spiral.links.insert(std::make_pair(id-1, id)); + spiral.links.insert(std::make_pair(id, id-1)); + spiral.length += spacing; + } + // Arc length ds = sqrt(r^2 + (dr/dtheta)^2) dtheta, stepped by `spacing`. + theta += spacing/std::sqrt(r*r + b*b); + r = radiusStart - b*theta; + } + return spiral; +} + +// The links between two poses closer than `maxDistance` in space but more than +// `minTrajectoryGap` meters apart along the trajectory: the proximity links a session +// would have added between neighboring turns, and the shortcuts that make the +// fewest-links path and the shortest-in-meters path two different paths. The gap is what +// makes them proximity links rather than trajectory ones: two poses a few steps apart are +// within `maxDistance` of each other as well, but linking them adds no shortcut, it just +// short-circuits the chain. +std::multimap proximityLinks( + const Spiral & spiral, + float maxDistance, + float minTrajectoryGap = 2.0f, + int * added = 0) +{ + std::multimap links = spiral.links; + const size_t minStep = (size_t)std::ceil(minTrajectoryGap/SPACING); + int count = 0; + for(size_t i=0; i constraints(const Spiral & spiral, const std::multimap & links) +{ + std::multimap constraints; + const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1); + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + const int from = iter->first, to = iter->second; + if(from > to) + { + continue; // the other direction of a pair already written + } + const Transform & a = spiral.poses.at(from); + const Transform & b = spiral.poses.at(to); + // Consecutive poses are the trajectory, the rest are the proximity detections. + const Link::Type type = (to == from+1) ? Link::kNeighbor : Link::kLocalSpaceClosure; + constraints.insert(std::make_pair(from, Link(from, to, type, a.inverse()*b, information))); + } + return constraints; +} + +// The graph these numbers were measured on, written next to the results so that it can be +// looked at (rtabmap-graphViewer, g2o_viewer) or replayed by another tool. Overwritten on +// every run, under a stable name rather than a pid-suffixed one: the file is there to be +// opened, and makeSpiral() builds the same graph every time anyway. +void saveG2o(const Spiral & spiral, const std::multimap & links, const std::string & name) +{ + const std::string path = test::tempPath(uFormat("rtabmap_spiral_%s.g2o", name.c_str())); + if(graph::exportPoses(path, /*format=*/4, spiral.poses, constraints(spiral, links))) + { + std::cout << "[ ] graph saved to " << path << std::endl; + } + else + { + std::cout << "[ ] could not save the graph to " << path << std::endl; + } +} + +// What Rtabmap::process() did before: one A* per candidate, from the last node, and the +// candidate is kept when the path it found is short enough. Returns the ids each path +// walks through, `from` first: its node count is what the old code compared against +// RGBD/ProximityMaxGraphDepth, one more than the depth graph::computePathDepths() gives. +std::map > > pathsWithAStar( + const std::map & poses, + const std::multimap & links, + int from, + const std::vector & targets) +{ + std::map > > paths; + for(size_t i=0; i > path = + graph::computePath(poses, links, from, targets[i]); + if(!path.empty()) + { + // As a vector, which is what the checks below (and graph::computePathLength()) take. + paths.insert(std::make_pair(targets[i], + std::vector >(path.begin(), path.end()))); + } + } + return paths; +} + +// A* needs one search per node, BFS answers for every node in the one search. +void report(size_t nodes, double aStarTime, double bfsTime) +{ + printf("[ ] A* %8.2f ms (%ld searches), BFS %6.2f ms (1 search), speedup x%.0f\n", + aStarTime*1000.0, (long)nodes, bfsTime*1000.0, + bfsTime > 0.0 ? aStarTime/bfsTime : 0.0); +} + +// The spirals compared. All of them have a pose every 30 cm and turns 50 cm apart; what +// changes is how far out they start, and so how many nodes they hold. +struct SpiralSize +{ + float radiusStart; + const char * name; + const char * fileName; +}; +static const SpiralSize SPIRAL_SIZES[] = { + {2.0f, "2 m to 1 m", "2m_to_1m"}, + {5.0f, "5 m to 1 m", "5m_to_1m"}, + {10.0f, "10 m to 1 m", "10m_to_1m"}, +}; +static const size_t SPIRAL_COUNT = sizeof(SPIRAL_SIZES)/sizeof(SPIRAL_SIZES[0]); + +} + +// The depths of every node of the spiral, from its last node, both ways. The spiral is a +// chain, so there is only one path between two of its nodes and both approaches have to +// agree: the A* path holds one more node than the BFS depth, the start node itself. +TEST(GraphPerfTest, PathDepthsOnSpiral) +{ + for(size_t s=0; s > > aStarPaths = + pathsWithAStar(spiral.poses, spiral.links, from, spiral.ids); + const double aStarTime = timer.ticks(); + + const std::map depths = graph::computePathDepths(spiral.links, from); + const double bfsTime = timer.ticks(); + + report(spiral.ids.size(), aStarTime, bfsTime); + + ASSERT_EQ(depths.size(), spiral.ids.size()); + ASSERT_EQ(aStarPaths.size(), spiral.ids.size()); + EXPECT_EQ(depths.at(from), 0); + for(size_t i=0; i > & path = aStarPaths.at(id); + ASSERT_EQ((int)path.size(), depths.at(id)+1) << "node " << id; + for(size_t j=0; j links = proximityLinks(spiral, /*maxDistance=*/0.6f, + /*minTrajectoryGap=*/2.0f, &added); + const int from = spiral.ids.back(); + std::cout << "[ ] spiral " << SPIRAL_SIZES[s].name << ": " << spiral.ids.size() + << " nodes, " << added << " proximity links added between turns" << std::endl; + saveG2o(spiral, links, uFormat("%s_proximity", SPIRAL_SIZES[s].fileName)); + + UTimer timer; + const std::map > > aStarPaths = + pathsWithAStar(spiral.poses, links, from, spiral.ids); + const double aStarTime = timer.ticks(); + + const std::map depths = graph::computePathDepths(links, from); + const double bfsTime = timer.ticks(); + + report(spiral.ids.size(), aStarTime, bfsTime); + + ASSERT_EQ(depths.size(), spiral.ids.size()); + ASSERT_EQ(aStarPaths.size(), spiral.ids.size()); + int overestimated = 0, maxOverestimation = 0; + for(size_t i=0; i 0) + { + ++overestimated; + maxOverestimation = std::max(maxOverestimation, over); + } + } + printf("[ ] A* counted more links than the depth on %d of the %ld nodes" + " (up to %d more)\n", overestimated, (long)spiral.ids.size(), maxOverestimation); + } +}