PolarScanMatching(PSM)  1
Classes | Macros | Functions | Variables
polar_match.h File Reference
#include <stdio.h>

Go to the source code of this file.

Classes

struct  PMScan
 Structure describing a laser scan. More...
 

Macros

#define PM_PSD_SCANNER   0
 
#define PM_HOKUYO_URG_04LX   1
 
#define PM_SICK_LMS200   2
 
#define PM_HOKUYO_UTM_30LX   3
 
#define PM_LASER   PM_HOKUYO_UTM_30LX
 
#define PM_LASER_NAME   "HOKUYO UTM-30LX"
 The name of the laser range finder. More...
 
#define PM_L_POINTS   1081
 Maximum number of points in a scan. More...
 
#define PM_FOV   270
 Field of view of the laser range finder in degrees. More...
 
#define PM_MAX_RANGE   700
 [cm] Maximum valid laser range. (3000 for this sensor.) More...
 
#define PM_MIN_VALID_POINTS   300
 Minimum number of valid points for scan matching. More...
 
#define PM_SEARCH_WINDOW   200
 Half window size which is searched for correct orientation. More...
 
#define PM_CORRIDOR_THRESHOLD   25.0
 Threshold for angle variation between points to determine if scan was taken of a corridor. More...
 
#define PM_TIME_DELAY   0
 [s] Time delay (time registration error) in the laser measurements. Use 0.02 for SLAMbot. More...
 
#define PM_LASER_Y   0
 [cm] Y coordinate of the laser on the robot. More...
 
#define PM_MIN_RANGE   10.0f
 [cm] Minimum valid laser range for the reprojected current scan. More...
 
#define PM_SEG_MAX_DIST   20.0
 The distance between points to break a segment. More...
 
#define PM_WEIGHTING_FACTOR   70*70
 Parameter used for weighting associated points in the position calculation of PSM. Try setting it to 30*30 for laser odometry. More...
 
#define PM_CHANGE_WEIGHT_ITER   10
 The number of iterations after which the weighting factor is reduced to weight down outliers. More...
 
#define PM_TYPE   float
 The variable type used in calculations. Change it to double for higher accuracy and lower speed. More...
 
#define PM_MAX_ERROR   100
 [cm] Maximum distance between associated points used in pose estimation. Try setting it to 30 for laser odometry. More...
 
#define PM_STOP_COND   0.4
 If the pose change (|dx|+|dy|+|dth|) is smaller than this PSM scan matching stops. More...
 
#define PM_MAX_ITER   30
 Maximum number of iterations for PSM. More...
 
#define PM_MAX_ITER_ICP   60
 Maximum number of iterations for ICP. More...
 
#define PM_STOP_COND_ICP   0.1
 Stopping condition for ICP. The pose change has to be smaller than this. More...
 
#define PM_MIN_STD_XY   20.0
 [cm] The minimum match result standard deviation in X or Y direction. Used in covariance estimation. More...
 
#define PM_MIN_STD_ORIENTATION   4.0
 [degrees] The minimum standard deviation of the orientation match. Used in covariance estimation. More...
 
#define PM_MATCH_ERROR_OFFSET   5.0
 [cm] Offset subtracted from average range residual when scaling the covariance matrix. More...
 
#define PM_ODO   -1
 Show results with odometry only in mapping_with_matching(). More...
 
#define PM_PSM   1
 Polar scan matching - matching bearing association rule. More...
 
#define PM_ICP   3
 Scan matching with iterative closest point association rule. More...
 
#define PM_TIME_FILE   "results/iterations.txt"
 When generating results, timing is data is saved into this file from the matching algorithms. i|time|ax|ay|ath[deg] is saved. Does not work in the moment. More...
 
#define PM_RANGE   1
 Measurement tag: range reading is longer than PM_MAX_RANGE. More...
 
#define PM_MOVING   2
 Measurement tag: range reading corresponds to a moving point. Not implemented yet. More...
 
#define PM_MIXED   4
 Measurement tag: range reading is a mixed pixel. More...
 
