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 ¶ms); static void printLaserData(LDP &laser_data); -- GitLab