diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index 56e9485b..e80f142f 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -38,7 +38,6 @@ namespace rtabmap { BayesFilter::BayesFilter(const ParametersMap & parameters) : _virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()), _fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()), - _badSignaturesIgnored(Parameters::defaultRtabmapCreateIntermediateNodes()), _totalPredictionLCValues(0.0f) { this->setPredictionLC(Parameters::defaultBayesPredictionLC()); @@ -57,7 +56,6 @@ void BayesFilter::parseParameters(const ParametersMap & parameters) } Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior); Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate); - Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _badSignaturesIgnored); 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 // ADD prob for each neighbors - std::map neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); + std::map neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true); std::list idsLoopMargin; //filter neighbors in STM for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end();) @@ -476,7 +474,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, } if(i neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); + std::map neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true); float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap); this->normalize(prediction, i, sum, newIds[0]<0); ++added; @@ -496,7 +494,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, int modified = 0; for(std::set::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) { - std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); + std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true); int index = newIdToIndexMap.at(*iter); float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); this->normalize(prediction, index, sum, newIds[0]<0); diff --git a/corelib/src/BayesFilter.h b/corelib/src/BayesFilter.h index 4d00cd76..24d85a88 100644 --- a/corelib/src/BayesFilter.h +++ b/corelib/src/BayesFilter.h @@ -58,7 +58,6 @@ public: float getVirtualPlacePrior() const {return _virtualPlacePrior;} const std::vector & getPredictionLC() const; // {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 & ids) const; @@ -80,7 +79,6 @@ private: float _virtualPlacePrior; std::vector _predictionLC; // {Vp, Lc, l1, l2, l3, l4...} bool _fullPredictionUpdate; - bool _badSignaturesIgnored; float _totalPredictionLCValues; }; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 9b3cda7d..fbab3e72 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1116,7 +1116,6 @@ bool Rtabmap::process( //============================================================ ULOGGER_INFO("computing likelihood..."); - // select only not empty signatures (may happen often if intermediate nodes are created) std::list signaturesToCompare; for(std::map::const_iterator iter=_memory->getWorkingMem().begin(); iter!=_memory->getWorkingMem().end(); @@ -1126,7 +1125,7 @@ bool Rtabmap::process( { const Signature * s = _memory->getSignature(iter->first); UASSERT(s!=0); - if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) + if(s->getWeight() != -1) // ignore intermediate nodes { signaturesToCompare.push_back(iter->first); } @@ -2780,7 +2779,7 @@ void Rtabmap::dumpPrediction() const { const Signature * s = _memory->getSignature(iter->first); UASSERT(s!=0); - if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) + if(s->getWeight() != -1) // ignore intermediate nodes { signaturesToCompare.push_back(iter->first); }