#define PM_OCCLUDED   8
 Measurement tag: range reading is occluded. More...
 
#define PM_EMPTY   16
 Measurement tag: no measurment (between 2 segments there is no interpolation!) More...
 

Functions

void pm_init (const char *filename=NULL, FILE **fin=NULL)
 Initialises internal variables and opens a log file. More...
 
int pm_readScan (FILE *fin, PMScan *ls)
 Reads one scan from file fin and stores it in ls. More...
 
void pm_save_scan (PMScan *act, const char *filename)
 Saves scan in a text file. More...
 
void pm_preprocessScan (PMScan *ls)
 Prepares a scan for scan matching. More...
 
PM_TYPE pm_psm (const PMScan *lsr, PMScan *lsa)
 Match two laser scans using polar scan matching. More...
 
PM_TYPE pm_icp (const PMScan *lsr, PMScan *lsa)
 Matches two laser scans using the iterative closest point method. More...
 
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. More...
 
void pm_plotScan (PMScan *ls, const char *col, double diameter=2.0, bool connect_lines=false)
 Plots scan ls. More...
 
void pm_show_segmentation (const PMScan *ls)
 Shows segmentation results by plotting segments with different colours. More...
 
void pm_plotScan4Thesis (PMScan *lsr, PMScan *lsa)
 Plots current and reference scan in the way scans appeared in my thesis. More...
 
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. More...
 
bool pm_is_corridor (PMScan *act)
 Guesses if a scan was taken on a corridor. More...
 
PM_TYPE pm_error_index (PMScan *lsr, PMScan *lsa)
 Calculates an error index expressing the quality of a match. More...
 
PM_TYPE pm_error_index2 (PMScan *ref, PMScan *cur, int *associatedPoints=NULL)
 More quickly calculates an error index expressing the quality of a match. More...
 
PM_TYPE pm_corridor_angle (PMScan *act)
 Determines the orientation of a corridor. More...
 
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. More...
 
void pm_unit_test (int matching_alg=PM_PSM, bool interactive=true)
 Performs unit tests on scan matching. More...
 

Variables

PM_TYPE pm_fi [PM_L_POINTS]
 Contains precomputed range bearings. More...
 
PM_TYPE pm_si [PM_L_POINTS]
 Contains the sinus of each bearing. More...
 
PM_TYPE pm_co [PM_L_POINTS]
 Contains the cosinus of each bearing. More...
 
const PM_TYPE PM_D2R
 Conversion factor for converting degrees to radians. More...
 
const PM_TYPE PM_R2D
 Conversion factor for converting radians to degrees. More...
 

Macro Definition Documentation

#define PM_CHANGE_WEIGHT_ITER   10

The number of iterations after which the weighting factor is reduced to weight down outliers.

#define PM_CORRIDOR_THRESHOLD   25.0

Threshold for angle variation between points to determine if scan was taken of a corridor.

#define PM_EMPTY   16

Measurement tag: no measurment (between 2 segments there is no interpolation!)

#define PM_FOV   270

Field of view of the laser range finder in degrees.

#define PM_HOKUYO_URG_04LX   1
#define PM_HOKUYO_UTM_30LX   3
#define PM_ICP   3

Scan matching with iterative closest point association rule.

#define PM_L_POINTS   1081

Maximum number of points in a scan.

#define PM_LASER   PM_HOKUYO_UTM_30LX
#define PM_LASER_NAME   "HOKUYO UTM-30LX"

The name of the laser range finder.

#define PM_LASER_Y   0

[cm] Y coordinate of the laser on the robot.

Set to 0 for ground thruth, simulation, mapping_with_matching. Set to 0 when in doubt and handle the coordinate transforms yourself. Set it to 31.3 for Slambot. The Y axis points in the forward motion direction of a differential drive robot.

#define PM_MATCH_ERROR_OFFSET   5.0

[cm] Offset subtracted from average range residual when scaling the covariance matrix.

