documented icp params and changed types

src/icp.cpp

@@ -44,8 +44,8 @@ class LDWrapper
                 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_ and
-                    0 <= it and it <= 100)
+                        it <= scan_params.range_max_ and
+                        0 <= it and it <= 100)
                 {
                     laser_data->readings[i] = it;
                     laser_data->valid[i] = 1;
@@ -95,105 +95,86 @@ icpOutput ICP::align(const LaserScan &_current_ls,
                      const icpParams &_icp_params,
                      Eigen::Vector3s &_transf_ref_current)
 {
-  // Uncomment to enable debug messages from the CSM library
-  //sm_debug_write(true);
-
-  LDWrapper laser_scan_current = LDWrapper(_current_ls, _current_scan_params);
-  LDWrapper laser_scan_ref = LDWrapper(_ref_ls, _ref_scan_params);
-
-  int num_rays = _current_ls.ranges_raw_.size();
-
-  sm_params csm_input{};
-  sm_result csm_output{};
-
-  csm_input.min_reading = fmax(_current_scan_params.range_min_, _ref_scan_params.range_min_);
-  csm_input.max_reading = fmin(_current_scan_params.range_max_, _ref_scan_params.range_max_);
-  csm_input.sigma = _ref_scan_params.range_std_dev_;
-
-  csm_input.laser_ref = laser_scan_ref.laser_data;
-  csm_input.laser_sens = laser_scan_current.laser_data;
-
-  csm_input.first_guess[0] = _transf_ref_current(0);
-  csm_input.first_guess[1] = _transf_ref_current(1);
-  csm_input.first_guess[2] = _transf_ref_current(2);
-
-  csm_input.use_point_to_line_distance = _icp_params.use_point_to_line_distance;
-  csm_input.max_correspondence_dist = _icp_params.max_correspondence_dist;
-  csm_input.max_iterations = _icp_params.max_iterations;
-  csm_input.use_corr_tricks = _icp_params.use_corr_tricks;
-  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 = _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;
-
-  //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);
-
-  icpOutput result{};
-  result.nvalid = csm_output.nvalid;
-  result.valid = csm_output.valid;
-  result.error = csm_output.error;
-
-  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();
-  }
-
-  return result;
+    // enable/disable debug messages from the CSM library
+    sm_debug_write(_icp_params.verbose);
+
+    LDWrapper laser_scan_current = LDWrapper(_current_ls, _current_scan_params);
+    LDWrapper laser_scan_ref = LDWrapper(_ref_ls, _ref_scan_params);
+
+    int num_rays = _current_ls.ranges_raw_.size();
+
+    sm_params csm_input{};
+    sm_result csm_output{};
+
+    csm_input.min_reading = fmax(_current_scan_params.range_min_, _ref_scan_params.range_min_);
+    csm_input.max_reading = fmin(_current_scan_params.range_max_, _ref_scan_params.range_max_);
+    csm_input.sigma = _ref_scan_params.range_std_dev_;
+
+    csm_input.laser_ref = laser_scan_ref.laser_data;
+    csm_input.laser_sens = laser_scan_current.laser_data;
+
+    csm_input.first_guess[0] = _transf_ref_current(0);
+    csm_input.first_guess[1] = _transf_ref_current(1);
+    csm_input.first_guess[2] = _transf_ref_current(2);
+
+    csm_input.use_point_to_line_distance = _icp_params.use_point_to_line_distance ? 1 : 0;
+    csm_input.max_correspondence_dist = _icp_params.max_correspondence_dist;
+    csm_input.max_iterations = _icp_params.max_iterations;
+    csm_input.use_corr_tricks = _icp_params.use_corr_tricks ? 1 : 0;
+    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 = _icp_params.do_compute_covariance ? 1 : 0;
+
+    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 ? 1 : 0;
+    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 ? 1 : 0;
+    csm_input.do_alpha_test_thresholdDeg = _icp_params.do_alpha_test_thresholdDeg;
+    csm_input.do_visibility_test = _icp_params.do_visibility_test ? 1 : 0;
+    csm_input.outliers_remove_doubles = _icp_params.outliers_remove_doubles ? 1 : 0;
+    csm_input.debug_verify_tricks = _icp_params.debug_verify_tricks ? 1 : 0;
+    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 ? 1 : 0;
+    csm_input.use_sigma_weights = _icp_params.use_sigma_weights ? 1 : 0;
+
+    sm_icp(&csm_input, &csm_output);
+
+    icpOutput result{};
+    result.nvalid = csm_output.nvalid;
+    result.valid = csm_output.valid == 1;
+    result.error = csm_output.error;
+
+    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;
+
+        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();
+    }
+
+    return result;
 }
 
 //Legacy code
