Skip to content
Snippets Groups Projects
Commit ebc3cb77 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Added icp parameters as a parameter to the ICP matching function

parent 26da6eb1
No related branches found
No related tags found
1 merge request!1Resolve "icp: develop matching tools"
...@@ -53,7 +53,7 @@ ICP::~ICP() ...@@ -53,7 +53,7 @@ ICP::~ICP()
} }
icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf) icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf)
{ {
// Uncomment to enable debug messages from the CSM library // Uncomment to enable debug messages from the CSM library
// sm_debug_write(true); // sm_debug_write(true);
...@@ -76,19 +76,17 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar ...@@ -76,19 +76,17 @@ 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 = 1; csm_input.use_point_to_line_distance = icp_params.use_point_to_line_distance;
csm_input.max_correspondence_dist = icp_params.max_correspondence_dist;
csm_input.max_correspondence_dist = 1; csm_input.max_iterations = icp_params.max_iterations;
csm_input.max_iterations = 20; csm_input.use_corr_tricks = icp_params.use_corr_tricks;
csm_input.outliers_maxPerc = icp_params.outliers_maxPerc;
csm_input.use_corr_tricks = 1; csm_input.outliers_adaptive_order = icp_params.outliers_adaptive_order;
csm_input.outliers_maxPerc = 0.9; csm_input.outliers_adaptive_mult = icp_params.outliers_adaptive_mult;
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);
icp_output result{}; icpOutput 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];
......
...@@ -8,19 +8,27 @@ ...@@ -8,19 +8,27 @@
namespace laserscanutils{ namespace laserscanutils{
struct icp_output{ struct icpOutput{
Eigen::Vector3s res_transf; Eigen::Vector3s res_transf;
int num_points; int num_points;
int error_points; int error_points;
}; };
struct icpParams{
int use_point_to_line_distance;
int max_correspondence_dist;
int max_iterations;
int use_corr_tricks;
double outliers_maxPerc;
double outliers_adaptive_order;
double outliers_adaptive_mult;
};
class ICP class ICP
{ {
public: public:
ICP(); ICP();
~ICP(); ~ICP();
static icp_output matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, Eigen::Vector3s &_last_transf); static icpOutput matchPC(LaserScan &_last_ls, LaserScan &_reference_ls, LaserScanParams& params, icpParams &icp_params, Eigen::Vector3s &_last_transf);
static void printTwoLaserData(sm_params &params); static void printTwoLaserData(sm_params &params);
static void printLaserData(LDP &laser_data); 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