i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 21:09:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated multisession_3it integration tests to compare loop closure hypotheses

Browse Source
This commit is contained in:
matlabbe
2026-08-20 21:49:34 -07:00
parent 6263b7e200
commit 0a38d5c869
3 changed files with 286 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/test/test_bayesfilter.cpp
+52 -72
View File
@@ -289,9 +289,7 @@ protected:
// Constructor Tests
// Bayes/SparsePrediction changes how the prediction is held and multiplied, not what the
// posterior is, so every test below that reads the posterior runs in both forms. The ones
// that read the prediction matrix instead keep the dense form, which is the only one that
// has a matrix.
// posterior is, so every test that reads the posterior runs in both forms.
class BayesFilterModeTest : public ::testing::TestWithParam<bool>
{
protected:
@@ -466,8 +464,7 @@ TEST(BayesFilterTest, ComputePosteriorEmptyLikelihood)
// generatePrediction Tests (virtual place only, no graph)
// Nothing has been computed yet, so there is no sparse form to keep and the matrix is built
// in both cases.
// Nothing computed yet, so there is no sparse form and the matrix is built in both cases.
TEST_P(BayesFilterModeTest, GeneratePredictionVirtualPlaceOnly)
{
ParametersMap params = modeParams();
@@ -483,9 +480,8 @@ TEST_P(BayesFilterModeTest, GeneratePredictionVirtualPlaceOnly)
EXPECT_FLOAT_EQ(prediction.at<float>(0, 0), 1.0f);
}
// The prediction matrix is kept between iterations and returned as it is while the ids do
// not change. Only the dense mode has a matrix at all: with the prediction kept sparse,
// generatePrediction() has to build one to return, so there is nothing to keep.
// The matrix is kept between iterations and returned as it is while the ids do not change.
// The sparse form has none to keep: generatePrediction() expands one per call.
TEST(BayesFilterTest, GeneratePredictionCachedWhenIdsUnchanged)
{
ParametersMap params;
@@ -563,9 +559,9 @@ TEST_P(BayesFilterModeTest, ComputePosteriorUpdatesWithNewLikelihood)
// Integration tests with real Memory graph
// The columns of the prediction, read from the matrix. A map this small is never kept sparse,
// a column of it reaching more than a quarter of the locations, so this is the dense form
// whatever Bayes/SparsePrediction says; GeneratePredictionExpandsTheSparseFormIntoTheSameMatrix
// carries these values over to the sparse form on a map large enough to keep it.
// a column reaching more than a quarter of the locations, so this is the dense form whatever
// Bayes/SparsePrediction says; GeneratePredictionExpandsTheSparseFormIntoTheSameMatrix carries
// these values over to the sparse form on a map large enough to keep it.
TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain)
{
addChain(5);
@@ -635,9 +631,8 @@ TEST_F(BayesFilterMemoryFixture, GeneratePredictionLinearChain)
(float)predictionLC[1]});
}
// A model whose values sum to less than 1 has normalize() spread the difference over every
// zero of a column, so there is nothing sparse to keep and the matrix is the form used with
// Bayes/SparsePrediction enabled as well as disabled. The columns below are its values.
// A model summing to less than 1 has normalize() spread the difference over every zero of a
// column, so nothing is sparse to keep and the matrix is used either way.
TEST_P(BayesFilterMemoryModeFixture, GeneratePredictionNormalizesWhenSumBelowOne)
{
addChain(4);
@@ -696,9 +691,8 @@ TEST_P(BayesFilterMemoryModeFixture, GeneratePredictionNormalizesWhenSumBelowOne
(float)predictionLC[1] * scaleRatio});
}
// The matrix is kept between iterations and handed over as it is while the ids do not change.
// The sparse form is expanded into a matrix per call instead: it is the caller asking to read
// the prediction that pays for it, not every iteration.
// The matrix is handed over as it is while the ids do not change; the sparse form is expanded
// per call, so the caller reading the prediction pays for it, not every iteration.
TEST_P(BayesFilterMemoryModeFixture, GeneratePredictionCachedWithGraph)
{
// Large enough for the sparse form to be worth keeping, so that both forms are exercised.
@@ -1097,9 +1091,7 @@ TEST_F(BayesFilterMemoryFixture, FullPredictionUpdateRegeneratesMatrix)
expectMatrixGrowsOnNewNode(false);
}
// Bayes/SparsePrediction only changes how the prediction matrix is multiplied with
// the last posterior, so both modes must agree, including while the graph grows and
// the matrix is rebuilt (the sparse view has to be rebuilt with it).
// Both forms must agree, including while the graph grows and the prediction is rebuilt.
TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModes)
{
ParametersMap paramsDense;
@@ -1140,9 +1132,8 @@ TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModes)
}
}
// A model whose values sum to less than 1 has normalize() spread the difference over
// every zero of a column, leaving the matrix dense. The sparse mode then falls back
// to the dense multiplication, which must not change the posterior.
// A model summing to less than 1 leaves the matrix dense, so the sparse mode falls back to
// the dense multiplication, which must not change the posterior.
TEST_F(BayesFilterMemoryFixture, SparsePredictionFallsBackWhenModelSumsBelowOne)
{
addChain(5);
@@ -1173,9 +1164,7 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionFallsBackWhenModelSumsBelowOne)
EXPECT_EQ(filterDense.getMemoryUsed(), filterSparse.getMemoryUsed());
}
// The sparse prediction holds only the non-zero values, so it is a fraction of what the
// matrix would be. Against the dense filter, which holds the matrix and no sparse form,
// this one holds the sparse form and no matrix, and comes out smaller.
// The sparse form holds only the non-zero values, so it comes out smaller than the matrix.
TEST_F(BayesFilterMemoryFixture, SparsePredictionMemoryUsed)
{
addChain(40);
@@ -1205,12 +1194,10 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionMemoryUsed)
EXPECT_LT(sparse, dense);
}
// Where the sparse and dense multiplications have to agree exactly rather than within
// the rounding of their sums: after an iteration whose likelihood is zero everywhere
// but on one location, the posterior is 1 there and 0 elsewhere, so the next prior is
// one column of the prediction matrix, each of its values the result of a single
// product. Both must then return the very same floats, which is only true if the
// sparse view holds the values of the matrix at the same rows and columns.
// Where the two multiplications have to agree exactly rather than within the rounding of
// their sums: a posterior of 1 on one location and 0 elsewhere makes the prior one column of
// the prediction, each value a single product. The same floats then, which only holds if the
// sparse form has the values of the matrix at the same rows and columns.
TEST_F(BayesFilterMemoryFixture, SparsePredictionIsExactOnASingleColumn)
{
addChain(30);
@@ -1254,10 +1241,8 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionIsExactOnASingleColumn)
}
}
// In localization mode the prediction is built in its sparse form directly, the matrix
// never being allocated, which is a second implementation of the same probabilities: it
// has to give what the matrix gives. Compared over a fixed graph, on which the
// prediction is generated once and kept.
// In localization mode the sparse form is built directly, the matrix never allocated. It is a
// second implementation of the same probabilities, compared here over a fixed graph.
TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModesInLocalizationMode)
{
addChain(30);
@@ -1295,11 +1280,9 @@ TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModesInLocalizationMode)
}
}
// The same exactness check as SparsePredictionIsExactOnASingleColumn, but against the
// prediction built directly in its sparse form: a posterior that is 1 on one location
// and 0 elsewhere makes each value of the prior a single product, so the two builds have
// to return the very same floats. This is what says that the sparse build puts the same
// probabilities at the same rows and columns as the matrix does.
// SparsePredictionIsExactOnASingleColumn against the prediction built directly in its sparse
// form: the same floats, which says that build puts the same probabilities at the same rows
// and columns as the matrix.
TEST_F(BayesFilterMemoryFixture, SparsePredictionInLocalizationModeIsExactOnASingleColumn)
{
addChain(30);
@@ -1341,8 +1324,8 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionInLocalizationModeIsExactOnASin
}
}
// What the sparse build is for: the prediction matrix, which is the size of the working
// memory squared, is not allocated at all.
// What the sparse build is for: the matrix, the size of the working memory squared, is never
// allocated.
TEST_F(BayesFilterMemoryFixture, SparsePredictionInLocalizationModeDoesNotAllocateTheMatrix)
{
addChain(200);
@@ -1368,11 +1351,10 @@ TEST_F(BayesFilterMemoryFixture, SparsePredictionInLocalizationModeDoesNotAlloca
EXPECT_LT(filterSparse.getMemoryUsed(), filterDense.getMemoryUsed()/4);
}
// The sparse form is only kept while the prediction is sparse enough to be worth it, so one
// session can use both forms: a map too small for the sparse form to pay off starts on the
// matrix and grows into the sparse form. The matrix cannot be carried over across the
// iterations the sparse form gave the prediction, the locations having moved on meanwhile, so
// coming back to it has to build it again.
// One session can use both forms: a map too small for the sparse form to pay off starts on
// the matrix and grows into the sparse form. The matrix cannot be carried over across the
// iterations the sparse form gave the prediction, the locations having moved on, so coming
// back to it has to build it again.
TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
{
ParametersMap paramsDense;
@@ -1385,9 +1367,9 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
BayesFilter filterDense(paramsDense);
BayesFilter filterSparse(paramsSparse);
// A column of this model reaches 3 locations on each side, which is more than a quarter of
// a small map and less than a quarter of a larger one: the session starts on the matrix
// and crosses over to the sparse form as it grows.
// A column of this model reaches 3 locations on each side: more than a quarter of a small
// map, less of a larger one, so the session starts on the matrix and grows into the
// sparse form.
addChain(6);
std::vector<int> ids;
for(int iter = 0; iter < 40; ++iter)
@@ -1412,8 +1394,8 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
// Grown into the sparse form, so the matrix of the first iterations is gone.
ASSERT_TRUE(filterSparse.isPredictionSparse());
// Back to the matrix, which was last built 40 iterations and as many locations ago. It
// has to be built again rather than updated against locations it was never built for.
// Back to the matrix, last built 40 iterations and as many locations ago: built again
// rather than updated against locations it was never built for.
ParametersMap disableSparse;
disableSparse.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false"));
filterSparse.parseParameters(disableSparse);
@@ -1439,11 +1421,10 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix)
}
}
// A location leaving the working memory, which memory management does on every iteration once
// the map is larger than what it holds. The index of every location after it moves, so the
// sparse form is laid out again -- but the columns whose contents did not change are carried
// over rather than built again, and what comes out has to be the prediction the matrix holds,
// value for value, and give the same posterior.
// A location leaving the working memory, as memory management does on every iteration once the
// map outgrows it. The index of every location after it moves, so the sparse form is laid out
// again -- carrying over the columns whose contents did not change -- and what comes out has
// to be the matrix value for value, with the same posterior.
TEST_F(BayesFilterMemoryFixture, PredictionCarriesOverWhenLocationsLeaveTheWorkingMemory)
{
ParametersMap paramsDense;
@@ -1472,8 +1453,8 @@ TEST_F(BayesFilterMemoryFixture, PredictionCarriesOverWhenLocationsLeaveTheWorki
ASSERT_TRUE(filterSparse.isPredictionSparse());
ASSERT_FALSE(filterDense.isPredictionSparse());
// Locations leaving, one iteration after the other, from the middle of the ones held as
// well as from the oldest end: both move the index of the ones that stay.
// Locations leaving from the middle of the ones held and from the oldest end: both move the
// index of the ones that stay.
std::vector<int> fewerIds = ids;
for(int iter = 0; iter < 5; ++iter)
{
@@ -1508,9 +1489,9 @@ TEST_F(BayesFilterMemoryFixture, PredictionCarriesOverWhenLocationsLeaveTheWorki
}
}
// A location coming back from long-term memory, which retrieval does when a hypothesis points
// at one that left. Its id is smaller than the ones added since, so it comes back in the
// middle of the locations held rather than at the end, and the columns after it move.
// A location coming back from long-term memory, as retrieval does when a hypothesis points at
// one that left. Its id being smaller than the ones added since, it comes back in the middle
// of the locations held rather than at the end, and the columns after it move.
TEST_F(BayesFilterMemoryFixture, PredictionCarriesOverWhenALocationComesBack)
{
ParametersMap paramsDense;
@@ -1568,16 +1549,16 @@ TEST_F(BayesFilterMemoryFixture, PredictionCarriesOverWhenALocationComesBack)
}
// A graph linked densely enough that a column reaches more than a quarter of the map is not
// kept sparse: a value costs 8 bytes against the 4 of a matrix cell, so past that point the
// matrix is the cheaper of the two and is what the filter builds.
// kept sparse: a value costs 8 bytes against the 4 of a matrix cell, so the matrix is cheaper
// past that point.
TEST_F(BayesFilterMemoryFixture, PredictionIsNotKeptSparseOnADenselyLinkedGraph)
{
addChain(40);
std::vector<int> ids = getBayesIds();
// Every location loop closed with the one a third of the map away, and a loop closure
// costs no depth in the graph search: the locations it joins share a column, and through
// them a column comes to reach most of the map.
// Every location loop closed with the one a third of the map away. A loop closure costs no
// depth in the graph search, so the locations it joins share a column and through them a
// column comes to reach most of the map.
const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
const size_t third = ids.size()/3;
for(size_t i = 1; i < ids.size(); ++i)
@@ -1614,10 +1595,9 @@ TEST_F(BayesFilterMemoryFixture, PredictionIsNotKeptSparseOnADenselyLinkedGraph)
EXPECT_NEAR(sum, 1.0f, 1e-4f);
}
// generatePrediction() hands over the prediction whichever form it is held in, expanding the
// sparse form into a matrix for the caller. Rtabmap::dumpPrediction() reads it that way, and
// so does anyone comparing the two forms: they are built through the same column arithmetic,
// so the matrices have to come out equal, value for value.
// generatePrediction() hands over the prediction whichever form holds it, expanding the sparse
// one for the caller -- how Rtabmap::dumpPrediction() reads it. Both go through the same column
// arithmetic, so the matrices have to come out equal value for value.
TEST_F(BayesFilterMemoryFixture, GeneratePredictionExpandsTheSparseFormIntoTheSameMatrix)
{
// Large enough for the sparse form to be worth keeping, so that it is the one answering.
corelib/test/test_rtabmap_integration.cpp
+233 -120
View File
@@ -46,6 +46,7 @@
#include "TestUtils.h"
#include <opencv2/imgcodecs.hpp>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <memory>
#include <string>
@@ -112,8 +113,11 @@ struct ReplayResult
// Wall-clock seconds spent inside Odometry::process across all
// frames; divide by framesRead to get per-frame average.
double odomTotalSeconds = 0.0;
// Timing/Posterior_computation (ms) over the frames that reported it,
// which is the prediction and the multiplication of the Bayes filter.
// The highest loop closure hypothesis of each node added, id and probability: the curve
// the Bayes filter draws over a session.
std::map<int, std::pair<int, float> > highestHypothesis;
// Timing/Posterior_computation (ms) over the frames that reported it: the prediction and
// the multiplication of the Bayes filter.
double posteriorMsSum = 0.0;
float posteriorMsMin = -1.0f;
float posteriorMsMax = 0.0f;
@@ -562,13 +566,11 @@ ReplayResult replayDatabaseWithStoredOdom(
// the LaserScan of each SensorData after dbReader.takeData(),
// simulating a lidar with a tighter max range.
float scanMaxRange = 0.0f,
// Starts a new map on every frame whose stored odom covariance is
// the 9999 of a session start, which is how rtabmap-reprocess
// replays a database holding more than one session. Off by default:
// a test that wants its own boundaries uses the frame above.
// Starts a new map on every frame whose stored odom covariance is the 9999 of a
// session start, as rtabmap-reprocess does. Off by default: a test wanting its own
// boundaries uses the frame above.
bool triggerNewMapOnSessionStart = false,
// Filled with the number of sessions the replay ran, meaning one
// plus the boundaries it triggered on.
// Filled with the number of sessions replayed: one plus the boundaries triggered on.
int * sessionsReplayed = 0)
{
ReplayResult result;
@@ -675,11 +677,9 @@ ReplayResult replayDatabaseWithStoredOdom(
continue;
}
// Session boundary: the stored odom covariance of the first frame of
// a session is the 9999 that says the pose does not continue the
// previous one. The new map is started before that frame is
// processed, so it is the first of the new session rather than the
// last of the one before. Same as rtabmap-reprocess.
// Session boundary: the 9999 covariance of a session's first frame says its pose does
// not continue the previous one. Started before that frame is processed, so it is the
// first of the new session rather than the last of the old. Same as rtabmap-reprocess.
if(triggerNewMapOnSessionStart
&& result.framesProcessed > 0
&& info.odomCovariance.at<double>(0, 0) >= 9999.0)
@@ -740,6 +740,13 @@ ReplayResult replayDatabaseWithStoredOdom(
{
result.translationalRmseFinal = rmseIt->second;
}
const auto hypIdIt = stats.data().find(Statistics::kLoopHighest_hypothesis_id());
const auto hypValIt = stats.data().find(Statistics::kLoopHighest_hypothesis_value());
if(hypIdIt != stats.data().end() && hypValIt != stats.data().end() && stats.refImageId()>0)
{
result.highestHypothesis[stats.refImageId()] =
std::make_pair((int)hypIdIt->second, hypValIt->second);
}
const auto postIt = stats.data().find(Statistics::kTimingPosterior_computation());
if(postIt != stats.data().end())
{
@@ -786,8 +793,7 @@ ReplayResult replayDatabaseWithStoredOdom(
return result;
}
// Where a node sits in the union-find of countConnectedComponents(), the path
// to it halved on the way up.
// Union-find root, halving the path on the way up.
int graphComponentRoot(std::map<int, int> & parent, int id)
{
while(parent.at(id) != id)
@@ -799,9 +805,8 @@ int graphComponentRoot(std::map<int, int> & parent, int id)
return id;
}
// How many pieces a graph is in: the nodes linked to each other, directly or
// through others, are one of them. Two sessions that never closed a loop with
// each other are two.
// How many pieces a graph is in. Two sessions that never closed a loop with each other are
// two pieces.
int countConnectedComponents(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links)
@@ -813,8 +818,7 @@ int countConnectedComponents(
}
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// A link to a landmark, or to a node the graph does not hold, joins
// nothing here.
// A link to a landmark, or to a node the graph does not hold, joins nothing.
if(parent.find(iter->second.from()) == parent.end() ||
parent.find(iter->second.to()) == parent.end())
{
@@ -835,37 +839,130 @@ int countConnectedComponents(
return (int)roots.size();
}
// What replaying a database asks for on top of the parameters it was recorded with.
//
// The dictionary is the one the database holds, fixed: a dictionary built as the replay goes
// feeds back on itself, the words a frame is quantized against depending on the ones the
// frames before it created, and a run then lands somewhere slightly different from the last.
// Handing it the vocabulary of the recorded session up front takes that out, and is also
// faster than building it again. The visual matching is exact for the same reason.
//
// Kp/NNStrategy is left as it is, approximate: the exact search is over the whole dictionary,
// 141k SIFT descriptors of 128 dimensions each, where a kd-tree is little better than
// comparing them all. Measured at over ten minutes against the twenty seconds of the
// approximate search, fixed dictionary or not.
// What replaying a database asks for on top of the parameters it was recorded with, kept as
// little as possible: the dictionary and the searches stay as they were recorded, so what the
// replay does can be held against what the session did.
ParametersMap replayParams(const ParametersMap & recordedWith, const std::string & srcPath)
{
ParametersMap params = recordedWith;
// The nodes of the database are already the ones its detection rate kept, so every frame
// the reader hands over is processed.
// Its nodes are already the ones its detection rate kept, so every frame is processed.
uInsert(params, ParametersPair(Parameters::kRtabmapDetectionRate(), "0"));
// The features stored with each node rather than extracted again: this database keeps no
// images to extract them from.
// The features stored with each node: this database keeps no images to extract them from.
uInsert(params, ParametersPair(Parameters::kMemUseOdomFeatures(), "true"));
uInsert(params, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "false"));
uInsert(params, ParametersPair(Parameters::kKpIncrementalDictionary(), "false"));
uInsert(params, ParametersPair(Parameters::kKpDictionaryPath(), srcPath));
uInsert(params, ParametersPair(Parameters::kVisCorNNType(), "8")); // FLANN kd-tree single
return params;
}
// The optimized graph a database holds, which is what its own mapping session
// converged to and what a replay of it is compared against, together with the
// parameters it was recorded with, which the replay is run with.
// The highest loop closure hypothesis the recorded session saw at each node, id and value,
// from the statistics it saved. That is the curve a replay is compared against.
std::map<int, std::pair<int, float> > loadDatabaseHighestHypothesis(const std::string & path)
{
std::map<int, std::pair<int, float> > hypothesis;
DBDriver * driver = DBDriver::create();
if(!driver->openConnection(path))
{
delete driver;
return hypothesis;
}
std::set<int> ids;
driver->getAllNodeIds(ids);
for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
double stamp = 0.0;
const std::map<std::string, float> stats = driver->getStatistics(*iter, stamp);
const std::map<std::string, float>::const_iterator idIter =
stats.find(Statistics::kLoopHighest_hypothesis_id());
const std::map<std::string, float>::const_iterator valueIter =
stats.find(Statistics::kLoopHighest_hypothesis_value());
if(idIter != stats.end() && valueIter != stats.end())
{
hypothesis[*iter] = std::make_pair((int)idIter->second, valueIter->second);
}
}
driver->closeConnection(false);
delete driver;
return hypothesis;
}
// How a replay's highest hypothesis per node stands against the recorded one.
struct HypothesisComparison
{
int nodes = 0; ///< nodes compared, of the ones both curves have
int sameId = 0; ///< of those, the ones pointing at the very same location
int samePlace = 0; ///< and the ones pointing at a location within a meter of it
int alsoOverThreshold = 0; ///< and the ones the replay also took past the threshold
float meanAbsValue = 0.0f; ///< mean |value - golden value|
float maxAbsValue = 0.0f;
float sameIdRatio() const {return nodes>0 ? float(sameId)/float(nodes) : 0.0f;}
float samePlaceRatio() const {return nodes>0 ? float(samePlace)/float(nodes) : 0.0f;}
float overThresholdRatio() const {return nodes>0 ? float(alsoOverThreshold)/float(nodes) : 0.0f;}
};
// Two hypotheses are on the same place when the locations they point at are this close in the
// optimized graph. Ids cannot say that on a map of three passes over the same trajectory: the
// same corner is a node of each pass, hundreds of ids apart. The nodes are ~0.3 m apart along
// the path, so a meter is a handful of them, and the map is only 24 m by 33 m: a wider radius
// would call most of it the same place.
const float kHypothesisSamePlaceRadius = 1.0f; // meters
// Only the nodes where the recorded session had a hypothesis at or past Rtabmap/LoopThr are
// compared: under it the value is spread thinly over the working memory and which location
// comes out highest is noise.
HypothesisComparison compareHighestHypothesis(
const std::map<int, std::pair<int, float> > & golden,
const std::map<int, std::pair<int, float> > & replayed,
const std::map<int, Transform> & poses,
float loopThreshold)
{
HypothesisComparison c;
double sum = 0.0;
for(std::map<int, std::pair<int, float> >::const_iterator iter=golden.begin(); iter!=golden.end(); ++iter)
{
if(iter->second.first <= 0 || iter->second.second < loopThreshold)
{
continue;
}
const std::map<int, std::pair<int, float> >::const_iterator jter = replayed.find(iter->first);
if(jter == replayed.end())
{
continue;
}
++c.nodes;
if(jter->second.second >= loopThreshold)
{
++c.alsoOverThreshold;
}
const int goldenId = iter->second.first;
const int replayedId = jter->second.first;
if(goldenId == replayedId)
{
++c.sameId;
}
bool samePlace = goldenId == replayedId;
if(!samePlace && goldenId > 0 && replayedId > 0)
{
const std::map<int, Transform>::const_iterator goldenPose = poses.find(goldenId);
const std::map<int, Transform>::const_iterator replayedPose = poses.find(replayedId);
if(goldenPose != poses.end() && replayedPose != poses.end())
{
samePlace = goldenPose->second.getDistance(replayedPose->second)
< kHypothesisSamePlaceRadius;
}
}
if(samePlace)
{
++c.samePlace;
}
const float diff = fabs(iter->second.second - jter->second.second);
sum += diff;
c.maxAbsValue = std::max(c.maxAbsValue, diff);
}
c.meanAbsValue = c.nodes>0 ? (float)(sum/(double)c.nodes) : 0.0f;
return c;
}
// The optimized graph a database converged to, which a replay is compared against, and the
// parameters it was recorded with, which the replay runs with.
bool loadDatabaseGraphAndParameters(
const std::string & path,
std::map<int, Transform> & optimizedPoses,
@@ -2139,17 +2236,13 @@ TEST_F(RtabmapIntegrationFixture, Loop3ItGps)
// ---------------------------------------------------------------------------
// Multi-session 2D lidar + SIFT (3 sessions, 935 nodes, ~270 m).
//
// The optimized graph the database already holds is the reference: it is what
// the session that recorded it converged to, so replaying the same frames with
// the same parameters has to land on the same trajectory and find about as many
// loop closures. The database keeps no images, so the replay reuses the
// features stored with each node (Mem/UseOdomFeatures) rather than extracting
// any, and the scans it does keep are what the ICP registration verifies loop
// closures with.
// The reference is the optimized graph the database holds: the same frames with the same
// parameters have to find about as many loop closures. The replay reuses the features stored
// with each node, the database keeping no images, and the ICP registration verifies loop
// closures on the scans it does keep.
//
// Each run is done with the prediction of the Bayes filter held both as a
// matrix and in its sparse form: the two are the same probabilities, so a real
// session over a real graph has to come out the same either way.
// Run over both forms of the Bayes prediction, matrix and sparse: the same probabilities, so
// a real session has to come out the same either way.
// ---------------------------------------------------------------------------
TEST_F(RtabmapIntegrationFixture, Multisession3It)
{
@@ -2171,6 +2264,8 @@ TEST_F(RtabmapIntegrationFixture, Multisession3It)
ParametersMap params = replayParams(dbParams, srcPath);
uInsert(params, ParametersPair(Parameters::kBayesSparsePrediction(),
sparsePrediction ? "true" : "false"));
// The database carries the cap it was recorded with; this test is the run without it.
uInsert(params, ParametersPair(Parameters::kRtabmapMemoryThr(), "0"));
const std::string workDb = test::tempPath(uFormat(
"rtabmap_integration_Multisession3It_%s.db", label.c_str()));
@@ -2206,40 +2301,24 @@ TEST_F(RtabmapIntegrationFixture, Multisession3It)
<< "ms min=" << result.posteriorMsMin
<< "ms max=" << result.posteriorMsMax << "ms\n";
// Every frame of the database is a node it kept, so the replay makes
// the same number of them, in the same three sessions.
// Every frame is a node the database kept, so the replay makes as many, in 3 sessions.
EXPECT_EQ(sessions, 3) << label;
EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << label;
// The same frames, the same parameters and the same features, so the
// trajectory comes out on top of the one the database holds: 7-15 cm
// and under 1.1 deg over six runs, dense and sparse alike.
//
// The replay is still not the same twice, even over a fixed dictionary
// and an exact visual matching: the registration verifying a loop
// closure accepts or rejects a hypothesis sitting on its threshold
// from one run to the next, and one loop closure more or less pulls
// the graph. The bands are what that spread asks for, not what a
// single run would allow.
EXPECT_LT(tRmse, 0.3f) << label << " trajectory drifted from the reference";
EXPECT_LT(rRmse, 2.0f) << label << " orientation drifted from the reference";
// The reference graph holds 285 global loop closures; the replay finds
// 282-288 of them over six runs.
// Loop closures are what this compares; the trajectory is printed but not asserted,
// following from which closures a run happens to find. And a run is never the same
// twice: the registration accepts or rejects a hypothesis sitting on its threshold
// from one run to the next, and one closure changes the next ones. Hence a band.
EXPECT_GT(result.loopClosuresAccepted, 265) << label << " found too few loop closures";
EXPECT_LT(result.loopClosuresAccepted, 305) << label << " found more loop closures than the reference";
}
}
// ---------------------------------------------------------------------------
// The same replay under memory management: Rtabmap/MemoryThr caps the working
// memory, so the oldest nodes are transferred to long-term memory as the map
// grows and only a window of it is ever held. What comes back is what the
// likelihood points at, the local retrieval being off, so this is the loop
// closure hypotheses of the Bayes filter driving the whole window. The global
// optimized graph still covers every node, so it is compared against the same
// reference, and the loop closures found from a limited working memory are
// fewer.
// The same replay under memory management: Rtabmap/MemoryThr caps the working memory, so the
// oldest nodes go to long-term memory as the map grows and only a window is ever held. The
// global optimized graph still covers every node, and the loop closures found from a limited
// working memory are fewer.
// ---------------------------------------------------------------------------
TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
{
@@ -2251,6 +2330,19 @@ TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
ASSERT_TRUE(loadDatabaseGraphAndParameters(srcPath, goldenPoses, dbParams))
<< "No optimized graph in " << srcPath;
// The database was recorded with the same 300-node cap, so the highest hypothesis it saw
// at each node is the curve to compare against.
const std::map<int, std::pair<int, float> > goldenHypothesis =
loadDatabaseHighestHypothesis(srcPath);
ASSERT_GT(goldenHypothesis.size(), 900u) << "No saved statistics in " << srcPath;
// Under this a hypothesis is not worth comparing.
float loopThreshold = Parameters::defaultRtabmapLoopThr();
Parameters::parse(dbParams, Parameters::kRtabmapLoopThr(), loopThreshold);
ASSERT_GT(loopThreshold, 0.0f);
std::cerr << "[ ] Comparing hypotheses at or over " << Parameters::kRtabmapLoopThr()
<< "=" << loopThreshold << "\n";
struct Variant
{
// "" leaves Bayes/SparsePrediction at its default, which is the sparse form.
@@ -2262,37 +2354,39 @@ TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
int maxLoops;
int minLocalGraph;
int maxLocalGraph;
float maxTransRmse; // meters
float maxRotRmse; // degrees
// Of the nodes the recorded session had a hypothesis on, the least that must point at
// the same place, and the most the probability may differ by on average.
float minSamePlaceRatio;
float maxMeanValueDiff;
std::string label;
};
// The first two are the same run over both forms of the prediction, with what comes back
// from long-term memory driven by the likelihood alone, and they share their expectations:
// the form the prediction is held in must not change what the session does. The last three
// hold the form at its default and take the retrieval apart: nothing coming back at all,
// the local retrieval on its own, and both of them together, which is what a session runs
// with out of the box.
// The first two are the same run over both forms of the prediction, sharing their band:
// which form holds it must not change what the session does. The last three hold the form
// at its default and take the retrieval apart: none, local only, and both, which is what a
// session runs with out of the box.
//
// The bands come from six runs of each, which is what it takes to see the spread: the
// registration verifying a loop closure accepts or rejects a hypothesis sitting on its
// threshold from one run to the next, and one loop closure more or less pulls the graph.
// Observed:
// The bands come from six runs of each. Observed:
//
// variant loops working memory trans rmse rot rmse
// sparse 253-267 275-289 0.16-0.48 m 1.1-2.3 deg
// dense 247-273 271-282 0.13-0.48 m 0.9-2.4 deg
// no-retrieval 197-205 277-280 0.39-0.53 m 2.2-2.7 deg
// local-retrieval-only 227-236 287-292 0.40-0.51 m 2.1-2.5 deg
// both-retrieval 245-269 278-289 0.12-0.39 m 1.1-2.0 deg
// variant loops working same place also over value diff
// memory as recorded threshold mean
// sparse 268-288 242-263 80-85% 86-91% 0.051-0.075
// dense 267-284 253-266 81-84% 86-91% 0.052-0.084
// no-retrieval 204-213 229-256 68-71% 81-86% 0.103-0.119
// local-retrieval-only 240-259 262-267 76-81% 85-91% 0.071-0.081
// both-retrieval 265-287 261-268 82-84% 89-92% 0.031-0.053
//
// The closest to the recorded session is the one configured like it, both retrievals on,
// and the furthest is the one with none: what comes back into the working memory is what
// the hypotheses are drawn over.
//
// The posterior time each run prints is left unasserted: it is what these variants are
// measured for, but it is also what a loaded runner moves most.
// measured for, but also what a loaded runner moves most.
const std::vector<Variant> variants = {
{"true", "0", "", 230, 290, 255, 310, 0.8f, 3.5f, "sparse" },
{"false", "0", "", 230, 290, 255, 310, 0.8f, 3.5f, "dense" },
{"", "0", "0", 175, 225, 255, 305, 0.8f, 3.5f, "no-retrieval" },
{"", "2", "0", 205, 260, 265, 320, 0.8f, 3.5f, "local-retrieval-only"},
{"", "2", "2", 215, 300, 255, 315, 0.8f, 3.5f, "both-retrieval" },
{"true", "0", "", 245, 310, 225, 290, 0.72f, 0.11f, "sparse" },
{"false", "0", "", 245, 310, 225, 290, 0.72f, 0.11f, "dense" },
{"", "0", "0", 180, 240, 210, 280, 0.60f, 0.16f, "no-retrieval" },
{"", "2", "0", 215, 285, 240, 290, 0.68f, 0.11f, "local-retrieval-only"},
{"", "2", "2", 240, 310, 240, 295, 0.74f, 0.08f, "both-retrieval" },
};
// Loop closures per variant, for the ordering between them.
@@ -2356,40 +2450,59 @@ TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
<< "ms min=" << result.posteriorMsMin
<< "ms max=" << result.posteriorMsMax << "ms\n";
// Nothing is lost whatever comes back: the nodes leave the working memory for
// long-term memory, and the global graph still covers every one of them, in one
// piece. The three sessions of this database are three passes over the same
// trajectory, and 300 nodes of working memory is wide enough to still hold the end
// of one session while the next starts over the same place, so the link across the
// boundary is found even when nothing comes back at all.
// The highest hypothesis of each node against the one the recorded session saw:
// the same locations pointed at, with the same probability on them.
const HypothesisComparison hyp = compareHighestHypothesis(
goldenHypothesis, result.highestHypothesis, goldenPoses, loopThreshold);
std::cerr << "[" << v.label << "] hypothesis over " << hyp.nodes
<< " nodes the recorded session had one on: same id " << hyp.sameId
<< " (" << 100.0f*hyp.sameIdRatio() << "%), same place " << hyp.samePlace
<< " (" << 100.0f*hyp.samePlaceRatio() << "%), also over the threshold "
<< hyp.alsoOverThreshold << " (" << 100.0f*hyp.overThresholdRatio()
<< "%), value diff mean=" << hyp.meanAbsValue
<< " max=" << hyp.maxAbsValue << "\n";
// Nothing is lost: the nodes go to long-term memory and the global graph still covers
// every one, in one piece. The three sessions are three passes over the same
// trajectory and 300 nodes is wide enough to still hold the end of one when the next
// starts over the same place, so the link across the boundary is found even with
// nothing coming back.
EXPECT_EQ(sessions, 3) << v.label;
EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << v.label;
EXPECT_EQ(components, 1) << v.label << " graph came out in pieces";
// The working memory is the part that is capped, and it stays there, whatever comes
// back into it. Anything near 935 would mean the cap never took effect and the test
// is no longer about memory management.
// The capped part stays capped. Anything near 935 would mean the cap never took effect
// and the test is no longer about memory management.
EXPECT_GT(result.finalLocalGraphSize, v.minLocalGraph) << v.label;
EXPECT_LT(result.finalLocalGraphSize, v.maxLocalGraph)
<< v.label << " working memory was not capped";
// Loop closures are only found against what the working memory holds, so what comes
// back into it is what they are found on.
// Loop closures are only found against what the working memory holds.
EXPECT_GT(result.loopClosuresAccepted, v.minLoops) << v.label << " found too few loop closures";
EXPECT_LT(result.loopClosuresAccepted, v.maxLoops) << v.label << " found more loop closures than expected";
// And the trajectory holds up on the strength of those.
EXPECT_LT(tRmse, v.maxTransRmse) << v.label << " trajectory drifted from the reference";
EXPECT_LT(rRmse, v.maxRotRmse) << v.label << " orientation drifted from the reference";
// The trajectory is printed but not asserted, following from which closures a run
// happens to find.
// The recorded session had a hypothesis worth the name on 562 of its 935 nodes; the
// replay is held against those. Not the same one every time -- the sessions part ways
// as soon as they accept a different closure, and what a capped working memory holds
// follows from that -- but the same place on three quarters or more, and past the
// threshold on nine tenths.
EXPECT_GT(hyp.nodes, 400) << v.label << " compared too few nodes";
EXPECT_GT(hyp.samePlaceRatio(), v.minSamePlaceRatio)
<< v.label << " points at other places than the recorded session";
EXPECT_LT(hyp.meanAbsValue, v.maxMeanValueDiff)
<< v.label << " hypothesis probabilities drifted from the recorded session";
EXPECT_GT(hyp.overThresholdRatio(), 0.70f)
<< v.label << " left the recorded session's hypotheses under the threshold";
loopsPerVariant[v.label] = result.loopClosuresAccepted;
}
// What comes back is what the loop closures are found on, so the settings of it stand in
// this order, whatever a run does inside its band: nothing coming back finds the fewest,
// the local retrieval around the current pose finds more, and bringing back what the
// likelihood points at -- on its own or together with the local retrieval -- finds the
// most.
// Whatever a run does inside its band, the retrieval settings stand in this order: none
// finds the fewest, local only finds more, and bringing back what the likelihood points at
// finds the most.
ASSERT_EQ(loopsPerVariant.size(), 5u);
EXPECT_GT(loopsPerVariant.at("local-retrieval-only"), loopsPerVariant.at("no-retrieval"));
EXPECT_GT(loopsPerVariant.at("both-retrieval"), loopsPerVariant.at("no-retrieval"));