improving icp cov treatment

src/icp.cpp

@@ -165,9 +165,9 @@ icpOutput ICP::align(const LaserScan &_current_ls,
 
     if (result.valid)
     {
-        result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
-        result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
-        result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
+        result.res_transf(0) = csm_output.x[0];
+        result.res_transf(1) = csm_output.x[1];
+        result.res_transf(2) = csm_output.x[2];
 
         if (csm_input.do_compute_covariance)
         {
@@ -176,21 +176,21 @@ icpOutput ICP::align(const LaserScan &_current_ls,
                     result.res_covar(i, j) = _icp_params.cov_factor *
                     csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];
             
-            // Multiply variance in bigger eigenvalue direction
-            if (abs(_icp_params.cov_factor - 1) > 1e3)
+            // Grow covariance in the biggest eigenvalue direction
+            if (_icp_params.cov_max_eigv_factor - 1 > 1e-6)
             {
-                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(result.res_covar);
+                Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(result.res_covar.topLeftCorner<2,2>());
 
                 if (eigensolver.info() == Eigen::Success)
                 {
-                    Eigen::Vector3d eigvs = eigensolver.eigenvalues();
+                    Eigen::Vector2d eigvs = eigensolver.eigenvalues();
                     Eigen::Index maxRow, maxCol;
                     float max_eigv = eigvs.maxCoeff(&maxRow, &maxCol);
 
-                    eigvs(maxRow) = _icp_params.cov_factor * max_eigv;
-                    result.res_covar = eigensolver.eigenvectors() *
-                                       eigvs.asDiagonal() *
-                                       eigensolver.eigenvectors().transpose();
+                    eigvs(maxRow) = _icp_params.cov_max_eigv_factor * max_eigv;
+                    result.res_covar.topLeftCorner<2,2>() = eigensolver.eigenvectors() *
+                                                            eigvs.asDiagonal() *
+                                                            eigensolver.eigenvectors().transpose();
                 }
             }
         }