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

Neigbor refining can be done with any Registration approach. Memory::computeIcpTransformMulti(): correspondences ratio increased by two times to accept transform

Browse Source
This commit is contained in:
matlabbe
2018-02-21 12:23:12 -05:00
parent 489ab86ac7
commit 1bfde1f9f0
4 changed files with 75 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -223,7 +223,6 @@ public:
Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const; Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false); Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
Transform computeIcpTransformMulti( Transform computeIcpTransformMulti(
int newId, int newId,
int oldId, int oldId,
@@ -320,7 +319,7 @@ private:
bool _parallelized; bool _parallelized;
Registration * _registrationPipeline; Registration * _registrationPipeline;
RegistrationIcp * _registrationIcp; RegistrationIcp * _registrationIcpMulti;
OccupancyGrid * _occupancy; OccupancyGrid * _occupancy;
}; };

97
corelib/src/Memory.cpp
View File

@@ -115,7 +115,14 @@ Memory::Memory(const ParametersMap & parameters) :
_feature2D = Feature2D::create(parameters); _feature2D = Feature2D::create(parameters);
_vwd = new VWDictionary(parameters); _vwd = new VWDictionary(parameters);
_registrationPipeline = Registration::create(parameters); _registrationPipeline = Registration::create(parameters);
_registrationIcp = new RegistrationIcp(parameters);
// for local scan matching, correspondences ratio should be two times higher as we expect more matches
float corRatio = Parameters::defaultIcpCorrespondenceRatio();
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), corRatio);
ParametersMap paramsMulti = parameters;
paramsMulti.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(corRatio*2.0f)));
_registrationIcpMulti = new RegistrationIcp(paramsMulti);
_occupancy = new OccupancyGrid(parameters); _occupancy = new OccupancyGrid(parameters);
this->parseParameters(parameters); this->parseParameters(parameters);
} }
@@ -418,9 +425,9 @@ Memory::~Memory()
{ {
delete _registrationPipeline; delete _registrationPipeline;
} }
if(_registrationIcp) if(_registrationIcpMulti)
{ {
delete _registrationIcp; delete _registrationIcpMulti;
} }
if(_occupancy) if(_occupancy)
{ {
@@ -555,9 +562,22 @@ void Memory::parseParameters(const ParametersMap & parameters)
_registrationPipeline->parseParameters(params); _registrationPipeline->parseParameters(params);
} }
if(_registrationIcp) if(_registrationIcpMulti)
{ {
_registrationIcp->parseParameters(params); if(uContains(params, Parameters::kIcpCorrespondenceRatio()))
{
// for local scan matching, correspondences ratio should be two times higher as we expect more matches
// for local scan matching, correspondences ratio should be two times higher as we expect more matches
float corRatio = Parameters::defaultIcpCorrespondenceRatio();
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), corRatio);
ParametersMap paramsMulti = params;
paramsMulti.at(Parameters::kIcpCorrespondenceRatio()) = uNumber2Str(corRatio*2.0f);
_registrationIcpMulti->parseParameters(paramsMulti);
}
else
{
_registrationIcpMulti->parseParameters(params);
}
} }
if(_occupancy) if(_occupancy)
@@ -2393,7 +2413,15 @@ Transform Memory::computeTransform(
UDEBUG(""); UDEBUG("");
// verify if it is a 180 degree transform, well verify > 90 // verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw; float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); if(guess.isNull())
{
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
}
else
{
Transform guessError = guess.inverse() * transform;
guessError.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
}
if(fabs(pitch) > CV_PI/2 || if(fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2) fabs(yaw) > CV_PI/2)
{ {
@@ -2411,61 +2439,6 @@ Transform Memory::computeTransform(
return transform; return transform;
} }
// compute transform fromId -> toId
Transform Memory::computeIcpTransform(
int fromId,
int toId,
Transform guess,
RegistrationInfo * info)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
if(fromS && toS && _dbDriver)
{
std::list<Signature*> depthsToLoad;
//if image is already here, scan should be or it is null
if(fromS->sensorData().imageCompressed().empty() &&
fromS->sensorData().laserScanCompressed().isEmpty())
{
depthsToLoad.push_back(fromS);
}
if(toS->sensorData().imageCompressed().empty() &&
toS->sensorData().laserScanCompressed().isEmpty())
{
depthsToLoad.push_back(toS);
}
if(depthsToLoad.size())
{
_dbDriver->loadNodeData(depthsToLoad, false, true, false, false);
}
}
Transform t;
if(fromS && toS)
{
//make sure data are uncompressed
LaserScan tmp1, tmp2;
fromS->sensorData().uncompressData(0, 0, &tmp1);
toS->sensorData().uncompressData(0, 0, &tmp2);
// compute transform fromId -> toId
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
}
else
{
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(info)
{
info->rejectedMsg = msg;
}
UWARN(msg.c_str());
}
return t;
}
// compute transform fromId -> multiple toId // compute transform fromId -> multiple toId
Transform Memory::computeIcpTransformMulti( Transform Memory::computeIcpTransformMulti(
int fromId, int fromId,
@@ -2606,7 +2579,7 @@ Transform Memory::computeIcpTransformMulti(
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity())); fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
Transform guess = poses.at(fromId).inverse() * poses.at(toId); Transform guess = poses.at(fromId).inverse() * poses.at(toId);
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info); t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);
} }
return t; return t;

81
corelib/src/Rtabmap.cpp
View File

@@ -1110,55 +1110,52 @@ bool Rtabmap::process(
//============================================================ //============================================================
// Refine neighbor links // Refine neighbor links
//============================================================ //============================================================
if(!signature->sensorData().laserScanCompressed().isEmpty()) UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
RegistrationInfo info;
Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())
{ {
UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str()); UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
RegistrationInfo info; oldId,
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info); signature->id(),
if(!t.isNull()) info.covariance.at<double>(0,0),
{ info.covariance.at<double>(5,5),
UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s", guess.prettyPrint().c_str(),
oldId, t.prettyPrint().c_str());
signature->id(), UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
info.covariance.at<double>(0,0), _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
info.covariance.at<double>(5,5),
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
if(_optimizeFromGraphEnd) if(_optimizeFromGraphEnd)
{
// 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("Odometry refining rejected: %s", info.rejectedMsg.c_str()); // update all previous nodes
if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0) // 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)
{ {
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv())); iter->second = mapCorrectionInv * up * iter->second;
} }
} }
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().size());
} }
else
{
UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
{
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().size());
} }
timeNeighborLinkRefining = timer.ticks(); timeNeighborLinkRefining = timer.ticks();
ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining); ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);

2
guilib/src/DatabaseViewer.cpp
View File

@@ -477,7 +477,6 @@ void DatabaseViewer::readSettings()
ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt()); ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool()); ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool());
ui_->checkBox_alignPosesWithGroundTruth->setChecked(settings.value("alignGroundTruth", ui_->checkBox_alignPosesWithGroundTruth->isChecked()).toBool());
ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool()); ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool()); ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool());
@@ -558,7 +557,6 @@ void DatabaseViewer::writeSettings()
settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex()); settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked()); settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked());
settings.setValue("alignGroundTruth", ui_->checkBox_alignPosesWithGroundTruth->isChecked());
settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()); settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked());
settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked()); settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked());