RegistrationIcp: Fixed invalid variance set when structural complexity is low and input doesn't have normals. DBViewer: added check on angular error after optimizing the graph when adding a new constraint. Memory: added warning if odom angular variance detected is very high.

corelib/src/Memory.cpp

@@ -801,6 +801,15 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
 					UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
+					double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
+					if(maxAngVar != 1.0 && maxAngVar > 0.1)
+					{
+						UWARN("Very large angular variance (%f) detected! Please fix odometry "
+								"twist covariance, otherwise poor graph optimizations are "
+								"expected and wrong loop closure detections creating a lot "
+								"of errors in the map could be accepted.", maxAngVar);
+					}
+
 					cv::Mat infMatrix;
 					if(_covOffDiagonalIgnored)
 					{

corelib/src/RegistrationIcp.cpp

@@ -1046,17 +1046,29 @@ Transform RegistrationIcp::computeTransformationImpl(
 							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
 							icpT = guess * t.inverse() * guessInv;
 
-							// we were using normals, so compute correspondences using normals
+							if(fromScan.hasNormals() && toScan.hasNormals())
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
+							{
-							pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+								// we were using normals, so compute correspondences using normals
+								pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
+								pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
 
-							util3d::computeVarianceAndCorrespondences(
+								util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
+										fromCloudNormalsRegistered,
-									toCloudNormals,
+										toCloudNormals,
-									_maxCorrespondenceDistance,
+										_maxCorrespondenceDistance,
-									_maxRotation,
+										_maxRotation,
-									variance,
+										variance,
-									correspondences);
+										correspondences);
+							}
+							else
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudRegistered,
+										toCloudFiltered,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
+							}
 						}
 						else
 						{
@@ -1097,6 +1109,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					// verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
 					int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
 					UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
+
 					if(maxLaserScans)
 					{
 						correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -1125,10 +1138,17 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);
 
-					info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					if(correspondences == 0)
+					{
+						UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
+					}
+					else
+					{
+						info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					}
 					info.icpInliersRatio = correspondencesRatio;
 
-					if(correspondencesRatio < _correspondenceRatio)
+					if(correspondencesRatio <= _correspondenceRatio)
 					{
 						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
 								correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);

corelib/src/Rtabmap.cpp

@@ -1203,6 +1203,7 @@ bool Rtabmap::process(
 						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
 						if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
 						{
+							std::cout << info.covariance << std::endl;
 							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 						}
 					}

guilib/src/DatabaseViewer.cpp

@@ -6277,7 +6277,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
 		linksIn.insert(std::make_pair(newLink.from(), newLink));
 		const Link * maxLinearLink = 0;
+		const Link * maxAngularLink = 0;
 		float maxLinearErrorRatio = 0.0f;
+		float maxAngularErrorRatio = 0.0f;
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
@@ -6293,38 +6295,52 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		std::string msg;
 		if(poses.size())
 		{
+			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())
 				{
-					UASSERT(poses.find(iter->second.from())!=poses.end());
+					Transform t1 = uValue(poses, iter->second.from(), Transform());
-					UASSERT(poses.find(iter->second.to())!=poses.end());
+					Transform t2 = uValue(poses, iter->second.to(), Transform());
-					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()));
-					float stddev = sqrt(iter->second.transVariance());
+					float opt_roll,opt__pitch,opt__yaw;
-					float linearErrorRatio = linearError/stddev;
+					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 ratio = %f (link %d->%d)", maxLinearErrorRatio, maxLinearLink->from(), maxLinearLink->to());
+				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()));
-			}
+				if(maxLinearErrorRatio > maxOptimizationError)
-
+				{
-			if(maxLinearErrorRatio > maxOptimizationError)
+					msg = uFormat("Rejecting edge %d->%d because "
-			{
-				msg = uFormat("Rejecting edge %d->%d because "
 						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
@@ -6335,6 +6351,25 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 						  sqrt(maxLinearLink->transVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
+				}
+			}
+			if(maxAngularLink)
+			{
+				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+				if(maxAngularErrorRatio > maxOptimizationError)
+				{
+					msg = uFormat("Rejecting edge %d->%d because "
+						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
+						  "\"%s\" is %f.",
+						  newLink.from(),
+						  newLink.to(),
+						  maxAngularErrorRatio,
+						  maxAngularLink->from(),
+						  maxAngularLink->to(),
+						  sqrt(maxAngularLink->rotVariance()),
+						  Parameters::kRGBDOptimizeMaxError().c_str(),
+						  maxOptimizationError);
+				}
 			}
 		}
 		else