From 70174adc19ae7add3451caaa172d0a9d1c3a95d7 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Mon, 9 Nov 2015 20:12:59 +0100 Subject: [PATCH 1/2] fix minor clang warning --- src/egsl/egsl.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/egsl/egsl.cpp b/src/egsl/egsl.cpp index 6ab5c092..3209bbf3 100644 --- a/src/egsl/egsl.cpp +++ b/src/egsl/egsl.cpp @@ -6,7 +6,6 @@ #include #include #include -using namespace std; #include "egsl.h" #include "egsl_imp.h" From 9fe62d37c2572aa6568e50e19ab792862b3b527c Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Thu, 17 Mar 2016 18:22:02 +0100 Subject: [PATCH 2/2] recompute correspondences in case of restarts recompute the correspondences in case of restarts. This computes the covariance using the best estimate and reports the used correspondences to the caller in the given LDP data. --- src/icp/icp.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/icp/icp.cpp b/src/icp/icp.cpp index 66915c9c..e22a2f28 100644 --- a/src/icp/icp.cpp +++ b/src/icp/icp.cpp @@ -93,6 +93,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { } else { /* It was succesfull */ + int restarted = 0; double best_error = error; gsl_vector * best_x = gsl_vector_alloc(3); gsl_vector_memcpy(best_x, x_new); @@ -100,6 +101,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { if(params->restart && (error/nvalid)>(params->restart_threshold_mean_error) ) { sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error)); + restarted = 1; double dt = params->restart_dt; double dth = params->restart_dtheta; sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth)); @@ -140,6 +142,13 @@ void sm_icp(struct sm_params*params, struct sm_result*res) { vector_to_array(best_x, res->x); sm_debug("icp: final x = %s \n", gsl_friendly_pose(best_x)); + if (restarted) { // recompute correspondences in case of restarts + ld_compute_world_coords(laser_sens, res->x); + if(params->use_corr_tricks) + find_correspondences_tricks(params); + else + find_correspondences(params); + } if(params->do_compute_covariance) {