diff --git a/corelib/test/test_bayesfilter.cpp b/corelib/test/test_bayesfilter.cpp index 3101e630..e585652e 100644 --- a/corelib/test/test_bayesfilter.cpp +++ b/corelib/test/test_bayesfilter.cpp @@ -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 { 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(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 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 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 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. diff --git a/corelib/test/test_rtabmap_integration.cpp b/corelib/test/test_rtabmap_integration.cpp index f1e5c148..2453c14a 100644 --- a/corelib/test/test_rtabmap_integration.cpp +++ b/corelib/test/test_rtabmap_integration.cpp @@ -46,6 +46,7 @@ #include "TestUtils.h" #include #include +#include #include #include #include @@ -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 > 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(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 & parent, int id) { while(parent.at(id) != id) @@ -799,9 +805,8 @@ int graphComponentRoot(std::map & 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 & poses, const std::multimap & links) @@ -813,8 +818,7 @@ int countConnectedComponents( } for(std::multimap::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 > loadDatabaseHighestHypothesis(const std::string & path) +{ + std::map > hypothesis; + DBDriver * driver = DBDriver::create(); + if(!driver->openConnection(path)) + { + delete driver; + return hypothesis; + } + std::set ids; + driver->getAllNodeIds(ids); + for(std::set::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter) + { + double stamp = 0.0; + const std::map stats = driver->getStatistics(*iter, stamp); + const std::map::const_iterator idIter = + stats.find(Statistics::kLoopHighest_hypothesis_id()); + const std::map::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 > & golden, + const std::map > & replayed, + const std::map & poses, + float loopThreshold) +{ + HypothesisComparison c; + double sum = 0.0; + for(std::map >::const_iterator iter=golden.begin(); iter!=golden.end(); ++iter) + { + if(iter->second.first <= 0 || iter->second.second < loopThreshold) + { + continue; + } + const std::map >::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::const_iterator goldenPose = poses.find(goldenId); + const std::map::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 & 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 > 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 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")); diff --git a/data/tests/manifest.txt b/data/tests/manifest.txt index e692f5e1..9b239373 100644 --- a/data/tests/manifest.txt +++ b/data/tests/manifest.txt @@ -25,7 +25,7 @@ pr2_scan2d_corridor_50s.db 18ClEPTRM98icorrlDYBzKTWRwVOU8-bh f518cb4d9284b24a150 robust_graph_optimization_stereo.db https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/robust_graph_optimization_stereo.db 247694b5bdb82168ebe88bb6bb5bc31c7142234f8c64567afd93e464418cf02e loop_3it_gps.db https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/RobustGraphOptimization/loop_3it_gps.db 7aefeb573107a27b0a7826cb87ea03db495367aa56868b4d2b2543e6cacad3c8 stereo_20Hz.db https://github.com/introlab/rtabmap/releases/download/0.23.1/stereo_20Hz.db 94e219e1c96e540cbb1490cc23c81c65b9e7b132e8d61eda05e7990bc13393b7 -multisession_3it.7z 10ulRDMqQy5V_3_kx_IeNzAJ8iUMdkkBZ 7b0b02108f05516870eea42cbb519aa6c46a0ffd1eb3e525a9920d3b1997b6f4 multisession_3it.db +multisession_3it.7z 10ulRDMqQy5V_3_kx_IeNzAJ8iUMdkkBZ 3d7b139e6b4c608185e44774288ce078b6364bfaeecd0c46c51007c95230c987 multisession_3it.db superpoint_v1.pth https://github.com/magicleap/SuperPointPretrainedNetwork/raw/refs/heads/master/superpoint_v1.pth 52b6708629640ca883673b5d5c097c4ddad37d8048b33f09c8ca0d69db12c40e superpoint_v6_from_tf.pth https://github.com/rpautrat/SuperPoint/raw/refs/heads/master/weights/superpoint_v6_from_tf.pth cd5d19a5061848e248c17728878ea166b66512076d43c77dbcf27f4a88a56084 demo_superpoint.py https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py 613706ae7e9ce3fbc2cfe042fc3f37d739838cc2e1dc8ddd08f8ec037765df04