diff --git a/include/laser_scan_utils/icp.h b/include/laser_scan_utils/icp.h
index 71ba9678023671b5c2f3ceeb11887cfe39444a3a..c05911203fa1000a5d0e9391bd9526fd80a147f2 100644
--- a/include/laser_scan_utils/icp.h
+++ b/include/laser_scan_utils/icp.h
@@ -26,171 +26,193 @@
#include <chrono>
#include <random>
#include <csm/csm_all.h>
-#undef max //undefine macro of csm that may interfere with std::max
-#undef min //undefine macro of csm that may interfere with std::min
+#undef max // undefine macro of csm that may interfere with std::max
+#undef min // undefine macro of csm that may interfere with std::min
-namespace laserscanutils{
-
-struct icpOutput{
- bool valid; // If the result is valid
- Eigen::Vector3s res_transf; // Transformation found
- Eigen::Matrix3s res_covar; // Covariance of the transformation
- int nvalid; // Number of valid correspondences in the match
- double error; // Total correspondence error
-};
-
-struct icpParams
+namespace laserscanutils
{
- bool verbose; // prints debug messages
-
- // Algorithm options ---------------------------------------------------
- bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false).
- double max_angular_correction_deg; // Maximum angular displacement between scans (deg)
- double max_linear_correction; // Maximum translation between scans (m)
-
- /** 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;
-
- // Stopping criteria
- int max_iterations; // maximum iterations
- double epsilon_xy; // distance change
- double epsilon_theta; // angle change
-
- // Restart algorithm
- bool restart; // Enable restarting
- double restart_threshold_mean_error; // Threshold for restarting
- double restart_dt; // Displacement for restarting
- double restart_dtheta; // Displacement for restarting
-
- // Discarding points or correspondences ---------------------------------------------------
- /** 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;
-
- // 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
- double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm
-
- void print() const
+
+ struct icpOutput
+ {
+ bool valid; // If the result is valid
+ Eigen::Vector3s res_transf; // Transformation found
+ Eigen::Matrix3s res_covar; // Covariance of the transformation
+ int nvalid; // Number of valid correspondences in the match
+ double error; // Total correspondence error
+ };
+
+ struct icpParams
+ {
+ bool verbose; // prints debug messages
+
+ // Algorithm options ---------------------------------------------------
+ bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false).
+ double max_angular_correction_deg; // Maximum angular displacement between scans (deg)
+ double max_linear_correction; // Maximum translation between scans (m)
+
+ /** 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;
+
+ // Stopping criteria
+ int max_iterations; // maximum iterations
+ double epsilon_xy; // distance change
+ double epsilon_theta; // angle change
+
+ // Restart algorithm
+ bool restart; // Enable restarting
+ double restart_threshold_mean_error; // Threshold for restarting
+ double restart_dt; // Displacement for restarting
+ double restart_dtheta; // Displacement for restarting
+
+ // Discarding points or correspondences ---------------------------------------------------
+ /** 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;
+
+ // 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
+ double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm
+
+ void print() const
+ {
+ std::cout << "verbose: " << std::to_string(verbose) << std::endl;
+ std::cout << "use_point_to_line_distance: " << std::to_string(use_point_to_line_distance) << std::endl;
+ std::cout << "max_angular_correction_deg: " << std::to_string(max_angular_correction_deg) << std::endl;
+ std::cout << "max_linear_correction: " << std::to_string(max_linear_correction) << std::endl;
+ std::cout << "max_correspondence_dist: " << std::to_string(max_correspondence_dist) << std::endl;
+ std::cout << "use_corr_tricks: " << std::to_string(use_corr_tricks) << std::endl;
+ std::cout << "debug_verify_tricks: " << std::to_string(debug_verify_tricks) << std::endl;
+ std::cout << "max_iterations: " << std::to_string(max_iterations) << std::endl;
+ std::cout << "epsilon_xy: " << std::to_string(epsilon_xy) << std::endl;
+ std::cout << "epsilon_theta: " << std::to_string(epsilon_theta) << std::endl;
+ std::cout << "restart: " << std::to_string(restart) << std::endl;
+ std::cout << "restart_threshold_mean_error: " << std::to_string(restart_threshold_mean_error) << std::endl;
+ std::cout << "restart_dt: " << std::to_string(restart_dt) << std::endl;
+ std::cout << "restart_dtheta: " << std::to_string(restart_dtheta) << std::endl;
+ std::cout << "min_reading: " << std::to_string(min_reading) << std::endl;
+ std::cout << "max_reading: " << std::to_string(max_reading) << std::endl;
+ std::cout << "outliers_maxPerc: " << std::to_string(outliers_maxPerc) << std::endl;
+ std::cout << "outliers_adaptive_order: " << std::to_string(outliers_adaptive_order) << std::endl;
+ std::cout << "outliers_adaptive_mult: " << std::to_string(outliers_adaptive_mult) << std::endl;
+ std::cout << "outliers_remove_doubles: " << std::to_string(outliers_remove_doubles) << std::endl;
+ std::cout << "do_visibility_test: " << std::to_string(do_visibility_test) << std::endl;
+ std::cout << "do_alpha_test: " << std::to_string(do_alpha_test) << std::endl;
+ std::cout << "do_alpha_test_thresholdDeg: " << std::to_string(do_alpha_test_thresholdDeg) << std::endl;
+ std::cout << "clustering_threshold: " << std::to_string(clustering_threshold) << std::endl;
+ std::cout << "orientation_neighbourhood: " << std::to_string(orientation_neighbourhood) << std::endl;
+ std::cout << "use_ml_weights: " << std::to_string(use_ml_weights) << std::endl;
+ std::cout << "use_sigma_weights: " << std::to_string(use_sigma_weights) << std::endl;
+ 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;
+ }
+ };
+
+ const icpParams icp_params_default = {
+ false, // bool verbose (prints debug messages)
+ true, // bool use_point_to_line_distance
+ 5.0, // double max_angular_correction_deg
+ 1, // double max_linear_correction
+ 0.5, // double max_correspondence_dist
+ false, // bool use_corr_tricks
+ false, // bool debug_verify_tricks
+ 50, // int max_iterations
+ 1e-4, // double epsilon_xy
+ 1e-3, // double epsilon_theta
+ false, // bool restart
+ 0, // double restart_threshold_mean_error
+ 0, // double restart_dt
+ 0, // double restart_dtheta
+ 0.023, // double min_reading
+ 60, // max_reading
+ 1, // double outliers_maxPerc
+ 0.8, // double outliers_adaptive_order
+ 2, // double outliers_adaptive_mult
+ false, // bool outliers_remove_doubles
+ false, // bool do_visibility_test
+ false, // bool do_alpha_test
+ 10, // double do_alpha_test_thresholdDeg
+ 0.5, // double clustering_threshold
+ 4, // int orientation_neighbourhood
+ false, // bool use_ml_weights
+ false, // bool use_sigma_weights
+ 0.2, // double sigma
+ true, // bool do_compute_covariance
+ 5, // double cov_factor
+ 2 // double cov_max_eigv_factor
+ };
+
+ class ICP
{
- std::cout << "verbose: " << std::to_string(verbose) << std::endl;
- std::cout << "use_point_to_line_distance: " << std::to_string(use_point_to_line_distance) << std::endl;
- std::cout << "max_angular_correction_deg: " << std::to_string(max_angular_correction_deg) << std::endl;
- std::cout << "max_linear_correction: " << std::to_string(max_linear_correction) << std::endl;
- std::cout << "max_correspondence_dist: " << std::to_string(max_correspondence_dist) << std::endl;
- std::cout << "use_corr_tricks: " << std::to_string(use_corr_tricks) << std::endl;
- std::cout << "debug_verify_tricks: " << std::to_string(debug_verify_tricks) << std::endl;
- std::cout << "max_iterations: " << std::to_string(max_iterations) << std::endl;
- std::cout << "epsilon_xy: " << std::to_string(epsilon_xy) << std::endl;
- std::cout << "epsilon_theta: " << std::to_string(epsilon_theta) << std::endl;
- std::cout << "restart: " << std::to_string(restart) << std::endl;
- std::cout << "restart_threshold_mean_error: " << std::to_string(restart_threshold_mean_error) << std::endl;
- std::cout << "restart_dt: " << std::to_string(restart_dt) << std::endl;
- std::cout << "restart_dtheta: " << std::to_string(restart_dtheta) << std::endl;
- std::cout << "min_reading: " << std::to_string(min_reading) << std::endl;
- std::cout << "max_reading: " << std::to_string(max_reading) << std::endl;
- std::cout << "outliers_maxPerc: " << std::to_string(outliers_maxPerc) << std::endl;
- std::cout << "outliers_adaptive_order: " << std::to_string(outliers_adaptive_order) << std::endl;
- std::cout << "outliers_adaptive_mult: " << std::to_string(outliers_adaptive_mult) << std::endl;
- std::cout << "outliers_remove_doubles: " << std::to_string(outliers_remove_doubles) << std::endl;
- std::cout << "do_visibility_test: " << std::to_string(do_visibility_test) << std::endl;
- std::cout << "do_alpha_test: " << std::to_string(do_alpha_test) << std::endl;
- std::cout << "do_alpha_test_thresholdDeg: " << std::to_string(do_alpha_test_thresholdDeg) << std::endl;
- std::cout << "clustering_threshold: " << std::to_string(clustering_threshold) << std::endl;
- std::cout << "orientation_neighbourhood: " << std::to_string(orientation_neighbourhood) << std::endl;
- std::cout << "use_ml_weights: " << std::to_string(use_ml_weights) << std::endl;
- std::cout << "use_sigma_weights: " << std::to_string(use_sigma_weights) << std::endl;
- 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;
- }
-};
-
-const icpParams icp_params_default = {
- false, //bool verbose; // prints debug messages
- true, 5.0, 1, // bool use_point_to_line_distance; double max_angular_correction_deg; double max_linear_correction;
- 0.5, false, false, // double max_correspondence_dist; bool use_corr_tricks; bool debug_verify_tricks;
- 50, 1e-4, 1e-3, // int max_iterations; double epsilon_xy; double epsilon_theta;
- false, 0, 0, 0, // bool restart; double restart_threshold_mean_error; double restart_dt; double restart_dtheta;
- 0.023, 60, // double min_reading, max_reading;
- 1, 0.8, 2, // double outliers_maxPerc; double outliers_adaptive_order; double outliers_adaptive_mult;
- 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, 2 // bool do_compute_covariance; double cov_factor; double cov_max_eigv_factor;
-};
-
-class ICP
-{
public:
ICP();
~ICP();
- static icpOutput align(const LaserScan &_current_ls,
- const LaserScan &_ref_ls,
- const LaserScanParams &_current_scan_params,
- const LaserScanParams &_ref_scan_params,
- const icpParams &_icp_params,
- const Eigen::Vector3s &_initial_guess);
- static icpOutput align(const LaserScan &_last_ls,
- const LaserScan &_reference_ls,
- const LaserScanParams &scan_params,
- const icpParams &icp_params,
- const Eigen::Vector3s &_initial_guess);
-
- static void printTwoLaserData(sm_params & params);
- static void printLaserData(LDP & laser_data);
- };
+ static icpOutput align(const LaserScan &_current_ls,
+ const LaserScan &_ref_ls,
+ const LaserScanParams &_current_scan_params,
+ const LaserScanParams &_ref_scan_params,
+ const icpParams &_icp_params,
+ const Eigen::Vector3s &_initial_guess);
+ static icpOutput align(const LaserScan &_last_ls,
+ const LaserScan &_reference_ls,
+ const LaserScanParams &scan_params,
+ const icpParams &icp_params,
+ const Eigen::Vector3s &_initial_guess);
+
+ static void printTwoLaserData(sm_params ¶ms);
+ static void printLaserData(LDP &laser_data);
+ };
}
#endif