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