MbICP2.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 /*      J. Minguez, F. Lamiraux and L. Montesano                                                                                 */
00010 /*      Metric-Based Iterative Closest Point,                                                                                    */
00011 /*  Scan Matching for Mobile Robot Displacement Estimation                                                       */
00012 /*      IEEE Transactions on Roboticics (2006)                                                                               */
00013 /*                                                                                   */
00014 /*************************************************************************************/
00015 /*
00016  *  This program is free software; you can redistribute it and/or modify
00017  *  it under the terms of the GNU General Public License as published by
00018  *  the Free Software Foundation; either version 2 of the License, or
00019  *  (at your option) any later version.
00020  *
00021  *  This program is distributed in the hope that it will be useful,
00022  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00023  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00024  *  GNU General Public License for more details.
00025  *
00026  *  You should have received a copy of the GNU General Public License
00027  *  along with this program; if not, write to the Free Software
00028  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00029  *
00030  */
00031 
00032 /* **************************************************************************************** */
00033 //      This file contains inner information of the MbICP that you want to see from the outside
00034 /* **************************************************************************************** */
00035 
00036 #ifndef MbICP2
00037 #define MbICP2
00038 
00039 //#include "MbICP.h"
00040 #include "TData.h"
00041 
00042 #ifdef __cplusplus
00043 extern "C" {
00044 #endif
00045 
00046 // ---------------------------------------------------------------
00047 // ---------------------------------------------------------------
00048 // Types definition
00049 // ---------------------------------------------------------------
00050 // ---------------------------------------------------------------
00051 
00052 
00053 // ************************
00054 // Associations information
00055 
00056 /*
00057 typedef struct{
00058   float rx,ry,nx,ny,dist;               // Point (nx,ny), static corr (rx,ry), dist
00059   int numDyn;                                   // Number of dynamic associations
00060   float unknown;                                // Unknown weight
00061   int index;                                    // Index within the original scan
00062   int L,R;
00063 }TAsoc;
00064 */
00065 
00066 // ************************
00067 // Scan inner matching parameters
00068 typedef struct{
00069         /* --------------------- */
00070         /* --- Thresold parameters */
00071         /* Bw: maximum angle diference between points of different scans */
00072         /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */
00073         /* This is a speed up parameter */
00074         float Bw;
00075 
00076         /* Br: maximum distance difference between points of different scans */
00077         /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */
00078         float Br;
00079 
00080         /* --------------------- */
00081         /* --- Inner parameters */
00082 
00083         /* L: value of the metric */
00084         /* When L tends to infinity you are using the standart ICP */
00085     /* When L tends to 0 you use the metric (more importance to rotation */
00086         float LMET;
00087 
00088         /* laserStep: selects points of each scan with an step laserStep  */
00089         /* When laserStep=1 uses all the points of the scans */
00090         /* When laserStep=2 uses one each two ... */
00091         /* This is an speed up parameter */
00092         int laserStep;
00093 
00094         /* ProjectionFilter: */
00095         /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */
00096         /* It works well for angles < 45 \circ*/
00097         /* 1 : activates the filter */
00098         /* 0 : desactivates the filter */
00099         int ProjectionFilter;
00100 
00101         /* MaxDistInter: maximum distance to interpolate between points in the ref scan */
00102         /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */
00103         float MaxDistInter;
00104 
00105         /* filtrado: in [0,1] sets the % of asociations NOT considered spurious */
00106         float filter;
00107 
00108         /* AsocError: in [0,1] */
00109         /* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */
00110         /* When the number of associations is below AsocError, the main function will return error in associations step */
00111         float AsocError;
00112 
00113         /* --------------------- */
00114         /* --- Exit parameters */
00115         /* MaxIter: sets the maximum number of iterations for the algorithm to exit */
00116         /* More iterations more chance you give the algorithm to be more accurate   */
00117         int MaxIter;
00118 
00119         /* error_th: in [0,1] sets the maximum error ratio between iterations to exit */
00120         /* In each iteration, the error is the residual of the minimization */
00121         /* When error_th tends to 1 more precise is the solution of the scan matching */
00122         float error_th;
00123 
00124         /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */
00125         /* In each iteration, the error is the residual of the minimization in each component */
00126         /* The condition is (lower than errx_out && lower than erry_out && lower than errt_out */
00127         /* When error_XXX tend to 0 more precise is the solution of the scan matching */
00128         float errx_out,erry_out, errt_out;
00129 
00130         /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria */
00131         /* (error_th) OR (errorx_out && errory_out && errt_out) */
00132         /* With this parameter >1 avoids random solutions */
00133         int IterSmoothConv;
00134 
00135 }TSMparams;
00136 
00137 // ************************
00138 // Structure to store the scans in polar and cartesian coordinates
00139 
00140 /*
00141 typedef struct {
00142   int numPuntos;
00143   Tpf laserC[MAXLASERPOINTS];  // Cartesian coordinates
00144   Tpfp laserP[MAXLASERPOINTS]; // Polar coordinates
00145 }Tscan;
00146 */
00147 
00148 // ---------------------------------------------------------------
00149 // ---------------------------------------------------------------
00150 // Variables definition
00151 // ---------------------------------------------------------------
00152 // ---------------------------------------------------------------
00153 
00154 
00155 // ************************
00156 // Static structure to initialize the SM parameters
00157 extern TSMparams params;
00158 
00159 // Original points to be aligned
00160 extern Tscan ptosRef;
00161 extern Tscan ptosNew;
00162 
00163 // At each step::
00164 
00165 // Those points removed by the projection filter (see Lu&Millios -- IDC)
00166 extern Tscan ptosNoView; // Only with ProjectionFilter=1;
00167 
00168 // Structure of the associations before filtering
00169 extern TAsoc cp_associations[MAXLASERPOINTS];
00170 extern int cntAssociationsT;
00171 
00172 // Filtered Associations
00173 extern TAsoc cp_associationsTemp[MAXLASERPOINTS];
00174 extern int cntAssociationsTemp;
00175 
00176 // Current motion estimation
00177 extern Tsc motion2;
00178 
00179 #ifdef __cplusplus
00180 }
00181 #endif
00182 
00183 #endif

Last updated 12 September 2005 21:38:45