diff --git a/src/icp.cpp b/src/icp.cpp index d58d8117095c00ae0b001cf281868628605b172f..52b5a459be059f2df0ecd7fe661411d98b6a0f2e 100644 --- a/src/icp.cpp +++ b/src/icp.cpp @@ -72,14 +72,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar 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.first_guess[0] = dis(generator)*3; - csm_input.first_guess[1] = dis(generator)*3; - double aux = dis(generator); - csm_input.first_guess[2] = -1.14*(1-aux) + 1.14*aux; + 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 = 1; @@ -98,9 +93,9 @@ icp_output ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPar 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'; + // 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; }