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