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
Transform newPose;
if(signature->getLinks().size() == 1)
if(signature->getLinks().size() == 1 && !smallDisplacement)
{
int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId);
@@ -1112,7 +1112,10 @@ bool Rtabmap::process(
_mapCorrection = newPose * signature->getPose().inverse();
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

21
corelib/src/VWDictionary.cpp
View File

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