From 6b5990aa0424b3164624c8f19b53466d1f84ad88 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 2 Oct 2018 18:02:55 -0400
Subject: [PATCH] odom variance verification limited to yaw if 3dof
 registration/optimization is done

---
 corelib/src/Memory.cpp | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp
index 92152687..314b8f23 100644
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -824,14 +824,23 @@ 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));
+					double maxAngVar = 0.0;
+					if(_registrationPipeline->force3DoF())
+					{
+						maxAngVar = covariance.at<double>(5,5);
+					}
+					else
+					{
+						maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
+					}
+
 					if(maxAngVar != 1.0 && maxAngVar > 0.1)
 					{
 						static bool warned = false;
 						if(!warned)
 						{
 							UWARN("Very large angular variance (%f) detected! Please fix odometry "
-									"twist covariance, otherwise poor graph optimizations are "
+									"covariance, otherwise poor graph optimizations are "
 									"expected and wrong loop closure detections creating a lot "
 									"of errors in the map could be accepted. This message is only "
 									"printed once.", maxAngVar);