diff --git a/sm/csm/icp/icp.c b/sm/csm/icp/icp.c
index a96838ca90973ae0e9e7c1d2abaef55f3d71d653..4472ea2792afd6bd7c0b7d71f7d807c3ef864737 100644
--- a/sm/csm/icp/icp.c
+++ b/sm/csm/icp/icp.c
@@ -175,10 +175,11 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 		res->iterations = iterations;
 		res->nvalid = nvalid;
 
-		gsl_vector_free(x_new);
-		gsl_vector_free(x_old);
 		gsl_vector_free(best_x);
 	}
+	gsl_vector_free(x_new);
+	gsl_vector_free(x_old);
+
 
 	egsl_pop_named("sm_icp");