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