From 26da6eb1c3d7c16fe58537ccbf7860b0a5b991e9 Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Fri, 31 May 2019 12:40:29 +0200
Subject: [PATCH] conflicts: solved conficts after pull
---
src/icp.cpp | 17 ++++++-----------
1 file changed, 6 insertions(+), 11 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index d58d811..52b5a45 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -72,14 +72,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
csm_input.laser_ref = origin.laser_data;
csm_input.laser_sens = last.laser_data;
- // csm_input.first_guess[0] = _last_transf(0);
- // csm_input.first_guess[1] = _last_transf(1);
- // csm_input.first_guess[2] = _last_transf(2);
-
- csm_input.first_guess[0] = dis(generator)*3;
- csm_input.first_guess[1] = dis(generator)*3;
- double aux = dis(generator);
- csm_input.first_guess[2] = -1.14*(1-aux) + 1.14*aux;
+ csm_input.first_guess[0] = _last_transf(0);
+ csm_input.first_guess[1] = _last_transf(1);
+ csm_input.first_guess[2] = _last_transf(2);
csm_input.use_point_to_line_distance = 1;
@@ -98,9 +93,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
- 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';
+ // 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';
return result;
}
--
GitLab