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

sm: matchICP() is working after some tuning

See comments in the code and in the csm library
parent f8516946
No related branches found
No related tags found
1 merge request!1Resolve "icp: develop matching tools"
......@@ -18,7 +18,7 @@ public:
laser_data->readings[i] = it;
laser_data->valid[i] = 1;
}else{
laser_data->readings[i] = -1;
laser_data->readings[i] = NAN;
laser_data->valid[i] = 0;
}
laser_data->cluster[i] = -1;
......@@ -53,7 +53,8 @@ ICP::~ICP()
icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf)
{
sm_debug_write(true);
// Uncomment to enable debug messages from the CSM library
// sm_debug_write(true);
LDWrapper last = LDWrapper(_last_ls, params);
LDWrapper origin = LDWrapper(_origin_ls, params);
......@@ -73,15 +74,43 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
csm_input.first_guess[1] = _last_transf(1);
csm_input.first_guess[2] = _last_transf(2);
csm_input.use_point_to_line_distance = true;
csm_input.use_point_to_line_distance = 1;
csm_input.max_correspondence_dist = 1;
csm_input.max_iterations = 20;
csm_input.use_corr_tricks = 1;
csm_input.outliers_maxPerc = 0.9;
csm_input.outliers_adaptive_order = 0.7;
csm_input.outliers_adaptive_mult = 1.5;
sm_icp(&csm_input, &csm_output);
// std::cout << "Result: " << csm_output.valid << '\n';
// std::cout << "My solution " << csm_output.x[0] << "," << csm_output.x[1] << "," << csm_output.x[2] << std::endl;
icp_output 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];
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)
{
std::cout << "Laser Reading: " << laser_data->readings[0] << '\n';
}
void ICP::printTwoLaserData(sm_params &params)
{
for (int ii=0; ii<params.laser_ref->nrays-1; ++ii)
{
std::cout << "Theta: " << params.laser_ref->theta[ii] << "; Readings: "
<< params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
<< '\n';
}
}
......@@ -19,6 +19,9 @@ class ICP
~ICP();
static icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf);
static void printTwoLaserData(sm_params &params);
static void 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