From 21a912cbc587f7423ff74605fe66bf34d8f1e0d6 Mon Sep 17 00:00:00 2001
From: Rainer Kuemmerle <rainer.kuemmerle@gmail.com>
Date: Thu, 17 Mar 2016 18:29:44 +0100
Subject: [PATCH] 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.
---
sm/csm/icp/icp.c | 13 +++++++++++--
1 file changed, 11 insertions(+), 2 deletions(-)
diff --git a/sm/csm/icp/icp.c b/sm/csm/icp/icp.c
index 8c5cb52..ec9362c 100644
--- a/sm/csm/icp/icp.c
+++ b/sm/csm/icp/icp.c
@@ -97,7 +97,8 @@ 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);
@@ -105,6 +106,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));
@@ -145,7 +147,14 @@ 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) {
val cov0_x, dx_dy1, dx_dy2;
--
GitLab