From 284d3dcc37cdc7448d98eb1284d59ca79caf9c13 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 22 Jul 2019 09:55:24 +0200
Subject: [PATCH] Getting Censi's covariance and transforming it to Eigen

---
 src/icp.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/icp.cpp b/src/icp.cpp
index cf83798..55a92f4 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -84,6 +84,8 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
     csm_input.outliers_adaptive_order = icp_params.outliers_adaptive_order;
     csm_input.outliers_adaptive_mult = icp_params.outliers_adaptive_mult;
 
+    csm_input.do_compute_covariance = 1;
+
     sm_icp(&csm_input, &csm_output);
 
     icpOutput result{};
@@ -95,7 +97,9 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
     result.error  = csm_output.error;
 
 
-    result.res_covar = Eigen::Matrix3s::Identity()*0.1;
+    for(int i = 0; i < 3; ++i)
+        for(int j = 0; j < 3; ++j)
+            result.res_covar(i,j) = gsl_matrix_get(csm_output.cov_x_m, i, j);
 
     // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
     // std::cout << "Number of iterations: " << csm_output.iterations << '\n';
-- 
GitLab