#define PM_MAX_ERROR   100

[cm] Maximum distance between associated points used in pose estimation. Try setting it to 30 for laser odometry.

#define PM_MAX_ITER   30

Maximum number of iterations for PSM.

#define PM_MAX_ITER_ICP   60

Maximum number of iterations for ICP.

#define PM_MAX_RANGE   700

[cm] Maximum valid laser range. (3000 for this sensor.)

#define PM_MIN_RANGE   10.0f

[cm] Minimum valid laser range for the reprojected current scan.

#define PM_MIN_STD_ORIENTATION   4.0

[degrees] The minimum standard deviation of the orientation match. Used in covariance estimation.

#define PM_MIN_STD_XY   20.0

[cm] The minimum match result standard deviation in X or Y direction. Used in covariance estimation.

#define PM_MIN_VALID_POINTS   300

Minimum number of valid points for scan matching.

#define PM_MIXED   4

Measurement tag: range reading is a mixed pixel.

#define PM_MOVING   2

Measurement tag: range reading corresponds to a moving point. Not implemented yet.

#define PM_OCCLUDED   8

Measurement tag: range reading is occluded.

#define PM_ODO   -1

Show results with odometry only in mapping_with_matching().

#define PM_PSD_SCANNER   0
#define PM_PSM   1

Polar scan matching - matching bearing association rule.

#define PM_RANGE   1

Measurement tag: range reading is longer than PM_MAX_RANGE.

#define PM_SEARCH_WINDOW   200

Half window size which is searched for correct orientation.

#define PM_SEG_MAX_DIST   20.0

The distance between points to break a segment.

#define PM_SICK_LMS200   2
#define PM_STOP_COND   0.4

If the pose change (|dx|+|dy|+|dth|) is smaller than this PSM scan matching stops.

#define PM_STOP_COND_ICP   0.1

Stopping condition for ICP. The pose change has to be smaller than this.

#define PM_TIME_DELAY   0

[s] Time delay (time registration error) in the laser measurements. Use 0.02 for SLAMbot.

#define PM_TIME_FILE   "results/iterations.txt"

When generating results, timing is data is saved into this file from the matching algorithms. i|time|ax|ay|ath[deg] is saved. Does not work in the moment.

#define PM_TYPE   float

The variable type used in calculations. Change it to double for higher accuracy and lower speed.

#define PM_WEIGHTING_FACTOR   70*70

Parameter used for weighting associated points in the position calculation of PSM. Try setting it to 30*30 for laser odometry.

Function Documentation

PM_TYPE pm_corridor_angle ( PMScan act)

Determines the orientation of a corridor.

TODO: NEEDS A REWRITE - TOO MESSY.

Assuming the scan was taken of a corridor, determines the orientation of the corridor by finding the maximum in a 180 degree wide angle histogram. The input into the histogram are angles of lines created by connecting neighbouring points.

Normally, this function is used on the reference scan and not the current scan.

Parameters
actThe scan of which oriention is to be determined.
Returns
The orientation of the corridor.
void pm_cov_est ( PM_TYPE  err,
double *  c11,
double *  c12,
double *  c22,
double *  c33,
bool  corridor,
PM_TYPE  corr_angle 
)

Estimates the covariance matrix of a match.

Estimates elements (c11,c12,c22,c33) of a match result covariance matrix of the following structure:
[c11 c12 0.0]
[c12 c22 0.0]
[0.0 0.0 c33]

This function scales a base covariance matrix by an error index:
C = C0(PM_MIN_STD_XY, PM_MIN_STD_ORIENTATION) * (err - PM_MATCH_ERROR_OFFSET).
For corridors it scales and rotates a covariance matrix stretched in the direction of the corridor:
C = Rot(C0, corr_angle) * (err - PM_MATCH_ERROR_OFFSET).
If (err - PM_MATCH_ERROR_OFFSET) < 1 then C = C0.

