Add all CSM params

src/icp.cpp

@@ -13,7 +13,6 @@ public:
     {
         int num_rays = scan.ranges_raw_.size();
         laser_data = ld_alloc_new(num_rays);
-
         int i = 0;
         for(auto it : scan.ranges_raw_){
             laser_data->theta[i] = scan_params.angle_min_ + i*scan_params.angle_step_;
@@ -29,9 +28,6 @@ public:
             ++i;
         }
 
-        // for(int i = 0; i < num_rays; ++i){
-        //     laser_data->theta[i] = laser_data->theta[i] * 0.0175;
-        // }
         laser_data->min_theta = laser_data->theta[0];
         laser_data->max_theta = laser_data->theta[num_rays-1];
 
@@ -89,8 +85,70 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
   csm_input.outliers_maxPerc = _icp_params.outliers_maxPerc;
   csm_input.outliers_adaptive_order = _icp_params.outliers_adaptive_order;
   csm_input.outliers_adaptive_mult = _icp_params.outliers_adaptive_mult;
-
-  csm_input.do_compute_covariance = 1;
+  csm_input.do_compute_covariance = _icp_params.do_compute_covariance;
+
+  csm_input.max_angular_correction_deg = _icp_params.max_angular_correction_deg;
+  csm_input.max_linear_correction = _icp_params.max_linear_correction;
+  csm_input.epsilon_xy = _icp_params.epsilon_xy;
+  csm_input.epsilon_theta = _icp_params.epsilon_theta;
+  csm_input.sigma = _icp_params.sigma;
+  csm_input.restart = _icp_params.restart;
+  csm_input.restart_threshold_mean_error = _icp_params.restart_threshold_mean_error;
+  csm_input.restart_dt = _icp_params.restart_dt;
+  csm_input.restart_dtheta = _icp_params.restart_dtheta;
+  csm_input.clustering_threshold = _icp_params.clustering_threshold;
+  csm_input.orientation_neighbourhood = _icp_params.orientation_neighbourhood;
+  csm_input.do_alpha_test = _icp_params.do_alpha_test;
+  csm_input.do_alpha_test_thresholdDeg = _icp_params.do_alpha_test_thresholdDeg;
+  csm_input.do_visibility_test = _icp_params.do_visibility_test;
+  csm_input.outliers_remove_doubles = _icp_params.outliers_remove_doubles;
+  csm_input.debug_verify_tricks = _icp_params.debug_verify_tricks;
+  csm_input.gpm_theta_bin_size_deg = _icp_params.gpm_theta_bin_size_deg;
+  csm_input.gpm_extend_range_deg = _icp_params.gpm_extend_range_deg;
+  csm_input.gpm_interval = _icp_params.gpm_interval;
+
+  // std::cout<< "=========================================================================================================================================\n";
+  // std::cout << "csm max_angular_correction_deg " << csm_input.max_angular_correction_deg << " icp max_angular_correction_deg " << _icp_params.max_angular_correction_deg << "\n";
+  // std::cout << "csm max_linear_correction " << csm_input.max_linear_correction << " icp max_linear_correction " << _icp_params.max_linear_correction << "\n";
+  // std::cout << "csm epsilon_xy " << csm_input.epsilon_xy << " icp epsilon_xy " << _icp_params.epsilon_xy << "\n";
+  // std::cout << "csm epsilon_theta " << csm_input.epsilon_theta << " icp epsilon_theta " << _icp_params.epsilon_theta << "\n";
+  // std::cout << "csm sigma " << csm_input.sigma << " icp sigma " << _icp_params.sigma << "\n";
+  // std::cout << "csm restart " << csm_input.restart << " icp restart " << _icp_params.restart << "\n";
+  // std::cout << "csm restart_threshold_mean_error " << csm_input.restart_threshold_mean_error << " icp restart_threshold_mean_error " << _icp_params.restart_threshold_mean_error << "\n";
+  // std::cout << "csm restart_dt " << csm_input.restart_dt << " icp restart_dt " << _icp_params.restart_dt << "\n";
+  // std::cout << "csm restart_dtheta " << csm_input.restart_dtheta << " icp restart_dtheta " << _icp_params.restart_dtheta << "\n";
+  // std::cout << "csm clustering_threshold " << csm_input.clustering_threshold << " icp clustering_threshold " << _icp_params.clustering_threshold << "\n";
+  // std::cout << "csm orientation_neighbourhood " << csm_input.orientation_neighbourhood << " icp orientation_neighbourhood " << _icp_params.orientation_neighbourhood << "\n";
+  // std::cout << "csm do_alpha_test " << csm_input.do_alpha_test << " icp do_alpha_test " << _icp_params.do_alpha_test << "\n";
+  // std::cout << "csm do_alpha_test_thresholdDeg " << csm_input.do_alpha_test_thresholdDeg << " icp do_alpha_test_thresholdDeg " << _icp_params.do_alpha_test_thresholdDeg << "\n";
+  // std::cout << "csm do_visibility_test " << csm_input.do_visibility_test << " icp do_visibility_test " << _icp_params.do_visibility_test << "\n";
+  // std::cout << "csm outliers_remove_doubles " << csm_input.outliers_remove_doubles << " icp outliers_remove_doubles " << _icp_params.outliers_remove_doubles << "\n";
+  // std::cout << "csm debug_verify_tricks " << csm_input.debug_verify_tricks << " icp debug_verify_tricks " << _icp_params.debug_verify_tricks << "\n";
+  // std::cout << "csm gpm_theta_bin_size_deg " << csm_input.gpm_theta_bin_size_deg << " icp gpm_theta_bin_size_deg " << _icp_params.gpm_theta_bin_size_deg << "\n";
+  // std::cout << "csm gpm_extend_range_deg " << csm_input.gpm_extend_range_deg << " icp gpm_extend_range_deg " << _icp_params.gpm_extend_range_deg << "\n";
+  // std::cout << "csm gpm_interval " << csm_input.gpm_interval << " icp gpm_interval " << _icp_params.gpm_interval << "\n";
+  // std::cout << "00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000"
+  //              "000000000000000000000000000000000\n";
+
+  //Not implemented (yet) in CSM
+  // csm_input.laser_x = _icp_params.laser_x;
+  // csm_input.laser_y = _icp_params.laser_y;
+  // csm_input.laser_theta = _icp_params.laser_theta;
+
+  csm_input.min_reading = _icp_params.min_reading;
+  csm_input.max_reading = _icp_params.max_reading;
+  csm_input.use_ml_weights = _icp_params.use_ml_weights;
+  csm_input.use_sigma_weights = _icp_params.use_sigma_weights;
+
+  // Not implemented (yet) in CSM
+  // csm_input.hsm_linear_cell_size = _icp_params.hsm_linear_cell_size;
+  // csm_input.hsm_angular_cell_size_deg = _icp_params.hsm_angular_cell_size_deg;
+  // csm_input.hsm_num_angular_hypotheses = _icp_params.hsm_num_angular_hypotheses;
+  // csm_input.hsm_xc_directions_min_distance_deg = _icp_params.hsm_xc_directions_min_distance_deg;
+  // csm_input.hsm_xc_ndirections = _icp_params.hsm_xc_ndirections;
+  // csm_input.hsm_angular_hyp_min_distance_deg = _icp_params.hsm_angular_hyp_min_distance_deg;
+  // csm_input.hsm_linear_xc_max_npeaks = _icp_params.hsm_linear_xc_max_npeaks;
+  // csm_input.hsm_linear_xc_peaks_min_distance = _icp_params.hsm_linear_xc_peaks_min_distance;
 
   sm_icp(&csm_input, &csm_output);
 

src/icp.h

@@ -25,6 +25,42 @@ struct icpParams{
     double outliers_maxPerc;
     double outliers_adaptive_order;
     double outliers_adaptive_mult;
+
+    int do_compute_covariance;
+    double max_angular_correction_deg;
+    double max_linear_correction;
+    double epsilon_xy;
+    double epsilon_theta;
+    double sigma;
+    int restart;
+    double restart_threshold_mean_error;
+    double restart_dt;
+    double restart_dtheta;
+    double clustering_threshold;
+    int orientation_neighbourhood;
+    int do_alpha_test;
+    double do_alpha_test_thresholdDeg;
+    int do_visibility_test;
+    int outliers_remove_doubles;
+    int debug_verify_tricks;
+    double gpm_theta_bin_size_deg;
+    double gpm_extend_range_deg;
+    double gpm_interval;
+    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;
 };
 
 class ICP

src/laser_scan.cpp

@@ -3,15 +3,15 @@
 namespace laserscanutils
 {
 void LaserScanParams::print() const{
-    std::cout << "PRINTING LASERSCANPARAMS ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
-    std::cout << "angle_min_: " << angle_min_ << std::endl;
-    std::cout << "angle_max_: " << angle_max_ << std::endl;
-    std::cout << "angle_step_: " << angle_step_ << std::endl;
-    std::cout << "scan_time_: " << scan_time_ << std::endl;
-    std::cout << "range_min_: " << range_min_ << std::endl;
-    std::cout << "range_max_: " << range_max_ << std::endl;
-    std::cout << "range_std_dev_: " << range_std_dev_ << std::endl;
-    std::cout << "angle_std_dev_: " << angle_std_dev_ << std::endl;
+    std::cout << "DBG PRINTING LASERSCANPARAMS ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl;
+    std::cout << "DBG angle_min_: " << angle_min_ << std::endl;
+    std::cout << "DBG angle_max_: " << angle_max_ << std::endl;
+    std::cout << "DBG angle_step_: " << angle_step_ << std::endl;
+    std::cout << "DBG scan_time_: " << scan_time_ << std::endl;
+    std::cout << "DBG range_min_: " << range_min_ << std::endl;
+    std::cout << "DBG range_max_: " << range_max_ << std::endl;
+    std::cout << "DBG range_std_dev_: " << range_std_dev_ << std::endl;
+    std::cout << "DBG angle_std_dev_: " << angle_std_dev_ << std::endl;
 }
 LaserScan::LaserScan() :
     is_raw_processed_(false)