From a7930fe814438aa0234ec9ed883389ab2d1104a5 Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Thu, 30 May 2019 15:55:52 +0200
Subject: [PATCH] Using random inicialization for debugging purposes

---
 src/icp.cpp | 15 +++++++++++----
 src/icp.h   |  2 ++
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/src/icp.cpp b/src/icp.cpp
index a7c938e..07bbd60 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -1,6 +1,8 @@
 #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;
 
diff --git a/src/icp.h b/src/icp.h
index 98c4527..25e5f4b 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -1,4 +1,6 @@
 // #include <csm/csm_all.h>
+#include <chrono>
+#include <random>
 #include "laser_scan.h"
 #include <csm/csm_all.h>
 
-- 
GitLab