checking if returned cov isCovariance ()

src/processor/processor_loop_closure_icp.cpp

 #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,