Skip to content
Snippets Groups Projects
Commit 26da6eb1 authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

conflicts: solved conficts after pull

parent 90ac2ca0
No related branches found
No related tags found
1 merge request!1Resolve "icp: develop matching tools"
......@@ -72,14 +72,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
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.first_guess[0] = dis(generator)*3;
csm_input.first_guess[1] = dis(generator)*3;
double aux = dis(generator);
csm_input.first_guess[2] = -1.14*(1-aux) + 1.14*aux;
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 = 1;
......@@ -98,9 +93,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
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';
// 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;
}
......
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