diff --git a/src/icp.cpp b/src/icp.cpp
index 066786e41941cf06cd10ef7b237bf9c66edd8b28..b32b58184b47af766fdf48024da48dce87eea39e 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -218,7 +218,7 @@ icpOutput ICP::align(const LaserScan &_current_ls,
         {
             std::cout << "Invalid result, trying again!" << std::endl;
         }
-        if (_icp_params.verbose and not result.valid and result.attempts < _icp_params.icp_attempts)
+        if (_icp_params.verbose and result.error > _icp_params.restart_threshold_mean_error and result.attempts < _icp_params.icp_attempts)
         {
             std::cout << "Error too big: " << result.error
                       << " ( should be < "