new param cov_factor

src/icp.cpp

@@ -164,11 +164,9 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
 
     for (int i = 0; i < 3; ++i)
       for (int j = 0; j < 3; ++j)
-        result.res_covar(i, j) =
-            // 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];//*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
+        result.res_covar(i, j) = _icp_params.cov_factor *
+            // 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]; //*_current_scan_params.range_max_/100*_current_scan_params.range_max_/100; // This does the same
   } else {
     std::cout << "ICP valid != 1, providing first guess transformation and "
                  "identity covariance\n";

src/icp.h

@@ -18,6 +18,8 @@ struct icpOutput{
 };
 
 struct icpParams{
+    double cov_factor;
+
     int use_point_to_line_distance;
     double max_correspondence_dist;
     int max_iterations;
@@ -46,21 +48,21 @@ struct icpParams{
     double gpm_theta_bin_size_deg;
     double gpm_extend_range_deg;
     double gpm_interval;
-    double laser_x;
-    double laser_y;
-    double laser_theta;
+    //double laser_x;
+    //double laser_y;
+    //double laser_theta;
     double min_reading;
     double max_reading;
     int use_ml_weights;
     int use_sigma_weights;
-    double hsm_linear_cell_size;
-    double hsm_angular_cell_size_deg;
-    double hsm_num_angular_hypotheses;
-    double hsm_xc_directions_min_distance_deg;
-    double hsm_xc_ndirections;
-    double hsm_angular_hyp_min_distance_deg;
-    double hsm_linear_xc_max_npeaks;
-    double hsm_linear_xc_peaks_min_distance;
+    //double hsm_linear_cell_size;
+    //double hsm_angular_cell_size_deg;
+    //double hsm_num_angular_hypotheses;
+    //double hsm_xc_directions_min_distance_deg;
+    //double hsm_xc_ndirections;
+    //double hsm_angular_hyp_min_distance_deg;
+    //double hsm_linear_xc_max_npeaks;
+    //double hsm_linear_xc_peaks_min_distance;
 };
 
 class ICP