From 238e73755cf44d89691e9bd21dd067ede671c727 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 20 Aug 2026 00:35:35 -0700 Subject: [PATCH] optimized sparse when transfer/retrieval happens (was slower than dense for that case) --- corelib/src/bayes/SparsePrediction.cpp | 264 ++++++++++++++++++++++--- corelib/src/bayes/SparsePrediction.h | 30 ++- corelib/test/test_bayesfilter.cpp | 164 ++++++++++++--- 3 files changed, 404 insertions(+), 54 deletions(-) diff --git a/corelib/src/bayes/SparsePrediction.cpp b/corelib/src/bayes/SparsePrediction.cpp index 20933478..991071ca 100644 --- a/corelib/src/bayes/SparsePrediction.cpp +++ b/corelib/src/bayes/SparsePrediction.cpp @@ -273,6 +273,63 @@ bool SparsePrediction::generate(const PredictionModel & model, const Memory * me return true; } +// One column, from the neighborhood of the location it is for. Read from the cache, which +// generate() filled and which the graph is only walked again for when a location came back +// from long-term memory after its neighborhood was dropped. +void SparsePrediction::buildColumn(const PredictionModel & model, const Memory * memory, int id, + int index, const std::vector & ids, const IdToIndexMap & idToIndex, + std::vector & buffer, NeighborsCache & cache) +{ + const std::map & neighbors = cachedNeighbors(memory, id, model.depth(), cache); + const float sum = model.addNeighborProb(&buffer[0], 1, neighbors, idToIndex); + model.normalize(&buffer[0], 1, (int)ids.size(), index, sum, ids[0]<0); + this->takeColumn(buffer, index, true); +} + +// Carries the prediction over to the locations of an iteration, which costs the columns whose +// contents changed rather than a walk of the graph per column. +// +// Returns false when there is nothing to carry over, which the caller answers by calling +// generate(). +bool SparsePrediction::update(const PredictionModel & model, const Memory * memory, + const std::vector & newIds, NeighborsCache & cache) +{ + if(ids_.empty() || newIds.empty() || columns_.size() != ids_.size()) + { + return false; + } + + // Appended to, or changed in any other way: the first keeps every index, the second has + // to lay the columns out again. + const bool appendedTo = + newIds.size() > ids_.size() && + memcmp(ids_.data(), newIds.data(), ids_.size()*sizeof(int)) == 0; + const bool updated = appendedTo + ? this->updateAppended(model, memory, newIds, cache) + : this->updateRemapped(model, memory, newIds, cache); + if(!updated) + { + return false; + } + + // Whether it is still worth keeping sparse, measured on the values themselves: past a + // quarter filled they cost more than the matrix, a value being 8 bytes against its 4. + // generate() has to estimate that from a sample of the graph before building anything, + // while here the count is already known. + const size_t maxValues = (size_t)newIds.size()*(size_t)newIds.size()/4; + if(used_ > maxValues) + { + UWARN("The prediction holds %ld values over %d locations, more than the quarter of " + "them past which the matrix costs less, so it is not kept sparse. Every loop " + "closure link widens a column, as one costs no depth in the graph search, and " + "so does a longer %s.", + (long)used_, (int)newIds.size(), Parameters::kBayesPredictionLC().c_str()); + this->clear(); + return false; + } + return true; +} + // The same prediction after locations were appended, without building it again. // // Every location that was already there keeps its index, so the columns already built @@ -280,23 +337,11 @@ bool SparsePrediction::generate(const PredictionModel & model, const Memory * me // column of the virtual place, whose values are shared out over however many locations // there are. What a column holds does not otherwise depend on how many there are, the // model summing to 1 leaving normalize() nothing to spread over the others. -// -// Returns false when the locations are not the previous ones with more appended, which the -// caller has to answer by building the prediction again: a location removed shifts the -// index of every one after it, and the dense update has to recompute every column then too. -bool SparsePrediction::update(const PredictionModel & model, const Memory * memory, +bool SparsePrediction::updateAppended(const PredictionModel & model, const Memory * memory, const std::vector & newIds, NeighborsCache & cache) { - const std::vector & oldIds = ids_; - if(oldIds.empty() || - newIds.size() <= oldIds.size() || - columns_.size() != oldIds.size() || - memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) != 0) - { - return false; - } - UTimer timer; + const std::vector & oldIds = ids_; const int size = (int)newIds.size(); IdToIndexMap newIdToIndexMap; @@ -339,13 +384,8 @@ bool SparsePrediction::update(const PredictionModel & model, const Memory * memo for(std::set::const_iterator iter=idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) { - const NeighborsCache::const_iterator kter = cache.find(*iter); - UASSERT_MSG(kter != cache.end(), - uFormat("Did not find %d (current index size=%d)", *iter, (int)cache.size()).c_str()); - const int index = newIdToIndexMap.at(*iter); - const float sum = model.addNeighborProb(&column[0], 1, kter->second, newIdToIndexMap); - model.normalize(&column[0], 1, size, index, sum, newIds[0]<0); - this->takeColumn(column, index, true); + this->buildColumn(model, memory, *iter, newIdToIndexMap.at(*iter), + newIds, newIdToIndexMap, column, cache); } // The virtual place shares what is left of its probability over the visited locations, @@ -364,11 +404,191 @@ bool SparsePrediction::update(const PredictionModel & model, const Memory * memo { this->compact(); } + const size_t appended = newIds.size()-oldIds.size(); ids_ = newIds; UDEBUG("Sparse prediction: %d locations appended, %d columns rebuilt of %d, %ld values, " "%ld left behind%s, updated in %fs", - (int)(newIds.size()-oldIds.size()), (int)idsToUpdate.size(), size, + (int)appended, (int)idsToUpdate.size(), size, + (long)used_, (long)waste, compacted?" (packed again)":"", + timer.ticks()); + return true; +} + +// The same prediction after the locations changed in any other way than being appended to: +// locations gone from the working memory as it is capped, locations back from long-term +// memory in the middle of the ones already there, or both at once. +// +// The index of a location moves, so the columns are laid out again -- but a column is only +// built again when what goes in it changed, which is when: +// * the location was not there before, so it has no column yet; +// * one of those is now part of its neighborhood, so it gains a value; +// * it shared its probability with a location that is gone, which normalize() now shares +// out over the ones that remain. +// Every other column is the same values at another row, which is a copy. That is what +// separates this from generate(): the graph is walked for the columns that changed, not for +// every one of them. +bool SparsePrediction::updateRemapped(const PredictionModel & model, const Memory * memory, + const std::vector & newIds, NeighborsCache & cache) +{ + UTimer timer; + const std::vector & oldIds = ids_; + const int size = (int)newIds.size(); + + // The virtual place appearing or disappearing changes every column, normalize() holding + // back its probability on all of them, so there would be nothing to carry over. + if((oldIds[0] < 0) != (newIds[0] < 0)) + { + return false; + } + + IdToIndexMap newIdToIndexMap; + IdToIndexMap oldIdToIndexMap; +#if __cplusplus >= 201103L + newIdToIndexMap.reserve(newIds.size()); + oldIdToIndexMap.reserve(oldIds.size()); +#endif + for(int i=0; i0) + { + newIdToIndexMap[newIds[i]] = i; + } + } + for(size_t i=0; i0) + { + oldIdToIndexMap[oldIds[i]] = (int)i; + } + } + + // Where each location went, and which ones are gone. The virtual place is the first of + // both, so it does not move. + std::vector oldToNew(oldIds.size(), -1); + size_t removed = 0; + for(size_t i=0; isecond; + } + } + + // The locations that were not there before, and the ones whose neighborhood they are + // part of. + std::set idsToBuild; + for(int i=0; i & neighbors = cachedNeighbors(memory, newIds[i], model.depth(), cache); + for(std::map::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) + { + if(iter->first > 0 && + newIdToIndexMap.find(iter->first) != newIdToIndexMap.end() && + oldIdToIndexMap.find(iter->first) != oldIdToIndexMap.end()) + { + idsToBuild.insert(iter->first); + } + } + } + + // And the ones holding a value on a row that is gone. + if(removed) + { + for(size_t i=0; i keptColumns(size); + std::vector > keptValues; + keptValues.reserve(used_); + size_t keptUsed = 0; + size_t carried = 0; + for(size_t i=0; i column(size, 0.0f); + for(std::set::const_iterator iter=idsToBuild.begin(); iter!=idsToBuild.end(); ++iter) + { + this->buildColumn(model, memory, *iter, newIdToIndexMap.at(*iter), + newIds, newIdToIndexMap, column, cache); + } + + // The virtual place shares what is left of its probability over the visited locations, + // so its column depends on how many of them there are. + if(newIds[0] < 0) + { + model.fillVirtualPlaceColumn(&column[0], 1, size); + this->takeColumn(column, 0, true); + } + + const size_t waste = values_.size() - used_; + const bool compacted = waste > used_/4; + if(compacted) + { + this->compact(); + } + ids_ = newIds; + + UDEBUG("Sparse prediction: %d locations removed, %d columns carried over and %d built " + "again of %d, %ld values, %ld left behind%s, updated in %fs", + (int)removed, (int)carried, (int)idsToBuild.size(), size, (long)used_, (long)waste, compacted?" (packed again)":"", timer.ticks()); return true; diff --git a/corelib/src/bayes/SparsePrediction.h b/corelib/src/bayes/SparsePrediction.h index b99f8b51..60ccef6a 100644 --- a/corelib/src/bayes/SparsePrediction.h +++ b/corelib/src/bayes/SparsePrediction.h @@ -72,15 +72,16 @@ public: const std::vector & ids, NeighborsCache * cache); /** - * @brief Carries it over to @p ids without building it again. + * @brief Carries it over to @p ids without walking the graph again. * - * Every location already there keeps its index when locations are only appended, so the - * columns already built still apply and only the ones the appended locations reach are - * built again. + * Only the columns whose contents changed are built again, from the neighborhoods of + * @p cache: the ones of the locations that were not there before, of their neighbors, and + * of the locations that shared their probability with one that is gone. Every other column + * is carried over, at another index when locations were removed. * - * @return False when @p ids is not the ids() it was built for with more appended, which the - * caller has to answer by calling generate(): a location removed shifts the index of - * every one after it. + * @return False when there is nothing to carry over: no prediction yet, the virtual place + * appearing or disappearing, or the result measured as too dense to keep sparse. + * The caller answers by calling generate(), which is also what fills @p cache. */ bool update(const PredictionModel & model, const Memory * memory, const std::vector & ids, NeighborsCache & cache); @@ -109,6 +110,21 @@ private: size_t capacity = 0; }; + /// update() when @p ids is the ids() it was built for with more appended: every location + /// keeps its index, so the columns are updated where they are. + bool updateAppended(const PredictionModel & model, const Memory * memory, + const std::vector & ids, NeighborsCache & cache); + + /// update() when locations were removed, or came back in the middle of the ones already + /// there: the index of a location moves, so the columns are laid out again. + bool updateRemapped(const PredictionModel & model, const Memory * memory, + const std::vector & ids, NeighborsCache & cache); + + /// Builds one column, from the neighborhood of the location it is for. + void buildColumn(const PredictionModel & model, const Memory * memory, int id, int index, + const std::vector & ids, const IdToIndexMap & idToIndex, + std::vector & buffer, NeighborsCache & cache); + void takeColumn(std::vector & column, int index, bool withRoomToGrow); void compact(); diff --git a/corelib/test/test_bayesfilter.cpp b/corelib/test/test_bayesfilter.cpp index 47ca2048..3101e630 100644 --- a/corelib/test/test_bayesfilter.cpp +++ b/corelib/test/test_bayesfilter.cpp @@ -1439,18 +1439,24 @@ TEST_F(BayesFilterMemoryFixture, PredictionCrossesFromSparseBackToTheMatrix) } } -// 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) +// 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. +TEST_F(BayesFilterMemoryFixture, PredictionCarriesOverWhenLocationsLeaveTheWorkingMemory) { - ParametersMap params; - params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); - params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "true")); - BayesFilter filter(params); + 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); + + // Grown past the size where the sparse form is worth keeping. addChain(6); std::vector ids; for(int iter = 0; iter < 40; ++iter) @@ -1459,10 +1465,119 @@ TEST_F(BayesFilterMemoryFixture, PredictionFallsBackToTheMatrixWhenTheGraphBecom 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))); + const std::map likelihood = uniformLikelihood(ids); + ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood)); } - ASSERT_TRUE(filter.isPredictionSparse()); + 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. + std::vector fewerIds = ids; + for(int iter = 0; iter < 5; ++iter) + { + fewerIds.erase(fewerIds.begin() + fewerIds.size()/2); + fewerIds.erase(fewerIds.begin() + 1); // the oldest, the virtual place being first + const std::map likelihood = uniformLikelihood(fewerIds); + ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood)); + + // Carried over, not built again from the graph and not given up on. + ASSERT_TRUE(filterSparse.isPredictionSparse()) << "iter=" << iter; + + const cv::Mat predictionDense = filterDense.generatePrediction(memory_, fewerIds); + const cv::Mat predictionSparse = filterSparse.generatePrediction(memory_, fewerIds); + ASSERT_EQ(predictionDense.rows, (int)fewerIds.size()) << "iter=" << iter; + ASSERT_EQ(predictionSparse.rows, (int)fewerIds.size()) << "iter=" << iter; + for(int col = 0; col < predictionDense.cols; ++col) + { + for(int row = 0; row < predictionDense.rows; ++row) + { + ASSERT_NEAR(predictionAt(predictionSparse, row, col), + predictionAt(predictionDense, row, col), 1e-6f) + << "iter=" << iter << " row=" << row << " col=" << col; + } + } + for(size_t i = 0; i < fewerIds.size(); ++i) + { + ASSERT_NEAR(posteriorOf(filterDense).at(fewerIds[i]), + posteriorOf(filterSparse).at(fewerIds[i]), 1e-5f) + << "iter=" << iter << " id=" << fewerIds[i]; + } + } +} + +// 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. +TEST_F(BayesFilterMemoryFixture, PredictionCarriesOverWhenALocationComesBack) +{ + 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); + + addChain(46); + const std::vector allIds = getBayesIds(); + ASSERT_GT(allIds.size(), 40u); + + // The oldest locations held back, as long-term memory holds them. + std::vector ids = allIds; + const std::vector heldBack(ids.begin()+1, ids.begin()+6); + ids.erase(ids.begin()+1, ids.begin()+6); + + std::map likelihood = uniformLikelihood(ids); + ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.isPredictionSparse()); + + // Back they come, one per iteration, each one in the middle of the ones already there. + for(size_t i = 0; i < heldBack.size(); ++i) + { + ids.insert(ids.begin()+1+i, heldBack[i]); + likelihood = uniformLikelihood(ids); + ASSERT_TRUE(filterDense.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.computePosterior(memory_, likelihood)); + ASSERT_TRUE(filterSparse.isPredictionSparse()) << "back=" << heldBack[i]; + + const cv::Mat predictionDense = filterDense.generatePrediction(memory_, ids); + const cv::Mat predictionSparse = filterSparse.generatePrediction(memory_, ids); + ASSERT_EQ(predictionSparse.rows, (int)ids.size()) << "back=" << heldBack[i]; + for(int col = 0; col < predictionDense.cols; ++col) + { + for(int row = 0; row < predictionDense.rows; ++row) + { + ASSERT_NEAR(predictionAt(predictionSparse, row, col), + predictionAt(predictionDense, row, col), 1e-6f) + << "back=" << heldBack[i] << " row=" << row << " col=" << col; + } + } + for(size_t k = 0; k < ids.size(); ++k) + { + ASSERT_NEAR(posteriorOf(filterDense).at(ids[k]), + posteriorOf(filterSparse).at(ids[k]), 1e-5f) + << "back=" << heldBack[i] << " id=" << ids[k]; + } + } +} + +// 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. +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. 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) @@ -1475,22 +1590,21 @@ TEST_F(BayesFilterMemoryFixture, PredictionFallsBackToTheMatrixWhenTheGraphBecom } } - // 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))); + ParametersMap params; + params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); + params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "true")); + BayesFilter filter(params); - // 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. - ASSERT_FALSE(filter.isPredictionSparse()); - const cv::Mat prediction = filter.generatePrediction(memory_, fewerIds); - ASSERT_EQ(prediction.rows, (int)fewerIds.size()); - ASSERT_EQ(prediction.cols, (int)fewerIds.size()); + ASSERT_TRUE(filter.computePosterior(memory_, uniformLikelihood(ids))); + + // The matrix, and a posterior that is still a distribution over the locations. + EXPECT_FALSE(filter.isPredictionSparse()); + const cv::Mat prediction = filter.generatePrediction(memory_, ids); + ASSERT_EQ(prediction.rows, (int)ids.size()); + ASSERT_EQ(prediction.cols, (int)ids.size()); const std::map & posterior = posteriorOf(filter); - ASSERT_EQ(posterior.size(), fewerIds.size()); + ASSERT_EQ(posterior.size(), ids.size()); float sum = 0.0f; for(std::map::const_iterator iter=posterior.begin(); iter!=posterior.end(); ++iter) {