algos.h

Go to the documentation of this file.

#ifndef H_SCAN_MATCHING_LIB
#define H_SCAN_MATCHING_LIB
 
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
 
#include "hsm/hsm.h"
#include "hsm/hsm_interface.h"
#include "laser_data.h"
 
 
struct sm_params {
        LDP laser_ref;
        LDP laser_sens;
 
        double first_guess[3]; 
 
        double max_angular_correction_deg;
        double max_linear_correction;
 
        int max_iterations;
        double epsilon_xy;
        double epsilon_theta;
        
        double max_correspondence_dist;
        int use_corr_tricks;
        
        int restart;
                double restart_threshold_mean_error;
                double restart_dt;
                double restart_dtheta;
        
 
        /* Functions concerning discarding correspondences.
           THESE ARE MAGIC NUMBERS -- and they need to be tuned. */
 
        double outliers_maxPerc;
 
                double outliers_adaptive_order; /* 0.7 */
                double outliers_adaptive_mult; /* 2 */
 
        int outliers_remove_doubles; 
 
 
        
        /* Functions that compute and use point orientation for defining matches. */
                double clustering_threshold;
                int orientation_neighbourhood;
                int do_alpha_test;
                double do_alpha_test_thresholdDeg;
                
                
        int do_visibility_test;
 
        int use_point_to_line_distance;
 
        int use_ml_weights;
        
        /* If 1, the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
        int use_sigma_weights;
        
        int do_compute_covariance;
 
        int debug_verify_tricks;
        
        double laser[3]; 
 
        double sigma;
 
        double min_reading, max_reading;
        
        /* Parameters specific to GPM (unfinished :-/ ) */
        double gpm_theta_bin_size_deg;
        double gpm_extend_range_deg; 
        int gpm_interval;
        /* Parameter specific to HSM (unfinished :-/ ) */
        struct hsm_params hsm;
};
 
 
struct sm_result {
        int valid;
        
        double x[3];
        
        int iterations;
        int nvalid;
        double error;
        
        #ifndef RUBY
                gsl_matrix *cov_x_m;    
                gsl_matrix *dx_dy1_m;
                gsl_matrix *dx_dy2_m;
        #endif
};
 
 
void sm_icp(struct sm_params*input, struct sm_result*output);
 
void sm_icp_xy(struct sm_params*input, struct sm_result*output);
void sm_gpm(struct sm_params*input, struct sm_result*output);
void sm_hsm(struct sm_params*input, struct sm_result*output);
 
/* Unfinished, untested :-/ */
void sm_mbcip(struct sm_params*input, struct sm_result*output);
 
 
 
void sm_journal_open(const char* file);
 
#endif