i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rtabmap: added addLink() function

Browse Source
This commit is contained in:
matlabbe
2020-02-11 23:33:18 -05:00
parent e1ecf55d03
commit 79b480da4b
2 changed files with 163 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

162
corelib/src/Rtabmap.cpp
View File

@@ -4770,6 +4770,168 @@ int Rtabmap::refineLinks()
return (int)linksRefined.size();
}
bool Rtabmap::addLink(const Link & link)
{
const Transform & t = link.transform();
if(!_rgbdSlamMode)
{
UERROR("Adding new link can be done only in RGBD-SLAM mode.");
return false;
}
if(!_memory)
{
UERROR("Memory is not initialized.");
return false;
}
if(t.isNull())
{
UERROR("Link's transform is null!");
return false;
}
if(_memory->getSignature(link.from()) == 0)
{
UERROR("Link's \"from id\" %d is not in working memory", link.from());
return false;
}
if(_memory->getSignature(link.to()) == 0)
{
UERROR("Link's \"to id\" %d is not in working memory", link.to());
return false;
}
std::map<int, Transform> poses;
std::multimap<int, Link> links;
this->getGraph(poses, links, true, true);
if(poses.find(link.from()) == poses.end())
{
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();
int to = link.to();
if(_optimizationMaxError > 0.0f)
{
//optimize the graph to see if the new constraint is globally valid
std::multimap<int, Link> linksIn = links;
linksIn.insert(std::make_pair(link.from(), link));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
std::map<int, Transform> optimizedPoses;
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
_graphOptimizer->getConnectedGraph(from, poses, linksIn, optimizedPoses, links);
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, from, to) != links.end());
int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
std::string msg;
if(optimizedPoses.size())
{
graph::computeMaxGraphErrors(
optimizedPoses,
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
if(maxLinearErrorRatio > _optimizationMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
"\"%s\" is %f.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearErrorRatio,
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
{
msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
from,
to);
}
if(!msg.empty())
{
UERROR("%s", msg.c_str());
return false;
}
}
else
{
int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
poses = _graphOptimizer->optimize(fromId, poses, links, 0);
if(poses.empty())
{
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)
{
std::map<int, Transform>::iterator jter = poses.find(iter->first);
if(jter != poses.end())
{
iter->second = jter->second;
}
}
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;
}
return false;
}
cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
{
cv::Mat information = covariance.inv();