PolarScanMatching(PSM)  1
polar_match.h
Go to the documentation of this file.
1 /* **************************************************************************
2  polar_match.h - Matching laser scans in polar coord system
3  -------------------
4  begin : Tue Nov 9 2004
5  version : 0.3
6  copyright : (C) 2004-2010 by Albert Diosi and Lindsay Kleeman
7  email : albert.diosi@gmail.com
8  comments : - range units are cm; angle units are radians or degrees
9  - the laser is on the robot's Y axis
10  - in scan projections, occluded ref scanpoints are not removed!
11  - TODO: Investigate why is checking the range difference necessary for
12  the Hokuyo UTM and not for the rest.
13  ***************************************************************************/
14 /****************************************************************************
15 Copyright (c) 2004-2015, Albert Diosi and Lindsay Kleeman
16 All rights reserved.
17 
18 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
19 
20  * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
21  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
22  * The name of the copyright holders may not be used to endorse or promote products derived from this software without specific prior written permission.
23 
24 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 ****************************************************************************/
26 
27 /*
28 First of all you need to select the laser range finder you have by
29 setting "#define PM_LASER "
30 to PM_SICK_LMS200, PM_HOKUYO_URG_04LX.
31 */
32 
33 #ifndef _POLAR_MATCH_
34 #define _POLAR_MATCH_
35 
36 #include <stdio.h>
37 
38 //TODO: get the result generation to work again:
39 //#define PM_GENERATE_RESULTS //If left uncommented scanmatching results are
40  //saved into the "results"directory if two required programs are present.
41 
42 //----------------- L A S E R S P E C I F I C P A R A M E T E R S------------
43 
44 // STEP 1) Define a name for your laser range finder here (if it hasn't been defined yet):
45 #define PM_PSD_SCANNER 0
46 #define PM_HOKUYO_URG_04LX 1
47 #define PM_SICK_LMS200 2
48 #define PM_HOKUYO_UTM_30LX 3
49 
50 // STEP 2) Set the type your laser range finder here:
51 // #define PM_LASER PM_SICK_LMS200
52 // #define PM_LASER PM_HOKUYO_URG_04LX
53 // #define PM_LASER PM_PSD_SCANNER
54  #define PM_LASER PM_HOKUYO_UTM_30LX
55 
56 // STEP 3) Add your laser range finder's parameters here if it is a different model (use centimeters)
57 
58 #if PM_LASER == PM_PSD_SCANNER
59  #define PM_LASER_NAME "PSD_Scanner"
60  #define PM_L_POINTS 200
61  #define PM_FOV 360
62  #define PM_MAX_RANGE 400
63  #define PM_MIN_VALID_POINTS 100
64  #define PM_SEARCH_WINDOW 50
65  #define PM_CORRIDOR_THRESHOLD 25.0
66 #elif PM_LASER == PM_HOKUYO_URG_04LX
67  #define PM_LASER_NAME "Hokuyo URG-04LX"
68  #define PM_L_POINTS 682
69  #define PM_FOV 240
70  #define PM_MAX_RANGE 530
71  #define PM_MIN_VALID_POINTS 200
72  #define PM_SEARCH_WINDOW 80
73  #define PM_CORRIDOR_THRESHOLD 25.0
74 #elif PM_LASER == PM_SICK_LMS200
75  #define PM_LASER_NAME "Sick LMS"
76  #define PM_L_POINTS 181
77  #define PM_FOV 180
78  #define PM_MAX_RANGE 1000
79  #define PM_MIN_VALID_POINTS 40
80  #define PM_SEARCH_WINDOW 20
81  #define PM_CORRIDOR_THRESHOLD 25.0
82 #elif PM_LASER == PM_HOKUYO_UTM_30LX
83  #define PM_LASER_NAME "HOKUYO UTM-30LX"
84  #define PM_L_POINTS 1081
85  #define PM_FOV 270
86  #define PM_MAX_RANGE 700
87  #define PM_MIN_VALID_POINTS 300
88  #define PM_SEARCH_WINDOW 200
89  #define PM_CORRIDOR_THRESHOLD 25.0
90 #endif
91 
92 // STEP 4) Set the time registration delay (the time between your time stamps and the
93 // time the scan was taken) and the distance of your laser from the odometry center
94 // in the forward direction.
95 #define PM_TIME_DELAY 0
96 #define PM_LASER_Y 0
97 #define PM_MIN_RANGE 10.0f
103 #define PM_SEG_MAX_DIST 20.0
104 #define PM_WEIGHTING_FACTOR 70*70
105 #define PM_CHANGE_WEIGHT_ITER 10
106 
107 #define PM_TYPE float
108 
109 #define PM_MAX_ERROR 100
110 #define PM_STOP_COND 0.4
111 #define PM_MAX_ITER 30
112 #define PM_MAX_ITER_ICP 60
113 #define PM_STOP_COND_ICP 0.1
114 
115 #define PM_MIN_STD_XY 20.0
116 #define PM_MIN_STD_ORIENTATION 4.0
117 #define PM_MATCH_ERROR_OFFSET 5.0
118 
119 #define PM_ODO -1
120 #define PM_PSM 1
121 #define PM_ICP 3
122 
123 #define PM_TIME_FILE "results/iterations.txt"
124 
125 // Description of range reading errors. Each range measurement may be tagged with one of these:
126 #define PM_RANGE 1
127 #define PM_MOVING 2
128 #define PM_MIXED 4
129 #define PM_OCCLUDED 8
130 #define PM_EMPTY 16
131 
132 extern PM_TYPE pm_fi[PM_L_POINTS];
133 extern PM_TYPE pm_si[PM_L_POINTS];
134 extern PM_TYPE pm_co[PM_L_POINTS];
135 extern const PM_TYPE PM_D2R;
136 extern const PM_TYPE PM_R2D;
137 
147 struct PMScan
148 {
149  double t;
157  int seg[PM_L_POINTS];
160 };
161 
162 void pm_init(const char* filename=NULL, FILE **fin=NULL);
163 int pm_readScan(FILE *fin, PMScan *ls);
164 void pm_save_scan(PMScan *act,const char *filename);
165 
166 void pm_preprocessScan(PMScan *ls);
167 
168 PM_TYPE pm_psm( const PMScan *lsr,PMScan *lsa);
169 PM_TYPE pm_icp( const PMScan *lsr,PMScan *lsa);
170 
171 void pm_plotScanAt(const PMScan *ls, PM_TYPE x,PM_TYPE y,PM_TYPE th,const char *col, double diameter = 2.0, bool connect_lines = false);
172 void pm_plotScan(PMScan *ls, const char *col,double diameter = 2.0, bool connect_lines = false);
173 void pm_show_segmentation(const PMScan *ls);
174 void pm_plotScan4Thesis(PMScan *lsr,PMScan *lsa);
175 void pm_plotTime4Thesis(PM_TYPE xt, PM_TYPE yt, PM_TYPE tht,int *iter=NULL,double *time=NULL);
176 
177 bool pm_is_corridor(PMScan *act);
179 PM_TYPE pm_error_index2 ( PMScan *ref,PMScan *cur, int* associatedPoints=NULL );
181 void pm_cov_est(PM_TYPE err, double *c11,double *c12, double *c22, double *c33,
182  bool corridor=false, PM_TYPE corr_angle=0);
183 
184 void pm_unit_test(int matching_alg = PM_PSM, bool interactive=true);
185 #endif
void pm_plotScan4Thesis(PMScan *lsr, PMScan *lsa)
Plots current and reference scan in the way scans appeared in my thesis.
Definition: polar_match.cpp:1553
void pm_show_segmentation(const PMScan *ls)
Shows segmentation results by plotting segments with different colours.
Definition: polar_match.cpp:444
void pm_plotScanAt(const PMScan *ls, PM_TYPE x, PM_TYPE y, PM_TYPE th, const char *col, double diameter=2.0, bool connect_lines=false)
Plots scan ls at robot/laser pose x, y, th.
Definition: polar_match.cpp:208
bool pm_is_corridor(PMScan *act)
Guesses if a scan was taken on a corridor.
Definition: polar_match.cpp:524
void pm_save_scan(PMScan *act, const char *filename)
Saves scan in a text file.
Definition: polar_match.cpp:1539
PM_TYPE r[PM_L_POINTS]
[cm] Laser range readings. 0 or negative ranges denote invalid readings.
Definition: polar_match.h:153
int pm_readScan(FILE *fin, PMScan *ls)
Reads one scan from file fin and stores it in ls.
Definition: polar_match.cpp:169
PM_TYPE ry
[cm] Robot odometry Y coordinate.
Definition: polar_match.h:151
void pm_unit_test(int matching_alg=PM_PSM, bool interactive=true)
Performs unit tests on scan matching.
Definition: polar_match.cpp:2097
PM_TYPE pm_error_index(PMScan *lsr, PMScan *lsa)
Calculates an error index expressing the quality of a match.
Definition: polar_match.cpp:678
const PM_TYPE PM_R2D
Conversion factor for converting radians to degrees.
Definition: polar_match.cpp:65
void pm_plotTime4Thesis(PM_TYPE xt, PM_TYPE yt, PM_TYPE tht, int *iter=NULL, double *time=NULL)
Generates a convergence speed plot from previously saved result.
Definition: polar_match.cpp:1625
double t
[s] Time when scan was taken.
Definition: polar_match.h:149
PM_TYPE x[PM_L_POINTS]
[cm] Laser reading X coordinates in Cartesian coordinates.
Definition: polar_match.h:154
#define PM_L_POINTS
Maximum number of points in a scan.
Definition: polar_match.h:84
PM_TYPE pm_error_index2(PMScan *ref, PMScan *cur, int *associatedPoints=NULL)
More quickly calculates an error index expressing the quality of a match.
Definition: polar_match.cpp:824
PM_TYPE pm_si[PM_L_POINTS]
Contains the sinus of each bearing.
Definition: polar_match.cpp:62
PM_TYPE rx
[cm] Robot odometry X coordinate.
Definition: polar_match.h:150
int bad[PM_L_POINTS]
Tag describing the validity of a range measurement. 0 if OK; sources of invalidity - out of range rea...
Definition: polar_match.h:156
void pm_init(const char *filename=NULL, FILE **fin=NULL)
Initialises internal variables and opens a log file.
Definition: polar_match.cpp:122
void pm_cov_est(PM_TYPE err, double *c11, double *c12, double *c22, double *c33, bool corridor=false, PM_TYPE corr_angle=0)
Estimates the covariance matrix of a match.
Definition: polar_match.cpp:979
int seg[PM_L_POINTS]
Describes which segment the range reading belongs to.
Definition: polar_match.h:159
PM_TYPE pm_icp(const PMScan *lsr, PMScan *lsa)
Matches two laser scans using the iterative closest point method.
Definition: polar_match.cpp:1192
PM_TYPE th
[rad] Robot orientation.
Definition: polar_match.h:152
#define PM_TYPE
The variable type used in calculations. Change it to double for higher accuracy and lower speed...
Definition: polar_match.h:107
#define PM_PSM
Polar scan matching - matching bearing association rule.
Definition: polar_match.h:120
Structure describing a laser scan.
Definition: polar_match.h:147
PM_TYPE pm_co[PM_L_POINTS]
Contains the cosinus of each bearing.
Definition: polar_match.cpp:63
void pm_preprocessScan(PMScan *ls)
Prepares a scan for scan matching.
Definition: polar_match.cpp:493
PM_TYPE pm_fi[PM_L_POINTS]
Contains precomputed range bearings.
Definition: polar_match.cpp:61
PM_TYPE pm_corridor_angle(PMScan *act)
Determines the orientation of a corridor.
Definition: polar_match.cpp:885
PM_TYPE y[PM_L_POINTS]
[cm] Laser reading Y coordinates in Cartesian coordinates.
Definition: polar_match.h:155
PM_TYPE pm_psm(const PMScan *lsr, PMScan *lsa)
Match two laser scans using polar scan matching.
Definition: polar_match.cpp:1034
void pm_plotScan(PMScan *ls, const char *col, double diameter=2.0, bool connect_lines=false)
Plots scan ls.
Definition: polar_match.cpp:276
const PM_TYPE PM_D2R
Conversion factor for converting degrees to radians.
Definition: polar_match.cpp:64