Parameters
errThe error index of the match.
c11,c12,c22,c33The estimated covariance matrix elements.
corridorIf true, a special estimate with large along-corridor-error is used.
corr_angleThe orientation of the corridor. Call pm_corridor_angle on the refernce scan to get this value.
PM_TYPE pm_error_index ( PMScan lsr,
PMScan lsa 
)

Calculates an error index expressing the quality of a match.

This function assesses how well is the current scan aligned with the reference scan. This function has to be called after a scan has been matched. The current scan's pose has to be expressed in the reference scan's coordinate system.

The current scan is compared to the reference scan and vice versa, then the maximum is taken. The comparisson is performed by calculating the average closest point distance. Far away points are ignored in the process. The number of non-far away points have to be larger than a threshold.

This function is computationally very expensive and takes a conservative guess on the match quality.

TODO: Improve the accuracy of the estimate. Speed it up. Perhaps should use the error output from scan matching instead. A proper test is necessary.

Parameters
lsrThe reference scan.
lraThe current scan.
Returns
The average minimum Euclidean distance.
PM_TYPE pm_error_index2 ( PMScan ref,
PMScan cur,
int *  associatedPoints 
)

More quickly calculates an error index expressing the quality of a match.

This function assesses how well is the current scan aligned with the reference scan. This function has to be called after a scan has been matched. The current scan's pose has to be expressed in the reference scan's coordinate system.

The current scan is compared to the reference scan by projecting the current scan where the reference scan was taken and calculating the average range residuals.

Parameters
lsrThe reference scan.
lraThe current scan.
Returns
The average minimum Euclidean distance.
PM_TYPE pm_icp ( const PMScan lsr,
PMScan lsa 
)

Matches two laser scans using the iterative closest point method.

Minimizes least square error of points through changing lsa->rx, lsa->ry, lsa->th by using ICP. It interpolates associated points. Only the best 80% of points are used in the pose calculation. Scan projection is done at each iteration.

For maintanence reasons changed scan projection to that of psm.

void pm_init ( const char *  filename,
FILE **  fin 
)

Initialises internal variables and opens a log file.

If filename is present, then it opens the scanfile and reads out the first line. The file pointer is then set to the corresponding file. Before performing any scan matching call this fuction once to initialize important global variables.

Upon failure an exception is thrown.

Parameters
filenameThe name of the optional logfile to be used.
finThe optional opened file pointer is returned here.
bool pm_is_corridor ( PMScan act)

Guesses if a scan was taken on a corridor.

Scan matching results on corridors are often inaccurate in the along-corridor direction. By detecting scans with a corridor-like appearance one can tailor a covariance matrix to incorporate the along-corridor uncertainty of the scan matching result.

Calculates the variance of angles between neighbouring Cartesian points as the "corridorness" criterion. On corridoors, the majority of vectors connecting neighbouring points are aligned along one line. Thus corridors generate a sharp peak in an angle histogram.

To solve the problem caused at the 180-0 degree transition point the calculations are repeated after a 30 degree shift. This is a hack which can be fixed easily.

The scan is assumed be pre-processed (segmentation and median filtering).

An exeption is thrown if there is less than 1 valid point.

TODO: Remove the double calculation of the variance.

Parameters
actThe scan which is examined for being corridor-like.
Returns
True if act seems to be taken of a corridor.
void pm_plotScan ( PMScan ls,
const char *  col,
double  diameter,
bool  connect_lines 
)

Plots scan ls.

The scan is shifted by PM_LASER_Y.

Parameters
lsThe laser scan to be plotted at the scan's pose.
colThe colour of the scan as "red", "green"... See dr_COLORS for more.
diameter[cm] The drawn diameter of the measured points.
connect_linesIf true, measured points are connected with lines.
void pm_plotScan4Thesis ( PMScan lsr,
PMScan lsa 
)

Plots current and reference scan in the way scans appeared in my thesis.

Parameters
lsrReference scan.
lsaCurrent scan.
void pm_plotScanAt ( const PMScan ls,
PM_TYPE  x,
PM_TYPE  y,
PM_TYPE  th,
const char *  col,
double  diameter,
bool  connect_lines 
)

