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 vector out of bound error (findNN() matchesNotIndexed vector). Fixed error "map correction should be identity" when small displacement detected before scan matching

Browse Source
This commit is contained in:
matlabbe
2015-09-09 20:11:18 -04:00
parent 9a037b33ff
commit e3161c3b04
2 changed files with 26 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/Rtabmap.cpp
View File

@@ -1052,7 +1052,7 @@ bool Rtabmap::process(
// Update optimizedPoses with the newly added node // Update optimizedPoses with the newly added node
Transform newPose; Transform newPose;
if(signature->getLinks().size() == 1) if(signature->getLinks().size() == 1 && !smallDisplacement)
{ {
int oldId = signature->getLinks().begin()->first; int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId); const Signature * oldS = _memory->getSignature(oldId);
@@ -1112,7 +1112,10 @@ bool Rtabmap::process(
_mapCorrection = newPose * signature->getPose().inverse(); _mapCorrection = newPose * signature->getPose().inverse();
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{ {
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
_mapCorrection.prettyPrint().c_str(),
newPose.prettyPrint().c_str(),
signature->getPose().prettyPrint().c_str());
} }
} }
else else

21
corelib/src/VWDictionary.cpp
View File

@@ -1193,17 +1193,20 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
} }
// not indexed.. // not indexed..
for(unsigned int j=0; j<matchesNotIndexed.at(i).size(); ++j) if(matchesNotIndexed.size())
{ {
float d = matchesNotIndexed.at(i).at(j).distance; for(unsigned int j=0; j<matchesNotIndexed.at(i).size(); ++j)
int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
if(d >= 0.0f && id > 0)
{ {
fullResults.insert(std::pair<float, int>(d, id)); float d = matchesNotIndexed.at(i).at(j).distance;
} int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
else if(d >= 0.0f && id > 0)
{ {
break; fullResults.insert(std::pair<float, int>(d, id));
}
else
{
break;
}
} }
} }