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 <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
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)  {