csm: algos.h Source File

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                 
00083         int do_visibility_test;
00084 
00086         int use_point_to_line_distance;
00087 
00094         int use_ml_weights;
00095         
00096         /* If 1, the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
00097         int use_sigma_weights;
00098         
00101         int do_compute_covariance;
00102 
00104         int debug_verify_tricks;
00105         
00108         double laser[3]; 
00109 
00111         double sigma;
00112 
00114         double min_reading, max_reading;
00115         
00116         /* Parameters specific to GPM (unfinished :-/ ) */
00117         double gpm_theta_bin_size_deg;
00118         double gpm_extend_range_deg; 
00119         int gpm_interval;
00120         /* Parameter specific to HSM (unfinished :-/ ) */
00121         struct hsm_params hsm;
00122 };
00123 
00124 
00125 struct sm_result {
00127         int valid;
00128         
00130         double x[3];
00131         
00133         int iterations;
00135         int nvalid;
00137         double error;
00138         
00140         #ifndef RUBY
00141                 gsl_matrix *cov_x_m;    
00142                 gsl_matrix *dx_dy1_m;
00143                 gsl_matrix *dx_dy2_m;
00144         #endif
00145 };
00146 
00147 
00148 void sm_icp(struct sm_params*input, struct sm_result*output);
00149 void sm_gpm(struct sm_params*input, struct sm_result*output);
00150 void sm_hsm(struct sm_params*input, struct sm_result*output);
00151 
00152 /* Unfinished, untested :-/ */
00153 void sm_mbcip(struct sm_params*input, struct sm_result*output);
00154 
00155 
00156 
00157 void sm_journal_open(const char* file);
00158 
00159 #endif