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

corelib/include/rtabmap/core/Graph.h

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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 = 1.0/information.at<double>(0,0);
-											loopClosureLinearVariance = info.covariance.at<double>(0,0);
+										loopClosureAngularVariance = 1.0/information.at<double>(5,5);
-											loopClosureAngularVariance = info.covariance.at<double>(3,3);
-										}
 									}
 								}
 								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
+					UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
-					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;
-						}
-					}
 				}
+
 				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)
+								graph::computeMaxGraphErrors(
-								{
+										optimizedPoses,
-									// ignore links with high variance
+										links,
-									if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
+										maxLinearErrorRatio,
-									{
+										maxAngularErrorRatio,
-										UASSERT(optimizedPoses.find(iter->second.from())!=optimizedPoses.end());
+										maxLinearError,
-										UASSERT(optimizedPoses.find(iter->second.to())!=optimizedPoses.end());
+										maxAngularError,
-										Transform t1 = optimizedPoses.at(iter->second.from());
+										&maxLinearLink,
-										Transform t2 = optimizedPoses.at(iter->second.to());
+										&maxAngularLink);
-										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;
-										}
-									}
-								}
 								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(maxAngularErrorRatio > _optimizationMaxError)
-
+									{
-								if(maxLinearError > _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). "
-									msg = uFormat("Rejecting edge %d->%d because "
+												  "\"%s\" is %f m.",
-											  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
+												  from,
-											  "\"%s\" is %f m.",
+												  to,
-											  from,
+												  maxAngularError*180.0f/M_PI,
-											  to,
+												  maxAngularLink->from(),
-											  maxLinearError,
+												  maxAngularLink->to(),
-											  maxLinearLink->from(),
+												  maxAngularErrorRatio,
-											  maxLinearLink->to(),
+												  sqrt(maxAngularLink->rotVariance()),
-											  maxAngularError*180.0f/M_PI,
+												  Parameters::kRGBDOptimizeMaxError().c_str(),
-											  maxAngularLink?maxAngularLink->from():0,
+												  _optimizationMaxError);
-											  maxAngularLink?maxAngularLink->to():0,
+									}
-											  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

@@ -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

@@ -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

@@ -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
+				for(int i=0; i<6; ++i)
-				info.covariance *= t.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
+					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)
+			graph::computeMaxGraphErrors(
-			{
+					poses,
-				// ignore links with high variance
+					links,
-				if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
+					maxLinearErrorRatio,
-				{
+					maxAngularErrorRatio,
-					Transform t1 = uValue(poses, iter->second.from(), Transform());
+					maxLinearError,
-					Transform t2 = uValue(poses, iter->second.to(), Transform());
+					maxAngularError,
-					Transform t = t1.inverse()*t2;
+					&maxLinearLink,
-					float linearError = uMax3(
+					&maxAngularLink);
-							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;
-					}
-				}
-			}
 			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

@@ -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

@@ -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)
+											float maxLinearErrorRatio = 0.0f;
-											{
+											float maxAngularErrorRatio = 0.0f;
-												// ignore links with high variance
+											graph::computeMaxGraphErrors(
-												if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
+													poses,
-												{
+													links,
-													UASSERT(poses.find(iter->second.from())!=poses.end());
+													maxLinearErrorRatio,
-													UASSERT(poses.find(iter->second.to())!=poses.end());
+													maxAngularErrorRatio,
-													Transform t1 = poses.at(iter->second.from());
+													maxLinearError,
-													Transform t2 = poses.at(iter->second.to());
+													maxAngularError,
-													UASSERT(!t1.isNull() && !t2.isNull());
+													&maxLinearLink,
-													Transform t = t1.inverse()*t2;
+													&maxAngularLink);
-													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;
-													}
-												}
-											}
 											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(maxAngularErrorRatio > optimizeMaxError)
-
+												{
-											if(maxLinearError > 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). "
-												msg = uFormat("Rejecting edge %d->%d because "
+															  "\"%s\" is %f m.",
-														  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
+															  from,
-														  "\"%s\" is %f m.",
+															  to,
-														  from,
+															  maxAngularError*180.0f/M_PI,
-														  to,
+															  maxAngularLink->from(),
-														  maxLinearError,
+															  maxAngularLink->to(),
-														  maxLinearLink->from(),
+															  maxAngularErrorRatio,
-														  maxLinearLink->to(),
+															  sqrt(maxAngularLink->rotVariance()),
-														  maxAngularError*180.0f/M_PI,
+															  Parameters::kRGBDOptimizeMaxError().c_str(),
-														  maxAngularLink?maxAngularLink->from():0,
+															  optimizeMaxError);
-														  maxAngularLink?maxAngularLink->to():0,
+												}
-														  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

@@ -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

@@ -94,9 +94,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-287</y>
+            <y>0</y>
-            <width>681</width>
+            <width>673</width>
-            <height>2943</height>
+            <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">
+                       <item row="11" 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">
                         <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">
+                       <item row="13" column="0">
-                        <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">
                         <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>