Skip to content
Snippets Groups Projects

Resolve "icp: develop matching tools"

Merged Pep Martí Saumell requested to merge 23-icp-develop-matching-tools into master
2 files
+ 37
5
Compare changes
  • Side-by-side
  • Inline
Files
2
+ 34
5
@@ -18,7 +18,7 @@ public:
@@ -18,7 +18,7 @@ public:
laser_data->readings[i] = it;
laser_data->readings[i] = it;
laser_data->valid[i] = 1;
laser_data->valid[i] = 1;
}else{
}else{
laser_data->readings[i] = -1;
laser_data->readings[i] = NAN;
laser_data->valid[i] = 0;
laser_data->valid[i] = 0;
}
}
laser_data->cluster[i] = -1;
laser_data->cluster[i] = -1;
@@ -53,7 +53,8 @@ ICP::~ICP()
@@ -53,7 +53,8 @@ ICP::~ICP()
icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf)
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 last = LDWrapper(_last_ls, params);
LDWrapper origin = LDWrapper(_origin_ls, params);
LDWrapper origin = LDWrapper(_origin_ls, params);
@@ -73,15 +74,43 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
@@ -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[1] = _last_transf(1);
csm_input.first_guess[2] = _last_transf(2);
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);
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{};
icp_output result{};
result.res_transf(0) = csm_output.x[0];
result.res_transf(0) = csm_output.x[0];
result.res_transf(1) = csm_output.x[1];
result.res_transf(1) = csm_output.x[1];
result.res_transf(2) = csm_output.x[2];
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;
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';
 
}
 
 
}
Loading