From 7b6902f507ea792e0c46057abb3ac419f1872533 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 3 May 2021 12:27:27 +0200
Subject: [PATCH] handling do_compute_covariance == false
---
src/icp.cpp | 23 +++++++++++++----------
1 file changed, 13 insertions(+), 10 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index 9f32bd3..74a79d1 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -157,24 +157,27 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
result.valid = csm_output.valid;
result.error = csm_output.error;
- if (result.valid == 1) {
+ if (result.valid == 1)
+ {
result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- result.res_covar(i, j) = _icp_params.cov_factor *
- // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
- csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
- } else {
- std::cout << "ICP valid != 1, providing first guess transformation and "
- "identity covariance\n";
+ if (csm_input.do_compute_covariance)
+ for (int i = 0; i < 3; ++i)
+ for (int j = 0; j < 3; ++j)
+ result.res_covar(i, j) = _icp_params.cov_factor *
+ csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
+ // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
+ }
+ else
+ {
+ std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
result.res_transf = _transf_ref_current;
result.res_covar = Eigen::Matrix3s::Identity();
}
- // std::cout << "Number of valid correspondences: " << csm_output.nvalid <<
+ //std::cout << "Number of valid correspondences: " << csm_output.nvalid <<
// '\n'; std::cout << "Number of iterations: " << csm_output.iterations <<
// '\n'; std::cout << "Error: " << csm_output.error << '\n';
--
GitLab