MbICP.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 /* */
10 /* J. Minguez, L. Montesano, and F. Lamiraux, "Metric-based iterative */
11 /* closest point scan matching for sensor displacement estimation," IEEE */
12 /* Transactions on Robotics, vol. 22, no. 5, pp. 1047 \u2013 1054, 2006. */
13 /*************************************************************************************/
14 /*
15  * This program is free software; you can redistribute it and/or modify
16  * it under the terms of the GNU General Public License as published by
17  * the Free Software Foundation; either version 2 of the License, or
18  * (at your option) any later version.
19  *
20  * This program is distributed in the hope that it will be useful,
21  * but WITHOUT ANY WARRANTY; without even the implied warranty of
22  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
23  * GNU General Public License for more details.
24  *
25  * You should have received a copy of the GNU General Public License
26  * along with this program; if not, write to the Free Software
27  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
28  *
29  */
30 
31 /*****************************************************************************/
32 //
33 // EVERYTHING IN THE INTERNATIONAL SYSTEM (METERS AND RADIANS)
34 //
35 /*****************************************************************************/
36 
37 #ifndef MbICP
38 #define MbICP
39 #include "TData.h"
40 
41 #ifdef __cplusplus
42 extern "C" {
43 #endif
44 
45 
46 // ----------------------------------------------------------------------------
47 // DEFINES
48 // ----------------------------------------------------------------------------
49 
50 /*
51 #define MAXLASERPOINTS 361
52 */
53 
54 // ----------------------------------------------------------------------------
55 // GENERIC TYPES
56 // ----------------------------------------------------------------------------
57 
58 /*typedef struct {
59  float x;
60  float y;
61 }Tpf;
62 
63 typedef struct {
64  float r;
65  float t;
66 }Tpfp;
67 
68 typedef struct {
69  int x;
70  int y;
71 }Tpi;
72 
73 typedef struct {
74  float x;
75  float y;
76  float tita;
77 }Tsc;
78 */
79 
80 // ----------------------------------------------------------------------------
81 // SPECIFIC TYPES
82 // ----------------------------------------------------------------------------
83 
84 
85 
86 // ----------------------------------------------------------------------------
87 // GLOBAL FUNCTIONS
88 // ----------------------------------------------------------------------------
89 
90 
91 // ************************
92 // Function that initializes the SM parameters
93 // ************************
94 
95 /* void InitScanMatching(float Bw, float Br,
96  float L, int laserStep,float MaxDistInter, float filtrado,
97  int MaxIter, float error_th, float exo, float eyo, float etitao, int IterSmoothConv); */
98 // in:::
99 
100  /* --------------------- */
101  /* --- Thresold parameters */
102  /* --------------------- */
103  /* Bw: maximum angle diference between points of different scans */
104  /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */
105  /* This is a speed up parameter */
106  //float Bw;
107 
108  /* Br: maximum distance difference between points of different scans */
109  /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */
110  //float Br;
111 
112  /* --------------------- */
113  /* --- Inner parameters */
114  /* --------------------- */
115  /* L: value of the metric */
116  /* When L tends to infinity you are using the standart ICP */
117  /* When L tends to 0 you use the metric (more importance to rotation) */
118  //float L;
119 
120  /* laserStep: selects points of each scan with an step laserStep */
121  /* When laserStep=1 uses all the points of the scans */
122  /* When laserStep=2 uses one each two ... */
123  /* This is an speed up parameter */
124  //int laserStep;
125 
126  /* ProjectionFilter: */
127  /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */
128  /* It works well for angles < 45 \circ*/
129  /* 1 : activates the filter */
130  /* 0 : desactivates the filter */
131  // int ProjectionFilter;
132 
133  /* MaxDistInter: maximum distance to interpolate between points in the ref scan */
134  /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */
135  //float MaxDistInter;
136 
137  /* filter: in [0,1] sets the % of asociations NOT considered spurious */
138  /* E.g. if filter=0.9 you use 90% of the associations */
139  /* The associations are ordered by distance and the (1-filter) with greater distance are not used */
140  /* This type of filtering is called "trimmed-ICP" */
141  //float filter;
142 
143  /* AsocError: in [0,1] */
144  /* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */
145  /* When the number of associations is below AsocError, the main function will return error in associations step */
146  // float AsocError;
147 
148  /* --------------------- */
149  /* --- Exit parameters */
150  /* --------------------- */
151  /* MaxIter: sets the maximum number of iterations for the algorithm to exit */
152  /* The more iterations, the more chance you give the algorithm to be more accurate */
153  //int MaxIter;
154 
155  /* errorRatio: in [0,1] sets the maximum error ratio between iterations to exit */
156  /* In iteration K, let be errorK the residual of the minimization */
157  /* Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is the solution of the scan matching */
158  //float error_th;
159 
160  /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */
161  /* In each iteration, the error is the residual of the minimization in each component */
162  /* The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out) */
163  /* When errorK tends to 0 the more precise is the solution of the scan matching */
164  //float errx_out,erry_out, errt_out;
165 
166  /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria (the two above criteria) */
167  /* (error_th) OR (errorx_out && errory_out && errt_out) */
168  /* With this parameter >1 avoids random solutions and estabilices the algorithm */
169  //int IterSmoothConv;
170 
171 
172 
173 void Init_MbICP_ScanMatching(
174  float max_laser_range,
175  float Bw,
176  float Br,
177  float L,
178  int laserStep,
179  float MaxDistInter,
180  float filter,
181  int ProjectionFilter,
182  float AsocError,
183  int MaxIter,
184  float errorRatio,
185  float errx_out,
186  float erry_out,
187  float errt_out,
188  int IterSmoothConv);
189 
190 // -------------------------------------------------------------
191 
192 // ************************
193 // Function that does the scan matching
194 // ************************
195 
196 /* int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
197  Tsc *sensorMotion, Tsc *solution); */
198 
199 // in:::
200 // laserK: is the reference scan in polar coordinates (max. num points is MAXLASERPOINTS)
201 // laserK1: is the new scan in polar coordinates (max. num points is MAXLASERPOINTS)
202 // sensorMotion: initial SENSOR motion estimation from location K to location K1
203 // solution: SENSOR motion solution from location K to location K1
204 // out:::
205 // 1 : Everything OK in less that the Maximum number of iterations
206 // 2 : Everything OK but reached the Maximum number of iterations
207 // -1: Failure in the association step
208 // -2: Failure in the minimization step
209 
210 int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
211  Tsc *sensorMotion, Tsc *solution);
212 
213 #ifdef __cplusplus
214 }
215 #endif
216 
217 #endif
Definition: TData.h:57
Definition: TData.h:47