00001 #ifndef _H_ICP_
00002 #define _H_ICP_
00003
00004 #include <gpc/gpc.h>
00005 #include "../csm_all.h"
00006
00007
00009 void sm_icp(struct sm_params*params, struct sm_result*res);
00010
00012 int icp_loop(struct sm_params*params, const double*q0, double*x_new,
00013 double*total_error, int*nvalid, int*iterations);
00014
00017 int compute_next_estimate(struct sm_params*params,
00018 const double x_old[3], double x_new[3]);
00019
00024 int termination_criterion(struct sm_params*params, const double*delta);
00025
00027 void find_correspondences(struct sm_params*params);
00029 void find_correspondences_tricks(struct sm_params*params);
00032 void debug_correspondences(struct sm_params * params);
00033
00034 void kill_outliers_trim(struct sm_params*params, double*total_error);
00035 void kill_outliers_double(struct sm_params*params);
00036
00037 void compute_covariance_exact(
00038 LDP laser_ref, LDP laser_sens, const gsl_vector*x,
00039 val *cov0_x, val *dx_dy1, val *dx_dy2);
00040
00041 void visibilityTest(LDP ld, const gsl_vector*x_old);
00042
00044 void ld_invalid_if_outside(LDP ld, double min_reading, double max_reading);
00045
00046 void swap_double(double*a,double*b);
00047
00048 #endif
