MbICP.h

00001 /*************************************************************************************/
00002 /*                                                                                   */
00003 /*  File:          MbICP.h                                                           */
00004 /*  Authors:       Luis Montesano and Javier Minguez                                 */
00005 /*  Modified:      1/3/2006                                                          */
00006 /*                                                                                   */
00007 /*  This library implements the:                                                     */
00008 /*                                                                                   */
00009 /*                                                                                   */
00010 /*      J. Minguez, L. Montesano, and F. Lamiraux, "Metric-based iterative              */
00011 /*      closest point scan matching for sensor displacement estimation," IEEE           */
00012 /*      Transactions on Robotics, vol. 22, no. 5, pp. 1047 \u2013 1054, 2006.           */
00013 /*************************************************************************************/
00014 
00015 
00016 /*****************************************************************************/
00017 //
00018 //      EVERYTHING IN THE INTERNATIONAL SYSTEM (METERS AND RADIANS)
00019 //
00020 /*****************************************************************************/
00021 
00022 #ifndef MbICP
00023 #define MbICP
00024 #include "TData.h"
00025 
00026 #ifdef __cplusplus
00027 extern "C" {
00028 #endif
00029 
00030 
00031 // ----------------------------------------------------------------------------
00032 // DEFINES
00033 // ----------------------------------------------------------------------------
00034 
00035 /*
00036 #define MAXLASERPOINTS 361
00037 */
00038 
00039 // ----------------------------------------------------------------------------
00040 // GENERIC TYPES
00041 // ----------------------------------------------------------------------------
00042 
00043 /*typedef struct {
00044   float x;
00045   float y;
00046 }Tpf;
00047 
00048 typedef struct {
00049   float r;
00050   float t;
00051 }Tpfp;
00052 
00053 typedef struct {
00054   int x;
00055   int y;
00056 }Tpi;
00057 
00058 typedef struct {
00059   float x;
00060   float y;
00061   float tita;
00062 }Tsc;
00063 */
00064 
00065 // ----------------------------------------------------------------------------
00066 // SPECIFIC TYPES
00067 // ----------------------------------------------------------------------------
00068 
00069 
00070 
00071 // ----------------------------------------------------------------------------
00072 // GLOBAL FUNCTIONS
00073 // ----------------------------------------------------------------------------
00074 
00075 
00076 // ************************
00077 // Function that initializes the SM parameters
00078 // ************************
00079 
00080 /* void InitScanMatching(float Bw, float Br,
00081                                                  float L, int laserStep,float MaxDistInter, float filtrado,
00082                                              int MaxIter, float error_th, float exo, float eyo, float etitao, int IterSmoothConv); */
00083 // in:::
00084 
00085         /* --------------------- */
00086         /* --- Thresold parameters */
00087         /* --------------------- */
00088         /* Bw: maximum angle diference between points of different scans */
00089         /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */
00090         /* This is a speed up parameter */
00091         //float Bw;
00092 
00093         /* Br: maximum distance difference between points of different scans */
00094         /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */
00095         //float Br;
00096 
00097         /* --------------------- */
00098         /* --- Inner parameters */
00099         /* --------------------- */
00100         /* L: value of the metric */
00101         /* When L tends to infinity you are using the standart ICP */
00102     /* When L tends to 0 you use the metric (more importance to rotation) */
00103         //float L;
00104 
00105         /* laserStep: selects points of each scan with an step laserStep  */
00106         /* When laserStep=1 uses all the points of the scans */
00107         /* When laserStep=2 uses one each two ... */
00108         /* This is an speed up parameter */
00109         //int laserStep;
00110 
00111         /* ProjectionFilter: */
00112         /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */
00113         /* It works well for angles < 45 \circ*/
00114         /* 1 : activates the filter */
00115         /* 0 : desactivates the filter */
00116         // int ProjectionFilter;
00117 
00118         /* MaxDistInter: maximum distance to interpolate between points in the ref scan */
00119         /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */
00120         //float MaxDistInter;
00121 
00122         /* filter: in [0,1] sets the % of asociations NOT considered spurious */
00123         /* E.g. if filter=0.9 you use 90% of the associations */
00124         /* The associations are ordered by distance and the (1-filter) with greater distance are not used */
00125         /* This type of filtering is called "trimmed-ICP" */
00126         //float filter;
00127 
00128         /* AsocError: in [0,1] */
00129         /* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */
00130         /* When the number of associations is below AsocError, the main function will return error in associations step */
00131         // float AsocError;
00132 
00133         /* --------------------- */
00134         /* --- Exit parameters */
00135         /* --------------------- */
00136         /* MaxIter: sets the maximum number of iterations for the algorithm to exit */
00137         /* The more iterations, the more chance you give the algorithm to be more accurate   */
00138         //int MaxIter;
00139 
00140         /* errorRatio: in [0,1] sets the maximum error ratio between iterations to exit */
00141         /* In iteration K, let be errorK the residual of the minimization */
00142         /* Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is the solution of the scan matching */
00143         //float error_th;
00144 
00145         /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */
00146         /* In each iteration, the error is the residual of the minimization in each component */
00147         /* The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out) */
00148         /* When errorK tends to 0 the more precise is the solution of the scan matching */
00149         //float errx_out,erry_out, errt_out;
00150 
00151         /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria (the two above criteria) */
00152         /* (error_th) OR (errorx_out && errory_out && errt_out) */
00153         /* With this parameter >1 avoids random solutions and estabilices the algorithm */
00154         //int IterSmoothConv;
00155 
00156 
00157 
00158 void Init_MbICP_ScanMatching(
00159                              float max_laser_range,
00160                              float Bw,
00161                              float Br,
00162                              float L,
00163                              int   laserStep,
00164                              float MaxDistInter,
00165                              float filter,
00166                              int   ProjectionFilter,
00167                              float AsocError,
00168                              int   MaxIter,
00169                              float errorRatio,
00170                              float errx_out,
00171                              float erry_out,
00172                              float errt_out,
00173                              int IterSmoothConv);
00174 
00175 // -------------------------------------------------------------
00176 
00177 // ************************
00178 // Function that does the scan matching
00179 // ************************
00180 
00181 /* int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
00182                                  Tsc *sensorMotion, Tsc *solution); */
00183 
00184 // in:::
00185 //      laserK: is the reference scan in polar coordinates (max. num points is MAXLASERPOINTS)
00186 //              laserK1: is the new scan in polar coordinates (max. num points is MAXLASERPOINTS)
00187 //              sensorMotion: initial SENSOR motion estimation from location K to location K1
00188 //              solution: SENSOR motion solution from location K to location K1
00189 // out:::
00190 //              1 : Everything OK in less that the Maximum number of iterations
00191 //              2 : Everything OK but reached the Maximum number of iterations
00192 //              -1: Failure in the association step
00193 //              -2: Failure in the minimization step
00194 
00195 int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
00196                                  Tsc *sensorMotion, Tsc *solution);
00197 
00198 #ifdef __cplusplus
00199 }
00200 #endif
00201 
00202 #endif

Last updated 12 September 2005 21:38:45