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  *  This program is free software; you can redistribute it and/or modify
00016  *  it under the terms of the GNU General Public License as published by
00017  *  the Free Software Foundation; either version 2 of the License, or
00018  *  (at your option) any later version.
00019  *
00020  *  This program is distributed in the hope that it will be useful,
00021  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00022  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023  *  GNU General Public License for more details.
00024  *
00025  *  You should have received a copy of the GNU General Public License
00026  *  along with this program; if not, write to the Free Software
00027  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00028  *
00029  */
00030 
00031 /*****************************************************************************/
00032 //
00033 //      EVERYTHING IN THE INTERNATIONAL SYSTEM (METERS AND RADIANS)
00034 //
00035 /*****************************************************************************/
00036 
00037 #ifndef MbICP
00038 #define MbICP
00039 #include "TData.h"
00040 
00041 #ifdef __cplusplus
00042 extern "C" {
00043 #endif
00044 
00045 
00046 // ----------------------------------------------------------------------------
00047 // DEFINES
00048 // ----------------------------------------------------------------------------
00049 
00050 /*
00051 #define MAXLASERPOINTS 361
00052 */
00053 
00054 // ----------------------------------------------------------------------------
00055 // GENERIC TYPES
00056 // ----------------------------------------------------------------------------
00057 
00058 /*typedef struct {
00059   float x;
00060   float y;
00061 }Tpf;
00062 
00063 typedef struct {
00064   float r;
00065   float t;
00066 }Tpfp;
00067 
00068 typedef struct {
00069   int x;
00070   int y;
00071 }Tpi;
00072 
00073 typedef struct {
00074   float x;
00075   float y;
00076   float tita;
00077 }Tsc;
00078 */
00079 
00080 // ----------------------------------------------------------------------------
00081 // SPECIFIC TYPES
00082 // ----------------------------------------------------------------------------
00083 
00084 
00085 
00086 // ----------------------------------------------------------------------------
00087 // GLOBAL FUNCTIONS
00088 // ----------------------------------------------------------------------------
00089 
00090 
00091 // ************************
00092 // Function that initializes the SM parameters
00093 // ************************
00094 
00095 /* void InitScanMatching(float Bw, float Br,
00096                                                  float L, int laserStep,float MaxDistInter, float filtrado,
00097                                              int MaxIter, float error_th, float exo, float eyo, float etitao, int IterSmoothConv); */
00098 // in:::
00099 
00100         /* --------------------- */
00101         /* --- Thresold parameters */
00102         /* --------------------- */
00103         /* Bw: maximum angle diference between points of different scans */
00104         /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */
00105         /* This is a speed up parameter */
00106         //float Bw;
00107 
00108         /* Br: maximum distance difference between points of different scans */
00109         /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */
00110         //float Br;
00111 
00112         /* --------------------- */
00113         /* --- Inner parameters */
00114         /* --------------------- */
00115         /* L: value of the metric */
00116         /* When L tends to infinity you are using the standart ICP */
00117     /* When L tends to 0 you use the metric (more importance to rotation) */
00118         //float L;
00119 
00120         /* laserStep: selects points of each scan with an step laserStep  */
00121         /* When laserStep=1 uses all the points of the scans */
00122         /* When laserStep=2 uses one each two ... */
00123         /* This is an speed up parameter */
00124         //int laserStep;
00125 
00126         /* ProjectionFilter: */
00127         /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */
00128         /* It works well for angles < 45 \circ*/
00129         /* 1 : activates the filter */
00130         /* 0 : desactivates the filter */
00131         // int ProjectionFilter;
00132 
00133         /* MaxDistInter: maximum distance to interpolate between points in the ref scan */
00134         /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */
00135         //float MaxDistInter;
00136 
00137         /* filter: in [0,1] sets the % of asociations NOT considered spurious */
00138         /* E.g. if filter=0.9 you use 90% of the associations */
00139         /* The associations are ordered by distance and the (1-filter) with greater distance are not used */
00140         /* This type of filtering is called "trimmed-ICP" */
00141         //float filter;
00142 
00143         /* AsocError: in [0,1] */
00144         /* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */
00145         /* When the number of associations is below AsocError, the main function will return error in associations step */
00146         // float AsocError;
00147 
00148         /* --------------------- */
00149         /* --- Exit parameters */
00150         /* --------------------- */
00151         /* MaxIter: sets the maximum number of iterations for the algorithm to exit */
00152         /* The more iterations, the more chance you give the algorithm to be more accurate   */
00153         //int MaxIter;
00154 
00155         /* errorRatio: in [0,1] sets the maximum error ratio between iterations to exit */
00156         /* In iteration K, let be errorK the residual of the minimization */
00157         /* Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is the solution of the scan matching */
00158         //float error_th;
00159 
00160         /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */
00161         /* In each iteration, the error is the residual of the minimization in each component */
00162         /* The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out) */
00163         /* When errorK tends to 0 the more precise is the solution of the scan matching */
00164         //float errx_out,erry_out, errt_out;
00165 
00166         /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria (the two above criteria) */
00167         /* (error_th) OR (errorx_out && errory_out && errt_out) */
00168         /* With this parameter >1 avoids random solutions and estabilices the algorithm */
00169         //int IterSmoothConv;
00170 
00171 
00172 
00173 void Init_MbICP_ScanMatching(
00174                              float max_laser_range,
00175                              float Bw,
00176                              float Br,
00177                              float L,
00178                              int   laserStep,
00179                              float MaxDistInter,
00180                              float filter,
00181                              int   ProjectionFilter,
00182                              float AsocError,
00183                              int   MaxIter,
00184                              float errorRatio,
00185                              float errx_out,
00186                              float erry_out,
00187                              float errt_out,
00188                              int IterSmoothConv);
00189 
00190 // -------------------------------------------------------------
00191 
00192 // ************************
00193 // Function that does the scan matching
00194 // ************************
00195 
00196 /* int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
00197                                  Tsc *sensorMotion, Tsc *solution); */
00198 
00199 // in:::
00200 //      laserK: is the reference scan in polar coordinates (max. num points is MAXLASERPOINTS)
00201 //              laserK1: is the new scan in polar coordinates (max. num points is MAXLASERPOINTS)
00202 //              sensorMotion: initial SENSOR motion estimation from location K to location K1
00203 //              solution: SENSOR motion solution from location K to location K1
00204 // out:::
00205 //              1 : Everything OK in less that the Maximum number of iterations
00206 //              2 : Everything OK but reached the Maximum number of iterations
00207 //              -1: Failure in the association step
00208 //              -2: Failure in the minimization step
00209 
00210 int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
00211                                  Tsc *sensorMotion, Tsc *solution);
00212 
00213 #ifdef __cplusplus
00214 }
00215 #endif
00216 
00217 #endif

Last updated 12 September 2005 21:38:45