From ebc3cb778233fc3a227c8604702c1dc62da8ac01 Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Tue, 11 Jun 2019 15:01:46 +0200
Subject: [PATCH] Added icp parameters as a parameter to the ICP matching
 function

---
 src/icp.cpp | 20 +++++++++-----------
 src/icp.h   | 14 +++++++++++---
 2 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/icp.cpp b/src/icp.cpp
index 52b5a45..b22a1ae 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -53,7 +53,7 @@ ICP::~ICP()
 
 }
 
-icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf)
+icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf)
 {
     // Uncomment to enable debug messages from the CSM library
     // sm_debug_write(true);
@@ -76,19 +76,17 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
     csm_input.first_guess[1] = _last_transf(1);
     csm_input.first_guess[2] = _last_transf(2);
 
-    csm_input.use_point_to_line_distance = 1;
-
-    csm_input.max_correspondence_dist = 1;
-    csm_input.max_iterations = 20;
-
-    csm_input.use_corr_tricks = 1;
-    csm_input.outliers_maxPerc = 0.9;
-    csm_input.outliers_adaptive_order = 0.7;
-    csm_input.outliers_adaptive_mult = 1.5;
+    csm_input.use_point_to_line_distance = icp_params.use_point_to_line_distance;
+    csm_input.max_correspondence_dist = icp_params.max_correspondence_dist;
+    csm_input.max_iterations = icp_params.max_iterations;
+    csm_input.use_corr_tricks = icp_params.use_corr_tricks;
+    csm_input.outliers_maxPerc = icp_params.outliers_maxPerc;
+    csm_input.outliers_adaptive_order = icp_params.outliers_adaptive_order;
+    csm_input.outliers_adaptive_mult = icp_params.outliers_adaptive_mult;
 
     sm_icp(&csm_input, &csm_output);
 
-    icp_output result{};
+    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];
diff --git a/src/icp.h b/src/icp.h
index f0c5a97..939565d 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -8,19 +8,27 @@
 
 namespace laserscanutils{
 
-struct icp_output{
+struct icpOutput{
     Eigen::Vector3s res_transf;
     int num_points;
     int error_points;
 };
-
+struct icpParams{
+    int use_point_to_line_distance;
+    int max_correspondence_dist;
+    int max_iterations;
+    int use_corr_tricks;
+    double outliers_maxPerc;
+    double outliers_adaptive_order;
+    double outliers_adaptive_mult;
+};
 class ICP
 {
     public:
         ICP();
         ~ICP();
 
-    static icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf);
+    static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf);
 
     static void printTwoLaserData(sm_params &params);
     static void printLaserData(LDP &laser_data);
-- 
GitLab