From 3443c3b8ead81aba0784cf65e7cff1aa679ec3c0 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 26 Jul 2017 15:27:54 -0400
Subject: [PATCH] RegistrationVis: fixed assert covariance in bundle
 adjustement when transform is perfect (covariance is null) by setting an
 epsilon to generated covariance is null or very close to 0

---
 corelib/src/Registration.cpp    | 12 ++++++------
 corelib/src/RegistrationVis.cpp | 18 ++++++++++++++++--
 2 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/corelib/src/Registration.cpp b/corelib/src/Registration.cpp
index 3f0d0f86..18282680 100644
--- a/corelib/src/Registration.cpp
+++ b/corelib/src/Registration.cpp
@@ -257,17 +257,17 @@ void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & t
 	}
 
 	double epsilon = 0.000001;
-	if(covariance.at<double>(0,0)<=0.0)
+	if(covariance.at<double>(0,0)<=epsilon)
 		covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(1,1)<=0.0)
+	if(covariance.at<double>(1,1)<=epsilon)
 		covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(2,2)<=0.0)
+	if(covariance.at<double>(2,2)<=epsilon)
 		covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(3,3)<=0.0)
+	if(covariance.at<double>(3,3)<=epsilon)
 		covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(4,4)<=0.0)
+	if(covariance.at<double>(4,4)<=epsilon)
 		covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(5,5)<=0.0)
+	if(covariance.at<double>(5,5)<=epsilon)
 		covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
 }
 
diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp
index ffec99c4..835498fa 100644
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -1234,6 +1234,20 @@ Transform RegistrationVis::computeTransformationImpl(
 					UINFO(msg.c_str());
 				}
 			}
+
+			double epsilon = 0.000001;
+			if(covariances[dir].at<double>(0,0)<=epsilon)
+				covariances[dir].at<double>(0,0) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(1,1)<=epsilon)
+				covariances[dir].at<double>(1,1) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(2,2)<=epsilon)
+				covariances[dir].at<double>(2,2) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(3,3)<=epsilon)
+				covariances[dir].at<double>(3,3) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(4,4)<=epsilon)
+				covariances[dir].at<double>(4,4) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(5,5)<=epsilon)
+				covariances[dir].at<double>(5,5) = epsilon; // epsilon if exact transform
 		}
 
 		if(!_forwardEstimateOnly)
@@ -1292,13 +1306,13 @@ Transform RegistrationVis::computeTransformationImpl(
 			poses.insert(std::make_pair(2, transforms[0]));
 
 			cv::Mat cov = covariances[0].clone();
-			normalizeCovariance(cov, transform);
+			normalizeCovariance(cov, transforms[0]);
 
 			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
 			if(!transforms[1].isNull() && inliers[1].size())
 			{
 				cov = covariances[1].clone();
-				normalizeCovariance(cov, transform);
+				normalizeCovariance(cov, transforms[1]);
 				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
 			}