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