MbICP.h
Go to the documentation of this file.
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 
16 /*****************************************************************************/
17 //
18 // EVERYTHING IN THE INTERNATIONAL SYSTEM (METERS AND RADIANS)
19 //
20 /*****************************************************************************/
21 
22 #ifndef MbICP
23 #define MbICP
24 #include "TData.h"
25 
26 #ifdef __cplusplus
27 extern "C" {
28 #endif
29 
30 
31 
32 // ----------------------------------------------------------------------------
33 // GLOBAL FUNCTIONS
34 // ----------------------------------------------------------------------------
35 
36 
37 // ************************
38 // Function that initializes the SM parameters
39 // ************************
40 
41 /* void InitScanMatching(float Bw, float Br,
42  float L, int laserStep,float MaxDistInter, float filtrado,
43  int MaxIter, float error_th, float exo, float eyo, float etitao, int IterSmoothConv); */
44 // in:::
45 
46  /* --------------------- */
47  /* --- Thresold parameters */
48  /* --------------------- */
49  /* Bw: maximum angle diference between points of different scans */
50  /* Points with greater Bw cannot be correspondent (eliminate spurius asoc.) */
51  /* This is a speed up parameter */
52  //float Bw;
53 
54  /* Br: maximum distance difference between points of different scans */
55  /* Points with greater Br cannot be correspondent (eliminate spurius asoc.) */
56  //float Br;
57 
58  /* --------------------- */
59  /* --- Inner parameters */
60  /* --------------------- */
61  /* L: value of the metric */
62  /* When L tends to infinity you are using the standart ICP */
63  /* When L tends to 0 you use the metric (more importance to rotation) */
64  //float L;
65 
66  /* laserStep: selects points of each scan with an step laserStep */
67  /* When laserStep=1 uses all the points of the scans */
68  /* When laserStep=2 uses one each two ... */
69  /* This is an speed up parameter */
70  //int laserStep;
71 
72  /* ProjectionFilter: */
73  /* Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97) */
74  /* It works well for angles < 45 \circ*/
75  /* 1 : activates the filter */
76  /* 0 : desactivates the filter */
77  // int ProjectionFilter;
78 
79  /* MaxDistInter: maximum distance to interpolate between points in the ref scan */
80  /* Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment */
81  //float MaxDistInter;
82 
83  /* filter: in [0,1] sets the % of asociations NOT considered spurious */
84  /* E.g. if filter=0.9 you use 90% of the associations */
85  /* The associations are ordered by distance and the (1-filter) with greater distance are not used */
86  /* This type of filtering is called "trimmed-ICP" */
87  //float filter;
88 
89  /* AsocError: in [0,1] */
90  /* One way to check if the algorithm diverges if to supervise if the number of associatios goes below a thresold */
91  /* When the number of associations is below AsocError, the main function will return error in associations step */
92  // float AsocError;
93 
94  /* --------------------- */
95  /* --- Exit parameters */
96  /* --------------------- */
97  /* MaxIter: sets the maximum number of iterations for the algorithm to exit */
98  /* The more iterations, the more chance you give the algorithm to be more accurate */
99  //int MaxIter;
100 
101  /* errorRatio: in [0,1] sets the maximum error ratio between iterations to exit */
102  /* In iteration K, let be errorK the residual of the minimization */
103  /* Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is the solution of the scan matching */
104  //float error_th;
105 
106  /* errx_out,erry_out, errt_out: minimum error of the asociations to exit */
107  /* In each iteration, the error is the residual of the minimization in each component */
108  /* The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out) */
109  /* When errorK tends to 0 the more precise is the solution of the scan matching */
110  //float errx_out,erry_out, errt_out;
111 
112  /* IterSmoothConv: number of consecutive iterations that satisfity the error criteria (the two above criteria) */
113  /* (error_th) OR (errorx_out && errory_out && errt_out) */
114  /* With this parameter >1 avoids random solutions and estabilices the algorithm */
115  //int IterSmoothConv;
116 
117 
118 
119 void Init_MbICP_ScanMatching(
120  float max_laser_range,
121  float Bw,
122  float Br,
123  float L,
124  int laserStep,
125  float MaxDistInter,
126  float filter,
127  int ProjectionFilter,
128  float AsocError,
129  int MaxIter,
130  float errorRatio,
131  float errx_out,
132  float erry_out,
133  float errt_out,
134  int IterSmoothConv);
135 
136 // -------------------------------------------------------------
137 
138 // ************************
139 // Function that does the scan matching
140 // ************************
141 
142 /* int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
143  Tsc *sensorMotion, Tsc *solution); */
144 
145 // in:::
146 // laserK: is the reference scan in polar coordinates (max. num points is MAXLASERPOINTS)
147 // laserK1: is the new scan in polar coordinates (max. num points is MAXLASERPOINTS)
148 // sensorMotion: initial SENSOR motion estimation from location K to location K1
149 // solution: SENSOR motion solution from location K to location K1
150 // out:::
151 // 1 : Everything OK in less that the Maximum number of iterations
152 // 2 : Everything OK but reached the Maximum number of iterations
153 // -1: Failure in the association step
154 // -2: Failure in the minimization step
155 
156 int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
157  Tsc *sensorMotion, Tsc *solution);
158 
159 #ifdef __cplusplus
160 }
161 #endif
162 
163 #endif
Tsc
Definition: TData.h:42
Init_MbICP_ScanMatching
void Init_MbICP_ScanMatching(float max_laser_range, float Bw, float Br, float L, int laserStep, float MaxDistInter, float filter, int ProjectionFilter, float AsocError, int MaxIter, float errorRatio, float errx_out, float erry_out, float errt_out, int IterSmoothConv)
Definition: MbICP.c:122
MbICPmatcher
int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1, Tsc *sensorMotion, Tsc *solution)
Definition: MbICP.c:162
Tpfp
Definition: TData.h:32

csm
Author(s): Andrea Censi
autogenerated on Tue May 11 2021 02:18:23