Plots scan ls at robot/laser pose x, y, th.

The scan is shifted by PM_LASER_Y.

Parameters
lsThe laser scan to be plotted. The scan's pose is ignored.
xThe X coordinate of the pose where the scan should be drawn.
yThe Y coordinate of the pose where the scan should be drawn.
thThe orientation coordinate of the pose where the scan should be drawn.
colThe colour of the scan as "red", "green"... See dr_COLORS for more.
diameter[cm] The drawn diameter of the measured points.
connect_linesIf true, measured point are connected with lines.
void pm_plotTime4Thesis ( PM_TYPE  xt,
PM_TYPE  yt,
PM_TYPE  tht,
int *  iter,
double *  time 
)

Generates a convergence speed plot from previously saved result.

If you want to see the convergence speed, then uncomment the definition of PM_GENERATE_RESULTS, recompile, match a scan and run this function.

Only works if PM_GENERATE_RESULTS was enabled in the previous match as only then are results saved in a file which is read here.

Parameters
xtThe true X coordinate where the solution should converge.
ytThe true Y coordinate where the solution should converge.
tht[degrees] The true orientation where the solution should converge.
void pm_preprocessScan ( PMScan ls)

Prepares a scan for scan matching.

Filters the scan using median filter, finds far away points and segments the scan.

Parameters
lsThe scan to be preprocessed.
PM_TYPE pm_psm ( const PMScan lsr,
PMScan lsa 
)

Match two laser scans using polar scan matching.

Minimizes the sum of square range residuals through changing lsa->rx, lsa->ry, lsa->th. The error is minimized by iterating a translation estimation step followed by an orientation search step.

PSM was not explicitly designed for laser scan matching based odometry where scans with small pose difference are matched with each other without any prior pose information. However when using PSM for this purpose, reduce the values of PM_MAX_ERROR, PM_WEIGHTING_FACTOR to reflect the small inter-scan motion. Also by reducing the value of PM_STOP_COND, larger matching accuracy can be achieved. The currently implemented error estimation functions are not useful for laser odometry error estimation.

Limitations: due to the nature of the association rule divergence in a slow rate may be experienced in rooms where there are not many features to constrain the solution in all directions. This can occur for examples in corridor-like environments including rooms where the room directly in front of the laser is outside of the range of the laser range finder.

Parameters
lsrThe reference scan.
lraThe current scan.
int pm_readScan ( FILE *  fin,
PMScan ls 
)

Reads one scan from file fin and stores it in ls.

Assumed data format:
time[s] x[m] y[m] theta[rad]
r0[cm]
r1[cm]
.
.
.

Parameters
finPointer to the file opened with pm_init().
lsThe read laser scan is returned here.
Returns
Returns 0 on success, -1 if there are no more scans.
void pm_save_scan ( PMScan act,
const char *  filename 
)

Saves scan in a text file.

Saves the scan in the format of:
range[0] bad[0] segment[0]
range[1] bad[1] segment[1]
...
The aim is to enable quick scan loading in Octave using the "scan=load(filename)"; command

Parameters
actThe scan to be saved.
filenamThe name of the file the scan is saved under.
void pm_show_segmentation ( const PMScan ls)

Shows segmentation results by plotting segments with different colours.

Parameters
lsThe scan to be plotted.
void pm_unit_test ( int  matching_alg,
bool  interactive 
)

Performs unit tests on scan matching.

It will assert when compiled in debug mode. Currently the test are very basic (too high level). Should add more tests with time.

Parameters
matching_algSpecify the scan matching algorithm to be tested (PM_PSM, PM_ICP).
interactiveIf true, graphically display results.

Variable Documentation

Contains the cosinus of each bearing.

const PM_TYPE PM_D2R

Conversion factor for converting degrees to radians.

Contains precomputed range bearings.

const PM_TYPE PM_R2D

Conversion factor for converting radians to degrees.

Contains the sinus of each bearing.