diff --git a/src/icp.cpp b/src/icp.cpp
index 4cd599a74fda199e9fcb843dc4abf785450c1e7b..32a41845f2f2b8650978af606fc9fae5816ee406 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -93,19 +93,28 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con
     sm_icp(&csm_input, &csm_output);
 
     icpOutput result{};
-    result.res_transf(0) = csm_output.x[0];
-    result.res_transf(1) = csm_output.x[1];
-    result.res_transf(2) = csm_output.x[2];
-
     result.nvalid = csm_output.nvalid;
+    result.valid  = csm_output.valid;
     result.error  = csm_output.error;
 
-
-    for(int i = 0; i < 3; ++i)
-        for(int j = 0; j < 3; ++j)
-            result.res_covar(i,j) =
-                    //gsl_matrix_get(csm_output.cov_x_m, i, j);                 // NOT COMPILING
-                    csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];  // This does the same
+    if (result.valid == 1)
+    {
+        result.res_transf(0) = csm_output.x[0];
+        result.res_transf(1) = csm_output.x[1];
+        result.res_transf(2) = csm_output.x[2];
+
+        for(int i = 0; i < 3; ++i)
+            for(int j = 0; j < 3; ++j)
+                result.res_covar(i,j) =
+                        //gsl_matrix_get(csm_output.cov_x_m, i, j);                 // NOT COMPILING
+                        csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];  // This does the same
+    }
+    else
+    {
+        std::cout << "ICP valid != 1, providing first guess transformation and identity covariance\n";
+        result.res_transf = _last_transf;
+        result.res_covar = Eigen::Matrix3s::Identity();
+    }
 
     // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
     // std::cout << "Number of iterations: " << csm_output.iterations << '\n';