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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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
43extern "C" {
44#endif
45
46// ---------------------------------------------------------------
47// ---------------------------------------------------------------
48// Types definition
49// ---------------------------------------------------------------
50// ---------------------------------------------------------------
51
52
53// ************************
54// Associations information
55
56/*
57typedef 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
68typedef 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/*
141typedef 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
157extern TSMparams params;
158
159// Original points to be aligned
160extern Tscan ptosRef;
161extern Tscan ptosNew;
162
163// At each step::
164
165// Those points removed by the projection filter (see Lu&Millios -- IDC)
166extern Tscan ptosNoView; // Only with ProjectionFilter=1;
167
168// Structure of the associations before filtering
169extern TAsoc cp_associations[MAXLASERPOINTS];
170extern int cntAssociationsT;
171
172// Filtered Associations
173extern TAsoc cp_associationsTemp[MAXLASERPOINTS];
174extern int cntAssociationsTemp;
175
176// Current motion estimation
177extern Tsc motion2;
178
179#ifdef __cplusplus
180}
181#endif
182
183#endif
Definition TData.h:73
Definition MbICP2.h:68
Definition TData.h:57
Definition TData.h:63