i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Small refactoring of the Bayes filter

Browse Source
This commit is contained in:
matlabbe
2015-06-22 13:55:15 -04:00
parent 290df19cc6
commit 91a4506956
3 changed files with 5 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/src/BayesFilter.cpp
View File

@@ -38,7 +38,6 @@ namespace rtabmap {
BayesFilter::BayesFilter(const ParametersMap & parameters) : BayesFilter::BayesFilter(const ParametersMap & parameters) :
_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()), _virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()), _fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
_badSignaturesIgnored(Parameters::defaultRtabmapCreateIntermediateNodes()),
_totalPredictionLCValues(0.0f) _totalPredictionLCValues(0.0f)
{ {
this->setPredictionLC(Parameters::defaultBayesPredictionLC()); this->setPredictionLC(Parameters::defaultBayesPredictionLC());
@@ -57,7 +56,6 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
} }
Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior); Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior);
Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate); Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate);
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _badSignaturesIgnored);
UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f); UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f);
} }
@@ -262,7 +260,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
// Set high values (gaussians curves) to loop closure neighbors // Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors // ADD prob for each neighbors
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true);
std::list<int> idsLoopMargin; std::list<int> idsLoopMargin;
//filter neighbors in STM //filter neighbors in STM
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();) for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -476,7 +474,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
} }
if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i])) if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
{ {
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true);
float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap); float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
this->normalize(prediction, i, sum, newIds[0]<0); this->normalize(prediction, i, sum, newIds[0]<0);
++added; ++added;
@@ -496,7 +494,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
int modified = 0; int modified = 0;
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{ {
std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
int index = newIdToIndexMap.at(*iter); int index = newIdToIndexMap.at(*iter);
float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
this->normalize(prediction, index, sum, newIds[0]<0); this->normalize(prediction, index, sum, newIds[0]<0);

2
corelib/src/BayesFilter.h
View File

@@ -58,7 +58,6 @@ public:
float getVirtualPlacePrior() const {return _virtualPlacePrior;} float getVirtualPlacePrior() const {return _virtualPlacePrior;}
const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...} const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...} std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
bool isBadSignaturesIgnored() const {return _badSignaturesIgnored;}
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const; cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
@@ -80,7 +79,6 @@ private:
float _virtualPlacePrior; float _virtualPlacePrior;
std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...} std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
bool _fullPredictionUpdate; bool _fullPredictionUpdate;
bool _badSignaturesIgnored;
float _totalPredictionLCValues; float _totalPredictionLCValues;
}; };

5
corelib/src/Rtabmap.cpp
View File

@@ -1116,7 +1116,6 @@ bool Rtabmap::process(
//============================================================ //============================================================
ULOGGER_INFO("computing likelihood..."); ULOGGER_INFO("computing likelihood...");
// select only not empty signatures (may happen often if intermediate nodes are created)
std::list<int> signaturesToCompare; std::list<int> signaturesToCompare;
for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin(); for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
iter!=_memory->getWorkingMem().end(); iter!=_memory->getWorkingMem().end();
@@ -1126,7 +1125,7 @@ bool Rtabmap::process(
{ {
const Signature * s = _memory->getSignature(iter->first); const Signature * s = _memory->getSignature(iter->first);
UASSERT(s!=0); UASSERT(s!=0);
if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) if(s->getWeight() != -1) // ignore intermediate nodes
{ {
signaturesToCompare.push_back(iter->first); signaturesToCompare.push_back(iter->first);
} }
@@ -2780,7 +2779,7 @@ void Rtabmap::dumpPrediction() const
{ {
const Signature * s = _memory->getSignature(iter->first); const Signature * s = _memory->getSignature(iter->first);
UASSERT(s!=0); UASSERT(s!=0);
if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) if(s->getWeight() != -1) // ignore intermediate nodes
{ {
signaturesToCompare.push_back(iter->first); signaturesToCompare.push_back(iter->first);
} }