i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 17:47:49 +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/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());
}
}
}