mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Neigbor refining can be done with any Registration approach. Memory::computeIcpTransformMulti(): correspondences ratio increased by two times to accept transform
This commit is contained in:
matlabbe
@@ -223,7 +223,6 @@ public:
|
||||
|
||||
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 computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
|
||||
Transform computeIcpTransformMulti(
|
||||
int newId,
|
||||
int oldId,
|
||||
@@ -320,7 +319,7 @@ private:
|
||||
bool _parallelized;
|
||||
|
||||
Registration * _registrationPipeline;
|
||||
RegistrationIcp * _registrationIcp;
|
||||
RegistrationIcp * _registrationIcpMulti;
|
||||
|
||||
OccupancyGrid * _occupancy;
|
||||
};
|
||||
|
||||
@@ -115,7 +115,14 @@ Memory::Memory(const ParametersMap & parameters) :
|
||||
_feature2D = Feature2D::create(parameters);
|
||||
_vwd = new VWDictionary(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);
|
||||
this->parseParameters(parameters);
|
||||
}
|
||||
@@ -418,9 +425,9 @@ Memory::~Memory()
|
||||
{
|
||||
delete _registrationPipeline;
|
||||
}
|
||||
if(_registrationIcp)
|
||||
if(_registrationIcpMulti)
|
||||
{
|
||||
delete _registrationIcp;
|
||||
delete _registrationIcpMulti;
|
||||
}
|
||||
if(_occupancy)
|
||||
{
|
||||
@@ -555,9 +562,22 @@ void Memory::parseParameters(const ParametersMap & parameters)
|
||||
_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)
|
||||
@@ -2393,7 +2413,15 @@ Transform Memory::computeTransform(
|
||||
UDEBUG("");
|
||||
// verify if it is a 180 degree transform, well verify > 90
|
||||
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 ||
|
||||
fabs(yaw) > CV_PI/2)
|
||||
{
|
||||
@@ -2411,61 +2439,6 @@ Transform Memory::computeTransform(
|
||||
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
|
||||
Transform Memory::computeIcpTransformMulti(
|
||||
int fromId,
|
||||
@@ -2606,7 +2579,7 @@ Transform Memory::computeIcpTransformMulti(
|
||||
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
|
||||
|
||||
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;
|
||||
|
||||
@@ -1110,55 +1110,52 @@ bool Rtabmap::process(
|
||||
//============================================================
|
||||
// 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());
|
||||
RegistrationInfo info;
|
||||
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
|
||||
if(!t.isNull())
|
||||
{
|
||||
UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
|
||||
oldId,
|
||||
signature->id(),
|
||||
info.covariance.at<double>(0,0),
|
||||
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()));
|
||||
UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
|
||||
oldId,
|
||||
signature->id(),
|
||||
info.covariance.at<double>(0,0),
|
||||
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)
|
||||
{
|
||||
// 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
|
||||
if(_optimizeFromGraphEnd)
|
||||
{
|
||||
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)
|
||||
// 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)
|
||||
{
|
||||
_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();
|
||||
ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
|
||||
|
||||
Reference in New Issue
Block a user