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