Merge branch 'master' into 26-implementation-of-falko-lib

src/icp.cpp

@@ -17,8 +17,8 @@ public:
         for(auto it : scan.ranges_raw_){
             laser_data->theta[i] = scan_params.angle_min_ + i*scan_params.angle_step_;
 
-            if(scan_params.range_min_ <= it and it <= scan_params.range_max_){
-                laser_data->readings[i] = it;
+            if(scan_params.range_min_ <= it and it <= scan_params.range_max_ and 0 <= it and it <= 100){
+                laser_data->readings[i] = it;//*100/scan_params.range_max_;
                 laser_data->valid[i] = 1;
             }else{
                 laser_data->readings[i] = NAN;
@@ -157,26 +157,27 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
   result.valid = csm_output.valid;
   result.error = csm_output.error;
 
-  if (result.valid == 1) {
-    result.res_transf(0) = csm_output.x[0];
-    result.res_transf(1) = csm_output.x[1];
-    result.res_transf(2) = csm_output.x[2];
-
-    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]; // This does the same
-  } else {
-    std::cout << "ICP valid != 1, providing first guess transformation and "
-                 "identity covariance\n";
+  if (result.valid == 1)
+  {
+    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;
+
+    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
+  }
+  else
+  {
+    std::cout << "ICP NOT VALID, providing first guess transformation and identity covariance\n";
     result.res_transf = _transf_ref_current;
     result.res_covar = Eigen::Matrix3s::Identity();
   }
 
-  // std::cout << "Number of valid correspondences: " << csm_output.nvalid <<
+  //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';
 

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