00001 #ifndef H_SCAN_MATCHING_LIB 00002 #define H_SCAN_MATCHING_LIB 00003 00004 #include <gsl/gsl_vector.h> 00005 #include <gsl/gsl_matrix.h> 00006 00007 #include "hsm/hsm.h" 00008 #include "hsm/hsm_interface.h" 00009 #include "laser_data.h" 00010 00011 00012 struct sm_params { 00014 LDP laser_ref; 00016 LDP laser_sens; 00017 00019 double first_guess[3]; 00020 00022 double max_angular_correction_deg; 00024 double max_linear_correction; 00025 00027 int max_iterations; 00029 double epsilon_xy; 00031 double epsilon_theta; 00032 00034 double max_correspondence_dist; 00036 int use_corr_tricks; 00037 00039 int restart; 00041 double restart_threshold_mean_error; 00043 double restart_dt; 00045 double restart_dtheta; 00046 00047 00048 /* Functions concerning discarding correspondences. 00049 THESE ARE MAGIC NUMBERS -- and they need to be tuned. */ 00050 00053 double outliers_maxPerc; 00054 00065 double outliers_adaptive_order; /* 0.7 */ 00066 double outliers_adaptive_mult; /* 2 */ 00067 00069 int outliers_remove_doubles; 00070 00071 00072 00073 /* Functions that compute and use point orientation for defining matches. */ 00075 double clustering_threshold; 00077 int orientation_neighbourhood; 00079 int do_alpha_test; 00080 double do_alpha_test_thresholdDeg; 00081 00082 00096 int do_visibility_test; 00097 00099 int use_point_to_line_distance; 00100 00107 int use_ml_weights; 00108 00109 /* If 1, the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */ 00110 int use_sigma_weights; 00111 00114 int do_compute_covariance; 00115 00117 int debug_verify_tricks; 00118 00121 double laser[3]; 00122 00124 double sigma; 00125 00127 double min_reading, max_reading; 00128 00129 /* Parameters specific to GPM (unfinished :-/ ) */ 00130 double gpm_theta_bin_size_deg; 00131 double gpm_extend_range_deg; 00132 int gpm_interval; 00133 /* Parameter specific to HSM (unfinished :-/ ) */ 00134 struct hsm_params hsm; 00135 }; 00136 00137 00138 struct sm_result { 00140 int valid; 00141 00143 double x[3]; 00144 00146 int iterations; 00148 int nvalid; 00150 double error; 00151 00153 #ifndef RUBY 00154 gsl_matrix *cov_x_m; 00155 gsl_matrix *dx_dy1_m; 00156 gsl_matrix *dx_dy2_m; 00157 #endif 00158 }; 00159 00160 00161 void sm_icp(struct sm_params*input, struct sm_result*output); 00162 void sm_icp_xy(struct sm_params*input, struct sm_result*output); 00163 void sm_gpm(struct sm_params*input, struct sm_result*output); 00164 void sm_hsm(struct sm_params*input, struct sm_result*output); 00165 00166 /* Unfinished, untested :-/ */ 00167 void sm_mbcip(struct sm_params*input, struct sm_result*output); 00168 00169 00170 00171 void sm_journal_open(const char* file); 00172 00173 #endif