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
1 file
+ 19
10
Compare changes
  • Side-by-side
  • Inline
+ 19
10
@@ -28,13 +28,13 @@ public:
++i;
}
laser_data->odometry[0] = 0.0;
laser_data->odometry[1] = 0.0;
laser_data->odometry[2] = 0.0;
laser_data->true_pose[0] = 0.0;
laser_data->true_pose[1] = 0.0;
laser_data->true_pose[2] = 0.0;
// laser_data->odometry[0] = 1/0.0;
// laser_data->odometry[1] = 1/0.0;
// laser_data->odometry[2] = 1/0.0;
//
// laser_data->true_pose[0] = 1/0.0;
// laser_data->true_pose[1] = 1/0.0;
// laser_data->true_pose[2] = 1/0.0;
}
~LDWrapper(){
ld_free(laser_data);
@@ -61,18 +61,27 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar
int num_rays = _last_ls.ranges_raw_.size();
sm_params csm_input{};
sm_result csm_output{};
csm_input.laser_ref = last.laser_data;
csm_input.laser_sens = origin.laser_data;
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.use_point_to_line_distance = true;
//TODO: min_theta and max_theta should come from LaserScanParams
last.laser_data->min_theta = last.laser_data->theta[0];
last.laser_data->max_theta = last.laser_data->theta[num_rays-1];
origin.laser_data->min_theta = origin.laser_data->theta[0];
origin.laser_data->max_theta = origin.laser_data->theta[num_rays-1];
sm_icp(&csm_input, &csm_output);
std::cout << "My solution " << csm_output.x[0] << "," << csm_output.x[1] << "," << csm_output.x[2] << std::endl;
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];
return result;
}
Loading