From 261bf9dfa2303e3a98447650b4222e8f0cd24973 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 19 Aug 2026 21:23:48 -0700 Subject: [PATCH] fixing CI --- corelib/include/rtabmap/core/BayesFilter.h | 5 - corelib/include/rtabmap/core/Parameters.h | 2 +- corelib/src/BayesFilter.cpp | 27 ++--- corelib/src/bayes/DensePrediction.cpp | 26 +++-- corelib/src/bayes/DensePrediction.h | 22 ++-- corelib/test/perf_bayesfilter.cpp | 2 +- corelib/test/test_bayesfilter.cpp | 130 +++++++++++++++++++++ 7 files changed, 177 insertions(+), 37 deletions(-) diff --git a/corelib/include/rtabmap/core/BayesFilter.h b/corelib/include/rtabmap/core/BayesFilter.h index 2edf7aab..c9188a2f 100644 --- a/corelib/include/rtabmap/core/BayesFilter.h +++ b/corelib/include/rtabmap/core/BayesFilter.h @@ -182,11 +182,6 @@ public: unsigned long getMemoryUsed() const; private: - /** - * @brief Whether the posterior is indexed by exactly @p ids, in that order. - */ - bool posteriorHasSameIds(const std::vector & ids) const; - /** * @brief Realigns the posterior with the ids of the likelihood. * diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 8612183a..c69dd412 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -388,7 +388,7 @@ class RTABMAP_CORE_EXPORT Parameters // BayesFilter RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9, "Virtual place prior. Considering that we are at a new place, this is the prior probability to move again to a new place (unvisited location). The prior probability to move to a previously visited location is 1 - VirtualPlacePriorThr (split equally against all previously visited locations)."); - RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}. Considering we are at a previously visited location, the first value is the probability to move to a new place (unvisited location), the second value is the probability to stay at the same location, the third value is the probability to move to a neighbor or loop closure at the first depth level, the fourth value is the probability to move to a neighbor or loop closure at the second depth level, etc. If the sum of the values is not 1, the difference is normalized against all remaining visited locations. Normally, the sum of these values should be 1."); + RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}. Considering we are at a previously visited location, the first value is the probability to move to a new place (unvisited location), the second value is the probability to stay at the same location, the third value is the probability to move to a neighbor or loop closure at the first depth level, the fourth value is the probability to move to a neighbor or loop closure at the second depth level, etc. If the sum of the values is not 1, the difference is normalized against all remaining visited locations. Normally, the sum of these values should be 1."); RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, false, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated)."); RTABMAP_PARAM(Bayes, SparsePrediction, bool, true, uFormat("Multiply the prediction matrix with the last posterior using a sparse (compressed sparse row) view of the prediction instead of a dense matrix multiplication. A column of the prediction matrix only holds the neighbors within the depth of %s, so on a large map the matrix is mostly zeros and the dense multiplication spends all of its time reading them. The sparse view is rebuilt only when the prediction matrix changes, thus it costs nothing in localization mode over a fixed graph. Ignored (the dense multiplication is used) when the values of %s sum to less than 1, as the missing probability is then spread over every zero of each column and the matrix is no longer sparse.", kBayesPredictionLC().c_str(), kBayesPredictionLC().c_str()).c_str()); diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index a6a5d9a6..e73a515f 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -114,11 +114,6 @@ float BayesFilter::getVirtualPlacePrior() const return _model->virtualPlacePrior(); } -bool BayesFilter::posteriorHasSameIds(const std::vector & ids) const -{ - return _posteriorIds == ids; -} - void BayesFilter::reset() { _posteriorIds.clear(); @@ -219,16 +214,23 @@ bool BayesFilter::computePosterior(const Memory * memory, const std::mapvalues()); } - if(!sparseBuilt) + if(sparseBuilt) + { + // The matrix is released as soon as the sparse form takes over. It is built for the + // locations of the iteration it was built on, and the locations move on while the + // sparse form is the one being used, so it can neither be multiplied nor carried over + // once the sparse form gives the prediction back. A fallback to the matrix builds it + // again. + _dense->clear(); + } + else { _sparse->clear(); if(_predictionChanged || _dense->empty()) { // Only when it has to be: over a fixed graph the matrix of the last iteration is - // the one this iteration wants. _posteriorIds is still the locations it was built - // for, the posterior being realigned below. - _dense->set(_dense->generate(*_model, memory, _posteriorIds, ids, - _fullPredictionUpdate, &_neighborsIndex)); + // the one this iteration wants. + _dense->generate(*_model, memory, ids, _fullPredictionUpdate, &_neighborsIndex); } UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _dense->matrix().rows, _dense->matrix().cols); @@ -297,13 +299,12 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector // The prediction is being kept sparse, so there is no matrix to return. return cv::Mat(); } - if(!_dense->empty() && this->posteriorHasSameIds(ids)) + if(!_dense->empty() && _dense->ids() == ids) { return _dense->matrix(); } UASSERT(memory && _model->valid() && ids.size()); - // The ids the matrix was built for, which the posterior is still indexed by. - return _dense->generate(*_model, memory, _posteriorIds, ids, _fullPredictionUpdate, &_neighborsIndex); + return _dense->generate(*_model, memory, ids, _fullPredictionUpdate, &_neighborsIndex); } unsigned long BayesFilter::getMemoryUsed() const diff --git a/corelib/src/bayes/DensePrediction.cpp b/corelib/src/bayes/DensePrediction.cpp index 1fba4ae3..dea1125f 100644 --- a/corelib/src/bayes/DensePrediction.cpp +++ b/corelib/src/bayes/DensePrediction.cpp @@ -38,15 +38,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { namespace bayes { -cv::Mat DensePrediction::generate(const PredictionModel & model, const Memory * memory, - const std::vector & previousIds, const std::vector & ids, - bool fullUpdate, NeighborsCache * cache) const +const cv::Mat & DensePrediction::generate(const PredictionModel & model, const Memory * memory, + const std::vector & ids, bool fullUpdate, NeighborsCache * cache) { - if(!fullUpdate && !matrix_.empty()) + // The update carries the matrix already there over, so it can only be done against the + // locations that matrix is built for. There is none to carry over when the sparse form has + // been used since, or when the model changed, and every column is built again. + if(!fullUpdate && !matrix_.empty() && ids_.size() == (size_t)matrix_.cols) { - return this->update(model, memory, previousIds, ids, cache); + matrix_ = this->update(model, memory, ids_, ids, cache); } - return this->generateFull(model, memory, ids, fullUpdate?0:cache); + else + { + matrix_ = this->generateFull(model, memory, ids, fullUpdate?0:cache); + } + ids_ = ids; + return matrix_; } void DensePrediction::multiply(const std::vector & posterior, std::vector & prior) const @@ -66,7 +73,12 @@ void DensePrediction::multiply(const std::vector & posterior, std::vector unsigned long DensePrediction::memoryUsed() const { - return matrix_.empty() ? 0 : (unsigned long)(matrix_.total() * matrix_.elemSize()); + unsigned long memory = ids_.capacity() * sizeof(int); + if(!matrix_.empty()) + { + memory += (unsigned long)(matrix_.total() * matrix_.elemSize()); + } + return memory; } // The matrix built column by column, every column from the neighborhood of one location. diff --git a/corelib/src/bayes/DensePrediction.h b/corelib/src/bayes/DensePrediction.h index b64c4145..231b453e 100644 --- a/corelib/src/bayes/DensePrediction.h +++ b/corelib/src/bayes/DensePrediction.h @@ -51,22 +51,23 @@ class DensePrediction public: bool empty() const {return matrix_.empty();} const cv::Mat & matrix() const {return matrix_;} - void set(const cv::Mat & matrix) {matrix_ = matrix;} - void clear() {matrix_ = cv::Mat();} + + /// The locations the matrix is built for, which the incremental update carries over. + const std::vector & ids() const {return ids_;} + + void clear() {matrix_ = cv::Mat(); ids_.clear();} /** - * @brief Builds the matrix for @p ids, without keeping it. + * @brief Builds the matrix for @p ids and keeps it, along with the ids it is built for. * - * The caller may only want to look at it, so what is returned is what set() takes. + * The matrix already there is carried over when it is built for locations @p ids only + * appends to; otherwise every column is built again. * - * @param previousIds The ids the kept matrix was built for, which the incremental update - * carries over. - * @param fullUpdate Rebuilds every column rather than carrying the kept matrix over. + * @param fullUpdate Rebuilds every column rather than carrying the matrix over. * @param cache Filled with the neighborhoods, for a later incremental update. */ - cv::Mat generate(const PredictionModel & model, const Memory * memory, - const std::vector & previousIds, const std::vector & ids, - bool fullUpdate, NeighborsCache * cache) const; + const cv::Mat & generate(const PredictionModel & model, const Memory * memory, + const std::vector & ids, bool fullUpdate, NeighborsCache * cache); /// prior = prediction x posterior. void multiply(const std::vector & posterior, std::vector & prior) const; @@ -81,6 +82,7 @@ private: NeighborsCache * cache) const; cv::Mat matrix_; + std::vector ids_; }; } // namespace bayes diff --git a/corelib/test/perf_bayesfilter.cpp b/corelib/test/perf_bayesfilter.cpp index e95b6e80..6893d2c4 100644 --- a/corelib/test/perf_bayesfilter.cpp +++ b/corelib/test/perf_bayesfilter.cpp @@ -46,7 +46,7 @@ static const int MAP_SIZES[] = {1000, 4000, 8000}; // slightly more than 1: below 1, normalize() spreads what is missing over every zero // of a column and the matrix is no longer sparse at all. static const char PREDICTION_DEFAULT[] = - "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 " + "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1e-06 4.8e-08 " "1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23"; static const char PREDICTION_TRUNCATED[] = "0.1 0.36003 0.30 0.16 0.062 0.0151 0.00255 0.000324"; diff --git a/corelib/test/test_bayesfilter.cpp b/corelib/test/test_bayesfilter.cpp index a09040ba..416d00cf 100644 --- a/corelib/test/test_bayesfilter.cpp +++ b/corelib/test/test_bayesfilter.cpp @@ -1291,6 +1291,136 @@ 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. +TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix) +{ + ParametersMap paramsDense; + paramsDense.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); + paramsDense.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false")); + + ParametersMap paramsSparse = paramsDense; + paramsSparse[Parameters::kBayesSparsePrediction()] = "true"; + + 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. + addChain(6); + std::vector ids; + for(int iter = 0; iter < 40; ++iter) + { + SensorData data(image_); + Transform pose(float(6 + iter), 0.0f, 0.0f, 0, 0, 0); + ASSERT_TRUE(memory_->update(data, pose, covariance_)); + + ids = getBayesIds(); + std::map likelihood = uniformLikelihood(ids); + likelihood[ids[ids.size()/2]] = 4.0f; + + ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood)); + for(size_t i = 0; i < ids.size(); ++i) + { + ASSERT_NEAR(posteriorOf(filterDense).at(ids[i]), + posteriorOf(filterSparse).at(ids[i]), 1e-5f) + << "iter=" << iter << " id=" << ids[i]; + } + } + // Grown into the sparse form: there is no matrix left to hand over. + ASSERT_TRUE(filterSparse.generatePrediction(memory_, ids).empty()); + + // 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. + ParametersMap disableSparse; + disableSparse.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false")); + filterSparse.parseParameters(disableSparse); + + SensorData data(image_); + Transform pose(46.0f, 0.0f, 0.0f, 0, 0, 0); + ASSERT_TRUE(memory_->update(data, pose, covariance_)); + + ids = getBayesIds(); + std::map likelihood = uniformLikelihood(ids); + likelihood[ids[ids.size()/2]] = 4.0f; + ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood)); + + const cv::Mat prediction = filterSparse.generatePrediction(memory_, ids); + ASSERT_EQ(prediction.rows, (int)ids.size()); + ASSERT_EQ(prediction.cols, (int)ids.size()); + for(size_t i = 0; i < ids.size(); ++i) + { + EXPECT_NEAR(posteriorOf(filterDense).at(ids[i]), + posteriorOf(filterSparse).at(ids[i]), 1e-5f) << "id=" << ids[i]; + } +} + +// The same crossing, taken because the graph became densely linked rather than because the +// parameter changed. A loop closure costs no depth in the graph search, so the locations it +// joins share a column and the neighborhoods that go in it; enough of them and a column +// reaches more than a quarter of the map, which is where the matrix costs less than the +// values of the sparse form. +TEST_F(BayesFilterMemoryFixture, PredictionFallsBackToTheMatrixWhenTheGraphBecomesDense) +{ + ParametersMap params; + params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); + params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "true")); + BayesFilter filter(params); + + addChain(6); + std::vector ids; + for(int iter = 0; iter < 40; ++iter) + { + SensorData data(image_); + Transform pose(float(6 + iter), 0.0f, 0.0f, 0, 0, 0); + ASSERT_TRUE(memory_->update(data, pose, covariance_)); + ids = getBayesIds(); + ASSERT_TRUE(filter.computePosterior(memory_, uniformLikelihood(ids))); + } + ASSERT_TRUE(filter.generatePrediction(memory_, ids).empty()); + + 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) + { + const int to = ids[(i+third)%ids.size()]; + if(ids[i] > 0 && to > 0 && ids[i] != to) + { + ASSERT_TRUE(memory_->addLink(Link(ids[i], to, Link::kGlobalClosure, + Transform::getIdentity(), infMatrix))); + } + } + + // A location leaving the working memory, which the sparse form cannot be carried over + // through: every location after it shifts index. So the prediction is built again, and it + // is on that build that the graph is measured as too dense to keep sparse. + std::vector fewerIds = ids; + fewerIds.erase(fewerIds.begin() + fewerIds.size()/2); + ASSERT_TRUE(filter.computePosterior(memory_, uniformLikelihood(fewerIds))); + + // The matrix is built for the locations of this iteration, not updated against the ones it + // was built for before the sparse form took over. + const cv::Mat prediction = filter.generatePrediction(memory_, fewerIds); + ASSERT_EQ(prediction.rows, (int)fewerIds.size()); + ASSERT_EQ(prediction.cols, (int)fewerIds.size()); + + const std::map & posterior = posteriorOf(filter); + ASSERT_EQ(posterior.size(), fewerIds.size()); + float sum = 0.0f; + for(std::map::const_iterator iter=posterior.begin(); iter!=posterior.end(); ++iter) + { + EXPECT_GE(iter->second, 0.0f) << "id=" << iter->first; + sum += iter->second; + } + EXPECT_NEAR(sum, 1.0f, 1e-4f); +} + // generatePrediction() hands over the prediction matrix, which only the dense mode has. // With the prediction kept sparse there is none, and it says so with an empty matrix rather // than building one: Rtabmap::dumpPrediction(), the one caller outside the filter, reports