Skip to content
Snippets Groups Projects
Commit 7b6902f5 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

handling do_compute_covariance == false

parent d3598bb5
No related branches found
No related tags found
No related merge requests found
...@@ -157,24 +157,27 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con ...@@ -157,24 +157,27 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
result.valid = csm_output.valid; result.valid = csm_output.valid;
result.error = csm_output.error; result.error = csm_output.error;
if (result.valid == 1) { if (result.valid == 1)
{
result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100; result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100; result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100; result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
for (int i = 0; i < 3; ++i) if (csm_input.do_compute_covariance)
for (int j = 0; j < 3; ++j) for (int i = 0; i < 3; ++i)
result.res_covar(i, j) = _icp_params.cov_factor * for (int j = 0; j < 3; ++j)
// gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING result.res_covar(i, j) = _icp_params.cov_factor *
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 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 { // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
std::cout << "ICP valid != 1, providing first guess transformation and " }
"identity covariance\n"; else
{
std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
result.res_transf = _transf_ref_current; result.res_transf = _transf_ref_current;
result.res_covar = Eigen::Matrix3s::Identity(); result.res_covar = Eigen::Matrix3s::Identity();
} }
// std::cout << "Number of valid correspondences: " << csm_output.nvalid << //std::cout << "Number of valid correspondences: " << csm_output.nvalid <<
// '\n'; std::cout << "Number of iterations: " << csm_output.iterations << // '\n'; std::cout << "Number of iterations: " << csm_output.iterations <<
// '\n'; std::cout << "Error: " << csm_output.error << '\n'; // '\n'; std::cout << "Error: " << csm_output.error << '\n';
......
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