diff --git a/src/icp.cpp b/src/icp.cpp index cee049597e9ca3928ea6d3fe2daa94ff81fa8c45..9f32bd33095df9142b2376a7655810f08e39d21a 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -164,11 +164,9 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con 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];//*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same + result.res_covar(i, j) = _icp_params.cov_factor * + // 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]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same } else { std::cout << "ICP valid != 1, providing first guess transformation and " "identity covariance\n"; diff --git a/src/icp.h b/src/icp.h index 3c10a43c89a30bfe83a91f41755ff6f3adeacd50..1f3195588ddbe58cef6cc58461487bc100eeca15 100644 --- a/src/icp.h +++ b/src/icp.h @@ -18,6 +18,8 @@ struct icpOutput{ }; struct icpParams{ + double cov_factor; + int use_point_to_line_distance; double max_correspondence_dist; int max_iterations; @@ -46,21 +48,21 @@ struct icpParams{ double gpm_theta_bin_size_deg; double gpm_extend_range_deg; double gpm_interval; - double laser_x; - double laser_y; - double laser_theta; + //double laser_x; + //double laser_y; + //double laser_theta; double min_reading; double max_reading; int use_ml_weights; int use_sigma_weights; - double hsm_linear_cell_size; - double hsm_angular_cell_size_deg; - double hsm_num_angular_hypotheses; - double hsm_xc_directions_min_distance_deg; - double hsm_xc_ndirections; - double hsm_angular_hyp_min_distance_deg; - double hsm_linear_xc_max_npeaks; - double hsm_linear_xc_peaks_min_distance; + //double hsm_linear_cell_size; + //double hsm_angular_cell_size_deg; + //double hsm_num_angular_hypotheses; + //double hsm_xc_directions_min_distance_deg; + //double hsm_xc_ndirections; + //double hsm_angular_hyp_min_distance_deg; + //double hsm_linear_xc_max_npeaks; + //double hsm_linear_xc_peaks_min_distance; }; class ICP