From 3fc3e4b4dafb5ae20a0ac1962df23011ff7286f7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 7 Aug 2019 10:56:04 +0200
Subject: [PATCH] Minor cosmetic changes.

---
 src/icp.cpp | 31 +++++++++++++++----------------
 src/icp.h   |  2 +-
 2 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/icp.cpp b/src/icp.cpp
index 936e196..0987553 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -53,38 +53,39 @@ ICP::~ICP()
 
 }
 
-icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf)
+icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& scan_params, icpParams &icp_params, Eigen::Vector3s &_last_transf)
 {
     // Uncomment to enable debug messages from the CSM library
     // sm_debug_write(true);
 
-    LDWrapper last   = LDWrapper(_last_ls, params);
-    LDWrapper origin = LDWrapper(_origin_ls, params);
+    LDWrapper last   = LDWrapper(_last_ls, scan_params);
+    LDWrapper origin = LDWrapper(_origin_ls, scan_params);
 
     int num_rays = _last_ls.ranges_raw_.size();
 
     sm_params csm_input{};
     sm_result csm_output{};
 
-    csm_input.min_reading = params.range_min_;
-    csm_input.max_reading = params.range_max_;
+    csm_input.min_reading   = scan_params.range_min_;
+    csm_input.max_reading   = scan_params.range_max_;
+    csm_input.sigma         = scan_params.range_std_dev_;
 
-    csm_input.laser_ref  = origin.laser_data;
-    csm_input.laser_sens = last.laser_data;
+    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.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;
+    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;
 
-    csm_input.do_compute_covariance = 1;
+    csm_input.do_compute_covariance     = 1;
 
     sm_icp(&csm_input, &csm_output);
 
@@ -107,8 +108,6 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
     // std::cout << "Number of iterations: " << csm_output.iterations << '\n';
     // std::cout << "Error: " << csm_output.error << '\n';
 
-    std::cout << __FILE__ << __LINE__ << "cov: " << result.res_covar << std::endl;
-
     return result;
 }
 
diff --git a/src/icp.h b/src/icp.h
index a4c2861..603b2c9 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -33,7 +33,7 @@ class ICP
         ICP();
         ~ICP();
 
-    static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf);
+    static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& scan_params, icpParams &icp_params, Eigen::Vector3s &_last_transf);
 
     static void printTwoLaserData(sm_params &params);
     static void printLaserData(LDP &laser_data);
-- 
GitLab