Resolve "icp: develop matching tools"

Closes #23 (closed)

src/icp.cpp

@@ -92,7 +92,10 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
     result.res_transf(2) = csm_output.x[2];
 
     result.nvalid = csm_output.nvalid;
-    result.error      = csm_output.error;
+    result.error  = csm_output.error;
+
+    std::cout << "ICP: add covariance!" << '\n';
+    // result.res_covar = Eigen::Zeros();
 
     // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
     // std::cout << "Number of iterations: " << csm_output.iterations << '\n';