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

merged master to 0.11.0

Browse Source
This commit is contained in:
matlabbe
2015-11-30 19:39:05 -05:00
parent e58f907440 38fd459c7b
commit fc47d07b85
9 changed files with 273 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
corelib/src/Memory.cpp
View File

@@ -80,6 +80,7 @@ Memory::Memory(const ParametersMap & parameters) :
_idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
_generateIds(Parameters::defaultMemGenerateIds()),
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
_imageDecimation(Parameters::defaultMemImageDecimation()),
_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
@@ -132,7 +133,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
DBDriver * tmpDriver = 0;
if(!_memoryChanged && !_linksChanged)
if((!_memoryChanged && !_linksChanged) || dbOverwritten)
{
if(_dbDriver)
{
@@ -392,6 +393,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
Parameters::parse(parameters, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
@@ -410,19 +412,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
UASSERT(_rehearsalMaxDistance >= 0.0f);
UASSERT(_rehearsalMaxAngle >= 0.0f);
// SLAM mode vs Localization mode
iter = parameters.find(Parameters::kMemIncrementalMemory());
if(iter != parameters.end())
{
bool value = uStr2Bool(iter->second.c_str());
if(value == false && _incrementalMemory)
{
// From SLAM to localization, change map id
this->incrementMapId();
}
_incrementalMemory = value;
}
if(_dbDriver)
{
_dbDriver->parseParameters(parameters);
@@ -479,7 +468,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
else if(_feature2D)
{
_feature2D->parseParameters(parameters);
}
}
if(_registrationVis)
{
@@ -488,7 +477,29 @@ void Memory::parseParameters(const ParametersMap & parameters)
if(_registrationIcp)
{
_registrationIcp->parseParameters(parameters);
}
}
// do this after all parameters are parsed
// SLAM mode vs Localization mode
iter = parameters.find(Parameters::kMemIncrementalMemory());
if(iter != parameters.end())
{
bool value = uStr2Bool(iter->second.c_str());
if(value == false && _incrementalMemory)
{
// From SLAM to localization, change map id
this->incrementMapId();
// The easiest way to make sure that the mapping session is saved
// is to save the memory in the database and reload it.
if((_memoryChanged || _linksChanged) && _dbDriver)
{
UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
this->init(_dbDriver->getUrl());
}
}
_incrementalMemory = value;
}
}
void Memory::preUpdate()
@@ -697,7 +708,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
}
}
}
else
else if(_mapLabelsAdded)
{
//Tag the first node of the map
std::string tag = uFormat("map%d", signature->mapId());

115
corelib/src/Rtabmap.cpp
View File

@@ -982,72 +982,85 @@ bool Rtabmap::process(
// Update optimizedPoses with the newly added node
Transform newPose;
if(signature->getLinks().size() == 1 &&
!smallDisplacement &&
_memory->isIncremental()) // ignore pose matching in localization mode
if(_neighborLinkRefining &&
signature->getLinks().size() == 1 &&
_memory->isIncremental() && // ignore pose matching in localization mode
rehearsedId == 0) // don't do it if rehearsal happened
{
int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
//============================================================
// Scan matching
//============================================================
if(_neighborLinkRefining &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform().inverse();
float variance = 1.0f;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, variance, variance);
Transform guess = signature->getLinks().begin()->second.transform().inverse();
if(_optimizeFromGraphEnd)
if(smallDisplacement)
{
if(signature->getLinks().begin()->second.transVariance() == 1)
{
// set small variance
UDEBUG("Set small variance. The robot is not moving.");
_memory->updateLink(signature->id(), oldId, guess, 0.0001, 0.0001);
}
}
else
{
//============================================================
// Scan matching
//============================================================
if(!signature->sensorData().laserScanCompressed().empty())
{
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform().inverse();
float variance = 1.0f;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
// update all previous nodes
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform u = guess * t.inverse();
std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
UASSERT(jter!=_optimizedPoses.end());
Transform up = jter->second * u * jter->second.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, variance, variance);
if(_optimizeFromGraphEnd)
{
iter->second = mapCorrectionInv * up * iter->second;
// update all previous nodes
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform u = guess * t.inverse();
std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
UASSERT(jter!=_optimizedPoses.end());
Transform up = jter->second * u * jter->second.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * up * iter->second;
}
}
}
}
else
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
if(variance > 0)
else
{
double sqrtVar = sqrt(variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
if(variance > 0)
{
double sqrtVar = sqrt(variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), variance);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), variance);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
}
timeNeighborLinkRefining = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeNeighborLinkRefining);
ULOGGER_INFO("timeScanMatching=%fs", timeNeighborLinkRefining);
UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId));