i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rtabmap::addLink() support multi-session

Browse Source
This commit is contained in:
matlabbe
2020-06-25 13:56:01 -04:00
parent 274903dd63
commit dacf724ea6
1 changed files with 103 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

222
corelib/src/Rtabmap.cpp
View File

@@ -4903,145 +4903,129 @@ bool Rtabmap::addLink(const Link & link)
return false; return false;
} }
std::map<int, Transform> poses; if(_optimizedPoses.find(link.from()) == _optimizedPoses.end() &&
std::multimap<int, Link> links; _optimizedPoses.find(link.to()) == _optimizedPoses.end())
this->getGraph(poses, links, true, false);
if(_memory->isIncremental())
{ {
if(poses.find(link.from()) == poses.end()) UERROR("Neither nodes %d or %d are in the local graph (size=%d). One of the 2 nodes should be in the local graph.", (int)_optimizedPoses.size(), link.from(), link.to());
{ return false;
UERROR("Link's \"from id\" %d is not in the graph", link.from());
return false;
}
if(poses.find(link.to()) == poses.end())
{
UERROR("Link's \"to id\" %d is not in the graph", link.to());
return false;
}
} }
int from = link.from(); // add temporary the link
int to = link.to(); if(!_memory->addLink(link))
if(_optimizationMaxError > 0.0f)
{ {
//optimize the graph to see if the new constraint is globally valid UERROR("Cannot add new link %d->%d to memory", link.from(), link.to());
std::multimap<int, Link> linksIn = links; return false;
linksIn.insert(std::make_pair(link.from(), link)); }
// optimize with new link
std::map<int, Transform> poses = _optimizedPoses;
std::multimap<int, Link> links;
cv::Mat covariance;
optimizeCurrentMap(this->getLastLocationId(), false, poses, covariance, &links);
if(poses.find(link.from()) == poses.end())
{
UERROR("Link's \"from id\" %d is not in the graph (size=%d)", link.from(), (int)poses.size());
_memory->removeLink(link.from(), link.to());
return false;
}
if(poses.find(link.to()) == poses.end())
{
UERROR("Link's \"to id\" %d is not in the graph (size=%d)", link.to(), (int)poses.size());
_memory->removeLink(link.from(), link.to());
return false;
}
std::string msg;
if(poses.empty())
{
msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to());
}
else if(_optimizationMaxError > 0.0f)
{
float maxLinearError = 0.0f;
float maxLinearErrorRatio = 0.0f;
float maxAngularError = 0.0f;
float maxAngularErrorRatio = 0.0f;
const Link * maxLinearLink = 0; const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0; const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f; graph::computeMaxGraphErrors(
float maxLinearErrorRatio = 0.0f; poses,
float maxAngularErrorRatio = 0.0f; links,
std::map<int, Transform> optimizedPoses; maxLinearErrorRatio,
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str()); maxAngularErrorRatio,
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str()); maxLinearError,
_graphOptimizer->getConnectedGraph(from, poses, linksIn, optimizedPoses, links); maxAngularError,
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str()); &maxLinearLink,
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str()); &maxAngularLink);
UASSERT(graph::findLink(links, from, to) != links.end()); if(maxLinearLink)
int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
std::string msg;
if(optimizedPoses.size())
{ {
graph::computeMaxGraphErrors( UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
optimizedPoses, if(maxLinearErrorRatio > _optimizationMaxError)
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{ {
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); msg = uFormat("Rejecting edge %d->%d because "
if(maxLinearErrorRatio > _optimizationMaxError) "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
{ "\"%s\" is %f.",
msg = uFormat("Rejecting edge %d->%d because " link.from(),
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " link.to(),
"\"%s\" is %f.", maxLinearError,
from, maxLinearLink->from(),
to, maxLinearLink->to(),
maxLinearError, maxLinearErrorRatio,
maxLinearLink->from(), sqrt(maxLinearLink->transVariance()),
maxLinearLink->to(), Parameters::kRGBDOptimizeMaxError().c_str(),
maxLinearErrorRatio, _optimizationMaxError);
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
}
else if(maxAngularLink)
{
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
if(maxAngularErrorRatio > _optimizationMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
"\"%s\" is %f m.",
from,
to,
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularErrorRatio,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
} }
} }
else else if(maxAngularLink)
{ {
msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
from, if(maxAngularErrorRatio > _optimizationMaxError)
to); {
} msg = uFormat("Rejecting edge %d->%d because "
if(!msg.empty()) "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
{ "\"%s\" is %f m.",
UERROR("%s", msg.c_str()); link.from(),
return false; link.to(),
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularErrorRatio,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
} }
} }
else if(!msg.empty())
{ {
int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first; UERROR("%s", msg.c_str());
poses = _graphOptimizer->optimize(fromId, poses, links, 0); _memory->removeLink(link.from(), link.to());
if(poses.empty()) return false;
{
UERROR("Rejecting edge %d->%d because graph optimization has failed!", from, to);
return false;
}
} }
if(_memory->addLink(link, false)) // Update optimized poses
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{ {
// Update optimized poses std::map<int, Transform>::iterator jter = poses.find(iter->first);
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) if(jter != poses.end())
{ {
std::map<int, Transform>::iterator jter = poses.find(iter->first); iter->second = jter->second;
if(jter != poses.end())
{
iter->second = jter->second;
}
} }
if(!_optimizeFromGraphEnd)
{
_mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
}
std::map<int, Transform> tmp;
// Update also the links if some have been added in WM
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
_memory->save2DMap(cv::Mat(), 0, 0, 0);
return true;
} }
if(!_optimizeFromGraphEnd)
{
_mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
}
std::map<int, Transform> tmp;
// Update also the links if some have been added in WM
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
_memory->save2DMap(cv::Mat(), 0, 0, 0);
return true;
} }
else // localization mode else // localization mode
{ {