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