From 1cc6ff5852d9b2c9baa1255ce739201fa84ee08b Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Wed, 3 Jul 2019 07:10:21 +0200
Subject: [PATCH] Minor changes

---
 src/icp.cpp | 5 ++++-
 src/icp.h   | 6 ++++--
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/icp.cpp b/src/icp.cpp
index a2ca9a9..617a09b 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -92,7 +92,10 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
     result.res_transf(2) = csm_output.x[2];
 
     result.nvalid = csm_output.nvalid;
-    result.error      = csm_output.error;
+    result.error  = csm_output.error;
+
+    std::cout << "ICP: add covariance!" << '\n';
+    // result.res_covar = Eigen::Zeros();
 
     // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
     // std::cout << "Number of iterations: " << csm_output.iterations << '\n';
diff --git a/src/icp.h b/src/icp.h
index eb58cac..fb222aa 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -3,15 +3,17 @@
 #include <random>
 #include "laser_scan.h"
 #include <csm/csm_all.h>
-
+#include <Eigen
 // using namespace CSM;
 
 namespace laserscanutils{
 
 struct icpOutput{
+    int valid; /** 1 if the result is valid */
     Eigen::Vector3s res_transf;
+    Eigen::Matrix3s res_covar;
     int nvalid;     /** Number of valid correspondence in the end */
-    double error;   	/** Total correspondence error */
+    double error;   	/** Total correspondence error */    
 };
 
 struct icpParams{
-- 
GitLab