diff --git a/src/icp.cpp b/src/icp.cpp index a7c938e9782dc9ae7646ffc3a82fed9ae52409fa..3656a171a64b2f6d18f042147d1c9e5c4be41036 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -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 ¶ms) +{ + + 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'; + } + +} diff --git a/src/icp.h b/src/icp.h index 98c4527ae3e6437094c9ee9f80b2a38d834895f4..ef9dee6152e08144c02a96b1a8e1e084c1220eb4 100644 --- a/src/icp.h +++ b/src/icp.h @@ -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 ¶ms); + static void printLaserData(LDP &laser_data); }; }