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

Only update weight on rehearsal if the displacement is too high (using parameters RGBD/LinearUpdate and RGBD/AngularUpdate)

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-17 18:51:37 -05:00
parent f708e7c040
commit 5a37393a45
3 changed files with 49 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -210,6 +210,8 @@ private:
bool _badSignaturesIgnored;
int _imageDecimation;
bool _localSpaceLinksKeptInWM;
float _rehearsalMaxDistance;
float _rehearsalMaxAngle;
int _idCount;
int _idMapCount;

39
corelib/src/Memory.cpp
View File

@@ -68,6 +68,8 @@ Memory::Memory(const ParametersMap & parameters) :
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_imageDecimation(Parameters::defaultMemImageDecimation()),
_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
@@ -387,6 +389,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -2429,7 +2433,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
//============================================================
int id = signature->getLinks().begin()->first;
UDEBUG("Comparing with last signature (%d)...", id);
const Signature * sB = this->getSignature(id);
Signature * sB = this->_getSignature(id);
if(!sB)
{
UFATAL("Signature %d null?!?", id);
@@ -2441,9 +2445,36 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
{
if(_incrementalMemory)
{
if(this->rehearsalMerge(id, signature->id()))
if(signature->getLinks().begin()->second.transform().isNull())
{
merged = id;
if(this->rehearsalMerge(id, signature->id()))
{
merged = id;
}
}
else
{
float x,y,z, roll,pitch,yaw;
signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if((_rehearsalMaxDistance>0.0f && (
fabs(x) > _rehearsalMaxDistance ||
fabs(y) > _rehearsalMaxDistance ||
fabs(z) > _rehearsalMaxDistance)) ||
(_rehearsalMaxAngle>0.0f && (
fabs(roll) > _rehearsalMaxAngle ||
fabs(pitch) > _rehearsalMaxAngle ||
fabs(yaw) > _rehearsalMaxAngle)))
{
// if the robot has moved, transfer only weight
signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
sB->setWeight(0);
UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)",
signature->getWeight(), signature->id(), id, _rehearsalMaxDistance, _rehearsalMaxAngle);
}
else if(this->rehearsalMerge(id, signature->id()))
{
merged = id;
}
}
}
else
@@ -2474,7 +2505,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
}
UASSERT(!newS->isSaved());
UDEBUG("Rehearsal merge %d and %d", oldS->id(), newS->id());
UINFO("Rehearsal merging %d and %d", oldS->id(), newS->id());
//remove mutual links
oldS->removeLink(newId);

21
corelib/src/Rtabmap.cpp
View File

@@ -817,7 +817,7 @@ bool Rtabmap::process(const SensorData & data)
{
_optimizedPoses.erase(rehearsedId);
}
else
else if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
{
//============================================================
// Minimum displacement required to add to Memory
@@ -827,14 +827,17 @@ bool Rtabmap::process(const SensorData & data)
{
float x,y,z, roll,pitch,yaw;
links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(x) < _rgbdLinearUpdate &&
fabs(y) < _rgbdLinearUpdate &&
fabs(z) < _rgbdLinearUpdate &&
fabs(roll) < _rgbdAngularUpdate &&
fabs(pitch) < _rgbdAngularUpdate &&
fabs(yaw) < _rgbdAngularUpdate)
if((_rgbdLinearUpdate==0.0f || (
fabs(x) < _rgbdLinearUpdate &&
fabs(y) < _rgbdLinearUpdate &&
fabs(z) < _rgbdLinearUpdate)) &&
(_rgbdAngularUpdate==0.0f || (
fabs(roll) < _rgbdAngularUpdate &&
fabs(pitch) < _rgbdAngularUpdate &&
fabs(yaw) < _rgbdAngularUpdate)))
{
UWARN("Ignoring location %d because the displacement is too small!", signature->id());
UWARN("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
_memory->deleteLocation(signature->id());
return false;
}
@@ -1367,7 +1370,7 @@ bool Rtabmap::process(const SensorData & data)
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
UWARN("Cannot compute a loop closure transform between %d and %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
UINFO("Cannot compute a loop closure transform between %d and %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
}
}
if(!rejectedHypothesis)