From 835e4803b4258549679608e66b6e2fb8a50fb042 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 1 Oct 2019 18:14:52 +0200
Subject: [PATCH] returning initial guess if valid != 1
---
src/icp.cpp | 29 +++++++++++++++++++----------
1 file changed, 19 insertions(+), 10 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index 4cd599a..32a4184 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -93,19 +93,28 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con
sm_icp(&csm_input, &csm_output);
icpOutput result{};
- result.res_transf(0) = csm_output.x[0];
- result.res_transf(1) = csm_output.x[1];
- result.res_transf(2) = csm_output.x[2];
-
result.nvalid = csm_output.nvalid;
+ result.valid = csm_output.valid;
result.error = csm_output.error;
-
- 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); // NOT COMPILING
- csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; // This does the same
+ if (result.valid == 1)
+ {
+ result.res_transf(0) = csm_output.x[0];
+ result.res_transf(1) = csm_output.x[1];
+ result.res_transf(2) = csm_output.x[2];
+
+ 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); // NOT COMPILING
+ csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; // This does the same
+ }
+ else
+ {
+ std::cout << "ICP valid != 1, providing first guess transformation and identity covariance\n";
+ result.res_transf = _last_transf;
+ result.res_covar = Eigen::Matrix3s::Identity();
+ }
// std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
// std::cout << "Number of iterations: " << csm_output.iterations << '\n';
--
GitLab