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

Sparse Bayes (#1748)

Browse Source 
* Sparse Bayes

* updated perf test

* improved tests with real data

* Making sparse works in incremental mapping

* bookkeeping optimization

* small opt

* refactoring

* splitting dense and sparse in different classes to make the code more lisible

* cleanup comments

* fixing CI

* Making all Bayes tests testing both dense and sparse

* Added multisession_3it integration test (test memory management, multisession and dense/sparse bayes in that settings)

* optimized sparse when transfer/retrieval happens (was slower than dense for that case)

* Testing retrieval param variants

* Updated multisession_3it integration tests to compare loop closure hypotheses

* bump version

* Fixed ui sum of prediction

* adding g2o gtsam to linux ci

* cleanup

* added debug crash log for ci

* Simplified Bayes/SparsePrediction description

* Dont show too dense for sparse on small maps (e.g., when we just started a new map)

* fixing amd64v3 issue with gtsam on ci ubuntu 26

* Dot not auto switch to dense based on map size.

* updating test range

* added coverage tests

* Adressing coverage

* ignore one line in coverage for purpose
This commit is contained in:
matlabbe
2026-08-23 13:21:46 -07:00
committed by GitHub
parent f647014f54
commit 9c1e117384
32 changed files with 82886 additions and 769 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/BayesFilter.h
+61 -27
View File
@@ -32,7 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/core/core.hpp>
#include <list>
#include <map>
#include <set>
#include <utility>
#include <vector>
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/core/Parameters.h"
@@ -41,6 +44,12 @@ namespace rtabmap {
class Memory;
class Signature;
namespace bayes {
class PredictionModel;
class DensePrediction;
class SparsePrediction;
}
/**
* @class BayesFilter
* @brief Recursive Bayesian filter for loop-closure hypothesis estimation in RTAB-Map.
@@ -64,6 +73,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
@@ -91,12 +101,14 @@ public:
* The prediction matrix is generated or updated from @ref Memory using the ids present
* in @p likelihood.
*
* Read the result with @ref getPosteriorIds() and @ref getPosteriorValues().
*
* @param memory Working memory instance (must not be null).
* @param likelihood Observation likelihood per signature id (must not be empty).
* @return Reference to the internal posterior map (id → probability). On error (null
* memory, empty likelihood, or invalid prediction model), returns the unchanged posterior.
* @return False on error (null memory, empty likelihood, or invalid prediction model),
* the posterior being left unchanged.
*/
const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
bool computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
/**
* @brief Clears posterior, prediction matrix and cached neighbor indices.
@@ -119,16 +131,30 @@ public:
void setPredictionLC(const std::string & prediction);
/**
* @brief Returns the current posterior probability map.
* @return Map of signature id to normalized posterior probability. This is the probability to be at the given location.
* @brief The locations the posterior is over, ascending by id.
*
* The virtual place (@ref Memory::kIdVirtual) is the first of them when it is one.
*/
const std::map<int, float> & getPosterior() const {return _posterior;}
const std::vector<int> & getPosteriorIds() const {return _posteriorIds;}
/**
* @brief The probability of each location of @ref getPosteriorIds(), in the same order.
*/
const std::vector<float> & getPosteriorValues() const {return _posteriorValues;}
/**
* @brief Returns the virtual place prior threshold.
* @return Value in [0, 1] used when building the virtual place row of the prediction matrix.
*/
float getVirtualPlacePrior() const {return _virtualPlacePrior;}
float getVirtualPlacePrior() const;
/**
* @brief Whether the prediction is being kept in its sparse form rather than as a matrix.
*
* False when @ref Parameters::kBayesSparsePrediction() is disabled, and over a model whose
* values sum to less than 1, which leaves no zero in a column to keep out of the values.
*/
bool isPredictionSparse() const;
/**
* @brief Returns the loop-closure prediction model as a vector of values.
@@ -149,6 +175,10 @@ public:
* transition probabilities according to @ref getPredictionLC(). When @p ids match the
* current posterior keys, the cached matrix may be returned without recomputation.
*
* When the prediction is being kept sparse, the matrix is expanded from it rather than
* kept: it costs the memory that keeping the prediction sparse is saving, so ask for it to
* read, dump or compare the prediction, not on every iteration.
*
* @param memory Working memory instance (must not be null).
* @param ids Ordered list of signature ids (often includes @ref Memory::kIdVirtual as first element).
* @return Square CV_32FC1 matrix of size ids.size() × ids.size().
@@ -163,32 +193,36 @@ public:
private:
/**
* @brief Incrementally updates the prediction matrix when ids are added or removed.
* @brief Realigns the posterior with the ids of the likelihood.
*
* Keeps the probability of the locations that are in both. Called only when the ids differ.
*/
cv::Mat updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds);
void updatePosterior(const Memory * memory, const std::map<int, float> & likelihood);
/**
* @brief Realigns the posterior map with the current set of likelihood ids.
* @brief Settles whether the prediction is kept sparse, from the parameter and the model.
*
* Called when either of the two changes rather than on every iteration, and releases the
* sparse form when the answer is no.
*/
void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
/**
* @brief Normalizes one row of the prediction matrix and applies the virtual place probability.
*/
void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
void updateKeepSparse();
private:
std::map<int, float> _posterior; ///< Current posterior (signature id → probability).
cv::Mat _prediction; ///< Cached prediction/transition matrix.
float _virtualPlacePrior; ///< Prior for virtual place transitions.
std::vector<double> _predictionLC; ///< Model `{Vp, Lc, l1, l2, ...}`.
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.
std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id.
std::vector<int> _posteriorIds; ///< The locations the posterior is over, ascending by id.
std::vector<float> _posteriorValues; ///< The probability of each of them, in the same order.
std::vector<int> _likelihoodIds; ///< The ids of the likelihood of an iteration, in its order.
std::vector<float> _likelihoodValues; ///< The likelihood of an iteration, in the same order.
std::vector<float> _priorValues; ///< The prior of an iteration, in the same order.
bayes::PredictionModel * _model; ///< The `{Vp, Lc, l1, ...}` model and the column arithmetic of it.
bayes::DensePrediction * _dense; ///< The prediction as a matrix, used when it is not kept sparse.
bayes::SparsePrediction * _sparse; ///< The prediction as its values only, one column at a time.
std::map<int, std::map<int, int> > _neighborsIndex; ///< Cached neighbor margins per signature id, for the incremental updates.
bool _fullPredictionUpdate; ///< If true, rebuild the whole prediction each time.
bool _sparsePrediction; ///< Keep the prediction sparse (Bayes/SparsePrediction).
bool _keepSparse; ///< Whether it is being kept sparse: the parameter, over a model that leaves nothing sparse to keep.
bool _predictionChanged; ///< True when the prediction has to be built again.
};
} // namespace rtabmap
corelib/include/rtabmap/core/Parameters.h
+2 -1
View File
@@ -388,8 +388,9 @@ 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("Use a sparse representation of the prediction instead of a dense matrix, which significantly reduces memory usage and processing time on large maps. Ignored when the values of %s sum to less than 1, as the prediction is then not sparse.", 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()));
corelib/src/BayesFilter.cpp
+206 -584
View File
@@ -24,33 +24,40 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
#include "bayes/DensePrediction.h"
#include "bayes/PredictionModel.h"
#include "bayes/SparsePrediction.h"
#include <set>
#if __cplusplus >= 201103L
#include <unordered_map>
#include <unordered_set>
#endif
#include "rtabmap/utilite/UtiLite.h"
namespace rtabmap {
BayesFilter::BayesFilter(const ParametersMap & parameters) :
_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
_model(new bayes::PredictionModel()),
_dense(new bayes::DensePrediction()),
_sparse(new bayes::SparsePrediction()),
_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
_totalPredictionLCValues(0.0f),
_predictionEpsilon(0.0f)
_sparsePrediction(Parameters::defaultBayesSparsePrediction()),
_keepSparse(false),
_predictionChanged(true)
{
_model->setVirtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr());
this->setPredictionLC(Parameters::defaultBayesPredictionLC());
this->parseParameters(parameters);
}
BayesFilter::~BayesFilter() {
BayesFilter::~BayesFilter()
{
delete _model;
delete _dense;
delete _sparse;
}
void BayesFilter::parseParameters(const ParametersMap & parameters)
@@ -60,371 +67,258 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
{
this->setPredictionLC((*iter).second);
}
Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior);
float virtualPlacePrior = _model->virtualPlacePrior();
if(Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), virtualPlacePrior))
{
UASSERT(virtualPlacePrior >= 0 && virtualPlacePrior <= 1.0f);
_model->setVirtualPlacePrior(virtualPlacePrior);
}
Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate);
UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f);
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;
this->updateKeepSparse();
}
}
// format = {Virtual place, Loop closure, level1, level2, l3, l4...}
void BayesFilter::setPredictionLC(const std::string & prediction)
{
std::list<std::string> strValues = uSplit(prediction, ' ');
if(strValues.size() < 2)
if(_model->set(prediction))
{
UERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
// A new model changes the values of the prediction, and whether any of it is worth
// keeping sparse.
_predictionChanged = true;
this->updateKeepSparse();
}
else
{
std::vector<double> tmpValues(strValues.size());
int i=0;
bool valid = true;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = uStr2Float((*iter).c_str());
//UINFO("%d=%e", i, tmpValues[i]);
if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
{
valid = false;
break;
}
++i;
}
}
if(!valid)
{
UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str());
}
else
{
_predictionLC = tmpValues;
}
}
_totalPredictionLCValues = 0.0f;
for(unsigned int j=0; j<_predictionLC.size(); ++j)
// Asked for by the parameter, and possible only over a model whose values sum to 1: below that,
// normalize() spreads the difference over every zero of a column and there is nothing sparse
// left to keep. Nothing else gives the sparse form up, however densely the graph is linked, so
// that what the parameter measures is the sparse form and not a fallback to the matrix.
void BayesFilter::updateKeepSparse()
{
_keepSparse = _sparsePrediction && !_model->spreadsOverAllLocations();
if(_sparsePrediction && !_keepSparse)
{
_totalPredictionLCValues += _predictionLC[j];
if(j==0 || _predictionLC[j] < _predictionEpsilon)
{
_predictionEpsilon = _predictionLC[j];
}
UWARN("%s is enabled but the values of %s sum to %f, less than 1: the difference is "
"spread over every location, which leaves no zero in a column for the sparse form "
"to keep out, so the prediction is held as a matrix instead.",
Parameters::kBayesSparsePrediction().c_str(),
Parameters::kBayesPredictionLC().c_str(), _model->total());
}
if(!_predictionLC.empty())
if(!_keepSparse)
{
UDEBUG("predictionEpsilon = %f", _predictionEpsilon);
_sparse->clear();
}
}
const std::vector<double> & BayesFilter::getPredictionLC() const
{
// {Vp, Lc, l1, l2, l3, l4...}
return _predictionLC;
return _model->values();
}
std::string BayesFilter::getPredictionLCStr() const
{
std::string values;
for(unsigned int i=0; i<_predictionLC.size(); ++i)
{
values.append(uNumber2Str(_predictionLC[i]));
if(i+1 < _predictionLC.size())
{
values.append(" ");
}
}
return values;
return _model->str();
}
float BayesFilter::getVirtualPlacePrior() const
{
return _model->virtualPlacePrior();
}
bool BayesFilter::isPredictionSparse() const
{
return !_sparse->empty();
}
void BayesFilter::reset()
{
_posterior.clear();
_prediction = cv::Mat();
_posteriorIds.clear();
_posteriorValues.clear();
_dense->clear();
_sparse->clear();
_predictionChanged = true;
_neighborsIndex.clear();
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
bool BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
{
ULOGGER_DEBUG("");
if(!memory)
{
ULOGGER_ERROR("Memory is Null!");
return _posterior;
return false;
}
if(!likelihood.size())
{
ULOGGER_ERROR("likelihood is empty!");
return _posterior;
return false;
}
if(_predictionLC.size() < 2)
{
ULOGGER_ERROR("Prediction is not valid!");
return _posterior;
}
UASSERT(_model->valid());
UTimer timer;
timer.start();
cv::Mat prior;
cv::Mat posterior;
// One walk of the likelihood: its values into a vector, and its ids against the ones the
// posterior is indexed by. Everything below then works on vectors.
_likelihoodIds.resize(likelihood.size());
_likelihoodValues.resize(likelihood.size());
bool sameIds = _posteriorIds.size() == likelihood.size();
{
size_t k = 0;
for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter, ++k)
{
_likelihoodIds[k] = iter->first;
_likelihoodValues[k] = iter->second;
if(sameIds && _posteriorIds[k] != iter->first)
{
sameIds = false;
}
}
}
const std::vector<int> & ids = _likelihoodIds;
float sum = 0;
int j=0;
// Recursive Bayes estimation...
// STEP 1 - Prediction : Prior*lastPosterior
_prediction = this->generatePrediction(memory, uKeys(likelihood));
//
// The prediction is kept in its sparse form only, the matrix never being allocated:
// built once, then carried over to the locations of the next iteration. Over a fixed
// graph nothing changes and there is nothing to do; while mapping, the appended
// locations reach only a few of the columns and only those are built again. A location
// leaving the working memory shifts the index of every one after it, and is answered by
// building the prediction again, which is what the dense update does then as well.
if(!sameIds)
{
_predictionChanged = true;
}
if(_keepSparse)
{
// Nothing to do at all when neither the prediction nor the locations changed.
if(_predictionChanged || _sparse->ids() != ids)
{
// The neighborhoods are kept only when locations can be added, which is what the
// update needs them for: over a fixed graph one per location is as much memory
// again as the values of the prediction.
if(_fullPredictionUpdate || !_sparse->update(*_model, memory, ids, _neighborsIndex))
{
_sparse->generate(*_model, memory, ids,
memory->isIncremental() ? &_neighborsIndex : 0);
}
}
UDEBUG("STEP1-generate prior=%fs, values=%d", timer.ticks(), (int)_sparse->values());
UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
//std::cout << "Prediction=" << _prediction << std::endl;
// 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.
_dense->generate(*_model, memory, ids, _fullPredictionUpdate, &_neighborsIndex);
}
UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(),
_dense->matrix().rows, _dense->matrix().cols);
//std::cout << "Prediction=" << _dense->matrix() << std::endl;
}
// Cleared once, after whichever of the two built it: the sparse form, or the matrix when
// the prediction is not kept sparse.
_predictionChanged = false;
// 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));
j=0;
for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
// reactivated or removed from the working memory. After the prediction, which is built
// against the ids the posterior still holds from the last iteration.
if(!sameIds)
{
((float*)posterior.data)[j++] = (*i).second;
this->updatePosterior(memory, likelihood);
}
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior rows=%d, _posterior size=%d", timer.ticks(), posterior.rows, (int)_posterior.size());
//std::cout << "LastPosterior=" << posterior << std::endl;
UASSERT(_posteriorValues.size() == likelihood.size());
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior size=%d", timer.ticks(), (int)_posteriorValues.size());
// 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());
// Held sparse, or as the matrix when updateKeepSparse() gave the sparse form up.
const bool sparse = !_sparse->empty();
if(sparse)
{
_sparse->multiply(_posteriorValues, _priorValues);
}
else
{
_dense->multiply(_posteriorValues, _priorValues);
}
const float * priorPtr = &_priorValues[0];
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<float> likelihoodValues = uValues(likelihood);
//std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
// STEP 2 - Update : Multiply with observations (likelihood)
j=0;
for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
// The likelihood, the posterior and the prior are all indexed the same way, so the three
// are walked side by side.
float sum = 0;
for(size_t k=0; k<_posteriorValues.size(); ++k)
{
std::map<int, float>::iterator p =_posterior.find((*i).first);
if(p!= _posterior.end())
{
(*p).second = (*i).second * ((float*)prior.data)[j++];
sum+=(*p).second;
}
else
{
ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
}
_posteriorValues[k] = _likelihoodValues[k] * priorPtr[k];
sum += _posteriorValues[k];
}
ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
//std::cout << "Posterior (before normalization)=" << _posterior << std::endl;
// Normalize
ULOGGER_DEBUG("sum=%f", sum);
if(sum != 0)
{
for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
for(size_t k=0; k<_posteriorValues.size(); ++k)
{
(*i).second /= sum;
_posteriorValues[k] /= sum;
}
}
ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
//std::cout << "Posterior=" << _posterior << std::endl;
return _posterior;
}
float addNeighborProb(cv::Mat & prediction,
unsigned int col,
const std::map<int, int> & neighbors,
const std::vector<double> & predictionLC,
#if __cplusplus >= 201103L
const std::unordered_map<int, int> & idToIndex
#else
const std::map<int, int> & idToIndex
#endif
)
{
UASSERT(col < (unsigned int)prediction.cols &&
col < (unsigned int)prediction.rows);
float sum=0.0f;
float * dataPtr = (float*)prediction.data;
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(iter->first>=0)
{
#if __cplusplus >= 201103L
std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
#else
std::map<int, int>::const_iterator jter = idToIndex.find(iter->first);
#endif
if(jter != idToIndex.end())
{
UASSERT((iter->second+1) < (int)predictionLC.size());
sum += dataPtr[col + jter->second*prediction.cols] = predictionLC[iter->second+1];
}
}
}
return sum;
return true;
}
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
{
std::vector<int> oldIds = uKeys(_posterior);
if(oldIds.size() == ids.size() &&
memcmp(oldIds.data(), ids.data(), oldIds.size()*sizeof(int)) == 0)
if(!_sparse->empty() && _sparse->ids() == ids)
{
return _prediction;
// Expanded from the sparse form, which holds the same prediction. The matrix costs
// what keeping it sparse is saving, so it is built to be read and not kept.
return _sparse->toMatrix();
}
if(!_fullPredictionUpdate && !_prediction.empty())
if(!_dense->empty() && _dense->ids() == ids)
{
return updatePrediction(_prediction, memory, oldIds, ids);
return _dense->matrix();
}
UDEBUG("");
UASSERT(memory &&
_predictionLC.size() >= 2 &&
ids.size());
UTimer timer;
timer.start();
UTimer timerGlobal;
timerGlobal.start();
#if __cplusplus >= 201103L
std::unordered_map<int,int> idToIndexMap;
idToIndexMap.reserve(ids.size());
#else
std::map<int,int> idToIndexMap;
#endif
for(unsigned int i=0; i<ids.size(); ++i)
{
if(ids[i]>0)
{
idToIndexMap[ids[i]] = i;
}
}
//int rows = prediction.rows;
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
// Each prior is a column vector
UDEBUG("_predictionLC.size()=%d",(int)_predictionLC.size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
{
if(idsDone.find(ids[i]) == idsDone.end())
{
if(ids[i] > 0)
{
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
std::map<int, int> 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<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::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]);
}
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
if(!_fullPredictionUpdate)
{
uInsert(_neighborsIndex, std::make_pair(*iter, neighbors));
}
float sum = 0.0f; // sum values added
int index = idToIndexMap.at(*iter);
sum += addNeighborProb(prediction, index, neighbors, _predictionLC, idToIndexMap);
idsDone.insert(*iter);
this->normalize(prediction, 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; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((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; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
}
}
}
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
return prediction;
UASSERT(memory && _model->valid() && ids.size());
return _dense->generate(*_model, memory, ids, _fullPredictionUpdate, &_neighborsIndex);
}
unsigned long BayesFilter::getMemoryUsed() const
{
long memoryUsage = sizeof(BayesFilter);
memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::map<int, float>::iterator)) + sizeof(std::map<int, float>);
if(!_prediction.empty())
{
memoryUsage += _prediction.total() * _prediction.elemSize();
}
memoryUsage += _predictionLC.size() * sizeof(double);
memoryUsage += _dense->memoryUsed();
memoryUsage += _sparse->memoryUsed();
memoryUsage += _model->memoryUsed();
// The vectors an iteration works on, indexed the same way as the posterior.
memoryUsage += _posteriorIds.capacity() * sizeof(int);
memoryUsage += _posteriorValues.capacity() * sizeof(float);
memoryUsage += _likelihoodIds.capacity() * sizeof(int);
memoryUsage += _likelihoodValues.capacity() * sizeof(float);
memoryUsage += _priorValues.capacity() * sizeof(float);
memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::map<int, std::map<int, int> >::iterator)) + sizeof(std::map<int, std::map<int, int> >);
for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
{
@@ -433,308 +327,36 @@ unsigned long BayesFilter::getMemoryUsed() const
return memoryUsage;
}
void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
{
UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);
int cols = prediction.cols;
// 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;
addedProbabilitiesSum+=delta;
}
float allOtherPlacesValue = 0;
if(_totalPredictionLCValues < 1)
{
allOtherPlacesValue = 1.0f - _totalPredictionLCValues;
}
// Set all loop events to small values according to the model
if(allOtherPlacesValue > 0 && cols>1)
{
float value = allOtherPlacesValue / float(cols - 1);
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
if(((float*)prediction.data)[index + j*cols] == 0)
{
((float*)prediction.data)[index + j*cols] = value;
addedProbabilitiesSum += ((float*)prediction.data)[index + j*cols];
}
}
}
//normalize this row
float maxNorm = 1 - (virtualPlaceUsed?_predictionLC[0]:0); // 1 - virtual place probability
if(addedProbabilitiesSum<maxNorm-0.0001 || addedProbabilitiesSum>maxNorm+0.0001)
{
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
((float*)prediction.data)[index + j*cols] *= maxNorm / addedProbabilitiesSum;
if(((float*)prediction.data)[index + j*cols] < _predictionEpsilon)
{
((float*)prediction.data)[index + j*cols] = 0.0f;
}
}
addedProbabilitiesSum = maxNorm;
}
// ADD virtual place prob
if(virtualPlaceUsed)
{
((float*)prediction.data)[index] = _predictionLC[0];
addedProbabilitiesSum += ((float*)prediction.data)[index];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
//}
if(addedProbabilitiesSum<0.99 || addedProbabilitiesSum > 1.01)
{
UWARN("Prediction is not normalized sum=%f", addedProbabilitiesSum);
}
}
cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds)
{
UTimer timer;
UDEBUG("");
UASSERT(memory &&
oldIds.size() &&
newIds.size() &&
oldIds.size() == (unsigned int)oldPrediction.cols &&
oldIds.size() == (unsigned int)oldPrediction.rows);
cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
UDEBUG("time creating prediction = %fs", timer.restart());
// Create id to index maps
#if __cplusplus >= 201103L
std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
#else
std::set<int> oldIdsSet(oldIds.begin(), oldIds.end());
#endif
UDEBUG("time creating old ids set = %fs", timer.restart());
#if __cplusplus >= 201103L
std::unordered_map<int,int> newIdToIndexMap;
newIdToIndexMap.reserve(newIds.size());
#else
std::map<int,int> newIdToIndexMap;
#endif
for(unsigned int i=0; i<newIds.size(); ++i)
{
if(newIds[i]>0)
{
newIdToIndexMap[newIds[i]] = i;
}
}
UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
//Get removed ids
std::set<int> removedIds;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
{
removedIds.insert(removedIds.end(), oldIds[i]);
_neighborsIndex.erase(oldIds[i]);
UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
}
}
UDEBUG("time getting removed ids = %fs", timer.restart());
bool oldAllCopied = false;
if(removedIds.empty() &&
newIds.size() > oldIds.size() &&
memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) == 0)
{
oldPrediction.copyTo(cv::Mat(prediction, cv::Range(0, oldPrediction.rows), cv::Range(0, oldPrediction.cols)));
oldAllCopied = true;
UDEBUG("Copied all old prediction: = %fs", timer.ticks());
}
int added = 0;
// get ids to update
std::set<int> idsToUpdate;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
{
if(i<oldIds.size())
{
if(removedIds.find(oldIds[i]) != removedIds.end())
{
unsigned int cols = oldPrediction.cols;
int count = 0;
for(unsigned int j=0; j<cols; ++j)
{
if(j!=i && removedIds.find(oldIds[j]) == removedIds.end())
{
//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
idsToUpdate.insert(oldIds[j]);
++count;
}
}
UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
}
}
if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
{
if(_neighborsIndex.find(newIds[i]) == _neighborsIndex.end())
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
std::map<int, std::map<int, int> >::iterator jter = _neighborsIndex.find(iter->first);
if(jter != _neighborsIndex.end())
{
uInsert(jter->second, std::make_pair(newIds[i], iter->second));
}
}
_neighborsIndex.insert(std::make_pair(newIds[i], neighbors));
}
const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]);
//std::map<int, int> 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);
++added;
int count = 0;
for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
removedIds.find(iter->first) == removedIds.end())
{
idsToUpdate.insert(iter->first);
++count;
}
}
UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
}
}
UDEBUG("time getting %d ids to update = %fs", (int)idsToUpdate.size(), timer.restart());
UTimer t1;
double e0=0,e1=0, e2=0, e3=0, e4=0;
// update modified/added ids
int modified = 0;
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{
int id = *iter;
if(id > 0)
{
int index = newIdToIndexMap.at(id);
e0 = t1.ticks();
std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id);
UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str());
const std::map<int, int> & neighbors = kter->second;
//std::map<int, int> neighbors = memory->getNeighborsId(id, _predictionLC.size()-1, 0, false, false, true, true);
e1+=t1.ticks();
float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap);
e3+=t1.ticks();
this->normalize(prediction, index, sum, newIds[0]<0);
++modified;
e4+=t1.ticks();
}
}
UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", (int)idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
int copied = 0;
if(!oldAllCopied)
{
//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
// copy not changed probabilities
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
{
for(int j=0; j<oldPrediction.cols; ++j)
{
if(oldIds[j]>0 && removedIds.find(oldIds[j]) == removedIds.end())
{
//UDEBUG("i=%d, j=%d", i, j);
//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
int ii = newIdToIndexMap.at(oldIds[i]);
int jj = newIdToIndexMap.at(oldIds[j]);
((float *)prediction.data)[ii + jj*prediction.cols] = v;
//if(ii != jj)
//{
// ((float *)prediction.data)[jj + ii*prediction.cols] = v;
//}
}
}
++copied;
}
}
UDEBUG("time copying = %fs", timer.restart());
}
//update virtual place
if(newIds[0] < 0)
{
if(prediction.cols>1) // The first must be the virtual place
{
((float*)prediction.data)[0] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(prediction.cols-1);
for(int j=1; j<prediction.cols; j++)
{
((float*)prediction.data)[j*prediction.cols] = val;
((float*)prediction.data)[j] = _predictionLC[0];
}
}
else if(prediction.cols>0)
{
((float*)prediction.data)[0] = 1;
}
}
UDEBUG("time updating virtual place = %fs", timer.restart());
UDEBUG("Modified=%d, Added=%d, Copied=%d", modified, added, copied);
return prediction;
}
void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
void BayesFilter::updatePosterior(const Memory * memory, const std::map<int, float> & likelihood)
{
ULOGGER_DEBUG("");
std::map<int, float> newPosterior;
for(std::vector<int>::const_iterator i=likelihoodIds.begin(); i != likelihoodIds.end(); ++i)
const bool wasEmpty = _posteriorIds.empty();
std::vector<int> ids;
std::vector<float> values;
ids.reserve(likelihood.size());
values.reserve(likelihood.size());
// Both the likelihood and the posterior are ascending by id, so the two are merged in one
// walk, k only ever moving forward: for each location of the likelihood, advance the
// posterior up to it. A location in both keeps its probability, a location removed from
// the working memory is left behind, and a location that came back gets 0 (1 on the very
// first iteration, where the posterior starts uniform).
size_t k = 0;
for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
{
std::map<int, float>::iterator post = _posterior.find(*i);
if(post == _posterior.end())
while(k < _posteriorIds.size() && _posteriorIds[k] < iter->first)
{
if(_posterior.size() == 0)
{
newPosterior.insert(std::pair<int, float>(*i, 1));
}
else
{
newPosterior.insert(std::pair<int, float>(*i, 0));
}
++k;
}
else
float value = wasEmpty ? 1.0f : 0.0f;
if(k < _posteriorIds.size() && _posteriorIds[k] == iter->first)
{
newPosterior.insert(std::pair<int, float>((*post).first, (*post).second));
value = _posteriorValues[k];
}
ids.push_back(iter->first);
values.push_back(value);
}
_posterior = newPosterior;
_posteriorIds.swap(ids);
_posteriorValues.swap(values);
}
} // namespace rtabmap
corelib/src/CMakeLists.txt
+3
View File
@@ -47,6 +47,9 @@ SET(SRC_FILES
VisualWord.cpp
VWDictionary.cpp
BayesFilter.cpp
bayes/PredictionModel.cpp
bayes/DensePrediction.cpp
bayes/SparsePrediction.cpp
Parameters.cpp
Signature.cpp
Features2d.cpp
corelib/src/Rtabmap.cpp
+20 -9
View File
@@ -1258,7 +1258,6 @@ bool Rtabmap::process(
std::map<int, float> adjustedLikelihood;
std::map<int, float> likelihood;
std::map<int, int> weights;
std::map<int, float> posterior;
std::list<std::pair<int, float> > reactivateHypotheses;
std::map<int, int> childCount;
@@ -2138,7 +2137,7 @@ bool Rtabmap::process(
ULOGGER_INFO("getting posterior...");
// Compute the posterior
posterior = _bayesFilter->computePosterior(_memory, likelihood);
_bayesFilter->computePosterior(_memory, likelihood);
timePosteriorCalculation = timer.ticks();
ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
@@ -2152,17 +2151,20 @@ bool Rtabmap::process(
// Select the highest hypothesis
//============================================================
ULOGGER_INFO("creating hypotheses...");
if(posterior.size())
const std::vector<int> & posteriorIds = _bayesFilter->getPosteriorIds();
const std::vector<float> & posteriorValues = _bayesFilter->getPosteriorValues();
if(posteriorIds.size())
{
for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
// Highest id first, so the highest id wins on equal probabilities.
for(size_t i=posteriorIds.size(); i-- > 0;)
{
if(iter->first > 0 && iter->second > _highestHypothesis.second)
if(posteriorIds[i] > 0 && posteriorValues[i] > _highestHypothesis.second)
{
_highestHypothesis = *iter;
_highestHypothesis = std::make_pair(posteriorIds[i], posteriorValues[i]);
}
}
// With the virtual place, use sum of LC probabilities (1 - virtual place hypothesis).
_highestHypothesis.second = 1-posterior.begin()->second;
_highestHypothesis.second = 1-posteriorValues[0];
}
timeHypothesesCreation = timer.ticks();
ULOGGER_INFO("Highest hypothesis=%d, value=%f, timeHypothesesCreation=%fs", _highestHypothesis.first, _highestHypothesis.second, timeHypothesesCreation);
@@ -2193,7 +2195,7 @@ bool Rtabmap::process(
if(_highestHypothesis.second >= loopThr)
{
rejectedLoopClosure = true;
if(posterior.size() <= 2 && loopThr>0.0f)
if(_bayesFilter->getPosteriorIds().size() <= 2 && loopThr>0.0f)
{
// Ignore loop closure if there is only one loop closure hypothesis
UDEBUG("rejected hypothesis: single hypothesis");
@@ -4194,7 +4196,9 @@ bool Rtabmap::process(
}
// Posterior is empty if a bad signature is detected
float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
// The virtual place is the first location of the posterior when it is one of them.
const std::vector<int> & vpIds = _bayesFilter->getPosteriorIds();
float vpHypothesis = (vpIds.size() && vpIds[0]==Memory::kIdVirtual)?_bayesFilter->getPosteriorValues()[0]:0.0f;
int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
// prepare statistics
@@ -4411,6 +4415,13 @@ bool Rtabmap::process(
statistics_.setWeights(weights);
if(_publishPdf)
{
const std::vector<int> & ids = _bayesFilter->getPosteriorIds();
const std::vector<float> & values = _bayesFilter->getPosteriorValues();
std::map<int, float> posterior;
for(size_t i=0; i<ids.size(); ++i)
{
posterior.insert(posterior.end(), std::make_pair(ids[i], values[i]));
}
statistics_.setPosterior(posterior);
}
if(_publishLikelihood)
corelib/src/bayes/DensePrediction.cpp
+369
View File
@@ -0,0 +1,369 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "bayes/DensePrediction.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/utilite/UtiLite.h"
#include <set>
#if __cplusplus >= 201103L
#include <unordered_set>
#endif
namespace rtabmap {
namespace bayes {
const cv::Mat & DensePrediction::generate(const PredictionModel & model, const Memory * memory,
const std::vector<int> & ids, bool fullUpdate, NeighborsCache * cache)
{
// 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)
{
matrix_ = this->update(model, memory, ids_, ids, cache);
}
else
{
matrix_ = this->generateFull(model, memory, ids, fullUpdate?0:cache);
}
ids_ = ids;
return matrix_;
}
void DensePrediction::multiply(const std::vector<float> & posterior, std::vector<float> & prior) const
{
UASSERT(!matrix_.empty());
UASSERT_MSG(matrix_.cols == (int)posterior.size(),
uFormat("posterior=%d prediction=%d", (int)posterior.size(), matrix_.cols).c_str());
// A header over the posterior, so the multiplication reads it where it is. The product
// itself is left to OpenCV to allocate: asked to write into a matrix of ours it takes a
// path orders of magnitude slower, and copying the result back is only one value per
// location.
const cv::Mat posteriorMat((int)posterior.size(), 1, CV_32FC1, (void*)&posterior[0]);
const cv::Mat priorMat = matrix_ * posteriorMat;
prior.assign((const float *)priorMat.data, (const float *)priorMat.data + priorMat.rows);
}
unsigned long DensePrediction::memoryUsed() const
{
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.
cv::Mat DensePrediction::generateFull(const PredictionModel & model, const Memory * memory,
const std::vector<int> & ids, NeighborsCache * cache) const
{
UASSERT(memory &&
model.values().size() >= 2 &&
ids.size());
UTimer timer;
timer.start();
UTimer timerGlobal;
timerGlobal.start();
IdToIndexMap idToIndexMap;
#if __cplusplus >= 201103L
idToIndexMap.reserve(ids.size());
#endif
for(unsigned int i=0; i<ids.size(); ++i)
{
if(ids[i]>0)
{
idToIndexMap[ids[i]] = i;
}
}
//int rows = prediction.rows;
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
// Each prior is a column vector
UDEBUG("model.values().size()=%d",(int)model.values().size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
{
if(idsDone.find(ids[i]) == idsDone.end())
{
if(ids[i] > 0)
{
// Set high values (gaussians curves) to loop closure neighbors
std::list<int> idsLoopMargin;
std::map<int, int> neighbors = resolveNeighbors(
memory, ids[i], model.depth(), idToIndexMap, idsLoopMargin, cache);
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
if(cache)
{
uInsert(*cache, std::make_pair(*iter, neighbors));
}
float sum = 0.0f; // sum values added
int index = idToIndexMap.at(*iter);
float * column = (float*)prediction.data + index;
sum += model.addNeighborProb(column, cols, neighbors, idToIndexMap);
idsDone.insert(*iter);
model.normalize(column, cols, cols, index, sum, ids[0]<0);
}
}
else
{
// Set the virtual place prior
model.fillVirtualPlaceColumn((float*)prediction.data + i, cols, cols);
}
}
}
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
return prediction;
}
cv::Mat DensePrediction::update(const PredictionModel & model, const Memory * memory,
const std::vector<int> & oldIds, const std::vector<int> & newIds,
NeighborsCache * cache) const
{
UTimer timer;
UDEBUG("");
UASSERT(memory &&
oldIds.size() &&
newIds.size() &&
oldIds.size() == (unsigned int)matrix_.cols &&
oldIds.size() == (unsigned int)matrix_.rows);
cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
UDEBUG("time creating prediction = %fs", timer.restart());
// Create id to index maps
#if __cplusplus >= 201103L
std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
#else
std::set<int> oldIdsSet(oldIds.begin(), oldIds.end());
#endif
UDEBUG("time creating old ids set = %fs", timer.restart());
IdToIndexMap newIdToIndexMap;
#if __cplusplus >= 201103L
newIdToIndexMap.reserve(newIds.size());
#endif
for(unsigned int i=0; i<newIds.size(); ++i)
{
if(newIds[i]>0)
{
newIdToIndexMap[newIds[i]] = i;
}
}
UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
//Get removed ids
std::set<int> removedIds;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
{
removedIds.insert(removedIds.end(), oldIds[i]);
(*cache).erase(oldIds[i]);
UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
}
}
UDEBUG("time getting removed ids = %fs", timer.restart());
bool oldAllCopied = false;
if(removedIds.empty() &&
newIds.size() > oldIds.size() &&
memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) == 0)
{
matrix_.copyTo(cv::Mat(prediction, cv::Range(0, matrix_.rows), cv::Range(0, matrix_.cols)));
oldAllCopied = true;
UDEBUG("Copied all old prediction: = %fs", timer.ticks());
}
int added = 0;
// get ids to update
std::set<int> idsToUpdate;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
{
if(i<oldIds.size())
{
if(removedIds.find(oldIds[i]) != removedIds.end())
{
unsigned int cols = matrix_.cols;
int count = 0;
for(unsigned int j=0; j<cols; ++j)
{
if(j!=i && removedIds.find(oldIds[j]) == removedIds.end())
{
//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)matrix_.data)[i + j*cols]);
idsToUpdate.insert(oldIds[j]);
++count;
}
}
UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
}
}
if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
{
if((*cache).find(newIds[i]) == (*cache).end())
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], model.depth(), 0, false, false, true, true);
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
std::map<int, std::map<int, int> >::iterator jter = (*cache).find(iter->first);
if(jter != (*cache).end())
{
uInsert(jter->second, std::make_pair(newIds[i], iter->second));
}
}
(*cache).insert(std::make_pair(newIds[i], neighbors));
}
const std::map<int, int> & neighbors = (*cache).at(newIds[i]);
//std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], model.depth(), 0, false, false, true, true);
float * column = (float*)prediction.data + i;
float sum = model.addNeighborProb(column, prediction.cols, neighbors, newIdToIndexMap);
model.normalize(column, prediction.cols, prediction.cols, i, sum, newIds[0]<0);
++added;
int count = 0;
for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
removedIds.find(iter->first) == removedIds.end())
{
idsToUpdate.insert(iter->first);
++count;
}
}
UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
}
}
UDEBUG("time getting %d ids to update = %fs", (int)idsToUpdate.size(), timer.restart());
UTimer t1;
double e0=0,e1=0, e2=0, e3=0, e4=0;
// update modified/added ids
int modified = 0;
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{
int id = *iter;
if(id > 0)
{
int index = newIdToIndexMap.at(id);
e0 = t1.ticks();
std::map<int, std::map<int, int> >::iterator kter = (*cache).find(id);
UASSERT_MSG(kter != (*cache).end(), uFormat("Did not find %d (current index size=%d)", id, (int)(*cache).size()).c_str());
const std::map<int, int> & neighbors = kter->second;
//std::map<int, int> neighbors = memory->getNeighborsId(id, model.depth(), 0, false, false, true, true);
e1+=t1.ticks();
float * column = (float*)prediction.data + index;
float sum = model.addNeighborProb(column, prediction.cols, neighbors, newIdToIndexMap);
e3+=t1.ticks();
model.normalize(column, prediction.cols, prediction.cols, index, sum, newIds[0]<0);
++modified;
e4+=t1.ticks();
}
}
UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", (int)idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
int copied = 0;
if(!oldAllCopied)
{
//UDEBUG("oldIds.size()=%d, matrix_.cols=%d, matrix_.rows=%d", oldIds.size(), matrix_.cols, matrix_.rows);
//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
// copy not changed probabilities
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
{
for(int j=0; j<matrix_.cols; ++j)
{
if(oldIds[j]>0 && removedIds.find(oldIds[j]) == removedIds.end())
{
//UDEBUG("i=%d, j=%d", i, j);
//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
float v = ((const float *)matrix_.data)[i + j*matrix_.cols];
int ii = newIdToIndexMap.at(oldIds[i]);
int jj = newIdToIndexMap.at(oldIds[j]);
((float *)prediction.data)[ii + jj*prediction.cols] = v;
//if(ii != jj)
//{
// ((float *)prediction.data)[jj + ii*prediction.cols] = v;
//}
}
}
++copied;
}
}
UDEBUG("time copying = %fs", timer.restart());
}
//update virtual place
if(newIds[0] < 0)
{
if(prediction.cols>1) // The first must be the virtual place
{
((float*)prediction.data)[0] = model.virtualPlacePrior();
float val = (1.0-model.virtualPlacePrior())/(prediction.cols-1);
for(int j=1; j<prediction.cols; j++)
{
((float*)prediction.data)[j*prediction.cols] = val;
((float*)prediction.data)[j] = model.values()[0];
}
}
else if(prediction.cols>0)
{
((float*)prediction.data)[0] = 1;
}
}
UDEBUG("time updating virtual place = %fs", timer.restart());
UDEBUG("Modified=%d, Added=%d, Copied=%d", modified, added, copied);
return prediction;
}
} // namespace bayes
} // namespace rtabmap
corelib/src/bayes/DensePrediction.h
+91
View File
@@ -0,0 +1,91 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RTABMAP_BAYES_DENSEPREDICTION_H_
#define RTABMAP_BAYES_DENSEPREDICTION_H_
#include "bayes/PredictionModel.h"
#include <opencv2/core/core.hpp>
#include <vector>
namespace rtabmap {
class Memory;
namespace bayes {
/**
* @brief The prediction as a matrix, one column per location.
*
* The matrix costs the number of locations squared, whatever the graph puts in it, which on a
* large map is most of what the Bayes filter holds and most of what an iteration reads. See
* SparsePrediction for the form that keeps only the values.
*/
class DensePrediction
{
public:
bool empty() const {return matrix_.empty();}
const cv::Mat & matrix() const {return matrix_;}
/// The locations the matrix is built for, which the incremental update carries over.
const std::vector<int> & ids() const {return ids_;}
void clear() {matrix_ = cv::Mat(); ids_.clear();}
/**
* @brief Builds the matrix for @p ids and keeps it, along with the ids it is built for.
*
* 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 fullUpdate Rebuilds every column rather than carrying the matrix over.
* @param cache Filled with the neighborhoods, for a later incremental update.
*/
const cv::Mat & generate(const PredictionModel & model, const Memory * memory,
const std::vector<int> & ids, bool fullUpdate, NeighborsCache * cache);
/// prior = prediction x posterior.
void multiply(const std::vector<float> & posterior, std::vector<float> & prior) const;
unsigned long memoryUsed() const;
private:
cv::Mat generateFull(const PredictionModel & model, const Memory * memory,
const std::vector<int> & ids, NeighborsCache * cache) const;
cv::Mat update(const PredictionModel & model, const Memory * memory,
const std::vector<int> & oldIds, const std::vector<int> & newIds,
NeighborsCache * cache) const;
cv::Mat matrix_;
std::vector<int> ids_;
};
} // namespace bayes
} // namespace rtabmap
#endif /* RTABMAP_BAYES_DENSEPREDICTION_H_ */
corelib/src/bayes/PredictionModel.cpp
+302
View File
@@ -0,0 +1,302 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "bayes/PredictionModel.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/utilite/UtiLite.h"
namespace rtabmap {
namespace bayes {
// format = {Virtual place, Loop closure, level1, level2, l3, l4...}
bool PredictionModel::set(const std::string & prediction)
{
bool set = false;
std::list<std::string> strValues = uSplit(prediction, ' ');
if(strValues.size() < 2)
{
UERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
}
else
{
std::vector<double> tmpValues(strValues.size());
int i=0;
bool valid = true;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = uStr2Float((*iter).c_str());
//UINFO("%d=%e", i, tmpValues[i]);
if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
{
valid = false;
break;
}
++i;
}
if(!valid)
{
UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str());
}
else
{
values_ = tmpValues;
set = true;
}
}
total_ = 0.0f;
for(unsigned int j=0; j<values_.size(); ++j)
{
total_ += values_[j];
if(j==0 || values_[j] < epsilon_)
{
epsilon_ = values_[j];
}
}
if(!values_.empty())
{
UDEBUG("predictionEpsilon = %f", epsilon_);
}
return set;
}
std::string PredictionModel::str() const
{
std::string values;
for(unsigned int i=0; i<values_.size(); ++i)
{
values.append(uNumber2Str(values_[i]));
if(i+1 < values_.size())
{
values.append(" ");
}
}
return values;
}
// 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 PredictionModel::addNeighborProb(float * column, size_t stride,
const std::map<int, int> & neighbors, const IdToIndexMap & idToIndex) const
{
float sum=0.0f;
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(iter->first>=0)
{
IdToIndexMap::const_iterator jter = idToIndex.find(iter->first);
if(jter != idToIndex.end())
{
UASSERT((iter->second+1) < (int)values_.size());
sum += column[jter->second*stride] = values_[iter->second+1];
}
}
}
return sum;
}
void PredictionModel::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 < total_-values_[0])
{
float delta = total_-values_[0]-addedProbabilitiesSum;
column[index*stride] += delta;
addedProbabilitiesSum+=delta;
}
float allOtherPlacesValue = 0;
if(total_ < 1)
{
allOtherPlacesValue = 1.0f - total_;
}
// Set all loop events to small values according to the model
if(allOtherPlacesValue > 0 && cols>1)
{
float value = allOtherPlacesValue / float(cols - 1);
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
if(column[j*stride] == 0)
{
column[j*stride] = value;
addedProbabilitiesSum += column[j*stride];
}
}
}
//normalize this row
float maxNorm = 1 - (virtualPlaceUsed?values_[0]:0); // 1 - virtual place probability
if(addedProbabilitiesSum<maxNorm-0.0001 || addedProbabilitiesSum>maxNorm+0.0001)
{
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
column[j*stride] *= maxNorm / addedProbabilitiesSum;
if(column[j*stride] < epsilon_)
{
column[j*stride] = 0.0f;
}
}
addedProbabilitiesSum = maxNorm;
}
// ADD virtual place prob
if(virtualPlaceUsed)
{
column[0] = values_[0];
addedProbabilitiesSum += column[0];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
//}
// Left out of the coverage report: no input gets here. Whatever the column held, the
// scaling above leaves addedProbabilitiesSum at maxNorm, which is 1 without the virtual
// place and 1 minus its probability with it -- and that probability is then added back.
// It is kept as a canary for whoever changes the arithmetic above.
if(addedProbabilitiesSum<0.99 || addedProbabilitiesSum > 1.01)
{
UWARN("Prediction is not normalized sum=%f", addedProbabilitiesSum); // LCOV_EXCL_LINE
}
}
// 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 PredictionModel::fillVirtualPlaceColumn(float * column, size_t stride, int size) const
{
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; j<size; ++j)
{
column[j*stride] = val;
}
}
else if(size>0)
{
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; j<size; ++j)
{
column[j*stride] = val;
}
}
else if(size>0)
{
column[0] = 1;
}
}
}
// 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.
//
// cache is filled when not null, for updatePrediction() to reuse.
std::map<int, int> resolveNeighbors(const Memory * memory, int id, int maxDepth,
const IdToIndexMap & idToIndexMap, std::list<int> & idsAtMargin0, NeighborsCache * cache)
{
std::map<int, int> neighbors = memory->getNeighborsId(id, maxDepth, 0, false, false, true, true);
if(cache)
{
uInsert(*cache, std::make_pair(id, neighbors));
}
idsAtMargin0.clear();
//filter neighbors in STM
for(std::map<int, int>::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;
}
// The neighborhood of a location, from the cache the incremental update needs, adding it
// there and to the neighborhoods of its own neighbors when it is not there yet.
const std::map<int, int> & cachedNeighbors(const Memory * memory, int id, int maxDepth,
NeighborsCache & cache)
{
std::map<int, std::map<int, int> >::const_iterator iter = cache.find(id);
if(iter == cache.end())
{
std::map<int, int> neighbors = memory->getNeighborsId(id, maxDepth, 0, false, false, true, true);
for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
{
std::map<int, std::map<int, int> >::iterator kter = cache.find(jter->first);
if(kter != cache.end())
{
uInsert(kter->second, std::make_pair(id, jter->second));
}
}
iter = cache.insert(std::make_pair(id, neighbors)).first;
}
return iter->second;
}
} // namespace bayes
} // namespace rtabmap
corelib/src/bayes/PredictionModel.h
+143
View File
@@ -0,0 +1,143 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RTABMAP_BAYES_PREDICTIONMODEL_H_
#define RTABMAP_BAYES_PREDICTIONMODEL_H_
#include <list>
#include <map>
#include <string>
#include <vector>
#if __cplusplus >= 201103L
#include <unordered_map>
#endif
namespace rtabmap {
class Memory;
namespace bayes {
/// Where each location sits in the prediction: its id to its row and column.
#if __cplusplus >= 201103L
typedef std::unordered_map<int, int> IdToIndexMap;
#else
typedef std::map<int, int> IdToIndexMap;
#endif
/// The neighborhood of the locations it was asked for, which the incremental updates of the
/// prediction read instead of walking the graph again.
typedef std::map<int, std::map<int, int> > NeighborsCache;
/**
* @brief The loop closure prediction model, and the column arithmetic that follows from it.
*
* Format `{Vp, Lc, l1, l2, ...}`: the probability of moving to a new place, of staying at the
* same location, then of moving to a neighbor at each depth of the graph. See
* Parameters::kBayesPredictionLC().
*
* A column of the prediction is the distribution over where the robot moves to from one
* location. Both the dense and the sparse prediction fill their columns through this, so the
* probabilities cannot end up differing between them. A column is given as the 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 fills one contiguous column at a time.
*/
class PredictionModel
{
public:
/**
* @brief Sets the model from a space separated list of probabilities.
* @return False when the string does not hold at least two values in [0, 1], the previous
* model being kept.
*/
bool set(const std::string & prediction);
const std::vector<double> & values() const {return values_;}
std::string str() const;
/// How deep in the graph a column reaches: one less than the number of values.
int depth() const {return (int)values_.size()-1;}
/// Whether the values leave probability for normalize() to spread over every other
/// location, which fills every zero of a column and leaves nothing sparse to keep.
bool spreadsOverAllLocations() const {return total_ < 1;}
float total() const {return total_;}
bool valid() const {return values_.size() >= 2;}
float virtualPlacePrior() const {return virtualPlacePrior_;}
void setVirtualPlacePrior(float prior) {virtualPlacePrior_ = prior;}
/**
* @brief Puts the probability of each neighbor into a column.
* @return The sum of what it put there, which normalize() takes.
*/
float addNeighborProb(float * column, size_t stride, const std::map<int, int> & neighbors,
const IdToIndexMap & idToIndex) const;
/**
* @brief Normalizes one column and applies the virtual place probability.
* @param index Index of the location the column is for, so of its diagonal value.
* @param addedProbabilitiesSum What 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;
/// Fills the column of the virtual place, the hypothesis of a location never visited.
void fillVirtualPlaceColumn(float * column, size_t stride, int size) const;
unsigned long memoryUsed() const {return values_.capacity() * sizeof(double);}
private:
std::vector<double> values_;
float total_ = 0.0f;
float epsilon_ = 0.0f; ///< Smallest probability of the model, under which normalize() drops a value.
float virtualPlacePrior_ = 0.0f;
};
/**
* @brief The neighbors of a location within the depth of the model, and the locations at
* margin 0 of it, meaning the same place: their columns hold this same neighborhood.
*
* @param cache Filled when not null, for the incremental updates to reuse.
*/
std::map<int, int> resolveNeighbors(const Memory * memory, int id, int maxDepth,
const IdToIndexMap & idToIndexMap, std::list<int> & idsAtMargin0, NeighborsCache * cache);
/**
* @brief The neighborhood of a location from @p cache, querying and caching it when absent.
*
* Caching it also adds the location to the neighborhoods of its own neighbors.
*/
const std::map<int, int> & cachedNeighbors(const Memory * memory, int id, int maxDepth,
NeighborsCache & cache);
} // namespace bayes
} // namespace rtabmap
#endif /* RTABMAP_BAYES_PREDICTIONMODEL_H_ */
corelib/src/bayes/SparsePrediction.cpp
+549
View File
@@ -0,0 +1,549 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "bayes/SparsePrediction.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/utilite/UtiLite.h"
namespace rtabmap {
namespace bayes {
void SparsePrediction::clear()
{
columns_.clear();
values_.clear();
ids_.clear();
used_ = 0;
}
cv::Mat SparsePrediction::toMatrix() const
{
const int size = (int)columns_.size();
cv::Mat matrix = cv::Mat::zeros(size, size, CV_32FC1);
for(int col=0; col<size; ++col)
{
const Column & slot = columns_[col];
for(size_t i=slot.offset; i<slot.offset+slot.size; ++i)
{
matrix.at<float>(values_[i].first, col) = values_[i].second;
}
}
return matrix;
}
unsigned long SparsePrediction::memoryUsed() const
{
return values_.capacity() * sizeof(std::pair<int, float>)
+ columns_.capacity() * sizeof(Column)
+ ids_.capacity() * sizeof(int);
}
// Takes the non zero values of a freshly built column into the prediction, and leaves the
// buffer zeroed for the next one, which saves clearing the whole of it every time.
//
// The values of every column live in one array, so that the multiplication reads them the
// way memory likes to be read. A column keeps the room it was given: rebuilt into fewer
// values it stays where it is, rebuilt into more than it has room for it is put at the end
// and the room it had is left behind, to be recovered by compact(). Asking
// for a little more than is needed, when the column is one being rebuilt, buys the room for
// it to grow a few times in place.
void SparsePrediction::takeColumn(std::vector<float> & column, int index, bool withRoomToGrow)
{
size_t count = 0;
for(size_t row=0; row<column.size(); ++row)
{
if(column[row] != 0.0f)
{
++count;
}
}
Column & slot = columns_[index];
used_ -= slot.size;
if(count > slot.capacity)
{
slot.offset = values_.size();
slot.capacity = withRoomToGrow ? count + count/8 + 4 : count;
values_.resize(slot.offset + slot.capacity);
}
slot.size = count;
used_ += count;
size_t i = slot.offset;
for(size_t row=0; row<column.size(); ++row)
{
if(column[row] != 0.0f)
{
values_[i++] = std::make_pair((int)row, column[row]);
column[row] = 0.0f;
}
}
}
// Packs the columns back into the order they are multiplied in, giving each the room it
// needs and no more. Called when the room left behind by rebuilt columns has grown to a
// quarter of what is in use, and at the end of a full build, whose columns are not built in
// the order of their index.
void SparsePrediction::compact()
{
std::vector<std::pair<int, float> > packed;
packed.reserve(used_);
for(size_t i=0; i<columns_.size(); ++i)
{
Column & slot = columns_[i];
const size_t offset = packed.size();
packed.insert(packed.end(),
values_.begin()+slot.offset,
values_.begin()+slot.offset+slot.size);
slot.offset = offset;
slot.capacity = slot.size;
}
values_.swap(packed);
}
// The prediction built 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. Every column keeps the room it was given in values_, so that update() can rebuild
// one of them without moving the others.
//
// 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.
//
// Always built, whatever its columns come to hold: the caller asked for the prediction sparse
// and gets it sparse, so that what it measures is the sparse form and not a fallback. The one
// prediction with nothing sparse to keep, of a model whose values sum to less than 1,
// normalize() spreading the difference over every zero of a column, never reaches here: the
// caller answers that one with the matrix without asking.
void SparsePrediction::generate(const PredictionModel & model, const Memory * memory,
const std::vector<int> & ids, NeighborsCache * cache)
{
UASSERT(memory && model.valid() && ids.size());
UTimer timer;
this->clear();
const int size = (int)ids.size();
IdToIndexMap idToIndexMap;
#if __cplusplus >= 201103L
idToIndexMap.reserve(ids.size());
#endif
for(int i=0; i<size; ++i)
{
if(ids[i]>0)
{
idToIndexMap[ids[i]] = i;
}
}
columns_.assign(size, Column());
std::vector<float> column(size, 0.0f);
std::set<int> idsDone;
for(int i=0; i<size; ++i)
{
if(idsDone.find(ids[i]) != idsDone.end())
{
continue;
}
if(ids[i] > 0)
{
std::list<int> idsLoopMargin;
std::map<int, int> neighbors = resolveNeighbors(
memory, ids[i], model.depth(), idToIndexMap, idsLoopMargin, cache);
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter=idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
if(cache)
{
uInsert(*cache, std::make_pair(*iter, neighbors));
}
const int index = idToIndexMap.at(*iter);
const float sum = model.addNeighborProb(&column[0], 1, neighbors, idToIndexMap);
model.normalize(&column[0], 1, size, index, sum, ids[0]<0);
this->takeColumn(column, index, false);
idsDone.insert(*iter);
}
}
else
{
model.fillVirtualPlaceColumn(&column[0], 1, size);
this->takeColumn(column, i, false);
}
}
// The columns were not built in the order of their index, so they are packed into it.
this->compact();
ids_ = ids;
const size_t nnz = used_;
UDEBUG("Sparse prediction: %ld/%ld values (%.2f%%), %ld MB against the %ld MB of the "
"matrix, built in %fs",
(long)nnz, (long)size*size, 100.0*double(nnz)/(double(size)*double(size)),
(long)(this->memoryUsed()/1048576),
(long)((size_t)size*(size_t)size*sizeof(float)/1048576),
timer.ticks());
}
// 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<int> & ids, const IdToIndexMap & idToIndex,
std::vector<float> & buffer, NeighborsCache & cache)
{
const std::map<int, int> & 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<int> & 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;
return appendedTo
? this->updateAppended(model, memory, newIds, cache)
: this->updateRemapped(model, memory, newIds, cache);
}
// 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
// still apply: only the ones the new locations reach have to be built again, and the
// 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.
bool SparsePrediction::updateAppended(const PredictionModel & model, const Memory * memory,
const std::vector<int> & newIds, NeighborsCache & cache)
{
UTimer timer;
const std::vector<int> & oldIds = ids_;
const int size = (int)newIds.size();
IdToIndexMap newIdToIndexMap;
#if __cplusplus >= 201103L
newIdToIndexMap.reserve(newIds.size());
#endif
for(int i=0; i<size; ++i)
{
if(newIds[i]>0)
{
newIdToIndexMap[newIds[i]] = i;
}
}
columns_.resize(size); // the appended columns start out empty
std::vector<float> column(size, 0.0f);
// The appended locations, and the ones that were already there whose neighborhood the
// appended ones are now part of.
std::set<int> idsToUpdate;
for(size_t i=oldIds.size(); i<newIds.size(); ++i)
{
// Every appended location is a visited one: the virtual place is the first of them
// and an append keeps the index of everything that was already there.
UASSERT(newIds[i] > 0);
const std::map<int, int> & neighbors = cachedNeighbors(memory, newIds[i], model.depth(), cache);
const float sum = model.addNeighborProb(&column[0], 1, neighbors, newIdToIndexMap);
model.normalize(&column[0], 1, size, (int)i, sum, newIds[0]<0);
this->takeColumn(column, (int)i, true);
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
const IdToIndexMap::const_iterator jter = newIdToIndexMap.find(iter->first);
if(jter != newIdToIndexMap.end() && (size_t)jter->second < oldIds.size())
{
idsToUpdate.insert(iter->first);
}
}
}
for(std::set<int>::const_iterator iter=idsToUpdate.begin(); iter!=idsToUpdate.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);
}
// The room left behind by the columns that outgrew their slot, once it is a quarter of
// what is in use.
const size_t waste = values_.size() - used_;
const bool compacted = waste > used_/4;
if(compacted)
{
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)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<int> & newIds, NeighborsCache & cache)
{
UTimer timer;
const std::vector<int> & 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; i<size; ++i)
{
if(newIds[i]>0)
{
newIdToIndexMap[newIds[i]] = i;
}
}
for(size_t i=0; i<oldIds.size(); ++i)
{
if(oldIds[i]>0)
{
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<int> oldToNew(oldIds.size(), -1);
size_t removed = 0;
for(size_t i=0; i<oldIds.size(); ++i)
{
if(oldIds[i] <= 0)
{
oldToNew[i] = 0;
continue;
}
const IdToIndexMap::const_iterator iter = newIdToIndexMap.find(oldIds[i]);
if(iter == newIdToIndexMap.end())
{
// Its neighborhood is no longer ours to keep, as the dense update does too.
cache.erase(oldIds[i]);
++removed;
}
else
{
oldToNew[i] = iter->second;
}
}
// The locations that were not there before, and the ones whose neighborhood they are
// part of.
std::set<int> idsToBuild;
for(int i=0; i<size; ++i)
{
if(newIds[i] <= 0 || oldIdToIndexMap.find(newIds[i]) != oldIdToIndexMap.end())
{
continue;
}
idsToBuild.insert(newIds[i]);
const std::map<int, int> & neighbors = cachedNeighbors(memory, newIds[i], model.depth(), cache);
for(std::map<int, int>::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<oldIds.size(); ++i)
{
if(oldIds[i] <= 0 || oldToNew[i] < 0 ||
idsToBuild.find(oldIds[i]) != idsToBuild.end())
{
continue;
}
const Column & slot = columns_[i];
for(size_t v=slot.offset; v<slot.offset+slot.size; ++v)
{
if(oldToNew[values_[v].first] < 0)
{
idsToBuild.insert(oldIds[i]);
break;
}
}
}
}
// The columns that are carried over, at their new index and packed as they go: the room
// left behind by the ones that are gone or built again is not carried with them.
std::vector<Column> keptColumns(size);
std::vector<std::pair<int, float> > keptValues;
keptValues.reserve(used_);
size_t keptUsed = 0;
size_t carried = 0;
for(size_t i=0; i<oldIds.size(); ++i)
{
const int index = oldToNew[i];
if(index < 0 || oldIds[i] <= 0 || idsToBuild.find(oldIds[i]) != idsToBuild.end())
{
continue;
}
const Column & slot = columns_[i];
Column & kept = keptColumns[index];
kept.offset = keptValues.size();
kept.size = slot.size;
kept.capacity = slot.size;
for(size_t v=slot.offset; v<slot.offset+slot.size; ++v)
{
// The rows of a column are ascending, and so are both id vectors, so a remapped
// row stays after the one before it.
keptValues.push_back(std::make_pair(oldToNew[values_[v].first], values_[v].second));
}
keptUsed += slot.size;
++carried;
}
columns_.swap(keptColumns);
values_.swap(keptValues);
used_ = keptUsed;
std::vector<float> column(size, 0.0f);
for(std::set<int>::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;
}
void SparsePrediction::multiply(const std::vector<float> & posterior, std::vector<float> & prior) const
{
const size_t size = columns_.size();
UASSERT(size > 0);
UASSERT_MSG(posterior.size() == size,
uFormat("posterior=%d prediction=%d", (int)posterior.size(), (int)size).c_str());
prior.assign(size, 0.0f);
const float * posteriorPtr = &posterior[0];
float * priorPtr = &prior[0];
// The prior is the sum of the columns of the prediction weighted by the posterior.
// Going by column is the order the values are stored in, and lets a location the
// posterior has ruled out be skipped whole.
for(size_t col=0; col<size; ++col)
{
const float weight = posteriorPtr[col];
if(weight == 0.0f)
{
continue;
}
const Column & slot = columns_[col];
for(size_t i=slot.offset; i<slot.offset+slot.size; ++i)
{
priorPtr[values_[i].first] += values_[i].second * weight;
}
}
}
} // namespace bayes
} // namespace rtabmap
corelib/src/bayes/SparsePrediction.h
+142
View File
@@ -0,0 +1,142 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RTABMAP_BAYES_SPARSEPREDICTION_H_
#define RTABMAP_BAYES_SPARSEPREDICTION_H_
#include "bayes/PredictionModel.h"
#include <opencv2/core/core.hpp>
#include <utility>
#include <vector>
namespace rtabmap {
class Memory;
namespace bayes {
/**
* @brief The prediction as its values only, one column at a time.
*
* A column holds the neighbors of one location within the depth of the model, so on a large
* map the matrix DensePrediction would build is mostly zeros: holding it costs the number of
* locations squared, against the far smaller number of values in it. Each column is built in a
* buffer of its own and only its non-zero values are kept, so nothing of that size is ever
* allocated.
*
* The values of every column live in one array, which the multiplication reads the way memory
* likes to be read, and a column keeps the room it was given so that update() can rebuild one
* without moving the others.
*/
class SparsePrediction
{
public:
bool empty() const {return columns_.empty();}
const std::vector<int> & ids() const {return ids_;}
size_t values() const {return used_;}
void clear();
/**
* @brief Builds it for @p ids, whatever its columns come to hold.
*
* The prediction of a model that leaves probability to spread has no zero left in a column
* and nothing sparse to keep, which the caller answers with the matrix rather than asking
* for this. Nothing else falls back to one.
*
* @param cache Filled with the neighborhoods when not null, which update() needs.
*/
void generate(const PredictionModel & model, const Memory * memory,
const std::vector<int> & ids, NeighborsCache * cache);
/**
* @brief Carries it over to @p ids without walking the graph 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 there is nothing to carry over: no prediction yet, or the virtual place
* appearing or disappearing. The caller answers by calling generate(), which is also
* what fills @p cache.
*/
bool update(const PredictionModel & model, const Memory * memory,
const std::vector<int> & ids, NeighborsCache & cache);
/// prior = prediction x posterior.
void multiply(const std::vector<float> & posterior, std::vector<float> & prior) const;
/**
* @brief The same prediction as a matrix, for the one caller that wants to look at it.
*
* The matrix costs what keeping the prediction sparse is saving, so this builds one to be
* read, dumped or compared against DensePrediction, and does not keep it.
*/
cv::Mat toMatrix() const;
unsigned long memoryUsed() const;
private:
/// Where a column sits in values_, and how much room it was given: a column rebuilt into
/// more values than it has room for is moved to the end, leaving its room behind until
/// compact() recovers it.
struct Column
{
size_t offset = 0;
size_t size = 0;
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<int> & 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<int> & 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<int> & ids, const IdToIndexMap & idToIndex,
std::vector<float> & buffer, NeighborsCache & cache);
void takeColumn(std::vector<float> & column, int index, bool withRoomToGrow);
void compact();
std::vector<Column> columns_;
std::vector<std::pair<int, float> > values_;
size_t used_ = 0; ///< How many of values_ belong to a column.
std::vector<int> ids_;
};
} // namespace bayes
} // namespace rtabmap
#endif /* RTABMAP_BAYES_SPARSEPREDICTION_H_ */
corelib/src/optimizer/OptimizerG2O.cpp
+93 -36
View File
@@ -2242,6 +2242,27 @@ bool OptimizerG2O::loadGraph(
std::vector<VertexEntry> verticesList;
std::vector<EdgeEntry> edgesList;
// The type of a link, which saveGraph() appends as a column past the fields the
// format defines: g2o's own loader reads the fields it knows and ignores what
// follows, so the column travels with the file without breaking it. A file written
// by anything else has no such column, and the type stays the one its tag implies.
// This is the only place the type of an edge can come from: the format has no field
// for it, so a loop closure and an odometry link are otherwise the same EDGE_SE2.
const auto readType = [](const std::vector<std::string> & v, size_t definedSize, Link::Type fallback)
{
if(v.size() > definedSize)
{
const int type = atoi(v[definedSize].c_str());
if(type >= 0 && type < Link::kEnd)
{
return (Link::Type)type;
}
UWARN("Ignoring link type \"%s\", not one of the %d types.",
v[definedSize].c_str(), (int)Link::kEnd);
}
return fallback;
};
char line[2048];
while(fgets(line, 2048, file) != NULL)
{
@@ -2301,7 +2322,7 @@ bool OptimizerG2O::loadGraph(
e.definitelyLandmark = true;
verticesList.push_back(e);
}
else if(tag == "EDGE_SE2" && v.size() == 12)
else if(tag == "EDGE_SE2" && v.size() >= 12)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2314,12 +2335,13 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[9]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]);
e.info.at<double>(5, 5) = uStr2Double(v[11]);
e.type = Link::kUndef; // disambiguated after we know landmarkOffset
// kUndef is disambiguated after we know landmarkOffset
e.type = readType(v, 12, Link::kUndef);
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE2_XY" && v.size() == 8)
else if(tag == "EDGE_SE2_XY" && v.size() >= 8)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2329,12 +2351,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(1, 1) = uStr2Double(v[7]);
e.type = Link::kLandmark;
e.type = readType(v, 8, Link::kLandmark);
e.isPrior = false;
e.hasLandmarkEndpoint = true;
edgesList.push_back(e);
}
else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31)
else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() >= 31)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2353,12 +2375,12 @@ bool OptimizerG2O::loadGraph(
}
// EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph
bool landmarkTag = (tag == "EDGE_SE3");
e.type = landmarkTag ? Link::kLandmark : Link::kUndef;
e.type = readType(v, 31, landmarkTag ? Link::kLandmark : Link::kUndef);
e.isPrior = false;
e.hasLandmarkEndpoint = landmarkTag;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13)
else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() >= 13)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2372,12 +2394,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]);
e.info.at<double>(2, 2) = uStr2Double(v[12]);
e.type = Link::kLandmark;
e.type = readType(v, 13, Link::kLandmark);
e.isPrior = false;
e.hasLandmarkEndpoint = true;
edgesList.push_back(e);
}
else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11)
else if(tag == "EDGE_PRIOR_SE2" && v.size() >= 11)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2390,12 +2412,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[8]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]);
e.info.at<double>(5, 5) = uStr2Double(v[10]);
e.type = Link::kPosePrior;
e.type = readType(v, 11, Link::kPosePrior);
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7)
else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() >= 7)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2407,12 +2429,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[6]);
// no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior;
e.type = readType(v, 7, Link::kPosePrior);
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31)
else if(tag == "EDGE_SE3_PRIOR" && v.size() >= 31)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2430,12 +2452,12 @@ bool OptimizerG2O::loadGraph(
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
e.type = Link::kPosePrior;
e.type = readType(v, 31, Link::kPosePrior);
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11)
else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() >= 11)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2450,12 +2472,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(2, 2) = uStr2Double(v[10]);
// no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior;
e.type = readType(v, 11, Link::kPosePrior);
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13)
else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() >= 13)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2469,12 +2491,12 @@ bool OptimizerG2O::loadGraph(
e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]);
e.info.at<double>(5, 5) = uStr2Double(v[12]);
e.type = Link::kUndef;
e.type = readType(v, 13, Link::kUndef);
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32)
else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() >= 32)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
@@ -2492,7 +2514,7 @@ bool OptimizerG2O::loadGraph(
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
e.type = Link::kUndef;
e.type = readType(v, 32, Link::kUndef);
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
@@ -2743,8 +2765,35 @@ bool OptimizerG2O::saveGraph(
}
int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0);
// A link is stored on both of the nodes it connects, so a caller iterating them
// hands us each one twice, once per direction. g2o has no notion of a reverse
// edge: it would read the two lines as two independent constraints and count the
// information of every link twice. Only the first direction of a pair is written,
// which is also half the file. Links on a single node (a prior, gravity) are not
// pairs and are left alone.
std::set<std::pair<int, int> > writtenPairs;
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
{
const std::pair<int, int> pair(
std::min(iter->second.from(), iter->second.to()),
std::max(iter->second.from(), iter->second.to()));
if(!writtenPairs.insert(pair).second)
{
continue;
}
}
// The type of the link, as a column past the fields the format defines. g2o's
// own loader reads the fields it knows and ignores what follows, so this
// travels with the file without breaking it, and loadGraph() reads it back.
// Without it the type is lost on export, and the type is what tells a loop
// closure from an odometry link.
const std::string typeSuffix = uFormat(" %d", (int)iter->second.type());
if (iter->second.type() == Link::kLandmark)
{
if (this->landmarksIgnored())
@@ -2760,7 +2809,7 @@ bool OptimizerG2O::saveGraph(
if(uValue(isLandmarkWithRotation, landmarkId, false))
{
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n",
fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
@@ -2771,19 +2820,21 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 5),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(5, 5));
iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
}
else
{
// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1));
iter->second.infMatrix().at<double>(1, 1),
typeSuffix.c_str());
}
}
else
@@ -2792,7 +2843,7 @@ bool OptimizerG2O::saveGraph(
{
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
@@ -2822,12 +2873,13 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(3, 5),
iter->second.infMatrix().at<double>(4, 4),
iter->second.infMatrix().at<double>(4, 5),
iter->second.infMatrix().at<double>(5, 5));
iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
}
else
{
// EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n",
fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f%s\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
PARAM_OFFSET,
@@ -2839,7 +2891,8 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2));
iter->second.infMatrix().at<double>(2, 2),
typeSuffix.c_str());
}
}
continue;
@@ -2911,7 +2964,7 @@ bool OptimizerG2O::saveGraph(
{
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
// EDGE_SE2_PRIOR observed_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f%s\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -2924,13 +2977,14 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 5),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 5),
iter->second.infMatrix().at<double>(5, 5));
iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
}
else
{
// EDGE_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
// EDGE_POINTXY_PRIOR x y inf_11 inf_12 inf_22
fprintf(file, "%s %d%s%s %f %f %f %f %f\n",
fprintf(file, "%s %d%s%s %f %f %f %f %f%s\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -2939,7 +2993,8 @@ bool OptimizerG2O::saveGraph(
iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1));
iter->second.infMatrix().at<double>(1, 1),
typeSuffix.c_str());
}
}
else
@@ -2949,7 +3004,7 @@ bool OptimizerG2O::saveGraph(
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
// EDGE_SE3_PRIOR observed_vertex_id offset_parameter_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f%s\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -2981,13 +3036,14 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(3, 5),
iter->second.infMatrix().at<double>(4, 4),
iter->second.infMatrix().at<double>(4, 5),
iter->second.infMatrix().at<double>(5, 5));
iter->second.infMatrix().at<double>(5, 5),
typeSuffix.c_str());
}
else
{
// EDGE_XYZ observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_13 inf_22 .. inf_33
// EDGE_POINTXYZ_PRIOR observed_vertex_id x y z inf_11 inf_12 .. inf_13 inf_22 .. inf_33
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f\n",
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f%s\n",
prefix.c_str(),
iter->second.from(),
to.c_str(),
@@ -3000,7 +3056,8 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2));
iter->second.infMatrix().at<double>(2, 2),
typeSuffix.c_str());
}
}
}
corelib/test/CMakeLists.txt
+18
View File
@@ -119,6 +119,24 @@ 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. Over synthetic graphs, and over
# the graph of a real map read from data/tests/large_reduced_graph.g2o, whose link
# types decide how much of the map a column of the prediction holds:
# bin/test_bayesfilter_perf
# bin/test_bayesfilter_perf --gtest_filter=-*LargeMap*:-*RealMap*
# Its own executable: it spends its time benchmarking rather than asserting, and the
# largest maps it builds allocate 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
corelib/test/perf_bayesfilter.cpp
+736
View File
@@ -0,0 +1,736 @@
// 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 <gtest/gtest.h>
#include <rtabmap/core/BayesFilter.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UTimer.h>
#include <algorithm>
#include <cmath>
#include <cstdio>
#include <iostream>
#include <map>
#include <vector>
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 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";
// 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<int> ids;
ids.reserve(nodes);
for(int i=0; i<nodes; ++i)
{
SensorData data(image);
UASSERT(memory_->update(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<int> bayesIds() const
{
std::vector<int> ids;
ids.push_back(Memory::kIdVirtual);
const std::set<int> & stm = memory_->getStMem();
for(std::map<int, double>::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<int, float> uniformLikelihood(const std::vector<int> & ids) const
{
std::map<int, float> likelihood;
for(size_t i=0; i<ids.size(); ++i)
{
likelihood.insert(std::make_pair(ids[i], 1.0f));
}
return likelihood;
}
private:
Memory * memory_ = nullptr;
int loopClosures_ = 0;
double buildTime_ = 0.0;
};
// A real map's graph, read from the g2o file it was exported to. What makes it worth
// measuring against the synthetic graphs above is its link types: the file holds mostly
// merged neighbor links, which cost a margin like an ordinary neighbor, and only a few
// hundred closures that Memory::getNeighborsId() follows without spending one. How many
// of those there are is what decides how much of the map a column of the prediction
// holds, so a real graph's answer is not a synthetic one's.
//
// The graph is rebuilt in a Memory rather than optimized: Memory::update() creates a
// signature per pose and links each to the previous one, so the links the file does not
// have are removed and the ones it has are added with their own type.
// A real map's graph, read from the g2o file it was exported to. What makes it worth
// measuring against the synthetic graphs above is its link types: how many links
// Memory::getNeighborsId() follows without spending a margin is what decides how much of
// the map a column of the prediction holds, and a real graph's answer is not a synthetic
// one's. A graph that went through the reduction holds mostly merged neighbor links,
// which cost a margin like an ordinary neighbor; one that did not holds none.
struct RealGraph
{
std::vector<int> ids; // the locations, in the order they were created
std::map<int, Transform> poses;
// The links between two locations, as indices into ids, each on the later of the two:
// that is the one a mapping session adds them on.
std::vector<std::vector<std::pair<size_t, Link::Type> > > linksTo;
std::map<int, int> byType;
int skippedLinks = 0;
double loadTime = 0.0;
};
bool loadRealGraph(const std::string & path, RealGraph & graph)
{
UTimer timer;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
if(!graph::importPoses(path, 4 /*g2o*/, poses, &links))
{
return false;
}
graph.loadTime = timer.ticks();
graph.poses = poses;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
graph.ids.push_back(iter->first);
}
}
std::sort(graph.ids.begin(), graph.ids.end());
std::map<int, size_t> indexOf;
for(size_t i=0; i<graph.ids.size(); ++i)
{
indexOf.insert(std::make_pair(graph.ids[i], i));
}
// One entry per pair of locations. Links on a single location (a prior, gravity) and
// landmark observations are left out: getNeighborsId() doesn't walk the first, and the
// second would need the landmark index of a memory that mapped them.
graph.linksTo.resize(graph.ids.size());
std::set<std::pair<size_t, size_t> > seen;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
const Link & link = iter->second;
if(link.from() == link.to() || link.from() < 0 || link.to() < 0)
{
++graph.skippedLinks;
continue;
}
const size_t a = indexOf.at(link.from()), b = indexOf.at(link.to());
if(!seen.insert(std::make_pair(std::min(a,b), std::max(a,b))).second)
{
continue;
}
graph.linksTo[std::max(a,b)].push_back(std::make_pair(std::min(a,b), link.type()));
++graph.byType[link.type()];
}
return true;
}
// Adds one location and the links the graph has between it and the ones already there.
// Memory::update() links each new signature to the previous one as a kNeighbor, so that
// one is dropped when the graph does not have it, or has it with another type.
void addRealNode(
Memory * memory,
const RealGraph & graph,
size_t index,
std::vector<int> & newIds,
int * removedLinks = 0)
{
static const cv::Mat image(8, 8, CV_8UC1, cv::Scalar(128));
static const cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0001;
static const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1);
SensorData data(image);
UASSERT(memory->update(data, graph.poses.at(graph.ids[index]), covariance));
newIds.push_back(memory->getLastSignatureId());
bool previousLinked = false;
for(size_t i=0; i<graph.linksTo[index].size(); ++i)
{
const size_t other = graph.linksTo[index][i].first;
const Link::Type type = graph.linksTo[index][i].second;
if(other == index-1 && type == Link::kNeighbor)
{
previousLinked = true; // update() already made this one
continue;
}
memory->addLink(Link(newIds[index], newIds[other], type,
Transform::getIdentity(), information));
}
if(index > 0 && !previousLinked)
{
// Either the graph has no link between these two, or it has one of another type
// which the loop above has just added.
memory->removeLink(newIds[index-1], newIds[index]);
if(removedLinks)
{
++(*removedLinks);
}
}
}
Memory * newRealMemory()
{
ParametersMap params;
params.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1"));
params.insert(ParametersPair(Parameters::kMemSTMSize(), "1"));
params.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
params.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
return new Memory(params);
}
// What Rtabmap passes to the filter: the virtual place followed by the locations of the
// working memory that are not in the short term memory.
std::vector<int> bayesIdsOf(const Memory * memory)
{
std::vector<int> ids;
ids.push_back(Memory::kIdVirtual);
const std::set<int> & stm = memory->getStMem();
for(std::map<int, double>::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;
}
std::map<int, float> uniformLikelihoodOf(const std::vector<int> & ids)
{
std::map<int, float> likelihood;
for(size_t i=0; i<ids.size(); ++i)
{
likelihood.insert(std::make_pair(ids[i], 1.0f));
}
return likelihood;
}
const char * linkTypeName(int type)
{
switch(type)
{
case Link::kNeighbor: return "kNeighbor";
case Link::kGlobalClosure: return "kGlobalClosure";
case Link::kLocalSpaceClosure: return "kLocalSpaceClosure";
case Link::kLocalTimeClosure: return "kLocalTimeClosure";
case Link::kUserClosure: return "kUserClosure";
case Link::kVirtualClosure: return "kVirtualClosure";
case Link::kNeighborMerged: return "kNeighborMerged";
case Link::kPosePrior: return "kPosePrior";
case Link::kLandmark: return "kLandmark";
case Link::kGravity: return "kGravity";
default: return "other";
}
}
void printGraph(const std::string & path, const RealGraph & graph, int removedLinks)
{
std::cout << "[ ] " << path << ": read in " << graph.loadTime << "s, "
<< graph.ids.size() << " locations, links:";
for(std::map<int,int>::const_iterator iter=graph.byType.begin(); iter!=graph.byType.end(); ++iter)
{
std::cout << " " << linkTypeName(iter->first) << "=" << iter->second;
}
std::cout << " (" << graph.skippedLinks << " on a single location or on a landmark not rebuilt, "
<< removedLinks << " gaps in the chain)" << std::endl;
}
// The posterior as a map, which the filter no longer builds: it holds the locations and
// their probabilities as two vectors, and a test reads them more easily as a map.
static std::map<int, float> posteriorOf(const BayesFilter & filter)
{
const std::vector<int> & ids = filter.getPosteriorIds();
const std::vector<float> & values = filter.getPosteriorValues();
std::map<int, float> posterior;
for(size_t i = 0; i < ids.size(); ++i)
{
posterior.insert(posterior.end(), std::make_pair(ids[i], values[i]));
}
return posterior;
}
struct Result
{
double firstIteration = 0.0; // includes generating the prediction, sparse or dense
double steadyState = 0.0; // fastest of the following iterations, the prediction being unchanged
unsigned long memoryUsed = 0;
std::map<int, float> posterior;
};
Result run(const Memory * memory,
const std::vector<int> & ids,
const std::map<int, float> & likelihood,
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);
Result result;
UTimer timer;
filter.computePosterior(memory, likelihood);
result.firstIteration = timer.ticks();
// A few untimed iterations to let the caches and the processor clock settle before
// measuring. A fixed count, the same whatever the mode: the filter is recursive, so
// how many iterations it has run decides where its posterior is, and warming up for a
// fixed duration instead would run hundreds of them in the fast mode against one in
// the slow one and leave the two posteriors nowhere near each other.
for(int i=0; i<3; ++i)
{
filter.computePosterior(memory, likelihood);
}
// The fastest iteration rather than the mean or the median of them. Everything that
// makes an iteration slower than its own best is the machine rather than the code
// being measured, and the dense multiplication reads the whole prediction matrix from
// memory, which makes it sensitive to whatever else is using that memory. The fastest
// is the one measurement of the run that is the least of it, so it is the one that
// compares between runs and between machines.
double best = 0.0;
for(int i=1; i<iterations; ++i)
{
timer.restart();
filter.computePosterior(memory, likelihood);
const double elapsed = timer.ticks();
if(best == 0.0 || elapsed < best)
{
best = elapsed;
}
}
result.steadyState = best > 0.0 ? best : result.firstIteration;
result.memoryUsed = filter.getMemoryUsed();
result.posterior = posteriorOf(filter);
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<int, float> & a, const std::map<int, float> & b)
{
if(a.size() != b.size())
{
return 1.0;
}
double maxDiff = 0.0;
double maxValue = 0.0;
for(std::map<int, float>::const_iterator iter=a.begin(); iter!=a.end(); ++iter)
{
std::map<int, float>::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, fastest iteration %8.2f ms, filter memory %7.1f MB\n",
name, result.firstIteration*1000.0, result.steadyState*1000.0, result.memoryUsed/1048576.0);
}
// sparseFirst measures the sparse mode before the dense one. It matters on a large map:
// the dense mode allocates the prediction matrix, and running it first leaves the
// allocator holding hundreds of megabytes, which the sparse measurement that follows then
// pays for. Measuring the two in separate processes is the only way to have both clean;
// within one, the cheaper mode is the one to protect.
void compare(const Memory * memory,
const std::vector<int> & ids,
const std::map<int, float> & likelihood,
const char * predictionLC,
int iterations,
bool sparseFirst = false)
{
const size_t size = ids.size();
Result dense, sparse;
if(sparseFirst)
{
sparse = run(memory, ids, likelihood, predictionLC, true, iterations);
dense = run(memory, ids, likelihood, predictionLC, false, iterations);
}
else
{
dense = run(memory, ids, likelihood, predictionLC, false, iterations);
sparse = run(memory, ids, likelihood, 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 enough
// iterations for the fastest of them to be a stable number.
const int iterations = 30;
for(size_t s=0; s<sizeof(MAP_SIZES)/sizeof(int); ++s)
{
const int nodes = MAP_SIZES[s];
// A moderately connected graph, so that this measures the effect of the size.
// How much the connectivity itself matters is measured by the test below.
SyntheticMap map(nodes, 100, 500);
const size_t size = map.bayesIds().size();
std::cout << "[ ] " << nodes << " nodes (" << size << " ids with the virtual place), "
<< map.loopClosures() << " loop closures, graph built in " << map.buildTime()
<< "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
const std::vector<int> ids = map.bayesIds();
compare(map.memory(), ids, map.uniformLikelihood(ids), PREDICTION_DEFAULT, iterations);
}
}
// How sparse the prediction matrix is, and so how much the sparse multiplication can
// win, is decided by two things: how connected the graph is, every loop closure being
// a shortcut that getNeighborsId() follows, and how deep the prediction model reaches.
// The default 18 values model stores neighbors up to 17 links away with probabilities
// down to 6.9e-23, which are numerically irrelevant next to the 0.36 of the first
// level but fill most of the matrix.
TEST(BayesFilterPerfTest, SparsityAgainstGraphConnectivityAndModelDepth)
{
const int nodes = 4000;
const int iterations = 30;
struct Connectivity { const char * name; int loopEvery; int loopSpan; };
const Connectivity connectivities[] = {
{"chain only, no loop closure", 0, 0},
{"a loop closure every 100 nodes, spanning 500", 100, 500},
{"a loop closure every 20 nodes, spanning 200", 20, 200},
};
for(size_t c=0; c<sizeof(connectivities)/sizeof(Connectivity); ++c)
{
SyntheticMap map(nodes, connectivities[c].loopEvery, connectivities[c].loopSpan);
std::cout << "[ ] " << nodes << " nodes, " << connectivities[c].name
<< " (" << map.loopClosures() << " loop closures)" << std::endl;
const std::vector<int> ids = map.bayesIds();
const std::map<int, float> likelihood = map.uniformLikelihood(ids);
std::cout << "[ ] 18 values model (default, depth 17):" << std::endl;
compare(map.memory(), ids, likelihood, PREDICTION_DEFAULT, iterations);
std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl;
compare(map.memory(), ids, likelihood, PREDICTION_TRUNCATED, iterations);
}
}
// The size of a real large map, over which a localization session iterates without
// ever changing the graph: the prediction matrix is generated once and every
// following iteration reuses it, so the sparse view is built once too. This is the
// case the sparse multiplication is for.
//
// The dense matrix alone is a gigabyte at that size, and takes seconds to generate;
// exclude this one with
// bin/test_bayesfilter_perf --gtest_filter=-*LargeMap*
TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnALargeMap)
{
const int nodes = 16384;
const int iterations = 10;
SyntheticMap map(nodes, 100, 500);
const size_t size = map.bayesIds().size();
std::cout << "[ ] " << nodes << " nodes (" << size << " ids with the virtual place), "
<< map.loopClosures() << " loop closures, graph built in " << map.buildTime()
<< "s, dense matrix = " << (size*size*sizeof(float))/1048576 << " MB" << std::endl;
const std::vector<int> ids = map.bayesIds();
const std::map<int, float> likelihood = map.uniformLikelihood(ids);
std::cout << "[ ] 18 values model (default, depth 17):" << std::endl;
compare(map.memory(), ids, likelihood, PREDICTION_DEFAULT, iterations);
std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl;
compare(map.memory(), ids, likelihood, PREDICTION_TRUNCATED, iterations);
}
// Mapping mode, over a graph that has stopped growing: the matrix is kept, because
// updatePrediction() needs it to carry its unchanged columns over whenever the graph does
// grow, and the sparse form is taken from it rather than built instead of it. It is worth
// taking here because the prediction outlasts an iteration, which is what
// DenseVsSparsePredictionWhileMappingARealSession does not have: there a location is added
// on every iteration and the sparse form is never built at all.
TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMapping)
{
const int nodes = 4000;
const int iterations = 30;
SyntheticMap map(nodes, 100, 500, false /*stay in mapping mode*/);
const size_t size = map.bayesIds().size();
std::cout << "[ ] " << nodes << " nodes, mapping mode (the matrix is kept), "
<< map.loopClosures() << " loop closures, dense matrix = "
<< (size*size*sizeof(float))/1048576 << " MB" << std::endl;
const std::vector<int> ids = map.bayesIds();
compare(map.memory(), ids, map.uniformLikelihood(ids), PREDICTION_DEFAULT, iterations);
}
// The graphs of real maps, against the synthetic ones above, in localization mode where
// the graph is fixed. Two of them: one that went through the graph reduction, whose merged
// neighbor links cost a margin like ordinary neighbors, and one that did not.
//
// Needs the same ~1 GB as the largest synthetic map for the dense prediction, and reads
// the graphs from data/tests. Exclude with
// bin/test_bayesfilter_perf --gtest_filter=-*RealMap*
TEST(BayesFilterPerfTest, DenseVsSparsePredictionOnRealMaps)
{
const int iterations = 10;
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in, needed to read the graphs";
}
const char * files[] = {"large_reduced_graph.g2o", "large_mapping_session.g2o"};
const char * labels[] = {"graph reduction applied", "no graph reduction"};
for(size_t f=0; f<sizeof(files)/sizeof(const char *); ++f)
{
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/" + files[f];
if(!UFile::exists(path))
{
std::cout << "[ ] " << path << " not found, skipped" << std::endl;
continue;
}
RealGraph graph;
ASSERT_TRUE(loadRealGraph(path, graph)) << "could not read " << path;
Memory * memory = newRealMemory();
std::vector<int> newIds;
int removedLinks = 0;
UTimer timer;
for(size_t i=0; i<graph.ids.size(); ++i)
{
addRealNode(memory, graph, i, newIds, &removedLinks);
}
const double buildTime = timer.ticks();
ParametersMap localization;
localization.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
memory->parseParameters(localization);
ASSERT_FALSE(memory->isIncremental());
const std::vector<int> ids = bayesIdsOf(memory);
std::cout << "[ ] " << files[f] << " (" << labels[f] << ")" << std::endl;
printGraph(path, graph, removedLinks);
std::cout << "[ ] rebuilt in " << buildTime << "s, " << ids.size()
<< " locations (with the virtual place), dense matrix = "
<< (ids.size()*ids.size()*sizeof(float))/1048576 << " MB" << std::endl;
const std::map<int, float> likelihood = uniformLikelihoodOf(ids);
std::cout << "[ ] 18 values model (default, depth 17):" << std::endl;
compare(memory, ids, likelihood, PREDICTION_DEFAULT, iterations, /*sparseFirst=*/true);
std::cout << "[ ] 8 values model (depth 7, every value above 1e-4):" << std::endl;
compare(memory, ids, likelihood, PREDICTION_TRUNCATED, iterations, /*sparseFirst=*/true);
delete memory;
}
}
// A mapping session as it runs: a location added, then an iteration of the filter, over and
// over. Every added location changes the prediction, so this is the case the sparse form
// cannot amortize -- unlike localization, where it is built once and reused for the rest of
// the session. What it costs to keep it up to date against what its multiplication saves is
// what this measures.
//
// The session is replayed from its end: the locations before the window are added without
// running the filter, so the per-location cost is measured at the size the map really
// reaches rather than at the sizes it passes through.
TEST(BayesFilterPerfTest, DenseVsSparsePredictionWhileMappingARealSession)
{
const size_t window = 15; // locations added one at a time, with an iteration each
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in, needed to read the graph";
}
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/large_mapping_session.g2o";
if(!UFile::exists(path))
{
GTEST_SKIP() << path << " not found";
}
RealGraph graph;
ASSERT_TRUE(loadRealGraph(path, graph)) << "could not read " << path;
ASSERT_GT(graph.ids.size(), window);
const size_t prepared = graph.ids.size() - window;
std::cout << "[ ] large_mapping_session.g2o, mapping mode: " << prepared
<< " locations already mapped, " << window
<< " more added one at a time with an iteration of the filter each" << std::endl;
std::map<int, float> lastPosterior[2];
for(int sparse=1; sparse>=0; --sparse) // the sparse mode first, see compare()
{
Memory * memory = newRealMemory();
std::vector<int> newIds;
int removedLinks = 0;
for(size_t i=0; i<prepared; ++i)
{
addRealNode(memory, graph, i, newIds, &removedLinks);
}
ParametersMap params;
params.insert(ParametersPair(Parameters::kBayesSparsePrediction(), sparse?"true":"false"));
BayesFilter filter(params);
// The first iteration generates the whole prediction, as it does at the start of a
// session; the ones after it are what a mapping session pays per location.
std::vector<int> ids = bayesIdsOf(memory);
UTimer timer;
filter.computePosterior(memory, uniformLikelihoodOf(ids));
const double first = timer.ticks();
double total = 0.0, best = 0.0, worst = 0.0;
for(size_t i=prepared; i<graph.ids.size(); ++i)
{
addRealNode(memory, graph, i, newIds, &removedLinks);
ids = bayesIdsOf(memory);
timer.restart();
filter.computePosterior(memory, uniformLikelihoodOf(ids));
const double elapsed = timer.ticks();
total += elapsed;
if(best == 0.0 || elapsed < best) best = elapsed;
if(elapsed > worst) worst = elapsed;
}
lastPosterior[sparse] = posteriorOf(filter);
printf("[ ] %-6s first iteration %8.1f ms, then per added location: "
"fastest %8.1f ms, mean %8.1f ms, slowest %8.1f ms, filter memory %7.1f MB\n",
sparse?"sparse":"dense", first*1000.0, best*1000.0, total*1000.0/double(window),
worst*1000.0, filter.getMemoryUsed()/1048576.0);
delete memory;
}
EXPECT_LT(maxPosteriorDifference(lastPosterior[0], lastPosterior[1]), 1e-3);
}
corelib/test/test_bayesfilter.cpp
+853 -45
View File
File diff suppressed because it is too large Load Diff
corelib/test/test_graph.cpp
+257 -4
View File
@@ -8,6 +8,8 @@
#include <cmath>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
using namespace rtabmap;
@@ -922,10 +924,10 @@ void expectLinksNearEqual(
const auto idxB = index(b);
ASSERT_EQ(idxA.size(), idxB.size())
<< label << " unique (from,to) link pair count differs";
// Note: graph file formats (TORO / g2o) store edges generically and
// don't preserve rtabmap's Link::Type tag, so we only round-trip
// from / to / transform / infMatrix here. The loader assigns a
// placeholder type for ordinary edges.
// Note: TORO's text format stores edges generically and doesn't preserve
// rtabmap's Link::Type tag, so from / to / transform / infMatrix are all
// that round-trip there. g2o carries the type in a column of its own,
// which G2oRoundTripPreservesLinkTypes below checks.
//
// Landmark links in g2o are written as EDGE_SE3_TRACKXYZ (3D point
// observation): only the translation and the 3x3 translation block
@@ -1260,3 +1262,254 @@ INSTANTIATE_TEST_SUITE_P(
+ "_"
+ (std::get<2>(info.param) ? "rotPrior" : "posPrior");
});
// -------------------------------------------------------------------------
// The type of a link (a loop closure against an odometry link, and which kind
// of loop closure) decides how the graph is traversed: Memory::getNeighborsId()
// follows a global closure without spending any depth, skips a proximity one
// and spends a depth on a neighbor. The g2o format defines no field for it, so
// OptimizerG2O writes it as a column past the ones it defines, which its own
// loader reads back and g2o's ignores.
//
// Also checks that a link handed over in both directions, which is how Memory
// stores it, is written once: g2o reads two lines as two constraints and would
// count the information of the link twice.
// -------------------------------------------------------------------------
TEST(GraphG2oTest, G2oRoundTripPreservesLinkTypes)
{
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in";
}
const Link::Type types[] = {
Link::kNeighbor,
Link::kNeighborMerged,
Link::kGlobalClosure,
Link::kLocalSpaceClosure,
Link::kLocalTimeClosure,
Link::kUserClosure,
};
const size_t typeCount = sizeof(types)/sizeof(Link::Type);
std::map<int, Transform> poses;
for(size_t i=0; i<=typeCount; ++i)
{
poses.insert(std::make_pair((int)i+1, Transform((float)i, 0.0f, 0.0f, 0, 0, 0)));
}
const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64F) * 100.0;
std::multimap<int, Link> links;
for(size_t i=0; i<typeCount; ++i)
{
const int from = (int)i+1, to = (int)i+2;
const Transform t = poses.at(from).inverse() * poses.at(to);
// Both directions, as Memory holds them: one link stored on each of the
// two nodes it connects.
links.insert(std::make_pair(from, Link(from, to, types[i], t, infMatrix)));
links.insert(std::make_pair(to, Link(to, from, types[i], t.inverse(), infMatrix)));
}
const std::string path = test::tempPath(
uFormat("rtabmap_graph_link_types_%d.g2o", test::getPid()));
UFile::erase(path);
ASSERT_TRUE(graph::exportPoses(path, 4 /*g2o*/, poses, links));
ASSERT_TRUE(UFile::exists(path));
// One line per link, not two, and each one carrying its type last.
std::ifstream file(path.c_str());
std::string line;
std::map<std::pair<int,int>, int> written;
while(std::getline(file, line))
{
if(line.compare(0, 5, "EDGE_") != 0)
{
continue;
}
std::istringstream in(line);
std::string tag;
int from = 0, to = 0;
in >> tag >> from >> to;
std::string last;
while(in >> last) {}
const std::pair<int,int> pair(std::min(from,to), std::max(from,to));
EXPECT_TRUE(written.insert(std::make_pair(pair, atoi(last.c_str()))).second)
<< "link " << from << "->" << to << " written more than once";
}
ASSERT_EQ(written.size(), typeCount);
for(size_t i=0; i<typeCount; ++i)
{
EXPECT_EQ(written.at(std::make_pair((int)i+1, (int)i+2)), (int)types[i])
<< "type column of link " << i+1 << "->" << i+2;
}
// And read back as the types they were.
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, posesOut, &linksOut));
ASSERT_EQ(linksOut.size(), typeCount);
std::map<std::pair<int,int>, Link::Type> loaded;
for(std::multimap<int, Link>::const_iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
{
loaded.insert(std::make_pair(
std::make_pair(std::min(iter->second.from(), iter->second.to()),
std::max(iter->second.from(), iter->second.to())),
iter->second.type()));
}
for(size_t i=0; i<typeCount; ++i)
{
const std::pair<int,int> pair((int)i+1, (int)i+2);
ASSERT_TRUE(loaded.find(pair) != loaded.end()) << "link " << i+1 << "->" << i+2 << " missing";
EXPECT_EQ(loaded.at(pair), types[i]) << "type of link " << i+1 << "->" << i+2;
}
UFile::erase(path);
}
// A file without the type column, which is every file g2o itself writes and
// every one rtabmap wrote before, still loads: the type stays the one its tag
// implies, as it did.
TEST(GraphG2oTest, G2oWithoutTypeColumnStillLoads)
{
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in";
}
const std::string path = test::tempPath(
uFormat("rtabmap_graph_no_type_column_%d.g2o", test::getPid()));
UFile::erase(path);
{
std::ofstream file(path.c_str());
file << "VERTEX_SE2 1 0 0 0\n";
file << "VERTEX_SE2 2 1 0 0\n";
file << "EDGE_SE2 1 2 1 0 0 100 0 0 100 0 100\n";
}
std::map<int, Transform> poses;
std::multimap<int, Link> links;
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, poses, &links));
EXPECT_EQ(poses.size(), 2u);
ASSERT_EQ(links.size(), 1u);
EXPECT_EQ(links.begin()->second.from(), 1);
EXPECT_EQ(links.begin()->second.to(), 2);
UFile::erase(path);
}
// The switchable edges of vertigo, which saveGraph() writes for a link that is not a neighbor
// when Optimizer/Robust is enabled: the tag inserts the id of a switch vertex of its own
// before the fields of the link, so the type column lands one field further than on the
// ordinary tags and the writer and the loader have to agree on where it sits.
TEST(GraphG2oTest, G2oRoundTripsSwitchableEdges)
{
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in";
}
// EDGE_SE2_SWITCHABLE in 2D, EDGE_SE3_SWITCHABLE in 3D.
for(int slam2d = 1; slam2d >= 0; --slam2d)
{
SCOPED_TRACE(slam2d ? "slam2d" : "slam3d");
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform(0.0f, 0.0f, 0.0f, 0, 0, 0)));
poses.insert(std::make_pair(2, Transform(1.0f, 0.0f, 0.0f, 0, 0, 0)));
poses.insert(std::make_pair(3, Transform(2.0f, 0.0f, 0.0f, 0, 0, 0)));
const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64F) * 100.0;
std::multimap<int, Link> links;
// The odometry links are written as the ordinary tag whatever Optimizer/Robust is,
// and the loop closure as the switchable one.
const int pairs[][3] = {
{1, 2, Link::kNeighbor},
{2, 3, Link::kNeighbor},
{1, 3, Link::kGlobalClosure}};
for(size_t i = 0; i < sizeof(pairs)/sizeof(pairs[0]); ++i)
{
const int from = pairs[i][0], to = pairs[i][1];
const Link::Type type = (Link::Type)pairs[i][2];
const Transform t = poses.at(from).inverse() * poses.at(to);
links.insert(std::make_pair(from, Link(from, to, type, t, infMatrix)));
links.insert(std::make_pair(to, Link(to, from, type, t.inverse(), infMatrix)));
}
ParametersMap params;
params.insert(ParametersPair(Parameters::kOptimizerRobust(), "true"));
params.insert(ParametersPair(Parameters::kRegForce3DoF(), slam2d ? "true" : "false"));
const std::string path = test::tempPath(
uFormat("rtabmap_graph_switchable_%d_%d.g2o", slam2d, test::getPid()));
UFile::erase(path);
ASSERT_TRUE(graph::exportPoses(path, 4 /*g2o*/, poses, links, std::map<int, double>(), params));
// Written as the switchable tag, which is what puts the type column one field further.
const std::string switchableTag = slam2d ? "EDGE_SE2_SWITCHABLE" : "EDGE_SE3_SWITCHABLE";
int switchableLines = 0;
{
std::ifstream file(path.c_str());
std::string line;
while(std::getline(file, line))
{
if(line.compare(0, switchableTag.size(), switchableTag) == 0)
{
++switchableLines;
}
}
}
EXPECT_EQ(switchableLines, 1);
std::map<int, Transform> posesOut;
std::multimap<int, Link> linksOut;
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, posesOut, &linksOut));
EXPECT_EQ(posesOut.size(), poses.size()); // the switch vertices are none of the poses
ASSERT_EQ(linksOut.size(), sizeof(pairs)/sizeof(pairs[0]));
std::map<std::pair<int,int>, Link::Type> loaded;
for(std::multimap<int, Link>::const_iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
{
loaded.insert(std::make_pair(
std::make_pair(std::min(iter->second.from(), iter->second.to()),
std::max(iter->second.from(), iter->second.to())),
iter->second.type()));
}
for(size_t i = 0; i < sizeof(pairs)/sizeof(pairs[0]); ++i)
{
const std::pair<int,int> pair(pairs[i][0], pairs[i][1]);
ASSERT_TRUE(loaded.find(pair) != loaded.end())
<< "link " << pair.first << "->" << pair.second << " missing";
EXPECT_EQ(loaded.at(pair), (Link::Type)pairs[i][2])
<< "type of link " << pair.first << "->" << pair.second;
}
UFile::erase(path);
}
}
// A type column holding something that is not one of the types, which nothing rtabmap writes
// but another writer of the same format could: it is ignored and the type stays the one the
// tag implies, so a column that means something else elsewhere cannot turn a link into a type
// it is not.
TEST(GraphG2oTest, G2oOutOfRangeTypeColumnIsIgnored)
{
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
GTEST_SKIP() << "g2o optimizer not built in";
}
const std::string path = test::tempPath(
uFormat("rtabmap_graph_bad_type_column_%d.g2o", test::getPid()));
UFile::erase(path);
{
std::ofstream file(path.c_str());
file << "VERTEX_SE2 1 0 0 0\n";
file << "VERTEX_SE2 2 1 0 0\n";
file << "EDGE_SE2 1 2 1 0 0 100 0 0 100 0 100 4242\n";
}
std::map<int, Transform> poses;
std::multimap<int, Link> links;
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, poses, &links));
EXPECT_EQ(poses.size(), 2u);
ASSERT_EQ(links.size(), 1u);
EXPECT_EQ(links.begin()->second.type(), Link::kUndef); // the type EDGE_SE2 implies
UFile::erase(path);
}
corelib/test/test_rtabmap_integration.cpp
+541 -13
View File
@@ -14,6 +14,7 @@
// pass.
#include <gtest/gtest.h>
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/Features2d.h>
#include <rtabmap/core/camera/CameraImages.h>
@@ -45,6 +46,7 @@
#include "TestUtils.h"
#include <opencv2/imgcodecs.hpp>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <memory>
#include <string>
@@ -94,6 +96,7 @@ struct ReplayResult
int finalGlobalGraphSize = 0; // poses returned by Rtabmap::getGraph(global=true)
std::map<int, Transform> finalLocalPoses; // optimized, global=false
std::map<int, Transform> finalGlobalPoses; // optimized, global=true
std::multimap<int, Link> finalGlobalLinks; // constraints of the global graph
// Occupancy-grid cell counts after assembling the global grid from per-
// node local maps (only populated when RGBD/CreateOccupancyGrid=true).
int gridEmptyCells = 0;
@@ -110,6 +113,19 @@ struct ReplayResult
// Wall-clock seconds spent inside Odometry::process across all
// frames; divide by framesRead to get per-frame average.
double odomTotalSeconds = 0.0;
// The highest loop closure hypothesis of each node added, id and probability: the curve
// the Bayes filter draws over a session.
std::map<int, std::pair<int, float> > highestHypothesis;
// Timing/Posterior_computation (ms) over the frames that reported it: the prediction and
// the multiplication of the Bayes filter.
double posteriorMsSum = 0.0;
float posteriorMsMin = -1.0f;
float posteriorMsMax = 0.0f;
int posteriorSamples = 0;
float posteriorMsAvg() const
{
return posteriorSamples > 0 ? (float)(posteriorMsSum/(double)posteriorSamples) : -1.0f;
}
};
// Synchronous replay: DBReader -> Odometry::process -> Rtabmap::process.
@@ -415,8 +431,7 @@ ReplayResult replayDatabase(
rtabmap.getGraph(result.finalLocalPoses, constraints,
/*optimized=*/true, /*global=*/false);
result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
constraints.clear();
rtabmap.getGraph(result.finalGlobalPoses, constraints,
rtabmap.getGraph(result.finalGlobalPoses, result.finalGlobalLinks,
/*optimized=*/true, /*global=*/true);
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
}
@@ -550,7 +565,13 @@ ReplayResult replayDatabaseWithStoredOdom(
// When >0, points beyond this range (in meters) are dropped from
// the LaserScan of each SensorData after dbReader.takeData(),
// simulating a lidar with a tighter max range.
float scanMaxRange = 0.0f)
float scanMaxRange = 0.0f,
// Starts a new map on every frame whose stored odom covariance is the 9999 of a
// session start, as rtabmap-reprocess does. Off by default: a test wanting its own
// boundaries uses the frame above.
bool triggerNewMapOnSessionStart = false,
// Filled with the number of sessions replayed: one plus the boundaries triggered on.
int * sessionsReplayed = 0)
{
ReplayResult result;
@@ -656,6 +677,20 @@ ReplayResult replayDatabaseWithStoredOdom(
continue;
}
// Session boundary: the 9999 covariance of a session's first frame says its pose does
// not continue the previous one. Started before that frame is processed, so it is the
// first of the new session rather than the last of the old. Same as rtabmap-reprocess.
if(triggerNewMapOnSessionStart
&& result.framesProcessed > 0
&& info.odomCovariance.at<double>(0, 0) >= 9999.0)
{
rtabmap.triggerNewMap();
if(sessionsReplayed)
{
++*sessionsReplayed;
}
}
SensorData rtabmapData = data;
if(throttle)
{
@@ -705,6 +740,24 @@ ReplayResult replayDatabaseWithStoredOdom(
{
result.translationalRmseFinal = rmseIt->second;
}
const auto hypIdIt = stats.data().find(Statistics::kLoopHighest_hypothesis_id());
const auto hypValIt = stats.data().find(Statistics::kLoopHighest_hypothesis_value());
if(hypIdIt != stats.data().end() && hypValIt != stats.data().end() && stats.refImageId()>0)
{
result.highestHypothesis[stats.refImageId()] =
std::make_pair((int)hypIdIt->second, hypValIt->second);
}
const auto postIt = stats.data().find(Statistics::kTimingPosterior_computation());
if(postIt != stats.data().end())
{
result.posteriorMsSum += postIt->second;
result.posteriorMsMax = std::max(result.posteriorMsMax, postIt->second);
if(result.posteriorMsMin < 0.0f || postIt->second < result.posteriorMsMin)
{
result.posteriorMsMin = postIt->second;
}
++result.posteriorSamples;
}
data = dbReader.takeData(&info);
applyScanRangeFilter(data);
}
@@ -714,8 +767,7 @@ ReplayResult replayDatabaseWithStoredOdom(
rtabmap.getGraph(result.finalLocalPoses, constraints,
/*optimized=*/true, /*global=*/false);
result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
constraints.clear();
rtabmap.getGraph(result.finalGlobalPoses, constraints,
rtabmap.getGraph(result.finalGlobalPoses, result.finalGlobalLinks,
/*optimized=*/true, /*global=*/true);
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
}
@@ -733,12 +785,202 @@ ReplayResult replayDatabaseWithStoredOdom(
<< " localGraph=" << result.finalLocalGraphSize
<< " globalGraph=" << result.finalGlobalGraphSize
<< " rmse=" << result.translationalRmseFinal << "m"
<< " posterior(avg/min/max)=" << result.posteriorMsAvg()
<< "/" << result.posteriorMsMin << "/" << result.posteriorMsMax << "ms"
<< " wall=" << result.replayWallSeconds << "s"
<< std::endl;
return result;
}
// Union-find root, halving the path on the way up.
int graphComponentRoot(std::map<int, int> & parent, int id)
{
while(parent.at(id) != id)
{
const int up = parent.at(id);
parent.at(id) = parent.at(up);
id = up;
}
return id;
}
// How many pieces a graph is in. Two sessions that never closed a loop with each other are
// two pieces.
int countConnectedComponents(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links)
{
std::map<int, int> parent;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
parent.insert(std::make_pair(iter->first, iter->first));
}
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// A link to a landmark, or to a node the graph does not hold, joins nothing.
if(parent.find(iter->second.from()) == parent.end() ||
parent.find(iter->second.to()) == parent.end())
{
continue;
}
const int a = graphComponentRoot(parent, iter->second.from());
const int b = graphComponentRoot(parent, iter->second.to());
if(a != b)
{
parent.at(a) = b;
}
}
std::set<int> roots;
for(std::map<int, int>::const_iterator iter=parent.begin(); iter!=parent.end(); ++iter)
{
roots.insert(graphComponentRoot(parent, iter->first));
}
return (int)roots.size();
}
// What replaying a database asks for on top of the parameters it was recorded with, kept as
// little as possible: the dictionary and the searches stay as they were recorded, so what the
// replay does can be held against what the session did.
ParametersMap replayParams(const ParametersMap & recordedWith, const std::string & srcPath)
{
ParametersMap params = recordedWith;
// Its nodes are already the ones its detection rate kept, so every frame is processed.
uInsert(params, ParametersPair(Parameters::kRtabmapDetectionRate(), "0"));
// The features stored with each node: this database keeps no images to extract them from.
uInsert(params, ParametersPair(Parameters::kMemUseOdomFeatures(), "true"));
uInsert(params, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "false"));
return params;
}
// The highest loop closure hypothesis the recorded session saw at each node, id and value,
// from the statistics it saved. That is the curve a replay is compared against.
std::map<int, std::pair<int, float> > loadDatabaseHighestHypothesis(const std::string & path)
{
std::map<int, std::pair<int, float> > hypothesis;
DBDriver * driver = DBDriver::create();
if(!driver->openConnection(path))
{
delete driver;
return hypothesis;
}
std::set<int> ids;
driver->getAllNodeIds(ids);
for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
double stamp = 0.0;
const std::map<std::string, float> stats = driver->getStatistics(*iter, stamp);
const std::map<std::string, float>::const_iterator idIter =
stats.find(Statistics::kLoopHighest_hypothesis_id());
const std::map<std::string, float>::const_iterator valueIter =
stats.find(Statistics::kLoopHighest_hypothesis_value());
if(idIter != stats.end() && valueIter != stats.end())
{
hypothesis[*iter] = std::make_pair((int)idIter->second, valueIter->second);
}
}
driver->closeConnection(false);
delete driver;
return hypothesis;
}
// How a replay's highest hypothesis per node stands against the recorded one.
struct HypothesisComparison
{
int nodes = 0; ///< nodes compared, of the ones both curves have
int sameId = 0; ///< of those, the ones pointing at the very same location
int samePlace = 0; ///< and the ones pointing at a location within a meter of it
int alsoOverThreshold = 0; ///< and the ones the replay also took past the threshold
float meanAbsValue = 0.0f; ///< mean |value - golden value|
float maxAbsValue = 0.0f;
float sameIdRatio() const {return nodes>0 ? float(sameId)/float(nodes) : 0.0f;}
float samePlaceRatio() const {return nodes>0 ? float(samePlace)/float(nodes) : 0.0f;}
float overThresholdRatio() const {return nodes>0 ? float(alsoOverThreshold)/float(nodes) : 0.0f;}
};
// Two hypotheses are on the same place when the locations they point at are this close in the
// optimized graph. Ids cannot say that on a map of three passes over the same trajectory: the
// same corner is a node of each pass, hundreds of ids apart. The nodes are ~0.3 m apart along
// the path, so a meter is a handful of them, and the map is only 24 m by 33 m: a wider radius
// would call most of it the same place.
const float kHypothesisSamePlaceRadius = 1.0f; // meters
// Only the nodes where the recorded session had a hypothesis at or past Rtabmap/LoopThr are
// compared: under it the value is spread thinly over the working memory and which location
// comes out highest is noise.
HypothesisComparison compareHighestHypothesis(
const std::map<int, std::pair<int, float> > & golden,
const std::map<int, std::pair<int, float> > & replayed,
const std::map<int, Transform> & poses,
float loopThreshold)
{
HypothesisComparison c;
double sum = 0.0;
for(std::map<int, std::pair<int, float> >::const_iterator iter=golden.begin(); iter!=golden.end(); ++iter)
{
if(iter->second.first <= 0 || iter->second.second < loopThreshold)
{
continue;
}
const std::map<int, std::pair<int, float> >::const_iterator jter = replayed.find(iter->first);
if(jter == replayed.end())
{
continue;
}
++c.nodes;
if(jter->second.second >= loopThreshold)
{
++c.alsoOverThreshold;
}
const int goldenId = iter->second.first;
const int replayedId = jter->second.first;
if(goldenId == replayedId)
{
++c.sameId;
}
bool samePlace = goldenId == replayedId;
if(!samePlace && goldenId > 0 && replayedId > 0)
{
const std::map<int, Transform>::const_iterator goldenPose = poses.find(goldenId);
const std::map<int, Transform>::const_iterator replayedPose = poses.find(replayedId);
if(goldenPose != poses.end() && replayedPose != poses.end())
{
samePlace = goldenPose->second.getDistance(replayedPose->second)
< kHypothesisSamePlaceRadius;
}
}
if(samePlace)
{
++c.samePlace;
}
const float diff = fabs(iter->second.second - jter->second.second);
sum += diff;
c.maxAbsValue = std::max(c.maxAbsValue, diff);
}
c.meanAbsValue = c.nodes>0 ? (float)(sum/(double)c.nodes) : 0.0f;
return c;
}
// The optimized graph a database converged to, which a replay is compared against, and the
// parameters it was recorded with, which the replay runs with.
bool loadDatabaseGraphAndParameters(
const std::string & path,
std::map<int, Transform> & optimizedPoses,
ParametersMap & parameters)
{
DBDriver * driver = DBDriver::create();
if(!driver->openConnection(path))
{
delete driver;
return false;
}
optimizedPoses = driver->loadOptimizedPoses();
parameters = driver->getLastParameters();
driver->closeConnection(false);
delete driver;
return !optimizedPoses.empty();
}
ParametersMap baseRtabmapParams()
{
ParametersMap params;
@@ -1990,16 +2232,302 @@ TEST_F(RtabmapIntegrationFixture, Loop3ItGps)
}
}
// ---------------------------------------------------------------------------
// Appearance-only loop closure on the 84-image `data/samples` set with the
// shipped `data/samples_GT.bmp` ground truth. Measures recall at 100%
// precision (the rtabmap "max recall while no false positive has appeared
// yet" metric — same definition as the legacy MATLAB getPrecisionRecall.m
// script) for every Features2D detector strategy that is available in this
// build. Detector strategies for which Feature2D::create() silently
// substitutes a different backend (e.g. SURF -> SIFT without nonfree,
// SuperPointTorch -> GFTT/ORB without RTABMAP_TORCH) are skipped.
// Multi-session 2D lidar + SIFT (3 sessions, 935 nodes, ~270 m).
//
// The reference is the optimized graph the database holds: the same frames with the same
// parameters have to find about as many loop closures. The replay reuses the features stored
// with each node, the database keeping no images, and the ICP registration verifies loop
// closures on the scans it does keep.
//
// Run over both forms of the Bayes prediction, matrix and sparse: the same probabilities, so
// a real session has to come out the same either way.
// ---------------------------------------------------------------------------
TEST_F(RtabmapIntegrationFixture, Multisession3It)
{
const std::string srcPath = testDataPath("multisession_3it.db");
SKIP_IF_MISSING(srcPath);
std::map<int, Transform> goldenPoses;
ParametersMap dbParams;
ASSERT_TRUE(loadDatabaseGraphAndParameters(srcPath, goldenPoses, dbParams))
<< "No optimized graph in " << srcPath;
std::cerr << "[ ] Reference graph: " << goldenPoses.size()
<< " poses, recorded with " << dbParams.size() << " parameters\n";
for(const bool sparsePrediction : {false, true})
{
const std::string label = sparsePrediction ? "sparse" : "dense";
SCOPED_TRACE(label);
ParametersMap params = replayParams(dbParams, srcPath);
uInsert(params, ParametersPair(Parameters::kBayesSparsePrediction(),
sparsePrediction ? "true" : "false"));
// The database carries the cap it was recorded with; this test is the run without it.
uInsert(params, ParametersPair(Parameters::kRtabmapMemoryThr(), "0"));
const std::string workDb = test::tempPath(uFormat(
"rtabmap_integration_Multisession3It_%s.db", label.c_str()));
std::cerr << "Working DB for " << label << ": " << workDb << "\n";
int sessions = 1;
const ReplayResult result = replayDatabaseWithStoredOdom(
srcPath, workDb, params,
/*triggerNewMapAfterFrame=*/-1,
/*overrideOdomAngularVariance=*/-1.0,
/*overrideOdomLinearVariance=*/-1.0,
/*scanMaxRange=*/0.0f,
/*triggerNewMapOnSessionStart=*/true,
&sessions);
ASSERT_GT(result.framesProcessed, 0) << label << " produced no frames";
ASSERT_GT(result.finalGlobalGraphSize, 0) << label << " produced an empty graph";
float tRmse=0, tMean=0, tMed=0, tStd=0, tMin=0, tMax=0;
float rRmse=0, rMean=0, rMed=0, rStd=0, rMin=0, rMax=0;
graph::calcRMSE(goldenPoses, result.finalGlobalPoses,
tRmse, tMean, tMed, tStd, tMin, tMax,
rRmse, rMean, rMed, rStd, rMin, rMax,
/*align2D=*/false);
std::cerr << "[" << label << "] sessions=" << sessions
<< " nodes=" << result.finalGlobalGraphSize
<< " loops=" << result.loopClosuresAccepted
<< " rejected=" << result.loopClosuresRejected
<< " proximity=" << result.proximityDetections
<< " trans rmse=" << tRmse << "m max=" << tMax << "m"
<< " rot rmse=" << rRmse << "deg max=" << rMax << "deg"
<< " posterior avg=" << result.posteriorMsAvg()
<< "ms min=" << result.posteriorMsMin
<< "ms max=" << result.posteriorMsMax << "ms\n";
// Every frame is a node the database kept, so the replay makes as many, in 3 sessions.
EXPECT_EQ(sessions, 3) << label;
EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << label;
// Loop closures are what this compares; the trajectory is printed but not asserted,
// following from which closures a run happens to find. And a run is never the same
// twice: the registration accepts or rejects a hypothesis sitting on its threshold
// from one run to the next, and one closure changes the next ones. Hence a band.
EXPECT_GT(result.loopClosuresAccepted, 265) << label << " found too few loop closures";
EXPECT_LT(result.loopClosuresAccepted, 305) << label << " found more loop closures than the reference";
}
}
// ---------------------------------------------------------------------------
// The same replay under memory management: Rtabmap/MemoryThr caps the working memory, so the
// oldest nodes go to long-term memory as the map grows and only a window is ever held. The
// global optimized graph still covers every node, and the loop closures found from a limited
// working memory are fewer.
// ---------------------------------------------------------------------------
TEST_F(RtabmapIntegrationFixture, Multisession3ItMemoryThr)
{
const std::string srcPath = testDataPath("multisession_3it.db");
SKIP_IF_MISSING(srcPath);
std::map<int, Transform> goldenPoses;
ParametersMap dbParams;
ASSERT_TRUE(loadDatabaseGraphAndParameters(srcPath, goldenPoses, dbParams))
<< "No optimized graph in " << srcPath;
// The database was recorded with the same 300-node cap, so the highest hypothesis it saw
// at each node is the curve to compare against.
const std::map<int, std::pair<int, float> > goldenHypothesis =
loadDatabaseHighestHypothesis(srcPath);
ASSERT_GT(goldenHypothesis.size(), 900u) << "No saved statistics in " << srcPath;
// Under this a hypothesis is not worth comparing.
float loopThreshold = Parameters::defaultRtabmapLoopThr();
Parameters::parse(dbParams, Parameters::kRtabmapLoopThr(), loopThreshold);
ASSERT_GT(loopThreshold, 0.0f);
std::cerr << "[ ] Comparing hypotheses at or over " << Parameters::kRtabmapLoopThr()
<< "=" << loopThreshold << "\n";
struct Variant
{
// "" leaves Bayes/SparsePrediction at its default, which is the sparse form.
std::string sparsePrediction;
// "" leaves the retrieval parameter at its default, which is 2 locations.
std::string maxLocalRetrieved;
std::string maxRetrieved;
int minLoops;
int maxLoops;
int minLocalGraph;
int maxLocalGraph;
// Of the nodes the recorded session had a hypothesis on, the least that must point at
// the same place, and the most the probability may differ by on average.
float minSamePlaceRatio;
float maxMeanValueDiff;
std::string label;
};
// The first two are the same run over both forms of the prediction, sharing their band:
// which form holds it must not change what the session does. The last three hold the form
// at its default and take the retrieval apart: none, local only, and both, which is what a
// session runs with out of the box.
//
// The bands come from six runs of each. Observed:
//
// variant loops working same place also over value diff
// memory as recorded threshold mean
// sparse 268-288 242-263 80-85% 86-91% 0.051-0.075
// dense 267-284 253-266 81-84% 86-91% 0.052-0.084
// no-retrieval 204-213 229-256 68-71% 81-86% 0.103-0.119
// local-retrieval-only 240-259 262-267 76-81% 85-91% 0.071-0.081
// both-retrieval 265-287 261-268 82-84% 89-92% 0.031-0.053
//
// The closest to the recorded session is the one configured like it, both retrievals on,
// and the furthest is the one with none: what comes back into the working memory is what
// the hypotheses are drawn over.
//
// The loop counts move with the environment, the feature extraction not seeing quite the
// same thing: local-retrieval-only has since been seen at 259 and at 271, over the 265-287
// of both-retrieval, which is why the two are no longer ordered on the count. The value
// diff keeps its order everywhere it has been run.
//
// The posterior time each run prints is left unasserted: it is what these variants are
// measured for, but also what a loaded runner moves most.
const std::vector<Variant> variants = {
{"true", "0", "", 245, 310, 225, 290, 0.72f, 0.11f, "sparse" },
{"false", "0", "", 245, 310, 225, 290, 0.72f, 0.11f, "dense" },
{"", "0", "0", 180, 240, 210, 280, 0.60f, 0.16f, "no-retrieval" },
{"", "2", "0", 215, 285, 240, 290, 0.68f, 0.11f, "local-retrieval-only"},
{"", "2", "2", 240, 310, 240, 295, 0.74f, 0.08f, "both-retrieval" },
};
// Loop closures and hypothesis agreement per variant, for the ordering between them.
std::map<std::string, int> loopsPerVariant;
std::map<std::string, float> valueDiffPerVariant;
for(const Variant & v : variants)
{
SCOPED_TRACE(v.label);
ParametersMap params = replayParams(dbParams, srcPath);
uInsert(params, ParametersPair(Parameters::kRtabmapMemoryThr(), "300"));
if(!v.sparsePrediction.empty())
{
uInsert(params, ParametersPair(Parameters::kBayesSparsePrediction(), v.sparsePrediction));
}
if(!v.maxLocalRetrieved.empty())
{
uInsert(params, ParametersPair(Parameters::kRGBDMaxLocalRetrieved(), v.maxLocalRetrieved));
}
if(!v.maxRetrieved.empty())
{
uInsert(params, ParametersPair(Parameters::kRtabmapMaxRetrieved(), v.maxRetrieved));
}
const std::string workDb = test::tempPath(uFormat(
"rtabmap_integration_Multisession3ItMemoryThr_%s.db", v.label.c_str()));
std::cerr << "Working DB for " << v.label << ": " << workDb << "\n";
int sessions = 1;
const ReplayResult result = replayDatabaseWithStoredOdom(
srcPath, workDb, params,
/*triggerNewMapAfterFrame=*/-1,
/*overrideOdomAngularVariance=*/-1.0,
/*overrideOdomLinearVariance=*/-1.0,
/*scanMaxRange=*/0.0f,
/*triggerNewMapOnSessionStart=*/true,
&sessions);
ASSERT_GT(result.framesProcessed, 0) << v.label << " produced no frames";
ASSERT_GT(result.finalGlobalGraphSize, 0) << v.label << " produced an empty graph";
const int components = countConnectedComponents(
result.finalGlobalPoses, result.finalGlobalLinks);
float tRmse=0, tMean=0, tMed=0, tStd=0, tMin=0, tMax=0;
float rRmse=0, rMean=0, rMed=0, rStd=0, rMin=0, rMax=0;
graph::calcRMSE(goldenPoses, result.finalGlobalPoses,
tRmse, tMean, tMed, tStd, tMin, tMax,
rRmse, rMean, rMed, rStd, rMin, rMax,
/*align2D=*/false);
std::cerr << "[" << v.label << "] sessions=" << sessions
<< " nodes=" << result.finalGlobalGraphSize
<< " components=" << components
<< " localGraph=" << result.finalLocalGraphSize
<< " loops=" << result.loopClosuresAccepted
<< " rejected=" << result.loopClosuresRejected
<< " proximity=" << result.proximityDetections
<< " trans rmse=" << tRmse << "m max=" << tMax << "m"
<< " rot rmse=" << rRmse << "deg max=" << rMax << "deg"
<< " posterior avg=" << result.posteriorMsAvg()
<< "ms min=" << result.posteriorMsMin
<< "ms max=" << result.posteriorMsMax << "ms\n";
// The highest hypothesis of each node against the one the recorded session saw:
// the same locations pointed at, with the same probability on them.
const HypothesisComparison hyp = compareHighestHypothesis(
goldenHypothesis, result.highestHypothesis, goldenPoses, loopThreshold);
std::cerr << "[" << v.label << "] hypothesis over " << hyp.nodes
<< " nodes the recorded session had one on: same id " << hyp.sameId
<< " (" << 100.0f*hyp.sameIdRatio() << "%), same place " << hyp.samePlace
<< " (" << 100.0f*hyp.samePlaceRatio() << "%), also over the threshold "
<< hyp.alsoOverThreshold << " (" << 100.0f*hyp.overThresholdRatio()
<< "%), value diff mean=" << hyp.meanAbsValue
<< " max=" << hyp.maxAbsValue << "\n";
// Nothing is lost: the nodes go to long-term memory and the global graph still covers
// every one, in one piece. The three sessions are three passes over the same
// trajectory and 300 nodes is wide enough to still hold the end of one when the next
// starts over the same place, so the link across the boundary is found even with
// nothing coming back.
EXPECT_EQ(sessions, 3) << v.label;
EXPECT_EQ(result.finalGlobalGraphSize, (int)goldenPoses.size()) << v.label;
EXPECT_EQ(components, 1) << v.label << " graph came out in pieces";
// The capped part stays capped. Anything near 935 would mean the cap never took effect
// and the test is no longer about memory management.
EXPECT_GT(result.finalLocalGraphSize, v.minLocalGraph) << v.label;
EXPECT_LT(result.finalLocalGraphSize, v.maxLocalGraph)
<< v.label << " working memory was not capped";
// Loop closures are only found against what the working memory holds.
EXPECT_GT(result.loopClosuresAccepted, v.minLoops) << v.label << " found too few loop closures";
EXPECT_LT(result.loopClosuresAccepted, v.maxLoops) << v.label << " found more loop closures than expected";
// The trajectory is printed but not asserted, following from which closures a run
// happens to find.
// The recorded session had a hypothesis worth the name on 562 of its 935 nodes; the
// replay is held against those. Not the same one every time -- the sessions part ways
// as soon as they accept a different closure, and what a capped working memory holds
// follows from that -- but the same place on three quarters or more, and past the
// threshold on nine tenths.
EXPECT_GT(hyp.nodes, 400) << v.label << " compared too few nodes";
EXPECT_GT(hyp.samePlaceRatio(), v.minSamePlaceRatio)
<< v.label << " points at other places than the recorded session";
EXPECT_LT(hyp.meanAbsValue, v.maxMeanValueDiff)
<< v.label << " hypothesis probabilities drifted from the recorded session";
EXPECT_GT(hyp.overThresholdRatio(), 0.70f)
<< v.label << " left the recorded session's hypotheses under the threshold";
loopsPerVariant[v.label] = result.loopClosuresAccepted;
valueDiffPerVariant[v.label] = hyp.meanAbsValue;
}
// Whatever a run does inside its band, retrieving nothing finds the fewest closures: the
// hypotheses are drawn over the working memory, and nothing comes back into it.
ASSERT_EQ(loopsPerVariant.size(), 5u);
EXPECT_GT(loopsPerVariant.at("local-retrieval-only"), loopsPerVariant.at("no-retrieval"));
EXPECT_GT(loopsPerVariant.at("both-retrieval"), loopsPerVariant.at("no-retrieval"));
EXPECT_GT(loopsPerVariant.at("sparse"), loopsPerVariant.at("no-retrieval"));
EXPECT_GT(loopsPerVariant.at("dense"), loopsPerVariant.at("no-retrieval"));
// Local retrieval against both is not asserted on the count: the upper tail of the one
// reaches into the band of the other, which a run on another machine walks into (271
// against 269) while both stay inside their own bands. Only a collapse is caught here.
EXPECT_GE(loopsPerVariant.at("both-retrieval"), loopsPerVariant.at("local-retrieval-only") - 15);
// What the retrieval buys is asserted on the hypotheses, where the three are ordered with
// room to spare -- the gaps are twice the spread of a variant: what comes back into the
// working memory is what the hypotheses are drawn over, so bringing back what the
// likelihood points at lands closest to the recorded session.
EXPECT_LT(valueDiffPerVariant.at("both-retrieval"), valueDiffPerVariant.at("local-retrieval-only"));
EXPECT_LT(valueDiffPerVariant.at("local-retrieval-only"), valueDiffPerVariant.at("no-retrieval"));
}
TEST_F(RtabmapIntegrationFixture, AppearanceOnly_PrecisionRecall)
{
const std::string samplesDir = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";