Resolve "icp: develop matching tools"

Closes #23 (closed)

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;
 }