icp new param factor to the max eigv of covariance

src/icp.cpp

@@ -170,11 +170,30 @@ icpOutput ICP::align(const LaserScan &_current_ls,
         result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
 
         if (csm_input.do_compute_covariance)
+        {
             for (int i = 0; i < 3; ++i)
                 for (int j = 0; j < 3; ++j)
                     result.res_covar(i, j) = _icp_params.cov_factor *
-                    csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
-        // gsl_matrix_get(csm_output.cov_x_m, i, j);   // NOT COMPILING
+                    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)
+            {
+                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(result.res_covar);
+
+                if (eigensolver.info() == Eigen::Success)
+                {
+                    Eigen::Vector3d 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();
+                }
+            }
+        }
     }
     else
     {

src/icp.h

@@ -118,6 +118,7 @@ struct icpParams
     // Covariance ------------------------------------------------------------------------
     bool do_compute_covariance; // Compute the matching covariance (method in http://purl.org/censi/2006/icpcov)
     double cov_factor;          // Factor multiplying the cov output of csm
+    double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm 
 
     void print() const
     {
@@ -151,6 +152,7 @@ struct icpParams
         std::cout << "sigma: " << std::to_string(sigma) << std::endl;
         std::cout << "do_compute_covariance: " << std::to_string(do_compute_covariance) << std::endl;
         std::cout << "cov_factor: " << std::to_string(cov_factor) << std::endl;
+        std::cout << "cov_max_eigv_factor: " << std::to_string(cov_max_eigv_factor) << std::endl;
     }
 };
 
@@ -165,7 +167,7 @@ const icpParams icp_params_default = {
     false, false, false, 10, // bool outliers_remove_doubles; bool do_visibility_test; bool do_alpha_test; double do_alpha_test_thresholdDeg;
     0.5, 4, // double clustering_threshold; int orientation_neighbourhood;
     false, false, 0.2, // bool use_ml_weights; bool use_sigma_weights; double sigma;
-    true, 5 // bool do_compute_covariance; double cov_factor;
+    true, 5, 2 // bool do_compute_covariance; double cov_factor; double cov_max_eigv_factor;
 };
 
 class ICP