Skip to content
Snippets Groups Projects
Commit 3999cba3 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Removed duplicate align code, now there is only different headers no duplicated code.

parent 27408130
No related branches found
No related tags found
No related merge requests found
...@@ -128,70 +128,7 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con ...@@ -128,70 +128,7 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
//Legacy code //Legacy code
icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_transf_guess) icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, const LaserScanParams& scan_params, const icpParams &icp_params, Eigen::Vector3s &_transf_guess)
{ {
// Uncomment to enable debug messages from the CSM library return align(_current_ls, _ref_ls, scan_params, scan_params, icp_params, _transf_guess);
// sm_debug_write(true);
LDWrapper current = LDWrapper(_current_ls, scan_params);
LDWrapper ref = LDWrapper(_ref_ls, scan_params);
int num_rays = _current_ls.ranges_raw_.size();
sm_params csm_input{};
sm_result csm_output{};
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 = ref.laser_data;
csm_input.laser_sens = current.laser_data;
csm_input.first_guess[0] = _transf_guess(0);
csm_input.first_guess[1] = _transf_guess(1);
csm_input.first_guess[2] = _transf_guess(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.do_compute_covariance = 1;
sm_icp(&csm_input, &csm_output);
icpOutput result{};
result.nvalid = csm_output.nvalid;
result.valid = csm_output.valid;
result.error = csm_output.error;
if (result.valid == 1)
{
result.res_transf(0) = csm_output.x[0];
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
result.res_covar(i,j) =
//gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; // This does the same
}
else
{
std::cout << "ICP valid != 1, providing first guess transformation and identity covariance\n";
result.res_transf = _transf_guess;
result.res_covar = Eigen::Matrix3s::Identity();
}
// std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
// std::cout << "Number of iterations: " << csm_output.iterations << '\n';
// std::cout << "Error: " << csm_output.error << '\n';
return result;
} }
void ICP::printLaserData(LDP &laser_data) void ICP::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