Resolve "icp: develop matching tools"

Closes #23 (closed)

src/icp.cpp

 #include "icp.h"
-
 using namespace laserscanutils;
+unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
+std::mt19937 generator (seed);
+std::uniform_real_distribution<double> dis(0.0, 1.0);
 
 class LDWrapper {
 public:
@@ -69,9 +71,14 @@ 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] = _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.use_point_to_line_distance = true;