diff --git a/include/laser_scan_utils/icp.h b/include/laser_scan_utils/icp.h
index ec849b4ad01a660b7dda27dd0d525c65a01053d6..6cfbdb6cbada3619c6347473c7f4786e389606c5 100644
--- a/include/laser_scan_utils/icp.h
+++ b/include/laser_scan_utils/icp.h
@@ -39,6 +39,8 @@ namespace laserscanutils
         Eigen::Matrix3s res_covar;  // Covariance of the transformation
         int nvalid;                 // Number of valid correspondences in the match
         double error;               // Total correspondence error
+        double mean_error;          // Average error (total error / nvalid)
+        double points_ratio;        // Ratio of valid correspondences of totall amount of points
         unsigned int attempts;      // Number of ICP calls to obtain a valid result (<= params.icp_attempts)
     };
 
@@ -124,7 +126,7 @@ namespace laserscanutils
         double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm
 
         // Attempts ------------------------------------------------------------------
-        unsigned int icp_attempts; // number of icp attempts if result fails (not valid or error > restart_threshold_mean_error)
+        unsigned int icp_attempts;        // number of icp attempts if result fails (not valid or error > restart_threshold_mean_error)
         double perturbation_new_attempts; // perturbation noise amplitude applied to initial guess in new attempts
 
         void print() const
diff --git a/src/icp.cpp b/src/icp.cpp
index b32b58184b47af766fdf48024da48dce87eea39e..cb23c4fd2eb028e59a69d8a06e0ed64761f12699 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -178,6 +178,8 @@ icpOutput ICP::align(const LaserScan &_current_ls,
         {
             result.nvalid = csm_output.nvalid;
             result.error = csm_output.error;
+            result.mean_error = csm_output.error / csm_output.nvalid;
+            result.points_ratio = ((double)csm_output.nvalid) / ((double)num_rays);
             result.res_transf(0) = csm_output.x[0];
             result.res_transf(1) = csm_output.x[1];
             result.res_transf(2) = csm_output.x[2];
@@ -218,13 +220,13 @@ icpOutput ICP::align(const LaserScan &_current_ls,
         {
             std::cout << "Invalid result, trying again!" << std::endl;
         }
-        if (_icp_params.verbose and result.error > _icp_params.restart_threshold_mean_error and result.attempts < _icp_params.icp_attempts)
+        if (_icp_params.verbose and result.mean_error > _icp_params.restart_threshold_mean_error and result.attempts < _icp_params.icp_attempts)
         {
-            std::cout << "Error too big: " << result.error
+            std::cout << "Error too big: " << result.mean_error
                       << " ( should be < "
                       << _icp_params.restart_threshold_mean_error << "). Trying again!" << std::endl;
         }
-    } while ((not result.valid or result.error > _icp_params.restart_threshold_mean_error) and
+    } while ((not result.valid or result.mean_error > _icp_params.restart_threshold_mean_error) and
              result.attempts < _icp_params.icp_attempts);
 
     return result;