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

DbViewer: fixed bug where null links were added when added links doen't have enough inliers. g2o/Optimizer: changed back to Levenberg as GaussNewton seems worst on Kinect multi-session mapping

Browse Source
This commit is contained in:
matlabbe
2016-12-01 11:59:49 -05:00
parent 40d0be5828
commit d0dddaf7cc
2 changed files with 65 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Parameters.h
View File

@@ -351,7 +351,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str())); RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod"); RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod");
RTABMAP_PARAM(g2o, Optimizer, int, 1, "0=Levenberg 1=GaussNewton"); RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
RTABMAP_PARAM(g2o, PixelVariance, double, 1.0, "Pixel variance used for bundle adjustment."); RTABMAP_PARAM(g2o, PixelVariance, double, 1.0, "Pixel variance used for bundle adjustment.");
// Odometry // Odometry

109
guilib/src/DatabaseViewer.cpp
View File

@@ -4294,7 +4294,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second; newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
} }
bool updateConstraints = true; bool updateConstraints = newLink.isValid();
float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError())); float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
if(newLink.isValid() && if(newLink.isValid() &&
maxOptimizationError > 0.0f && maxOptimizationError > 0.0f &&
@@ -4321,60 +4321,79 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::multimap<int, Link> links; std::multimap<int, Link> links;
optimizer->getConnectedGraph(fromId, poses_, linksIn, poses, links); optimizer->getConnectedGraph(fromId, poses_, linksIn, poses, links);
UASSERT(poses.find(fromId) != poses.end());
UASSERT(poses.find(newLink.from()) != poses.end());
UASSERT(poses.find(newLink.to()) != poses.end());
UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
poses = optimizer->optimize(fromId, poses, links); poses = optimizer->optimize(fromId, poses, links);
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter) std::string msg;
if(poses.size())
{ {
// ignore links with high variance for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
if(iter->second.transVariance() <= 1.0)
{ {
Transform t1 = uValue(poses, iter->second.from(), Transform()); // ignore links with high variance
Transform t2 = uValue(poses, iter->second.to(), Transform()); if(iter->second.transVariance() <= 1.0)
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
Eigen::Vector3f vA = t1.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = t2.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
if(linearError > maxLinearError)
{ {
maxLinearError = linearError; UASSERT(poses.find(iter->second.from())!=poses.end());
maxLinearLink = &iter->second; UASSERT(poses.find(iter->second.to())!=poses.end());
} Transform t1 = poses.at(iter->second.from());
if(angularError > maxAngularError) Transform t2 = poses.at(iter->second.to());
{ UASSERT(!t1.isNull() && !t2.isNull());
maxAngularError = angularError; Transform t = t1.inverse()*t2;
maxAngularLink = &iter->second; float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
Eigen::Vector3f vA = t1.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = t2.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
if(linearError > maxLinearError)
{
maxLinearError = linearError;
maxLinearLink = &iter->second;
}
if(angularError > maxAngularError)
{
maxAngularError = angularError;
maxAngularLink = &iter->second;
}
} }
} }
} if(maxLinearLink)
if(maxLinearLink) {
{ UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); }
} if(maxLinearLink)
if(maxLinearLink) {
{ UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); }
}
if(maxLinearError > maxOptimizationError) if(maxLinearError > maxOptimizationError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
"\"%s\" is %f m.",
newLink.from(),
newLink.to(),
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
Parameters::kRGBDOptimizeMaxError().c_str(),
maxOptimizationError);
}
}
else
{ {
std::string msg = uFormat("Rejecting edge %d->%d because " msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
"graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
"\"%s\" is %f m.",
newLink.from(), newLink.from(),
newLink.to(), newLink.to());
maxLinearError, }
maxLinearLink->from(), if(!msg.empty())
maxLinearLink->to(), {
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
Parameters::kRGBDOptimizeMaxError().c_str(),
maxOptimizationError);
UWARN("%s", msg.c_str()); UWARN("%s", msg.c_str());
if(!silent) if(!silent)
{ {
QMessageBox::warning(this, QMessageBox::warning(this,