diff --git a/corelib/include/rtabmap/core/BayesFilter.h b/corelib/include/rtabmap/core/BayesFilter.h index de7989b6..bbf1cde4 100644 --- a/corelib/include/rtabmap/core/BayesFilter.h +++ b/corelib/include/rtabmap/core/BayesFilter.h @@ -31,8 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include +#include #include #include +#include #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/core/Parameters.h" @@ -64,6 +66,7 @@ class Signature; * - @ref Parameters::kBayesPredictionLC() — transition probabilities per graph depth level. * - @ref Parameters::kBayesVirtualPlacePriorThr() — prior for the virtual place. * - @ref Parameters::kBayesFullPredictionUpdate() — regenerate the full prediction matrix each iteration. + * - @ref Parameters::kBayesSparsePrediction() — keep the prediction sparse and multiply it sparsely. * * @see Memory::getNeighborsId() * @see Rtabmap @@ -162,6 +165,11 @@ 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 Incrementally updates the prediction matrix when ids are added or removed. */ @@ -176,9 +184,78 @@ private: void updatePosterior(const Memory * memory, const std::vector & likelihoodIds); /** - * @brief Normalizes one row of the prediction matrix and applies the virtual place probability. + * @brief Fills the column of the virtual place (the unvisited location hypothesis). + * + * @param column First value of the column. + * @param stride Step between two values of the column (1 when it is contiguous, the + * width of the matrix when it is one of its columns). + * @param size Number of values in the column. */ - void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const; + void fillVirtualPlaceColumn(float * column, size_t stride, int size) const; + + /** + * @brief Normalizes one column of the prediction and applies the virtual place probability. + * + * @param column First value of the column, see @ref fillVirtualPlaceColumn() for @p stride + * and @p size. + * @param index Index of the location this column is for, so of its diagonal value. + * @param addedProbabilitiesSum Sum of the values @ref addNeighborProb() put in it. + * @param virtualPlaceUsed Whether the first location is the virtual place. + */ + void normalize(float * column, size_t stride, int size, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const; + + /** + * @brief Builds the prediction directly in its sparse form, without the matrix. + * + * One column at a time in a buffer of its own, so nothing of the size of the working + * memory squared is ever allocated. Used when the graph is fixed (localization mode), + * where no incremental matrix update needs the matrix to be kept. + * + * @param memory Working memory instance (must not be null). + * @param ids Ordered list of signature ids, as in @ref generatePrediction(). + * @return False when the prediction would not be sparse, which the caller has to answer + * by building the dense matrix. Happens when the values of + * @ref Parameters::kBayesPredictionLC() sum to less than 1, as @ref normalize() + * then spreads the missing probability over every zero of a column. + */ + bool generateSparsePrediction(const Memory * memory, const std::vector & ids); + + /** + * @brief Rebuilds the sparse prediction from the prediction matrix. + * + * Called only when the matrix has changed, and only when it is being kept anyway, which + * is the case while mapping. Leaves the sparse form empty, which makes + * @ref computePosterior() fall back to the dense multiplication, when the matrix is not + * sparse (see @ref generateSparsePrediction()). + */ + void updateSparsePredictionFromDense(); + + /** + * @brief Appends the non-zero values of a built column, and zeroes the buffer. + */ + void appendSparseColumn(std::vector & column, int index, + std::vector > & triplets) const; + + /** + * @brief Releases the sparse prediction and the memory it holds. + */ + void clearSparsePrediction(); + + /** + * @brief Approximate footprint of the sparse prediction, in bytes. + */ + unsigned long getSparsePredictionMemoryUsed() const; + + /** + * @brief Computes prior = prediction x posterior from the sparse prediction. + * + * Mathematically identical to the dense multiplication, up to the order the products of a + * row are summed in. + * + * @param posterior Column vector of the last posterior, as many rows as the prediction. + * @param prior Output column vector, allocated by this method. + */ + void multiplySparsePrediction(const cv::Mat & posterior, cv::Mat & prior) const; private: std::map _posterior; ///< Current posterior (signature id → probability). @@ -188,6 +265,9 @@ private: bool _fullPredictionUpdate; ///< If true, rebuild the full prediction matrix each time. float _totalPredictionLCValues; ///< Sum of all values in _predictionLC. float _predictionEpsilon; ///< Minimum non-zero probability in the model. + bool _sparsePrediction; ///< Multiply the prediction sparsely (Bayes/SparsePrediction). + bool _predictionChanged; ///< True when _prediction was rebuilt, so the sparse view is stale. + Eigen::SparseMatrix _sparsePredictionMatrix; ///< The prediction, sparse. Built instead of _prediction over a fixed graph. std::map > _neighborsIndex; ///< Cached neighbor margins per signature id. }; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 1a848faf..8612183a 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -390,6 +390,7 @@ class RTABMAP_CORE_EXPORT Parameters 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(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()); // Verify hypotheses RTABMAP_PARAM(VhEp, Enabled, bool, false, uFormat("Verify visual loop closure hypothesis by computing a fundamental matrix. This is done prior to transformation computation when %s is enabled.", kRGBDEnabled().c_str())); diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index 3d24a8ed..c1c08c0d 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -40,11 +40,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { +#if __cplusplus >= 201103L +typedef std::unordered_map IdToIndexMap; +#else +typedef std::map IdToIndexMap; +#endif + BayesFilter::BayesFilter(const ParametersMap & parameters) : _virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()), _fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()), _totalPredictionLCValues(0.0f), - _predictionEpsilon(0.0f) + _predictionEpsilon(0.0f), + _sparsePrediction(Parameters::defaultBayesSparsePrediction()), + _predictionChanged(true) { this->setPredictionLC(Parameters::defaultBayesPredictionLC()); this->parseParameters(parameters); @@ -62,6 +70,16 @@ void BayesFilter::parseParameters(const ParametersMap & parameters) } Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior); Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate); + if(Parameters::parse(parameters, Parameters::kBayesSparsePrediction(), _sparsePrediction)) + { + // The sparse view is rebuilt on the next posterior if it was just enabled, and + // released if it was just disabled. + _predictionChanged = true; + if(!_sparsePrediction) + { + this->clearSparsePrediction(); + } + } UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f); } @@ -113,6 +131,8 @@ void BayesFilter::setPredictionLC(const std::string & prediction) { UDEBUG("predictionEpsilon = %f", _predictionEpsilon); } + // A new model changes the values and the sparsity of the prediction matrix. + _predictionChanged = true; } const std::vector & BayesFilter::getPredictionLC() const @@ -135,10 +155,32 @@ std::string BayesFilter::getPredictionLCStr() const return values; } +// Whether the posterior is indexed by exactly these ids, in this order. Both are +// sorted by id, so they are compared side by side rather than collecting the keys of +// the posterior into a vector of their own to compare with. +bool BayesFilter::posteriorHasSameIds(const std::vector & ids) const +{ + if(_posterior.size() != ids.size()) + { + return false; + } + std::map::const_iterator iter = _posterior.begin(); + for(size_t i=0; ifirst != ids[i]) + { + return false; + } + } + return true; +} + void BayesFilter::reset() { _posterior.clear(); _prediction = cv::Mat(); + this->clearSparsePrediction(); + _predictionChanged = true; _neighborsIndex.clear(); } @@ -172,17 +214,56 @@ const std::map & BayesFilter::computePosterior(const Memory * memory float sum = 0; int j=0; + + // The ids of the likelihood, which both the prediction and the posterior are + // indexed by, taken once: there are as many of them as there are locations in the + // working memory, and walking the map to collect them is not free at that size. + const std::vector ids = uKeys(likelihood); + // Recursive Bayes estimation... // STEP 1 - Prediction : Prior*lastPosterior - _prediction = this->generatePrediction(memory, uKeys(likelihood)); + // + // The prediction is built in its sparse form only, the matrix never being + // allocated, when the graph is fixed: in localization mode there is no incremental + // update of the matrix to carry columns over, so nothing else needs it. While + // mapping, the matrix is built as before and the sparse form is taken from it. + const bool buildSparseDirectly = + _sparsePrediction && + _totalPredictionLCValues >= 1 && + !memory->isIncremental(); + bool sparseBuilt = false; + if(buildSparseDirectly) + { + if(_predictionChanged || _sparsePredictionMatrix.rows() != (int)ids.size() || + !this->posteriorHasSameIds(ids)) + { + sparseBuilt = this->generateSparsePrediction(memory, ids); + } + else + { + sparseBuilt = true; + } + UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), + (int)_sparsePredictionMatrix.rows(), (int)_sparsePredictionMatrix.cols()); + } + if(!sparseBuilt) + { + _prediction = this->generatePrediction(memory, ids); + UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols); + //std::cout << "Prediction=" << _prediction << std::endl; - UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols); - //std::cout << "Prediction=" << _prediction << std::endl; + if(_sparsePrediction && _predictionChanged) + { + // Only when the matrix has changed: over a fixed graph this happens once. + this->updateSparsePredictionFromDense(); + UDEBUG("STEP1-sparse prediction update time=%fs", timer.ticks()); + } + } // Adjust the last posterior if some images were // reactivated or removed from the working memory posterior = cv::Mat(likelihood.size(), 1, CV_32FC1); - this->updatePosterior(memory, uKeys(likelihood)); + this->updatePosterior(memory, ids); j=0; for(std::map::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i) { @@ -193,28 +274,42 @@ const std::map & BayesFilter::computePosterior(const Memory * memory // Multiply prediction matrix with the last posterior // (m,m) X (m,1) = (m,1) - prior = _prediction * posterior; - ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks()); + // The sparse form is empty when disabled, or when the prediction was found too + // dense for it to be worth it. + const bool sparse = _sparsePrediction && _sparsePredictionMatrix.rows() > 0; + if(sparse) + { + this->multiplySparsePrediction(posterior, prior); + } + else + { + prior = _prediction * posterior; + } + ULOGGER_DEBUG("STEP1-matrix mult time=%fs (sparse=%d)", timer.ticks(), sparse?1:0); //std::cout << "ResultingPrior=" << prior << std::endl; - ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks()); - std::vector likelihoodValues = uValues(likelihood); - //std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl; - // STEP 2 - Update : Multiply with observations (likelihood) + // The posterior holds the ids of the likelihood, updatePosterior() having just made + // sure of it, and both are sorted by id: they are walked side by side instead of + // looking up each id in the posterior, which at the size of the working memory is + // as many searches through the map. j=0; + std::map::iterator p = _posterior.begin(); for(std::map::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i) { - std::map::iterator p =_posterior.find((*i).first); - if(p!= _posterior.end()) + if(p == _posterior.end() || p->first != (*i).first) { - (*p).second = (*i).second * ((float*)prior.data)[j++]; - sum+=(*p).second; - } - else - { - ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first); + // Should not happen. Searched for rather than assumed if it ever does. + p = _posterior.find((*i).first); + if(p == _posterior.end()) + { + ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first); + continue; + } } + p->second = (*i).second * ((float*)prior.data)[j++]; + sum += p->second; + ++p; } ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks()); //std::cout << "Posterior (before normalization)=" << _posterior << std::endl; @@ -234,49 +329,91 @@ const std::map & BayesFilter::computePosterior(const Memory * memory return _posterior; } -float addNeighborProb(cv::Mat & prediction, - unsigned int col, +// A column of the prediction matrix, given as a pointer to its first value and the +// step between two of them: the matrix stores a column strided by its width, while the +// sparse build below fills one contiguous column at a time. Both go through this and +// through BayesFilter::normalize(), so that the probabilities cannot end up differing +// between the two. +float addNeighborProb(float * column, + size_t stride, const std::map & neighbors, const std::vector & predictionLC, -#if __cplusplus >= 201103L - const std::unordered_map & idToIndex -#else - const std::map & idToIndex -#endif - ) + const IdToIndexMap & idToIndex) { - UASSERT(col < (unsigned int)prediction.cols && - col < (unsigned int)prediction.rows); - float sum=0.0f; - float * dataPtr = (float*)prediction.data; for(std::map::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) { if(iter->first>=0) { -#if __cplusplus >= 201103L - std::unordered_map::const_iterator jter = idToIndex.find(iter->first); -#else - std::map::const_iterator jter = idToIndex.find(iter->first); -#endif + IdToIndexMap::const_iterator jter = idToIndex.find(iter->first); if(jter != idToIndex.end()) { UASSERT((iter->second+1) < (int)predictionLC.size()); - sum += dataPtr[col + jter->second*prediction.cols] = predictionLC[iter->second+1]; + sum += column[jter->second*stride] = predictionLC[iter->second+1]; } } } return sum; } +// The neighbors of a location within the depth of the prediction model, and the +// locations that are at margin 0 of it, meaning the same place: their columns all hold +// the probabilities of this same neighborhood. Shared by the dense and the sparse +// builds, this being the part that reads the graph. +// +// neighborsIndex is filled when not null, for updatePrediction() to reuse. +std::map resolveNeighbors( + const Memory * memory, + int id, + int maxDepth, + const IdToIndexMap & idToIndexMap, + std::list & idsAtMargin0, + std::map > * neighborsIndex) +{ + std::map neighbors = memory->getNeighborsId(id, maxDepth, 0, false, false, true, true); + + if(neighborsIndex) + { + uInsert(*neighborsIndex, std::make_pair(id, neighbors)); + } + + idsAtMargin0.clear(); + //filter neighbors in STM + for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end();) + { + if(memory->isInSTM(iter->first)) + { + neighbors.erase(iter++); + } + else + { + if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end()) + { + idsAtMargin0.push_back(iter->first); + } + ++iter; + } + } + + // should at least have 1 id in idsMarginLoop + if(idsAtMargin0.size() == 0) + { + UFATAL("No 0 margin neighbor for signature %d !?!?", id); + } + return neighbors; +} + cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector & ids) { - std::vector oldIds = uKeys(_posterior); - if(oldIds.size() == ids.size() && - memcmp(oldIds.data(), ids.data(), oldIds.size()*sizeof(int)) == 0) + if(this->posteriorHasSameIds(ids)) { return _prediction; } + std::vector oldIds = uKeys(_posterior); + + // Both paths below return a newly built matrix, so the sparse view of the + // previous one no longer applies. + _predictionChanged = true; if(!_fullPredictionUpdate && !_prediction.empty()) { @@ -293,11 +430,9 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector UTimer timerGlobal; timerGlobal.start(); + IdToIndexMap idToIndexMap; #if __cplusplus >= 201103L - std::unordered_map idToIndexMap; idToIndexMap.reserve(ids.size()); -#else - std::map idToIndexMap; #endif for(unsigned int i=0; i 0) { // Set high values (gaussians curves) to loop closure neighbors - - // ADD prob for each neighbors - std::map neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true, true); - - if(!_fullPredictionUpdate) - { - uInsert(_neighborsIndex, std::make_pair(ids[i], neighbors)); - } - std::list idsLoopMargin; - //filter neighbors in STM - for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end();) - { - if(memory->isInSTM(iter->first)) - { - neighbors.erase(iter++); - } - else - { - if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end()) - { - idsLoopMargin.push_back(iter->first); - } - ++iter; - } - } - - // should at least have 1 id in idsMarginLoop - if(idsLoopMargin.size() == 0) - { - UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]); - } + std::map neighbors = resolveNeighbors( + memory, ids[i], _predictionLC.size()-1, idToIndexMap, idsLoopMargin, + _fullPredictionUpdate?0:&_neighborsIndex); // same neighbor tree for loop signatures (margin = 0) for(std::list::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter) @@ -366,47 +473,16 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector float sum = 0.0f; // sum values added int index = idToIndexMap.at(*iter); - sum += addNeighborProb(prediction, index, neighbors, _predictionLC, idToIndexMap); + float * column = (float*)prediction.data + index; + sum += addNeighborProb(column, cols, neighbors, _predictionLC, idToIndexMap); idsDone.insert(*iter); - this->normalize(prediction, index, sum, ids[0]<0); + this->normalize(column, cols, cols, index, sum, ids[0]<0); } } else { // Set the virtual place prior - if(_virtualPlacePrior > 0) - { - if(cols>1) // The first must be the virtual place - { - ((float*)prediction.data)[i] = _virtualPlacePrior; - float val = (1.0-_virtualPlacePrior)/(cols-1); - for(int j=1; j0) - { - ((float*)prediction.data)[i] = 1; - } - } - else - { - // Only for some tests... - // when _virtualPlacePrior=0, set all priors to the same value - if(cols>1) - { - float val = 1.0/cols; - for(int j=0; j0) - { - ((float*)prediction.data)[i] = 1; - } - } + this->fillVirtualPlaceColumn((float*)prediction.data + i, cols, cols); } } } @@ -416,6 +492,222 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector return prediction; } +// Appends the non zero values of a freshly built column to the triplets of the sparse +// prediction, and leaves the buffer zeroed for the next one, which saves clearing the +// whole of it every time. +void BayesFilter::appendSparseColumn( + std::vector & column, + int index, + std::vector > & triplets) const +{ + for(size_t row=0; row((int)row, index, column[row])); + column[row] = 0.0f; + } + } +} + +// The prediction built directly in its sparse form, the matrix never being allocated. +// +// A column of the prediction only holds the neighbors of one location within the depth +// of the prediction model, so on a large map the matrix is mostly zeros, while holding +// it costs the size of the working memory squared against the far smaller size of the +// values in it. Each column is built in a buffer of its own instead, through the same +// addNeighborProb() and normalize() as the dense build, and only its non zero values +// are kept. +// +// The columns are not built in the order of their index: a column is built for every +// location at margin 0 of the one being expanded, so several are built at once. Eigen +// takes them as triplets and orders them. +// +// Returns false when the matrix would not be sparse, which the caller has to answer by +// building the dense one. Only reachable through a model whose values sum to less than +// 1, as normalize() then spreads the missing probability over every zero of a column. +bool BayesFilter::generateSparsePrediction(const Memory * memory, const std::vector & ids) +{ + UASSERT(memory && _predictionLC.size() >= 2 && ids.size()); + + UTimer timer; + this->clearSparsePrediction(); + _predictionChanged = false; + + const int size = (int)ids.size(); + + IdToIndexMap idToIndexMap; +#if __cplusplus >= 201103L + idToIndexMap.reserve(ids.size()); +#endif + for(int i=0; i0) + { + idToIndexMap[ids[i]] = i; + } + } + + // A value costs 12 bytes as a triplet and 8 in the matrix, against the 4 of the + // dense one, so past a quarter filled the sparse form is not worth building. + const size_t maxValues = (size_t)size*(size_t)size/4; + std::vector column(size, 0.0f); + std::vector > triplets; + + std::set idsDone; + for(int i=0; i 0) + { + std::list idsLoopMargin; + // The neighborhoods are not kept in _neighborsIndex: it is there for the + // incremental updatePrediction(), which this build replaces, and it holds + // one neighborhood per location, as much memory again as the values here. + std::map neighbors = resolveNeighbors( + memory, ids[i], _predictionLC.size()-1, idToIndexMap, idsLoopMargin, 0); + + // same neighbor tree for loop signatures (margin = 0) + for(std::list::iterator iter=idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter) + { + const int index = idToIndexMap.at(*iter); + float sum = addNeighborProb(&column[0], 1, neighbors, _predictionLC, idToIndexMap); + idsDone.insert(*iter); + this->normalize(&column[0], 1, size, index, sum, ids[0]<0); + this->appendSparseColumn(column, index, triplets); + } + } + else + { + this->fillVirtualPlaceColumn(&column[0], 1, size); + this->appendSparseColumn(column, i, triplets); + } + + if(triplets.size() > maxValues) + { + UWARN("The prediction has more than %ld non zero values, which is too dense " + "for %s to be worth it (%d of %d locations done). Building the matrix " + "instead. The values of %s summing to less than 1 is what fills it.", + (long)maxValues, Parameters::kBayesSparsePrediction().c_str(), i+1, size, + Parameters::kBayesPredictionLC().c_str()); + return false; + } + } + + _sparsePredictionMatrix.resize(size, size); + _sparsePredictionMatrix.setFromTriplets(triplets.begin(), triplets.end()); + + UDEBUG("Sparse prediction: %ld/%ld values (%.2f%%), %ld MB against the %ld MB of the " + "matrix, built in %fs", + (long)_sparsePredictionMatrix.nonZeros(), (long)size*size, + 100.0*double(_sparsePredictionMatrix.nonZeros())/(double(size)*double(size)), + (long)(this->getSparsePredictionMemoryUsed()/1048576), + (long)((size_t)size*(size_t)size*sizeof(float)/1048576), + timer.ticks()); + return true; +} + +// The same sparse prediction, but taken from the matrix rather than built instead of +// it. Used when the matrix is there anyway, which is the case while mapping: the +// incremental updatePrediction() needs it to carry the unchanged columns over. +void BayesFilter::updateSparsePredictionFromDense() +{ + this->clearSparsePrediction(); + _predictionChanged = false; + + if(_prediction.empty()) + { + return; + } + UASSERT(_prediction.type() == CV_32FC1); + UASSERT(_prediction.isContinuous()); + + // normalize() spreads the probability that the model doesn't account for over every + // zero of a column, so with such a model there is no zero left to skip. The + // condition is the one used there, so that both agree on the matrix content. + if(_totalPredictionLCValues < 1) + { + UWARN("%s is enabled but the values of %s sum to %f < 1, so the missing " + "probability is spread over all the other locations and the prediction " + "matrix has no zeros to skip. Using the dense multiplication instead. " + "Make the values sum to 1 to benefit from the sparse one.", + Parameters::kBayesSparsePrediction().c_str(), + Parameters::kBayesPredictionLC().c_str(), + _totalPredictionLCValues); + return; + } + + UTimer timer; + const int rows = _prediction.rows; + const int cols = _prediction.cols; + const float * data = (const float *)_prediction.data; + const size_t maxValues = (size_t)rows*(size_t)cols/4; + + // Read in the order the matrix is stored in, a column of it being strided. + std::vector > triplets; + for(int row=0; row(row, col, rowPtr[col])); + } + } + if(triplets.size() > maxValues) + { + UWARN("The prediction matrix has more than %ld non zero values, which is too " + "dense for %s to be worth it (%d of %d rows scanned). Using the dense " + "multiplication instead.", + (long)maxValues, Parameters::kBayesSparsePrediction().c_str(), row+1, rows); + return; + } + } + + _sparsePredictionMatrix.resize(rows, cols); + _sparsePredictionMatrix.setFromTriplets(triplets.begin(), triplets.end()); + + UDEBUG("Sparse prediction: %ld/%ld values (%.2f%%), %ld MB on top of the %ld MB of " + "the matrix, built in %fs", + (long)_sparsePredictionMatrix.nonZeros(), (long)rows*cols, + 100.0*double(_sparsePredictionMatrix.nonZeros())/(double(rows)*double(cols)), + (long)(this->getSparsePredictionMemoryUsed()/1048576), + (long)(_prediction.total()*_prediction.elemSize()/1048576), + timer.ticks()); +} + +void BayesFilter::clearSparsePrediction() +{ + // Assigned rather than resized: resize(0,0) keeps the buffers it has allocated. + _sparsePredictionMatrix = Eigen::SparseMatrix(); +} + +unsigned long BayesFilter::getSparsePredictionMemoryUsed() const +{ + typedef Eigen::SparseMatrix::StorageIndex StorageIndex; + return _sparsePredictionMatrix.nonZeros() * (sizeof(float)+sizeof(StorageIndex)) + + (_sparsePredictionMatrix.outerSize()+1) * sizeof(StorageIndex); +} + +void BayesFilter::multiplySparsePrediction(const cv::Mat & posterior, cv::Mat & prior) const +{ + UASSERT(_sparsePredictionMatrix.rows() > 0); + UASSERT(posterior.cols == 1 && posterior.type() == CV_32FC1); + UASSERT_MSG(posterior.rows == (int)_sparsePredictionMatrix.cols(), + uFormat("posterior=%d prediction=%d", posterior.rows, + (int)_sparsePredictionMatrix.cols()).c_str()); + + prior = cv::Mat((int)_sparsePredictionMatrix.rows(), 1, CV_32FC1); + Eigen::Map posteriorVector((const float *)posterior.data, posterior.rows); + Eigen::Map priorVector((float *)prior.data, prior.rows); + priorVector.noalias() = _sparsePredictionMatrix * posteriorVector; +} + unsigned long BayesFilter::getMemoryUsed() const { long memoryUsage = sizeof(BayesFilter); @@ -425,6 +717,7 @@ unsigned long BayesFilter::getMemoryUsed() const memoryUsage += _prediction.total() * _prediction.elemSize(); } memoryUsage += _predictionLC.size() * sizeof(double); + memoryUsage += this->getSparsePredictionMemoryUsed(); memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map)+sizeof(std::map >::iterator)) + sizeof(std::map >); for(std::map >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter) { @@ -433,16 +726,56 @@ unsigned long BayesFilter::getMemoryUsed() const return memoryUsage; } -void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const +// The column of the virtual place, the hypothesis of being at a location that was +// never visited: the probability of moving again to a new one, then the rest split +// equally over the visited ones. +void BayesFilter::fillVirtualPlaceColumn(float * column, size_t stride, int size) const { - UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols); + if(_virtualPlacePrior > 0) + { + if(size>1) // The first must be the virtual place + { + column[0] = _virtualPlacePrior; + float val = (1.0-_virtualPlacePrior)/(size-1); + for(int j=1; j0) + { + column[0] = 1; + } + } + else + { + // Only for some tests... + // when _virtualPlacePrior=0, set all priors to the same value + if(size>1) + { + float val = 1.0/size; + for(int j=0; j0) + { + column[0] = 1; + } + } +} - int cols = prediction.cols; +void BayesFilter::normalize(float * column, size_t stride, int size, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const +{ + UASSERT(index < (unsigned int)size); + + int cols = size; // ADD values of not found neighbors to loop closure if(addedProbabilitiesSum < _totalPredictionLCValues-_predictionLC[0]) { float delta = _totalPredictionLCValues-_predictionLC[0]-addedProbabilitiesSum; - ((float*)prediction.data)[index + index*cols] += delta; + column[index*stride] += delta; addedProbabilitiesSum+=delta; } @@ -458,10 +791,10 @@ void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float adde float value = allOtherPlacesValue / float(cols - 1); for(int j=virtualPlaceUsed?1:0; j= 201103L - std::unordered_map newIdToIndexMap; newIdToIndexMap.reserve(newIds.size()); -#else - std::map newIdToIndexMap; #endif for(unsigned int i=0; i & neighbors = _neighborsIndex.at(newIds[i]); //std::map neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true); - float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap); - this->normalize(prediction, i, sum, newIds[0]<0); + float * column = (float*)prediction.data + i; + float sum = addNeighborProb(column, prediction.cols, neighbors, _predictionLC, newIdToIndexMap); + this->normalize(column, prediction.cols, prediction.cols, i, sum, newIds[0]<0); ++added; int count = 0; @@ -643,10 +975,11 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, //std::map neighbors = memory->getNeighborsId(id, _predictionLC.size()-1, 0, false, false, true, true); e1+=t1.ticks(); - float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap); + float * column = (float*)prediction.data + index; + float sum = addNeighborProb(column, prediction.cols, neighbors, _predictionLC, newIdToIndexMap); e3+=t1.ticks(); - this->normalize(prediction, index, sum, newIds[0]<0); + this->normalize(column, prediction.cols, prediction.cols, index, sum, newIds[0]<0); ++modified; e4+=t1.ticks(); } @@ -714,6 +1047,13 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, void BayesFilter::updatePosterior(const Memory * memory, const std::vector & likelihoodIds) { ULOGGER_DEBUG(""); + if(this->posteriorHasSameIds(likelihoodIds)) + { + // Nothing was added to or removed from the working memory, which over a fixed + // graph is every iteration: the map below would be rebuilt identical, at the + // cost of allocating and freeing a node per location. + return; + } std::map newPosterior; for(std::vector::const_iterator i=likelihoodIds.begin(); i != likelihoodIds.end(); ++i) { diff --git a/corelib/test/CMakeLists.txt b/corelib/test/CMakeLists.txt index fa754022..da1c8d2f 100644 --- a/corelib/test/CMakeLists.txt +++ b/corelib/test/CMakeLists.txt @@ -119,6 +119,22 @@ IF(BUILD_PERF_TESTS) set_tests_properties(test_flann_index_perf PROPERTIES TIMEOUT ${_perf_timeout} LABELS "performance") + + # Comparison of the dense and the sparse prediction x posterior multiplication of + # BayesFilter (Bayes/SparsePrediction), against the size of the map, how connected + # its graph is and the depth of the prediction model: + # bin/test_bayesfilter_perf + # bin/test_bayesfilter_perf --gtest_filter=-*LargeMap* + # Its own executable: it spends its time benchmarking rather than asserting, and the + # largest map it builds allocates a gigabyte for the dense prediction matrix, + # which in a unit test shard would look like a leak. + add_executable(test_bayesfilter_perf perf_bayesfilter.cpp) + target_link_libraries(test_bayesfilter_perf gtest_main rtabmap_core) + + add_test(NAME test_bayesfilter_perf COMMAND test_bayesfilter_perf) + set_tests_properties(test_bayesfilter_perf PROPERTIES + TIMEOUT ${_perf_timeout} + LABELS "performance") ENDIF(BUILD_PERF_TESTS) # Rtabmap end-to-end replay of sample DBs (test data fetched by diff --git a/corelib/test/perf_bayesfilter.cpp b/corelib/test/perf_bayesfilter.cpp new file mode 100644 index 00000000..c224f586 --- /dev/null +++ b/corelib/test/perf_bayesfilter.cpp @@ -0,0 +1,342 @@ +// Comparison of the dense and the sparse (Parameters::kBayesSparsePrediction()) +// multiplication of the prediction matrix with the last posterior, which is where +// BayesFilter::computePosterior() spends nearly all of its time on a large map. +// +// Its own executable, run by ctest under the "performance" label, so that its +// seconds of benchmarking stay out of the unit test shards: +// ctest -L performance to run them +// ctest -LE performance to skip them +// bin/test_bayesfilter_perf --gtest_filter=*Growing* +// +// The times are reported rather than asserted on, as they depend on the machine. +// What is asserted is that both multiplications give the same posterior, so that +// the numbers below compare two ways of computing the same thing. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace rtabmap; + +namespace { + +// Sizes of the maps compared. The dense prediction matrix is n x n floats, so the +// largest one below already allocates 244 MB. +static const int MAP_SIZES[] = {1000, 4000, 8000}; + +// How many values of the prediction model decides how deep in the graph a column of +// the prediction matrix reaches, and thus how many values it holds. The default model +// has 18, so it stores neighbors up to 17 links away, with probabilities down to +// 6.9e-23. Truncating it to 8 keeps every value above 1e-4 and stops there. The +// difference is given to the loop closure probability so that the values still sum to +// 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 " + "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"; + +// A chain of signatures linked by odometry, with a global loop closure every +// loopEvery nodes back to the node loopSpan earlier. The loop closures matter here: +// Memory::getNeighborsId() follows them, so each one is a shortcut that widens the +// neighborhood a column of the prediction matrix holds. They are what decides how +// sparse the matrix is, so they are a knob of these benchmarks rather than a detail. +class SyntheticMap +{ +public: + // Built by a mapping session, then turned to localization mode unless asked + // otherwise: the graph is then fixed, which is what the sparse prediction is built + // for, and what the dense one is compared against here. + SyntheticMap(int nodes, int loopEvery, int loopSpan, bool localization = true) + { + ParametersMap params; + // No features extracted: the graph is what the prediction matrix is built from. + params.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1")); + // Only the latest signature stays in STM, the rest are WM nodes the filter uses. + params.insert(ParametersPair(Parameters::kMemSTMSize(), "1")); + params.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); + params.insert(ParametersPair(Parameters::kMemBinDataKept(), "false")); + memory_ = new Memory(params); + + const cv::Mat image(8, 8, CV_8UC1, cv::Scalar(128)); + const cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.01; + const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1); + + UTimer timer; + std::vector ids; + ids.reserve(nodes); + for(int i=0; iupdate(data, Transform(float(i), 0.0f, 0.0f, 0, 0, 0), covariance)); + ids.push_back(memory_->getLastSignatureId()); + if(loopEvery > 0 && i >= loopSpan && i % loopEvery == 0) + { + UASSERT(memory_->addLink(Link( + ids.back(), + ids[ids.size()-1-loopSpan], + Link::kGlobalClosure, + Transform::getIdentity(), + information))); + ++loopClosures_; + } + } + buildTime_ = timer.ticks(); + + if(localization) + { + ParametersMap localizationParams; + localizationParams.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false")); + memory_->parseParameters(localizationParams); + UASSERT(!memory_->isIncremental()); + } + } + + ~SyntheticMap() + { + delete memory_; + } + + const Memory * memory() const {return memory_;} + int loopClosures() const {return loopClosures_;} + double buildTime() const {return buildTime_;} + + // What Rtabmap passes to the filter: the virtual place (new location hypothesis) + // followed by the WM locations that are not in STM. + std::vector bayesIds() const + { + std::vector ids; + ids.push_back(Memory::kIdVirtual); + const std::set & stm = memory_->getStMem(); + for(std::map::const_iterator iter=memory_->getWorkingMem().begin(); + iter!=memory_->getWorkingMem().end(); + ++iter) + { + if(iter->first > 0 && stm.find(iter->first) == stm.end()) + { + ids.push_back(iter->first); + } + } + return ids; + } + + // Uniform, which is the worst case for the sparse multiplication: no location is + // ruled out, so no column of the prediction can be skipped whole. + std::map uniformLikelihood(const std::vector & ids) const + { + std::map likelihood; + for(size_t i=0; i posterior; +}; + +Result run(const SyntheticMap & map, const char * predictionLC, bool sparse, int iterations) +{ + ParametersMap params; + params.insert(ParametersPair(Parameters::kBayesPredictionLC(), predictionLC)); + params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), sparse?"true":"false")); + BayesFilter filter(params); + + const std::vector ids = map.bayesIds(); + const std::map likelihood = map.uniformLikelihood(ids); + + Result result; + UTimer timer; + filter.computePosterior(map.memory(), likelihood); + result.firstIteration = timer.ticks(); + + double total = 0.0; + for(int i=1; i 1 ? total/double(iterations-1) : result.firstIteration; + result.memoryUsed = filter.getMemoryUsed(); + result.posterior = filter.getPosterior(); + return result; +} + +// The two multiplications sum the products of a row in a different order, so the +// posteriors differ by the rounding of a few thousand float additions rather than +// being bit identical. Reported relative to the largest probability, which is the +// scale Rtabmap compares hypotheses at. +double maxPosteriorDifference(const std::map & a, const std::map & b) +{ + if(a.size() != b.size()) + { + return 1.0; + } + double maxDiff = 0.0; + double maxValue = 0.0; + for(std::map::const_iterator iter=a.begin(); iter!=a.end(); ++iter) + { + std::map::const_iterator jter = b.find(iter->first); + if(jter == b.end()) + { + return 1.0; + } + maxDiff = std::max(maxDiff, std::fabs(double(iter->second) - double(jter->second))); + maxValue = std::max(maxValue, std::fabs(double(iter->second))); + } + return maxValue > 0.0 ? maxDiff/maxValue : maxDiff; +} + +void report(const char * name, const Result & result) +{ + printf("[ ] %-7s first iteration %9.1f ms, steady state %8.2f ms, filter memory %7.1f MB\n", + name, result.firstIteration*1000.0, result.steadyState*1000.0, result.memoryUsed/1048576.0); +} + +void compare(const SyntheticMap & map, const char * predictionLC, int iterations) +{ + const size_t size = map.bayesIds().size(); + const Result dense = run(map, predictionLC, false, iterations); + const Result sparse = run(map, predictionLC, true, iterations); + + report("dense", dense); + report("sparse", sparse); + printf("[ ] steady state speedup x%.1f, dense/sparse memory x%.2f, " + "relative posterior difference %.1e\n", + sparse.steadyState > 0.0 ? dense.steadyState/sparse.steadyState : 0.0, + sparse.memoryUsed > 0 ? double(dense.memoryUsed)/double(sparse.memoryUsed) : 0.0, + maxPosteriorDifference(dense.posterior, sparse.posterior)); + + EXPECT_EQ(dense.posterior.size(), size); + // Same probabilities up to the rounding of the sums, which the iterations compound: + // the sums are of a few thousand floats spanning the whole range of the model, down + // to 6.9e-23 for the default one, and each iteration starts from the previous + // posterior. Which location comes out highest is not compared: the likelihood is + // uniform here, so the visited locations are all within rounding of each other and + // the highest is whichever the rounding favors. + EXPECT_LT(maxPosteriorDifference(dense.posterior, sparse.posterior), 1e-3); +} + +} // namespace + +// Both modes on the same graph, over maps of growing size. The dense multiplication +// reads the whole n x n matrix on every iteration, so its cost grows with the square +// of the number of nodes, while the sparse one grows with the number of values the +// graph actually puts in the matrix. +TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnGrowingMaps) +{ + // The steady state of the smaller maps is a fraction of a millisecond, so it is + // averaged over enough iterations that one hiccup doesn't carry the mean. + const int iterations = 30; + + for(size_t s=0; sparseParameters(params); + ASSERT_FALSE(memory_->isIncremental()); + } + void addChain(int count) { ASSERT_GT(count, 0); @@ -990,3 +1001,269 @@ TEST_F(BayesFilterMemoryFixture, FullPredictionUpdateRegeneratesMatrix) expectMatrixGrowsOnNewNode(true); 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). +TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModes) +{ + 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(4); + + for(int iter = 0; iter < 8; ++iter) + { + SensorData data(image_); + Transform pose(float(4 + iter), 0.0f, 0.0f, 0, 0, 0); + ASSERT_TRUE(memory_->update(data, pose, covariance_)); + + const std::vector ids = getBayesIds(); + std::map likelihood = uniformLikelihood(ids); + // Favor one location so that the posterior isn't uniform, and zeroes some + // others out, which is the case where the sparse multiplication skips columns. + likelihood[ids[ids.size()/2]] = 4.0f; + + filterDense.computePosterior(memory_, likelihood); + filterSparse.computePosterior(memory_, likelihood); + + const std::map & posteriorDense = filterDense.getPosterior(); + const std::map & posteriorSparse = filterSparse.getPosterior(); + ASSERT_EQ(posteriorDense.size(), posteriorSparse.size()); + for(size_t i = 0; i < ids.size(); ++i) + { + EXPECT_NEAR(posteriorDense.at(ids[i]), posteriorSparse.at(ids[i]), 1e-5f) + << "iter=" << iter << " id=" << ids[i]; + } + } +} + +// 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. +TEST_F(BayesFilterMemoryFixture, SparsePredictionFallsBackWhenModelSumsBelowOne) +{ + addChain(5); + + ParametersMap paramsDense; + paramsDense.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionSumBelowOne)); + paramsDense.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false")); + + ParametersMap paramsSparse = paramsDense; + paramsSparse[Parameters::kBayesSparsePrediction()] = "true"; + + const std::vector ids = getBayesIds(); + std::map likelihood = uniformLikelihood(ids); + + BayesFilter filterDense(paramsDense); + BayesFilter filterSparse(paramsSparse); + filterDense.computePosterior(memory_, likelihood); + filterSparse.computePosterior(memory_, likelihood); + + ASSERT_EQ(filterDense.getPosterior().size(), filterSparse.getPosterior().size()); + for(size_t i = 0; i < ids.size(); ++i) + { + EXPECT_NEAR(filterDense.getPosterior().at(ids[i]), + filterSparse.getPosterior().at(ids[i]), 1e-5f) << "id=" << ids[i]; + } + + // Falling back means the sparse view was left empty, so no extra memory is used. + EXPECT_EQ(filterDense.getMemoryUsed(), filterSparse.getMemoryUsed()); +} + +// The sparse view holds only the non-zero values of the prediction matrix, so it is a +// fraction of its size, and the reported memory reflects that it is an addition to it. +TEST_F(BayesFilterMemoryFixture, SparsePredictionMemoryUsed) +{ + addChain(40); + + ParametersMap params; + params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); + params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false")); + BayesFilter filterDense(params); + + params[Parameters::kBayesSparsePrediction()] = "true"; + BayesFilter filterSparse(params); + + const std::vector ids = getBayesIds(); + const std::map likelihood = uniformLikelihood(ids); + filterDense.computePosterior(memory_, likelihood); + filterSparse.computePosterior(memory_, likelihood); + + const unsigned long dense = filterDense.getMemoryUsed(); + const unsigned long sparse = filterSparse.getMemoryUsed(); + EXPECT_GT(sparse, dense); + // The matrix is ids x ids floats; a column of it only holds the neighbors within + // the depth of the model (3 here), so the view has to stay well under it. + EXPECT_LT(sparse - dense, ids.size()*ids.size()*sizeof(float)); +} + +// 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. +TEST_F(BayesFilterMemoryFixture, SparsePredictionIsExactOnASingleColumn) +{ + addChain(30); + + ParametersMap paramsDense; + paramsDense.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); + paramsDense.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false")); + + ParametersMap paramsSparse = paramsDense; + paramsSparse[Parameters::kBayesSparsePrediction()] = "true"; + + const std::vector ids = getBayesIds(); + ASSERT_GT(ids.size(), 4u); + + // Every column of the matrix, so that a value at the wrong row or column cannot be + // missed, whichever one it is. + for(size_t selected = 0; selected < ids.size(); ++selected) + { + BayesFilter filterDense(paramsDense); + BayesFilter filterSparse(paramsSparse); + + std::map oneHot; + for(size_t i = 0; i < ids.size(); ++i) + { + oneHot.insert(std::make_pair(ids[i], i == selected ? 1.0f : 0.0f)); + } + filterDense.computePosterior(memory_, oneHot); + filterSparse.computePosterior(memory_, oneHot); + ASSERT_FLOAT_EQ(filterDense.getPosterior().at(ids[selected]), 1.0f) << "column=" << selected; + + const std::map uniform = uniformLikelihood(ids); + filterDense.computePosterior(memory_, uniform); + filterSparse.computePosterior(memory_, uniform); + + for(size_t i = 0; i < ids.size(); ++i) + { + EXPECT_FLOAT_EQ(filterDense.getPosterior().at(ids[i]), + filterSparse.getPosterior().at(ids[i])) + << "column=" << selected << " id=" << ids[i]; + } + } +} + +// 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. +TEST_F(BayesFilterMemoryFixture, CompareSparsePredictionModesInLocalizationMode) +{ + addChain(30); + switchToLocalization(); + + 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); + + const std::vector ids = getBayesIds(); + ASSERT_GT(ids.size(), 4u); + + for(int iter = 0; iter < 8; ++iter) + { + std::map likelihood = uniformLikelihood(ids); + // Favor a different location on each iteration, so the posterior moves. + likelihood[ids[1 + (size_t)iter % (ids.size()-1)]] = 4.0f; + + filterDense.computePosterior(memory_, likelihood); + filterSparse.computePosterior(memory_, likelihood); + + ASSERT_EQ(filterDense.getPosterior().size(), filterSparse.getPosterior().size()); + for(size_t i = 0; i < ids.size(); ++i) + { + EXPECT_NEAR(filterDense.getPosterior().at(ids[i]), + filterSparse.getPosterior().at(ids[i]), 1e-5f) + << "iter=" << iter << " id=" << ids[i]; + } + } +} + +// 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. +TEST_F(BayesFilterMemoryFixture, SparsePredictionInLocalizationModeIsExactOnASingleColumn) +{ + addChain(30); + switchToLocalization(); + + ParametersMap paramsDense; + paramsDense.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); + paramsDense.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false")); + + ParametersMap paramsSparse = paramsDense; + paramsSparse[Parameters::kBayesSparsePrediction()] = "true"; + + const std::vector ids = getBayesIds(); + ASSERT_GT(ids.size(), 4u); + + for(size_t selected = 0; selected < ids.size(); ++selected) + { + BayesFilter filterDense(paramsDense); + BayesFilter filterSparse(paramsSparse); + + std::map oneHot; + for(size_t i = 0; i < ids.size(); ++i) + { + oneHot.insert(std::make_pair(ids[i], i == selected ? 1.0f : 0.0f)); + } + filterDense.computePosterior(memory_, oneHot); + filterSparse.computePosterior(memory_, oneHot); + + const std::map uniform = uniformLikelihood(ids); + filterDense.computePosterior(memory_, uniform); + filterSparse.computePosterior(memory_, uniform); + + for(size_t i = 0; i < ids.size(); ++i) + { + EXPECT_FLOAT_EQ(filterDense.getPosterior().at(ids[i]), + filterSparse.getPosterior().at(ids[i])) + << "column=" << selected << " id=" << ids[i]; + } + } +} + +// What the sparse build is for: the prediction matrix, which is the size of the working +// memory squared, is not allocated at all. +TEST_F(BayesFilterMemoryFixture, SparsePredictionInLocalizationModeDoesNotAllocateTheMatrix) +{ + addChain(200); + switchToLocalization(); + + ParametersMap params; + params.insert(ParametersPair(Parameters::kBayesPredictionLC(), kPredictionNewPlace10Stay50Neighbor25_15)); + params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), "false")); + BayesFilter filterDense(params); + + params[Parameters::kBayesSparsePrediction()] = "true"; + BayesFilter filterSparse(params); + + const std::vector ids = getBayesIds(); + const std::map likelihood = uniformLikelihood(ids); + filterDense.computePosterior(memory_, likelihood); + filterSparse.computePosterior(memory_, likelihood); + + const unsigned long matrix = ids.size()*ids.size()*sizeof(float); + EXPECT_GT(filterDense.getMemoryUsed(), matrix); + // Not the matrix, and not the neighborhood of every location either, which only the + // incremental update of the matrix needs. + EXPECT_LT(filterSparse.getMemoryUsed(), filterDense.getMemoryUsed()/4); +} diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index cc93d1b3..c665d000 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1132,6 +1132,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->general_doubleSpinBox_vp->setObjectName(Parameters::kBayesVirtualPlacePriorThr().c_str()); _ui->lineEdit_bayes_predictionLC->setObjectName(Parameters::kBayesPredictionLC().c_str()); _ui->checkBox_bayes_fullPredictionUpdate->setObjectName(Parameters::kBayesFullPredictionUpdate().c_str()); + _ui->checkBox_bayes_sparsePrediction->setObjectName(Parameters::kBayesSparsePrediction().c_str()); connect(_ui->lineEdit_bayes_predictionLC, SIGNAL(textChanged(const QString &)), this, SLOT(updatePredictionPlot())); //Keypoint-based diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 3925c54b..8ff2c42e 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -12591,14 +12591,14 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + - + QComboBox::AdjustToContents @@ -12628,7 +12628,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + Use GPS to filter likelihood (if GPS is recorded). Only locations inside the Local Radius (see RGBD-SLAM panel) of the current GPS location are considered for loop closure detection. @@ -12648,7 +12648,30 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + + + + + + + true + + + + + + + Keep the prediction in a sparse form and multiply it sparsely. A column of the prediction only holds the neighbors of one location within the depth of the prediction of loop closures above, so on a large map the matrix is mostly zeros and the dense multiplication spends all of its time reading them. In localization mode the sparse form is built directly and the matrix, which costs the size of the working memory squared, is not allocated at all. Ignored when the values above sum to less than 1, as the missing probability is then spread over every zero of each column and the prediction is no longer sparse. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -12658,7 +12681,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + 1.000000000000000 @@ -12671,7 +12694,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + The VP here is the probability to be in a new place since no loop closure was found on the last iteration. @@ -12684,7 +12707,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + Use the tf-idf method to compute the likelihood. Otherwise, images are compared with each other. @@ -12697,7 +12720,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + Global descriptor extractor. @@ -12710,7 +12733,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + Likelihood ratio for VP. @@ -12723,7 +12746,7 @@ see Sqlite3 doc 'PRAGMA temp_store'. - + QComboBox::AdjustToContentsOnFirstShow