From 9eaf1feb1d5ef649f61931b628d3230a982033ff Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Wed, 3 Jul 2019 14:05:34 +0200
Subject: [PATCH] Covar: Hardocded. Need to be correctly map from gsl
---
src/icp.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index 617a09b..cf83798 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -94,8 +94,8 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
result.nvalid = csm_output.nvalid;
result.error = csm_output.error;
- std::cout << "ICP: add covariance!" << '\n';
- // result.res_covar = Eigen::Zeros();
+
+ result.res_covar = Eigen::Matrix3s::Identity()*0.1;
// std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
// std::cout << "Number of iterations: " << csm_output.iterations << '\n';
--
GitLab