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