From 3fbe02363d7ebd40dc54ffef1fdbffc2a5013cc3 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 3 May 2021 17:45:46 +0200
Subject: [PATCH] checking if returned cov isCovariance ()

---
 src/processor/processor_loop_closure_icp.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index 32ab45917..cd3e809f4 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -1,5 +1,6 @@
 #include "laser/processor/processor_loop_closure_icp.h"
 #include "laser/sensor/sensor_laser_2d.h"
+#include "core/math/covariance.h"
 
 using namespace wolf;
 using namespace laserscanutils;
@@ -8,7 +9,8 @@ namespace wolf{
 // Constructor
 ProcessorLoopClosureIcp::ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params) :
     ProcessorBase("ProcessorLoopClosureIcp", 2, _params),
-    params_loop_closure_icp_(_params)
+    params_loop_closure_icp_(_params),
+    key_frames_skipped_(0)
 {
     icp_tools_ptr_ = std::make_shared<ICP>();
 }
@@ -218,6 +220,10 @@ CapturesAligned ProcessorLoopClosureIcp::bestCandidate(std::map<double, Captures
 }
 FactorBasePtr ProcessorLoopClosureIcp::emplaceFeatureAndFactor(CapturesAligned &_captures_aligned)
 {
+    assert(_captures_aligned.align_result.valid == 1);
+    if (not isCovariance(_captures_aligned.align_result.res_covar))
+        _captures_aligned.align_result.res_covar = 1e-4 * Eigen::Matrix3d::Identity();
+
     auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own,
                                                      _captures_aligned.align_result);
     return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr,
-- 
GitLab