Skip to content
Snippets Groups Projects
Commit 74fa166b authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

better identation

parent 3fbd35e3
No related branches found
No related tags found
No related merge requests found
Pipeline #18420 passed
Hide whitespace changes
Inline Side-by-side
include/laser_scan_utils/icp.h
+ 180
− 158
View file @ 74fa166b
...@@ -26,171 +26,193 @@ ...@@ -26,171 +26,193 @@
#include <chrono> #include <chrono>
#include <random> #include <random>
#include <csm/csm_all.h> #include <csm/csm_all.h>
#undef max //undefine macro of csm that may interfere with std::max #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 min // undefine macro of csm that may interfere with std::min
namespace laserscanutils{ 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
{ {
bool verbose; // prints debug messages
struct icpOutput
// Algorithm options --------------------------------------------------- {
bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false). bool valid; // If the result is valid
double max_angular_correction_deg; // Maximum angular displacement between scans (deg) Eigen::Vector3s res_transf; // Transformation found
double max_linear_correction; // Maximum translation between scans (m) Eigen::Matrix3s res_covar; // Covariance of the transformation
int nvalid; // Number of valid correspondences in the match
/** Maximum distance for a correspondence to be valid */ double error; // Total correspondence error
double max_correspondence_dist; };
/** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
bool use_corr_tricks; struct icpParams
/** Checks that find_correspondences_tricks give the right answer */ {
bool debug_verify_tricks; bool verbose; // prints debug messages
// Stopping criteria // Algorithm options ---------------------------------------------------
int max_iterations; // maximum iterations bool use_point_to_line_distance; // use PlICP (true) or use vanilla ICP (false).
double epsilon_xy; // distance change double max_angular_correction_deg; // Maximum angular displacement between scans (deg)
double epsilon_theta; // angle change double max_linear_correction; // Maximum translation between scans (m)
// Restart algorithm /** Maximum distance for a correspondence to be valid */
bool restart; // Enable restarting double max_correspondence_dist;
double restart_threshold_mean_error; // Threshold for restarting /** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
double restart_dt; // Displacement for restarting bool use_corr_tricks;
double restart_dtheta; // Displacement for restarting /** Checks that find_correspondences_tricks give the right answer */
bool debug_verify_tricks;
// Discarding points or correspondences ---------------------------------------------------
/** discard rays outside of this interval */ // Stopping criteria
double min_reading, max_reading; int max_iterations; // maximum iterations
/** Percentage of correspondences to consider: if 0.9, double epsilon_xy; // distance change
always discard the top 10% of correspondences with more error */ double epsilon_theta; // angle change
double outliers_maxPerc;
// Restart algorithm
/** Parameters describing a simple adaptive algorithm for discarding. bool restart; // Enable restarting
1) Order the errors. double restart_threshold_mean_error; // Threshold for restarting
2) Choose the percentile according to outliers_adaptive_order. double restart_dt; // Displacement for restarting
(if it is 0.7, get the 70% percentile) double restart_dtheta; // Displacement for restarting
3) Define an adaptive threshold multiplying outliers_adaptive_mult
with the value of the error at the chosen percentile. // Discarding points or correspondences ---------------------------------------------------
4) Discard correspondences over the threshold. /** discard rays outside of this interval */
double min_reading, max_reading;
This is useful to be conservative; yet remove the biggest errors. /** Percentage of correspondences to consider: if 0.9,
*/ always discard the top 10% of correspondences with more error */
double outliers_adaptive_order; // 0.7 double outliers_maxPerc;
double outliers_adaptive_mult; // 2
/** Parameters describing a simple adaptive algorithm for discarding.
/** Do not allow two different correspondences to share a point */ 1) Order the errors.
bool outliers_remove_doubles; 2) Choose the percentile according to outliers_adaptive_order.
(if it is 0.7, get the 70% percentile)
/** If initial guess, visibility test can be done to discard points that are not visible */ 3) Define an adaptive threshold multiplying outliers_adaptive_mult
bool do_visibility_test; with the value of the error at the chosen percentile.
4) Discard correspondences over the threshold.
/** Discard correspondences based on the angles */
bool do_alpha_test; This is useful to be conservative; yet remove the biggest errors.
double do_alpha_test_thresholdDeg; */
double outliers_adaptive_order; // 0.7
// Point orientation ------------------------------------------------------------------ double outliers_adaptive_mult; // 2
/** For now, a very simple max-distance clustering algorithm is used */
double clustering_threshold; /** Do not allow two different correspondences to share a point */
/** Number of neighbour rays used to estimate the orientation.*/ bool outliers_remove_doubles;
int orientation_neighbourhood;
/** If initial guess, visibility test can be done to discard points that are not visible */
// Weights --------------------------------------------------------------------------- bool do_visibility_test;
/** If the field "true_alpha" is used to compute the incidence
beta, and the factor (1/cos^2(beta)) used to weight the impact /** Discard correspondences based on the angles */
of each correspondence. This works fabolously if doing localization, bool do_alpha_test;
that is the first scan has no noise. double do_alpha_test_thresholdDeg;
If "true_alpha" is not available, it uses "alpha".
*/ // Point orientation ------------------------------------------------------------------
bool use_ml_weights; /** For now, a very simple max-distance clustering algorithm is used */
/* If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */ double clustering_threshold;
bool use_sigma_weights; /** Number of neighbour rays used to estimate the orientation.*/
/** Noise in the scan */ int orientation_neighbourhood;
double sigma;
// Weights ---------------------------------------------------------------------------
// Covariance ------------------------------------------------------------------------ /** If the field "true_alpha" is used to compute the incidence
bool do_compute_covariance; // Compute the matching covariance (method in http://purl.org/censi/2006/icpcov) beta, and the factor (1/cos^2(beta)) used to weight the impact
double cov_factor; // Factor multiplying the cov output of csm of each correspondence. This works fabolously if doing localization,
double cov_max_eigv_factor; // Factor multiplying the direction of the max eigenvalue of the cov output of csm that is the first scan has no noise.
If "true_alpha" is not available, it uses "alpha".
void print() const */
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: public:
ICP(); ICP();
~ICP(); ~ICP();
static icpOutput align(const LaserScan &_current_ls, static icpOutput align(const LaserScan &_current_ls,
const LaserScan &_ref_ls, const LaserScan &_ref_ls,
const LaserScanParams &_current_scan_params, const LaserScanParams &_current_scan_params,
const LaserScanParams &_ref_scan_params, const LaserScanParams &_ref_scan_params,
const icpParams &_icp_params, const icpParams &_icp_params,
const Eigen::Vector3s &_initial_guess); const Eigen::Vector3s &_initial_guess);
static icpOutput align(const LaserScan &_last_ls, static icpOutput align(const LaserScan &_last_ls,
const LaserScan &_reference_ls, const LaserScan &_reference_ls,
const LaserScanParams &scan_params, const LaserScanParams &scan_params,
const icpParams &icp_params, const icpParams &icp_params,
const Eigen::Vector3s &_initial_guess); const Eigen::Vector3s &_initial_guess);
static void printTwoLaserData(sm_params & params); static void printTwoLaserData(sm_params &params);
static void printLaserData(LDP & laser_data); static void printLaserData(LDP &laser_data);
}; };
} }
#endif #endif
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment