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