Player
Frontpage
Contents
User
Installation
Quick start
Supported devices
Tutorials
Utilities
Client libraries
FAQ
Help
Developer
Architecture
libplayercore
interfaces
libplayerdrivers
drivers
libplayercommon
libplayerutils
libplayersd
libplayertcp
libplayerxdr
TODO
Online
Homepage
Download
Project
Bugs
Help
server
drivers
position
mbicp
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
Tsc
Definition:
TData.h:57
TAsoc
Definition:
TData.h:73
TSMparams
Definition:
MbICP2.h:68
Tscan
Definition:
TData.h:63
Last updated 12 September 2005 21:38:45