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

returning initial guess if valid != 1

parent d61637c5
No related branches found
No related tags found
1 merge request!1Resolve "icp: develop matching tools"
......@@ -93,19 +93,28 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con
sm_icp(&csm_input, &csm_output);
icpOutput result{};
result.res_transf(0) = csm_output.x[0];
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
result.nvalid = csm_output.nvalid;
result.valid = csm_output.valid;
result.error = csm_output.error;
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
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 = _last_transf;
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';
......
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