From d3598bb54e47df054ab3df70f8b4934ae321e98e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 27 Apr 2021 15:27:56 +0200
Subject: [PATCH] new param cov_factor
---
src/icp.cpp | 8 +++-----
src/icp.h | 24 +++++++++++++-----------
2 files changed, 16 insertions(+), 16 deletions(-)
diff --git a/src/icp.cpp b/src/icp.cpp
index cee0495..9f32bd3 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -164,11 +164,9 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
- result.res_covar(i, j) =
- // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT
- // COMPILING
- 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
+ result.res_covar(i, j) = _icp_params.cov_factor *
+ // gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
+ 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
} else {
std::cout << "ICP valid != 1, providing first guess transformation and "
"identity covariance\n";
diff --git a/src/icp.h b/src/icp.h
index 3c10a43..1f31955 100644
--- a/src/icp.h
+++ b/src/icp.h
@@ -18,6 +18,8 @@ struct icpOutput{
};
struct icpParams{
+ double cov_factor;
+
int use_point_to_line_distance;
double max_correspondence_dist;
int max_iterations;
@@ -46,21 +48,21 @@ struct icpParams{
double gpm_theta_bin_size_deg;
double gpm_extend_range_deg;
double gpm_interval;
- double laser_x;
- double laser_y;
- double laser_theta;
+ //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;
+ //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
--
GitLab