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

[torjusti]

Hi, while profiling rtabmap-reprocess I noticed that the filtering of proximity loop candidates by graph distance was taking a surprisingly large amount of time (~136 ms per node on average on a dataset with around 40k nodes).

The current implementation runs an A* search per candidate node, so there is a lot of overlap between computations. The A* search also has no max depth currently and can end up exploring large sections of the graph.

By changing to a BFS that computes the path to all nodes within N hops instead I end up with ~5.5 ms per node on average instead. Results are probably dependent on the data set but in my case it seems to be a quick win :)

[matlabbe]

Sounds great, let me take a deeper look later this week.

[matlabbe]

Small refactor: I renamed computePathLengths() to computePathDepths() (less confusing as the returning length is not in meters, but in depth of the graph).

Also added performance tests to compare before and after. That new approach is just faster on all cases for that application.

$ ./bin/test_graph_perf 
[==========] Running 2 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 2 tests from GraphPerfTest
[ RUN      ] GraphPerfTest.PathDepthsOnSpiral
[          ] spiral 2 m to 1 m, turns 0.5 m apart, a pose every 0.3 m: 64 nodes, 18.9 m walked, depths from 64 (the innermost pose) to all of them
[          ]   graph saved to /tmp/rtabmap_spiral_2m_to_1m.g2o
[          ]   A*     0.69 ms (64 searches), BFS   0.01 ms (1 search), speedup x49
[          ] spiral 5 m to 1 m, turns 0.5 m apart, a pose every 0.3 m: 504 nodes, 150.901 m walked, depths from 504 (the innermost pose) to all of them
[          ]   graph saved to /tmp/rtabmap_spiral_5m_to_1m.g2o
[          ]   A*    49.25 ms (504 searches), BFS   0.08 ms (1 search), speedup x609
[          ] spiral 10 m to 1 m, turns 0.5 m apart, a pose every 0.3 m: 2075 nodes, 622.187 m walked, depths from 2075 (the innermost pose) to all of them
[          ]   graph saved to /tmp/rtabmap_spiral_10m_to_1m.g2o
[          ]   A*   766.89 ms (2075 searches), BFS   0.45 ms (1 search), speedup x1704
[       OK ] GraphPerfTest.PathDepthsOnSpiral (922 ms)
[ RUN      ] GraphPerfTest.PathDepthsOnSpiralWithProximityLinks
[          ] spiral 2 m to 1 m: 64 nodes, 70 proximity links added between turns
[          ]   graph saved to /tmp/rtabmap_spiral_2m_to_1m_proximity.g2o
[          ]   A*     0.28 ms (64 searches), BFS   0.02 ms (1 search), speedup x16
[          ]   A* counted more links than the depth on 0 of the 64 nodes (up to 0 more)
[          ] spiral 5 m to 1 m: 504 nodes, 977 proximity links added between turns
[          ]   graph saved to /tmp/rtabmap_spiral_5m_to_1m_proximity.g2o
[          ]   A*     4.56 ms (504 searches), BFS   0.19 ms (1 search), speedup x24
[          ]   A* counted more links than the depth on 2 of the 504 nodes (up to 1 more)
[          ] spiral 10 m to 1 m: 2075 nodes, 4329 proximity links added between turns
[          ]   graph saved to /tmp/rtabmap_spiral_10m_to_1m_proximity.g2o
[          ]   A*    38.53 ms (2075 searches), BFS   0.90 ms (1 search), speedup x43
[          ]   A* counted more links than the depth on 149 of the 2075 nodes (up to 2 more)
[       OK ] GraphPerfTest.PathDepthsOnSpiralWithProximityLinks (145 ms)
[----------] 2 tests from GraphPerfTest (1067 ms total)

[----------] Global test environment tear-down
[==========] 2 tests from 1 test suite ran. (1067 ms total)
[  PASSED  ] 2 tests.

The testing graphes look like these, from the last node in the center, we compute depth for all other nodes using old and new approaches.