Skip to content
Snippets Groups Projects

Resolve "icp: develop matching tools"

Merged Pep Martí Saumell requested to merge 23-icp-develop-matching-tools into master
2 files
+ 20
14
Compare changes
  • Side-by-side
  • Inline
Files
2
+ 9
11
@@ -53,7 +53,7 @@ ICP::~ICP()
@@ -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
// Uncomment to enable debug messages from the CSM library
// sm_debug_write(true);
// sm_debug_write(true);
@@ -76,19 +76,17 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
@@ -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[1] = _last_transf(1);
csm_input.first_guess[2] = _last_transf(2);
csm_input.first_guess[2] = _last_transf(2);
csm_input.use_point_to_line_distance = 1;
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_correspondence_dist = 1;
csm_input.max_iterations = icp_params.max_iterations;
csm_input.max_iterations = 20;
csm_input.use_corr_tricks = icp_params.use_corr_tricks;
csm_input.outliers_maxPerc = icp_params.outliers_maxPerc;
csm_input.use_corr_tricks = 1;
csm_input.outliers_adaptive_order = icp_params.outliers_adaptive_order;
csm_input.outliers_maxPerc = 0.9;
csm_input.outliers_adaptive_mult = icp_params.outliers_adaptive_mult;
csm_input.outliers_adaptive_order = 0.7;
csm_input.outliers_adaptive_mult = 1.5;
sm_icp(&csm_input, &csm_output);
sm_icp(&csm_input, &csm_output);
icp_output result{};
icpOutput result{};
result.res_transf(0) = csm_output.x[0];
result.res_transf(0) = csm_output.x[0];
result.res_transf(1) = csm_output.x[1];
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
result.res_transf(2) = csm_output.x[2];
Loading