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

25
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
@@ -2348,9 +2351,9 @@ bool Rtabmap::process(
// Pass this point signature should not be used, since it could have been transferred... // Pass this point signature should not be used, since it could have been transferred...
signature = 0; signature = 0;
timeMemoryCleanup = timer.ticks(); timeMemoryCleanup = timer.ticks();
ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
@@ -2518,7 +2521,7 @@ bool Rtabmap::process(
timeLocalTimeDetection, timeLocalTimeDetection,
timeLocalSpaceDetection, timeLocalSpaceDetection,
timeMapOptimization); timeMapOptimization);
std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
_loopClosureHypothesis.first, _loopClosureHypothesis.first,
_highestHypothesis.first, _highestHypothesis.first,
(int)signaturesRemoved.size(), (int)signaturesRemoved.size(),
@@ -3060,8 +3063,8 @@ void Rtabmap::getGraph(
std::map<int, Transform> & poses, std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints, std::multimap<int, Link> & constraints,
bool optimized, bool optimized,
bool global, bool global,
std::map<int, Signature> * signatures) std::map<int, Signature> * signatures)
{ {
if(_memory && _memory->getLastWorkingSignature()) if(_memory && _memory->getLastWorkingSignature())
{ {
@@ -3083,8 +3086,8 @@ void Rtabmap::getGraph(
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
} }
if(signatures) if(signatures)
{ {
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
@@ -3100,7 +3103,7 @@ void Rtabmap::getGraph(
weight, weight,
stamp, stamp,
label, label,
odomPose))); odomPose)));
} }
} }
} }
@@ -3243,7 +3246,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
links.insert(std::make_pair(jter->second.from(), jter->second.to())); links.insert(std::make_pair(jter->second.from(), jter->second.to()));
links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <->
} }
} }
UINFO("Time getting links = %fs", timer.ticks()); UINFO("Time getting links = %fs", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose); int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);

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;
}
} }
} }