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)
 					{