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 @@
#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 &params);
static void printLaserData(LDP &laser_data);
};
}
#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