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