diff --git a/src/icp.cpp b/src/icp.cpp
index b22a1aebdbf30861610a92b7690cec4905c8c711..a2ca9a996cc4f0abd3a4dacf60d32275c8a16916 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -91,6 +91,9 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
     result.res_transf(1) = csm_output.x[1];
     result.res_transf(2) = csm_output.x[2];
 
+    result.nvalid = csm_output.nvalid;
+    result.error      = csm_output.error;
+
     // 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';
diff --git a/src/icp.h b/src/icp.h
index 939565deae86071dac3997b64f9b89f891d883f4..eb58cacc73edfb098455a2087ef0f7433fc9741a 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -10,9 +10,10 @@ namespace laserscanutils{
 
 struct icpOutput{
     Eigen::Vector3s res_transf;
-    int num_points;
-    int error_points;
+    int nvalid;     /** Number of valid correspondence in the end */
+    double error;   	/** Total correspondence error */
 };
+
 struct icpParams{
     int use_point_to_line_distance;
     int max_correspondence_dist;
@@ -22,6 +23,7 @@ struct icpParams{
     double outliers_adaptive_order;
     double outliers_adaptive_mult;
 };
+
 class ICP
 {
     public: