From 78bdd4a087b598a8998e6c69cfaa5d7f9daf09c0 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 12 Jul 2017 16:41:47 -0400
Subject: [PATCH] Added check if covariance is invertible, if not an error is
 thrown

---
 corelib/src/Memory.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp
index 4935be27..c41807aa 100644
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -718,7 +718,15 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 				if(!signature->getPose().isNull() &&
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
+					UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
 					cv::Mat infMatrix = covariance.inv();
+					if((uIsFinite(covariance.at<double>(0,0) && covariance.at<double>(0,0)>0)) &&
+						!(uIsFinite(infMatrix.at<double>(0,0) && infMatrix.at<double>(0,0)>0)))
+					{
+						UERROR("Failed to invert the covariance matrix! Covariance matrix should be invertible!");
+						std::cout << "Covariance: " << covariance << std::endl;
+						infMatrix = cv::Mat::eye(6,6,CV_64FC1);
+					}
 					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));