@@ -213,8 +194,8 @@ void ICP::printTwoLaserData(sm_params &params)
     for (int ii=0; ii<params.laser_ref->nrays-1; ++ii)
     {
         std::cout << "Theta: " << params.laser_ref->theta[ii] << "; Readings: "
-                    << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
-                    << '\n';
+                << params.laser_ref->readings[ii] << "; " << params.laser_sens->readings[ii]
+                                                                                         << '\n';
     }
 
 }

src/icp.h

@@ -31,59 +31,96 @@
 namespace laserscanutils{
 
 struct icpOutput{
-    int valid; /** 1 if the result is valid */
+    bool valid; /** if the result is valid */
     Eigen::Vector3s res_transf;
     Eigen::Matrix3s res_covar;
     int nvalid;     /** Number of valid correspondence in the end */
     double error;   	/** Total correspondence error */
 };
 
-struct icpParams{
-    double cov_factor;
+struct icpParams
+{
+    bool verbose;                       // prints debug messages
 
-    int use_point_to_line_distance;
-    double max_correspondence_dist;
-    int max_iterations;
-    int use_corr_tricks;
-    double outliers_maxPerc;
-    double outliers_adaptive_order;
-    double outliers_adaptive_mult;
+    // Algorithm options ---------------------------------------------------
+    bool use_point_to_line_distance;     // use PlICP (true) or use vanilla ICP (false).
 
-    int do_compute_covariance;
+    /** Maximum angular displacement between scans (deg)*/
     double max_angular_correction_deg;
+    /** Maximum translation between scans (m) */
     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;
+
+    // Stopping criteria
+    int max_iterations;     // maximum iterations
+    double epsilon_xy;      // distance change
+    double epsilon_theta;   // angle change
+
+    /** Maximum distance for a correspondence to be valid */
+    double max_correspondence_dist;
+    /** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
+    bool use_corr_tricks;
+    /** Checks that find_correspondences_tricks give the right answer */
+    bool debug_verify_tricks;
+
+    // Restart algorithm
+    bool restart;
+    double restart_threshold_mean_error; // Threshold for restarting
+    double restart_dt;                   // Displacement for restarting
+    double restart_dtheta;               // Displacement for restarting
+
+    // Discarding points or correpondences ---------------------------------------------------
+    /** discard rays outside of this interval */
+    double min_reading, max_reading;
+    /** Percentage of correspondences to consider: if 0.9,
+        always discard the top 10% of correspondences with more error */
+    double outliers_maxPerc;
+
+    /** Parameters describing a simple adaptive algorithm for discarding.
+        1) Order the errors.
+        2) Choose the percentile according to outliers_adaptive_order.
+           (if it is 0.7, get the 70% percentile)
+        3) Define an adaptive threshold multiplying outliers_adaptive_mult
+           with the value of the error at the chosen percentile.
+        4) Discard correspondences over the threshold.
+
+        This is useful to be conservative; yet remove the biggest errors.
+    */
+    double outliers_adaptive_order; // 0.7
+    double outliers_adaptive_mult;  // 2
+
+    /** Do not allow two different correspondences to share a point */
+    bool outliers_remove_doubles;
+
+    /** If initial guess, visibility test can be done to discard points that are not visible */
+    bool do_visibility_test;
+
+    /** Discard correspondences based on the angles */
+    bool do_alpha_test;
+    double do_alpha_test_thresholdDeg;
+
+    // Point orientation ------------------------------------------------------------------
+    /** For now, a very simple max-distance clustering algorithm is used */
     double clustering_threshold;
+    /** Number of neighbour rays used to estimate the orientation.*/
     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;
+
+    // Weights ---------------------------------------------------------------------------
+    /** If the field "true_alpha" is used to compute the incidence
+        beta, and the factor (1/cos^2(beta)) used to weight the impact
+        of each correspondence. This works fabolously if doing localization,
+        that is the first scan has no noise.
+        If "true_alpha" is not available, it uses "alpha".
+    */
+    bool use_ml_weights;
+    /* If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
+    bool use_sigma_weights;
+    /** Noise in the scan */
+    double sigma;
+
+    // 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
+
 };
 
 class ICP