MbICP2.h
1 /*************************************************************************************/
2 /* */
3 /* File: MbICP.h */
4 /* Authors: Luis Montesano and Javier Minguez */
5 /* Modified: 1/3/2006 */
6 /* */
7 /* This library implements the: */
8 /* */
9 /* J. Minguez, F. Lamiraux and L. Montesano */
10 /* Metric-Based Iterative Closest Point, */
11 /* Scan Matching for Mobile Robot Displacement Estimation */
12 /* IEEE Transactions on Roboticics (2006) */
13 /* */
14 /*************************************************************************************/
15 /*
16  * This program is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation; either version 2 of the License, or
19  * (at your option) any later version.
20  *
21  * This program is distributed in the hope that it will be useful,
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24  * GNU General Public License for more details.
25  *
26  * You should have received a copy of the GNU General Public License
27  * along with this program; if not, write to the Free Software
28  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29  *
30  */
31 
32 /* **************************************************************************************** */
33 // This file contains inner information of the MbICP that you want to see from the outside
34 /* **************************************************************************************** */
35 
36 #ifndef MbICP2
37 #define MbICP2
38 
39 //#include "MbICP.h"
40 #include "TData.h"
41 
42 #ifdef __cplusplus
43 extern "C" {
44 #endif
45 
46 // ---------------------------------------------------------------
47 // ---------------------------------------------------------------
48 // Types definition
49 // ---------------------------------------------------------------
50 // ---------------------------------------------------------------
51 
52 
53 // ************************
54 // Associations information
55 
56 /*
57 typedef struct{
58  float rx,ry,nx,ny,dist; // Point (nx,ny), static corr (rx,ry), dist
59  int numDyn; // Number of dynamic associations
60  float unknown; // Unknown weight
61  int index; // Index within the original scan
62  int L,R;
63 }TAsoc;
64 */
65 
66 // ************************
67 // Scan inner matching parameters
68 typedef struct{
69  /* --------------------- */
70  /* --- Thresold parameters */
71  /* Bw: maximum angle diference between points of different scans */
72  /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */
73  /* This is a speed up parameter */
74  float Bw;
75 
76  /* Br: maximum distance difference between points of different scans */
77  /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */
78  float Br;
79 
80  /* --------------------- */
81  /* --- Inner parameters */
82 
83  /* L: value of the metric */
84  /* When L tends to infinity you are using the standart ICP */
85  /* When L tends to 0 you use the metric (more importance to rotation */
86  float LMET;
87 
88  /* laserStep: selects points of each scan with an step laserStep */
89  /* When laserStep=1 uses all the points of the scans */
90  /* When laserStep=2 uses one each two ... */
91  /* This is an speed up parameter */
92  int laserStep;
93 
94  /* ProjectionFilter: */
95  /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */
96  /* It works well for angles < 45 \circ*/
97  /* 1 : activates the filter */
98  /* 0 : desactivates the filter */
99  int ProjectionFilter;
100 
101  /* MaxDistInter: maximum distance to interpolate between points in the ref scan */
102  /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */
103  float MaxDistInter;
104 
105  /* filtrado: in [0,1] sets the % of asociations NOT considered spurious */
106  float filter;
107 
108  /* AsocError: in [0,1] */
109  /* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */
110  /* When the number of associations is below AsocError, the main function will return error in associations step */
111  float AsocError;
112 
113  /* --------------------- */
114  /* --- Exit parameters */
115  /* MaxIter: sets the maximum number of iterations for the algorithm to exit */
116  /* More iterations more chance you give the algorithm to be more accurate */
117  int MaxIter;
118 
119  /* error_th: in [0,1] sets the maximum error ratio between iterations to exit */
120  /* In each iteration, the error is the residual of the minimization */
121  /* When error_th tends to 1 more precise is the solution of the scan matching */
122  float error_th;
123 
124  /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */
125  /* In each iteration, the error is the residual of the minimization in each component */
126  /* The condition is (lower than errx_out && lower than erry_out && lower than errt_out */
127  /* When error_XXX tend to 0 more precise is the solution of the scan matching */
128  float errx_out,erry_out, errt_out;
129 
130  /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria */
131  /* (error_th) OR (errorx_out && errory_out && errt_out) */
132  /* With this parameter >1 avoids random solutions */
133  int IterSmoothConv;
134 
135 }TSMparams;
136 
137 // ************************
138 // Structure to store the scans in polar and cartesian coordinates
139 
140 /*
141 typedef struct {
142  int numPuntos;
143  Tpf laserC[MAXLASERPOINTS]; // Cartesian coordinates
144  Tpfp laserP[MAXLASERPOINTS]; // Polar coordinates
145 }Tscan;
146 */
147 
148 // ---------------------------------------------------------------
149 // ---------------------------------------------------------------
150 // Variables definition
151 // ---------------------------------------------------------------
152 // ---------------------------------------------------------------
153 
154 
155 // ************************
156 // Static structure to initialize the SM parameters
157 extern TSMparams params;
158 
159 // Original points to be aligned
160 extern Tscan ptosRef;
161 extern Tscan ptosNew;
162 
163 // At each step::
164 
165 // Those points removed by the projection filter (see Lu&Millios -- IDC)
166 extern Tscan ptosNoView; // Only with ProjectionFilter=1;
167 
168 // Structure of the associations before filtering
169 extern TAsoc cp_associations[MAXLASERPOINTS];
170 extern int cntAssociationsT;
171 
172 // Filtered Associations
173 extern TAsoc cp_associationsTemp[MAXLASERPOINTS];
174 extern int cntAssociationsTemp;
175 
176 // Current motion estimation
177 extern Tsc motion2;
178 
179 #ifdef __cplusplus
180 }
181 #endif
182 
183 #endif

Last updated 12 September 2005 21:38:45