diff --git a/src/icp.cpp b/src/icp.cpp
index 9f32bd33095df9142b2376a7655810f08e39d21a..74a79d18fab692822197d04ca7faadeba2fb4873 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -157,24 +157,27 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
   result.valid = csm_output.valid;
   result.error = csm_output.error;
 
-  if (result.valid == 1) {
+  if (result.valid == 1)
+  {
     result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
     result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
     result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
 
-    for (int i = 0; i < 3; ++i)
-      for (int j = 0; j < 3; ++j)
-        result.res_covar(i, j) = _icp_params.cov_factor *
-            // 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]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
-  } else {
-    std::cout << "ICP valid != 1, providing first guess transformation and "
-                 "identity covariance\n";
+    if (csm_input.do_compute_covariance)
+        for (int i = 0; i < 3; ++i)
+            for (int j = 0; j < 3; ++j)
+                result.res_covar(i, j) = _icp_params.cov_factor *
+                csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
+                // gsl_matrix_get(csm_output.cov_x_m, i, j);   // NOT COMPILING
+  }
+  else
+  {
+    std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
     result.res_transf = _transf_ref_current;
     result.res_covar = Eigen::Matrix3s::Identity();
   }
 
-  // std::cout << "Number of valid correspondences: " << csm_output.nvalid <<
+  //std::cout << "Number of valid correspondences: " << csm_output.nvalid <<
   // '\n'; std::cout << "Number of iterations: " << csm_output.iterations <<
   // '\n'; std::cout << "Error: " << csm_output.error << '\n';