i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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
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
@@ -2348,9 +2351,9 @@ bool Rtabmap::process(
// Pass this point signature should not be used, since it could have been transferred...
signature = 0;
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,
timeLocalSpaceDetection,
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,
_highestHypothesis.first,
(int)signaturesRemoved.size(),
@@ -3060,8 +3063,8 @@ void Rtabmap::getGraph(
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global,
std::map<int, Signature> * signatures)
bool global,
std::map<int, Signature> * signatures)
{
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);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
if(signatures)
if(signatures)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
@@ -3100,7 +3103,7 @@ void Rtabmap::getGraph(
weight,
stamp,
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.to(), jter->second.from())); // <->
}
}
}
UINFO("Time getting links = %fs", timer.ticks());
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..
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;
}
}
}