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