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

Last updated 12 September 2005 21:38:45