Merge pull request AndreaCensi#18 from RainerKuemmerle/csm_eigen

correspondences restart patch
clearpathrobotics · Mar 17, 2016 · d8ce2bf · d8ce2bf
2 parents b67ba21 + 9fe62d3
commit d8ce2bf
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 1 deletion.
diff --git a/src/egsl/egsl.cpp b/src/egsl/egsl.cpp
@@ -6,7 +6,6 @@
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
-using namespace std;
 
 #include "egsl.h"
 #include "egsl_imp.h"

diff --git a/src/icp/icp.cpp b/src/icp/icp.cpp
@@ -93,13 +93,15 @@ 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);
 
 		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)  {