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

Parameters: added RGBD/LoopCovLimited, refactored "detect more loop closures" in MainWindow/DBViewer/rtabmap

Browse Source
This commit is contained in:
matlabbe
2018-10-01 20:22:20 -04:00
parent 0059a4bc1b
commit b5dec56eaf
14 changed files with 468 additions and 319 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Graph.h
+12
View File
@@ -102,6 +102,18 @@ Transform RTABMAP_EXP calcRMSE(
float & rotational_min,
float & rotational_max);
void RTABMAP_EXP computeMaxGraphErrors(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
float & maxLinearErrorRatio,
float & maxAngularErrorRatio,
float & maxLinearError,
float & maxAngularError,
const Link ** maxLinearErrorLink = 0,
const Link ** maxAngularErrorLink = 0);
std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links);
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
corelib/include/rtabmap/core/Memory.h
+2
View File
@@ -206,6 +206,7 @@ public:
int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
const Feature2D * getFeature2D() const {return _feature2D;}
bool isGraphReduced() const {return _reduceGraph;}
const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;}
void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const;
@@ -315,6 +316,7 @@ private:
GPS _gpsOrigin;
std::vector<CameraModel> _rectCameraModels;
StereoCameraModel _rectStereoCameraModel;
std::vector<double> _odomMaxInf;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -353,6 +353,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false, "Extract features even if there are some already in the nodes.");
RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure, bool, false, "Do local bundle adjustment with neighborhood of the loop closure.");
RTABMAP_PARAM(RGBD, CreateOccupancyGrid, bool, false, "Create local occupancy grid maps. See \"Grid\" group for parameters.");
RTABMAP_PARAM(RGBD, LoopCovLimited, bool, false, "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
// Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM.");
corelib/include/rtabmap/core/Rtabmap.h
+2
View File
@@ -168,6 +168,7 @@ public:
std::map<int, Signature> * signatures = 0);
int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1, const ProgressState * state = 0);
int refineLinks();
cv::Mat getInformation(const cv::Mat & covariance) const;
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -259,6 +260,7 @@ private:
float _pathLinearVelocity;
float _pathAngularVelocity;
bool _savedLocalizationIgnored;
bool _loopCovLimited;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
corelib/src/Graph.cpp
+86
View File
@@ -818,6 +818,92 @@ Transform calcRMSE (
return t;
}
void computeMaxGraphErrors(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
float & maxLinearErrorRatio,
float & maxAngularErrorRatio,
float & maxLinearError,
float & maxAngularError,
const Link ** maxLinearErrorLink,
const Link ** maxAngularErrorLink)
{
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
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()));
float opt_roll,opt__pitch,opt__yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
UASSERT(iter->second.transVariance()>0.0);
float stddevLinear = sqrt(iter->second.transVariance());
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
if(maxLinearErrorLink)
{
*maxLinearErrorLink = &iter->second;
}
}
UASSERT(iter->second.rotVariance()>0.0);
float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
if(maxAngularErrorLink)
{
*maxAngularErrorLink = &iter->second;
}
}
}
}
}
std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)
{
std::vector<double> maxOdomInf(6,0.0);
maxOdomInf.resize(6,0.0);
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
{
for(int i=0; i<6; ++i)
{
const double & v = iter->second.infMatrix().at<double>(i,i);
if(maxOdomInf[i] < v)
{
maxOdomInf[i] = v;
}
}
}
}
if(maxOdomInf[0] == 0.0)
{
maxOdomInf.clear();
}
return maxOdomInf;
}
////////////////////////////////////////////
// Graph utilities
corelib/src/Memory.cpp
+39
View File
@@ -239,6 +239,29 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
// global loop closures.
_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
_workingMem.insert(std::make_pair((*iter)->id(), UTimer::now()));
//update odomMaxInf vector
std::multimap<int, Link> links = this->getAllLinks(true, true);
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor &&
iter->second.infMatrix().cols == 6 &&
iter->second.infMatrix().rows == 6)
{
if(_odomMaxInf.empty())
{
_odomMaxInf.resize(6, 0.0);
}
for(int i=0; i<6; ++i)
{
const double & v = iter->second.infMatrix().at<double>(i,i);
if(_odomMaxInf[i] < v)
{
_odomMaxInf[i] = v;
}
}
}
}
}
else
{
@@ -832,6 +855,21 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
std::cout << "Covariance: " << covariance << std::endl;
infMatrix = cv::Mat::eye(6,6,CV_64FC1);
}
UASSERT(infMatrix.rows == 6 && infMatrix.cols == 6);
if(_odomMaxInf.empty())
{
_odomMaxInf.resize(6, 0.0);
}
for(int i=0; i<6; ++i)
{
const double & v = infMatrix.at<double>(i,i);
if(_odomMaxInf[i] < v)
{
_odomMaxInf[i] = v;
}
}
motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));
@@ -1519,6 +1557,7 @@ void Memory::clear()
_gpsOrigin = GPS();
_rectCameraModels.clear();
_rectStereoCameraModel = StereoCameraModel();
_odomMaxInf.clear();
if(_dbDriver)
{
corelib/src/Rtabmap.cpp
+91 -101
View File
@@ -123,6 +123,7 @@ Rtabmap::Rtabmap() :
_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -464,6 +465,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1348,7 +1350,7 @@ bool Rtabmap::process(
transform.prettyPrint().c_str());
// Add a loop constraint
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.covariance.inv())))
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
{
++proximityDetectionsInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1916,11 +1918,6 @@ bool Rtabmap::process(
transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
loopClosureVisualInliers = info.inliers;
loopClosureVisualMatches = info.matches;
if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
{
loopClosureLinearVariance = info.covariance.at<double>(0,0);
loopClosureAngularVariance = info.covariance.at<double>(3,3);
}
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
@@ -1936,7 +1933,10 @@ bool Rtabmap::process(
{
// Make the new one the parent of the old one
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.covariance.inv()));
cv::Mat information = getInformation(info.covariance);
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
loopClosureAngularVariance = 1.0/information.at<double>(5,5);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
if(!rejectedHypothesis)
{
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -2043,7 +2043,8 @@ bool Rtabmap::process(
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, info.covariance.inv()));
cv::Mat information = getInformation(info.covariance);
_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, information));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
@@ -2053,11 +2054,9 @@ bool Rtabmap::process(
loopClosureVisualInliers = info.inliers;
loopClosureVisualMatches = info.matches;
if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
{
loopClosureLinearVariance = info.covariance.at<double>(0,0);
loopClosureAngularVariance = info.covariance.at<double>(3,3);
}
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
loopClosureAngularVariance = 1.0/information.at<double>(5,5);
}
}
else
@@ -2184,7 +2183,7 @@ bool Rtabmap::process(
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, (info.covariance*100.0).inv(), scanMatchingIds));
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++proximityDetectionsAddedByICPOnly;
@@ -2348,46 +2347,23 @@ bool Rtabmap::process(
optimizationIterations > 0 &&
constraints.size())
{
UINFO("Compute max graph errors...");
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
graph::computeMaxGraphErrors(
poses,
constraints,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink == 0 && maxAngularLink==0)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
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()));
float opt_roll,opt__pitch,opt__yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
float stddevLinear = sqrt(iter->second.transVariance());
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
maxLinearLink = &iter->second;
}
float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
maxAngularLink = &iter->second;
}
}
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
}
bool reject = false;
if(maxLinearLink)
{
@@ -3411,7 +3387,7 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
UERROR("Failed to optimize the graph! returning empty optimized poses...");
UWARN("Failed to optimize the graph! returning empty optimized poses...");
optimizedPoses.clear();
if(constraints)
{
@@ -3941,11 +3917,13 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
}
}
std::multimap<int, Link> linksIn = links;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.covariance.inv())));
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
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;
std::multimap<int, Link> links;
UASSERT(poses.find(fromId) != poses.end());
@@ -3960,60 +3938,52 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
std::string msg;
if(optimizedPoses.size())
{
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
UASSERT(optimizedPoses.find(iter->second.from())!=optimizedPoses.end());
UASSERT(optimizedPoses.find(iter->second.to())!=optimizedPoses.end());
Transform t1 = optimizedPoses.at(iter->second.from());
Transform t2 = optimizedPoses.at(iter->second.to());
UASSERT(!t1.isNull() && !t2.isNull());
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().linear()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = t2.toEigen3f().linear()*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;
}
}
}
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);
}
}
if(maxAngularLink)
else if(maxAngularLink)
{
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
}
if(maxLinearError > _optimizationMaxError)
{
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.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxAngularError*180.0f/M_PI,
maxAngularLink?maxAngularLink->from():0,
maxAngularLink?maxAngularLink->to():0,
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
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
@@ -4034,7 +4004,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
cv::Mat inf = info.covariance.inv();
cv::Mat inf = getInformation(info.covariance);
links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
@@ -4136,6 +4106,26 @@ int Rtabmap::refineLinks()
return (int)linksRefined.size();
}
cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
{
cv::Mat information = covariance.inv();
if(_loopCovLimited)
{
const std::vector<double> & odomMaxInf = _memory->getOdomMaxInf();
if(odomMaxInf.size() == 6)
{
for(int i=0; i<6; ++i)
{
if(information.at<double>(i,i) > odomMaxInf[i])
{
information.at<double>(i,i) = odomMaxInf[i];
}
}
}
}
return information;
}
void Rtabmap::clearPath(int status)
{
UINFO("status=%d", status);
corelib/src/optimizer/OptimizerGTSAM.cpp
+1 -1
View File
@@ -344,7 +344,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
catch(gtsam::IndeterminantLinearSystemException & e)
{
UERROR("GTSAM exception caught: %s", e.what());
UWARN("GTSAM exception caught: %s", e.what());
delete optimizer;
return optimizedPoses;
}
guilib/include/rtabmap/gui/DatabaseViewer.h
+1
View File
@@ -210,6 +210,7 @@ private:
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > generatedLocalMaps_; // < <ground, obstacles>, empty>
std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
std::map<int, cv::Mat> modifiedDepthImages_;
std::vector<double> odomMaxInf_;
OctoMap * octomap_;
ExportCloudsDialog * exportDialog_;
QDialog * editDepthDialog_;
guilib/src/DatabaseViewer.cpp
+36 -44
View File
@@ -3201,6 +3201,14 @@ void DatabaseViewer::detectMoreLoopClosures()
progressDialog->setMinimumWidth(800);
progressDialog->show();
const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
if(loopCovLimited)
{
odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_));
}
int iterations = ui_->spinBox_detectMore_iterations->value();
UASSERT(iterations > 0);
int added = 0;
@@ -3269,6 +3277,8 @@ void DatabaseViewer::detectMoreLoopClosures()
}
}
odomMaxInf_.clear();
if(added)
{
this->updateGraphView();
@@ -6157,6 +6167,14 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
Registration * reg = Registration::create(parameters);
bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
std::vector<double> odomMaxInf = odomMaxInf_;
if(loopCovLimited && odomMaxInf_.empty())
{
odomMaxInf = graph::getMaxOdomInf(updateLinksWithModifications(links_));
}
Transform t;
RegistrationInfo info;
@@ -6229,16 +6247,19 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
if(!t.isNull())
{
if(!t.isIdentity())
cv::Mat information = info.covariance.inv();
if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
{
// normalize variance
info.covariance *= t.getNorm();
if(info.covariance.at<double>(0,0)<=0.0)
for(int i=0; i<6; ++i)
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
if(information.at<double>(i,i) > odomMaxInf[i])
{
information.at<double>(i,i) = odomMaxInf[i];
}
}
}
newLink = Link(from, to, Link::kUserClosure, t, info.covariance.inv());
newLink = Link(from, to, Link::kUserClosure, t, information);
}
else if(!silent)
{
@@ -6297,44 +6318,15 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
{
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
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()));
float opt_roll,opt__pitch,opt__yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
float stddevLinear = sqrt(iter->second.transVariance());
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
maxLinearLink = &iter->second;
}
float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
maxAngularLink = &iter->second;
}
}
}
graph::computeMaxGraphErrors(
poses,
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
guilib/src/ExportCloudsDialog.cpp
+1 -1
View File
@@ -976,7 +976,7 @@ void ExportCloudsDialog::viewClouds(
{
viewer->setBackfaceCulling(true, false);
}
viewer->setLighting(true);
viewer->setLighting(false);
viewer->setDefaultBackgroundColor(QColor(40, 40, 40, 255));
viewer->buildPickingLocator(true);
guilib/src/MainWindow.cpp
+63 -60
View File
@@ -5465,6 +5465,14 @@ void MainWindow::postProcessing()
{
UDEBUG("");
bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
std::vector<double> odomMaxInf;
if(loopCovLimited)
{
odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
}
UASSERT(detectLoopClosureIterations>0);
for(int n=0; n<detectLoopClosureIterations && !_progressCanceled; ++n)
{
@@ -5573,18 +5581,19 @@ void MainWindow::postProcessing()
delete registration;
if(!transform.isNull())
{
if(!transform.isIdentity())
{
// normalize variance
info.covariance *= transform.getNorm();
if(info.covariance.at<double>(0,0)<=0.0)
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
}
//optimize the graph to see if the new constraint is globally valid
bool updateConstraint = true;
cv::Mat information = info.covariance.inv();
if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
{
for(int i=0; i<6; ++i)
{
if(information.at<double>(i,i) > odomMaxInf[i])
{
information.at<double>(i,i) = odomMaxInf[i];
}
}
}
if(optimizeMaxError > 0.0f && optimizeIterations > 0)
{
int fromId = from;
@@ -5599,7 +5608,7 @@ void MainWindow::postProcessing()
}
}
std::multimap<int, Link> linksIn = _currentLinksMap;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
@@ -5618,60 +5627,54 @@ void MainWindow::postProcessing()
std::string msg;
if(poses.size())
{
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
UASSERT(poses.find(iter->second.from())!=poses.end());
UASSERT(poses.find(iter->second.to())!=poses.end());
Transform t1 = poses.at(iter->second.from());
Transform t2 = poses.at(iter->second.to());
UASSERT(!t1.isNull() && !t2.isNull());
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().linear()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = t2.toEigen3f().linear()*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;
}
}
}
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
graph::computeMaxGraphErrors(
poses,
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 > optimizeMaxError)
{
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(),
optimizeMaxError);
}
}
if(maxAngularLink)
else if(maxAngularLink)
{
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
}
if(maxLinearError > optimizeMaxError)
{
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.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxAngularError*180.0f/M_PI,
maxAngularLink?maxAngularLink->from():0,
maxAngularLink?maxAngularLink->to():0,
Parameters::kRGBDOptimizeMaxError().c_str(),
optimizeMaxError);
if(maxAngularErrorRatio > optimizeMaxError)
{
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(),
optimizeMaxError);
}
}
}
else
@@ -5695,7 +5698,7 @@ void MainWindow::postProcessing()
addedLinks.insert(from);
addedLinks.insert(to);
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
++loopClosuresAdded;
_progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -864,6 +864,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDNeighborLinkRefining().c_str());
_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
_ui->rgbd_loopCovLimited->setObjectName(Parameters::kRGBDLoopCovLimited().c_str());
_ui->graphOptimization_type->setObjectName(Parameters::kOptimizerStrategy().c_str());
_ui->graphOptimization_iterations->setObjectName(Parameters::kOptimizerIterations().c_str());
guilib/src/ui/preferencesDialog.ui
+132 -112
View File
@@ -94,9 +94,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-287</y>
<width>681</width>
<height>2943</height>
<y>0</y>
<width>673</width>
<height>2939</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -117,7 +117,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>12</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -8896,51 +8896,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_153">
<property name="text">
<string>Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
<property name="suffix">
<string> rad</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="loopClosure_reextract">
<property name="text">
@@ -8967,7 +8925,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="11" column="1">
<item row="12" column="1">
<widget class="QLabel" name="label_scanMatching_5">
<property name="text">
<string>Ratio of working memory for which local nodes are immunized from transfer.</string>
@@ -9013,53 +8971,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_2">
<property name="text">
<string>When loading a database, ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_bunlde">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_space2">
<property name="text">
<string>Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_scanMatching_3">
<property name="text">
<string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="12" column="0">
<item row="13" column="0">
<widget class="QDoubleSpinBox" name="localDetection_radius">
<property name="suffix">
<string> m</string>
@@ -9076,7 +8988,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="11" column="0">
<item row="12" column="0">
<widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
<property name="suffix">
<string/>
@@ -9121,22 +9033,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
<property name="suffix">
<string> rad/s</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_scanMatching_10">
<property name="text">
@@ -9170,6 +9066,130 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_153">
<property name="text">
<string>Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
<property name="suffix">
<string> rad</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_2">
<property name="text">
<string>When loading a database, ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_bunlde">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_space2">
<property name="text">
<string>Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_scanMatching_3">
<property name="text">
<string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
<property name="suffix">
<string> rad/s</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_scanMatching_12">
<property name="text">
<string>Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QCheckBox" name="rgbd_loopCovLimited">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>