icp more outputs

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

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;