Skip to content
Snippets Groups Projects
Commit 3fc3e4b4 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Minor cosmetic changes.

parent b996b95a
No related branches found
No related tags found
1 merge request!1Resolve "icp: develop matching tools"
......@@ -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;
}
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment