Merge branch '23-icp-develop-matching-tools' of...

Merge branch '23-icp-develop-matching-tools' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/laser_scan_utils into 23-icp-develop-matching-tools

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:
@@ -70,9 +72,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 = 1;
 

src/icp.h

 // #include <csm/csm_all.h>
+#include <chrono>
+#include <random>
 #include "laser_scan.h"
 #include <csm/csm_all.h>