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

Fixed broken incremental prediction matrix from commit 977d21e

Browse Source
This commit is contained in:
matlabbe
2018-02-07 18:14:34 -05:00
parent a320eb5d8e
commit e7ceacc215
1 changed files with 13 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
corelib/src/BayesFilter.cpp
View File

@@ -561,6 +561,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
_neighborsIndex.insert(std::make_pair(newIds[i], neighbors)); _neighborsIndex.insert(std::make_pair(newIds[i], neighbors));
} }
const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]); 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); float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap);
this->normalize(prediction, i, sum, newIds[0]<0); this->normalize(prediction, i, sum, newIds[0]<0);
@@ -587,25 +588,23 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{ {
int id = *iter; int id = *iter;
if(id > 0 && id<(int)newIdToIndexMap.size()) if(id > 0)
{ {
int index = newIdToIndexMap.at(id); int index = newIdToIndexMap.at(id);
if(index > 0) e0 = t1.ticks();
{ std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id);
e0 = t1.ticks(); UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str());
std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id); const std::map<int, int> & neighbors = kter->second;
UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str()); //std::map<int, int> neighbors = memory->getNeighborsId(id, _predictionLC.size()-1, 0, false, false, true, true);
const std::map<int, int> & neighbors = kter->second; e1+=t1.ticks();
e1+=t1.ticks();
float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap); float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap);
e3+=t1.ticks(); e3+=t1.ticks();
this->normalize(prediction, index, sum, newIds[0]<0); this->normalize(prediction, index, sum, newIds[0]<0);
++modified; ++modified;
e4+=t1.ticks(); e4+=t1.ticks();
}
} }
} }
UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4); UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);