External polar scan matcher algorithm main header. More...
#include <stdio.h>
Go to the source code of this file.
Classes | |
| struct | PMScan |
| Structure describing a laser scan. More... | |
Defines | |
| #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 360 |
| 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 720 |
| Maximum number of points in a scan. | |
| #define | PM_LASER PM_VSCAN |
| #define | PM_LASER_NAME "VirtualScan" |
| The name of the laser range finder. | |
| #define | PM_LASER_Y 0 |
| [cm] Y coordinate of the laser on the 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 500 |
| Maximum number of iterations for PSM. | |
| #define | PM_MAX_ITER_ICP 500 |
| Maximum number of iterations for ICP. | |
| #define | PM_MAX_RANGE 100000 |
| [cm] Maximum valid laser range. | |
| #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 5.0 |
| [cm] The minimum match result standard deviation in X or Y direction. Used in covariance estimation. | |
| #define | PM_MIN_VALID_POINTS 80 |
| 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 10 |
| Half window size which is searched for correct orientation. | |
| #define | PM_SEG_MAX_DIST 80.0 |
| The distance between points to break a segment. | |
| #define | PM_SICK_LMS200 2 |
| #define | PM_STOP_COND 0.1 |
| 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 double |
| The variable type used in calculations. Change it to double for higher accuracy and lower speed. | |
| #define | PM_VSCAN 4 |
| #define | PM_WEIGHTING_FACTOR 30*30 |
| Parameter used for weighting associated points in the position calculation of PSM. Try setting it to 30*30 for laser odometry. | |
Functions | |
| PM_TYPE | pm_corridor_angle (PMScan *act) |
| Determines the orientation of a corridor. | |
| 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. | |
| PM_TYPE | pm_error_index (PMScan *lsr, PMScan *lsa) |
| Calculates an error index expressing the quality of a match. | |
| PM_TYPE | pm_error_index2 (PMScan *ref, PMScan *cur, int *associatedPoints=NULL) |
| More quickly calculates an error index expressing the quality of a match. | |
| PM_TYPE | pm_icp (const PMScan *lsr, PMScan *lsa) |
| Matches two laser scans using the iterative closest point method. | |
| void | pm_init (const char *filename=NULL, FILE **fin=NULL) |
| Initialises internal variables and opens a log file. | |
| bool | pm_is_corridor (PMScan *act) |
| Guesses if a scan was taken on a corridor. | |
| void | pm_plotScan (PMScan *ls, const char *col, double diameter=2.0, bool connect_lines=false) |
| void | pm_plotScan4Thesis (PMScan *lsr, PMScan *lsa) |
| 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) |
| void | pm_plotTime4Thesis (PM_TYPE xt, PM_TYPE yt, PM_TYPE tht, int *iter=NULL, double *time=NULL) |
| void | pm_preprocessScan (PMScan *ls) |
| Shows segmentation results by plotting segments with different colours. | |
| PM_TYPE | pm_psm (const PMScan *lsr, PMScan *lsa) |
| Match two laser scans using polar scan matching. | |
| int | pm_readScan (FILE *fin, PMScan *ls) |
| Reads one scan from file fin and stores it in ls. | |
| void | pm_save_scan (PMScan *act, const char *filename) |
| Saves scan in a text file. | |
| void | pm_show_segmentation (const PMScan *ls) |
| void | pm_unit_test (int matching_alg=PM_PSM, bool interactive=true) |
| Performs unit tests on scan matching. | |
Variables | |
| PM_TYPE | pm_co [PM_L_POINTS] |
| Contains the cosinus of each bearing. | |
| const PM_TYPE | PM_D2R |
| Conversion factor for converting degrees to radians. | |
| PM_TYPE | pm_fi [PM_L_POINTS] |
| Contains precomputed range bearings. | |
| const PM_TYPE | PM_R2D |
| Conversion factor for converting radians to degrees. | |
| PM_TYPE | pm_si [PM_L_POINTS] |
| Contains the sinus of each bearing. | |
External polar scan matcher algorithm main header.
Definition in file polar_match.h.
| #define PM_CHANGE_WEIGHT_ITER 10 |
The number of iterations after which the weighting factor is reduced to weight down outliers.
Definition at line 85 of file polar_match.h.
| #define PM_CORRIDOR_THRESHOLD 25.0 |
Threshold for angle variation between points to determine if scan was taken of a corridor.
Definition at line 70 of file polar_match.h.
| #define PM_EMPTY 16 |
Measurement tag: no measurment (between 2 segments there is no interpolation!).
Definition at line 111 of file polar_match.h.
| #define PM_FOV 360 |
Field of view of the laser range finder in degrees.
Definition at line 66 of file polar_match.h.
| #define PM_HOKUYO_URG_04LX 1 |
Definition at line 51 of file polar_match.h.
| #define PM_HOKUYO_UTM_30LX 3 |
Definition at line 53 of file polar_match.h.
| #define PM_ICP 3 |
Scan matching with iterative closest point association rule.
Definition at line 102 of file polar_match.h.
| #define PM_L_POINTS 720 |
Maximum number of points in a scan.
Definition at line 65 of file polar_match.h.
| #define PM_LASER PM_VSCAN |
Definition at line 60 of file polar_match.h.
| #define PM_LASER_NAME "VirtualScan" |
The name of the laser range finder.
Definition at line 64 of file polar_match.h.
| #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.
Definition at line 76 of file polar_match.h.
| #define PM_MATCH_ERROR_OFFSET 5.0 |
[cm] Offset subtracted from average range residual when scaling the covariance matrix.
Definition at line 98 of file polar_match.h.
| #define PM_MAX_ERROR 100 |
[cm] Maximum distance between associated points used in pose estimation. Try setting it to 30 for laser odometry.
Definition at line 89 of file polar_match.h.
| #define PM_MAX_ITER 500 |
Maximum number of iterations for PSM.
Definition at line 92 of file polar_match.h.
| #define PM_MAX_ITER_ICP 500 |
Maximum number of iterations for ICP.
Definition at line 93 of file polar_match.h.
| #define PM_MAX_RANGE 100000 |
[cm] Maximum valid laser range.
Definition at line 67 of file polar_match.h.
| #define PM_MIN_RANGE 10.0f |
[cm] Minimum valid laser range for the reprojected current scan.
Definition at line 82 of file polar_match.h.
| #define PM_MIN_STD_ORIENTATION 4.0 |
[degrees] The minimum standard deviation of the orientation match. Used in covariance estimation.
Definition at line 97 of file polar_match.h.
| #define PM_MIN_STD_XY 5.0 |
[cm] The minimum match result standard deviation in X or Y direction. Used in covariance estimation.
Definition at line 96 of file polar_match.h.
| #define PM_MIN_VALID_POINTS 80 |
Minimum number of valid points for scan matching.
Definition at line 68 of file polar_match.h.
| #define PM_MIXED 4 |
Measurement tag: range reading is a mixed pixel.
Definition at line 109 of file polar_match.h.
| #define PM_MOVING 2 |
Measurement tag: range reading corresponds to a moving point. Not implemented yet.
Definition at line 108 of file polar_match.h.
| #define PM_OCCLUDED 8 |
Measurement tag: range reading is occluded.
Definition at line 110 of file polar_match.h.
| #define PM_ODO -1 |
Show results with odometry only in mapping_with_matching().
Definition at line 100 of file polar_match.h.
| #define PM_PSD_SCANNER 0 |
Definition at line 50 of file polar_match.h.
| #define PM_PSM 1 |
Polar scan matching - matching bearing association rule.
Definition at line 101 of file polar_match.h.
| #define PM_RANGE 1 |
Measurement tag: range reading is longer than PM_MAX_RANGE.
Definition at line 107 of file polar_match.h.
| #define PM_SEARCH_WINDOW 10 |
Half window size which is searched for correct orientation.
Definition at line 69 of file polar_match.h.
| #define PM_SEG_MAX_DIST 80.0 |
The distance between points to break a segment.
Definition at line 83 of file polar_match.h.
| #define PM_SICK_LMS200 2 |
Definition at line 52 of file polar_match.h.
| #define PM_STOP_COND 0.1 |
If the pose change (|dx|+|dy|+|dth|) is smaller than this PSM scan matching stops.
Definition at line 91 of file polar_match.h.
| #define PM_STOP_COND_ICP 0.1 |
Stopping condition for ICP. The pose change has to be smaller than this.
Definition at line 94 of file polar_match.h.
| #define PM_TIME_DELAY 0 |
[s] Time delay (time registration error) in the laser measurements. Use 0.02 for SLAMbot.
Definition at line 75 of file polar_match.h.
| #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.
Definition at line 104 of file polar_match.h.
| #define PM_TYPE double |
The variable type used in calculations. Change it to double for higher accuracy and lower speed.
Definition at line 87 of file polar_match.h.
| #define PM_VSCAN 4 |
Definition at line 54 of file polar_match.h.
| #define PM_WEIGHTING_FACTOR 30*30 |
Parameter used for weighting associated points in the position calculation of PSM. Try setting it to 30*30 for laser odometry.
Definition at line 84 of file polar_match.h.
| 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.
| act | The scan of which oriention is to be determined. |
Definition at line 875 of file polar_match.cpp.
| 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.
| err | The error index of the match. | |
| c11,c12,c22,c33 | The estimated covariance matrix elements. | |
| corridor | If true, a special estimate with large along-corridor-error is used. | |
| corr_angle | The orientation of the corridor. Call pm_corridor_angle on the refernce scan to get this value. |
Definition at line 969 of file polar_match.cpp.
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.
| lsr | The reference scan. | |
| lsa | The current scan. |
Definition at line 667 of file polar_match.cpp.
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.
| ref | The reference scan. | |
| cur | The current scan. | |
| associatedPoints | number of associated points. |
Definition at line 814 of file polar_match.cpp.
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.
Definition at line 1182 of file polar_match.cpp.
| 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.
| filename | The name of the optional logfile to be used. | |
| fin | The optional opened file pointer is returned here. |
Definition at line 126 of file polar_match.cpp.
| 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.
| act | The scan which is examined for being corridor-like. |
Definition at line 513 of file polar_match.cpp.
| void pm_plotScan | ( | PMScan * | ls, | |
| const char * | col, | |||
| double | diameter = 2.0, |
|||
| bool | connect_lines = false | |||
| ) |
| 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 | |||
| ) |
| void pm_plotTime4Thesis | ( | PM_TYPE | xt, | |
| PM_TYPE | yt, | |||
| PM_TYPE | tht, | |||
| int * | iter = NULL, |
|||
| double * | time = NULL | |||
| ) |
| void pm_preprocessScan | ( | PMScan * | ls | ) |
Shows segmentation results by plotting segments with different colours.
| ls | The scan to be plotted. Prepares a scan for scan matching.Filters the scan using median filter, finds far away points and segments the scan. | |
| ls | The scan to be preprocessed. |
Definition at line 482 of file polar_match.cpp.
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.
| lsr | The reference scan. | |
| lsa | The current scan. |
Definition at line 1024 of file polar_match.cpp.
| 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]
.
.
.
| fin | Pointer to the file opened with pm_init(). | |
| ls | The read laser scan is returned here. |
Definition at line 177 of file polar_match.cpp.
| 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
| act | The scan to be saved. | |
| filename | The name of the file the scan is saved under. |
Definition at line 1529 of file polar_match.cpp.
| void pm_show_segmentation | ( | const PMScan * | ls | ) |
| 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.
| matching_alg | Specify the scan matching algorithm to be tested (PM_PSM, PM_ICP). | |
| interactive | If true, graphically display results. |
Definition at line 2089 of file polar_match.cpp.
| PM_TYPE pm_co[PM_L_POINTS] |
Contains the cosinus of each bearing.
Definition at line 67 of file polar_match.cpp.
| const PM_TYPE PM_D2R |
Conversion factor for converting degrees to radians.
Definition at line 68 of file polar_match.cpp.
| PM_TYPE pm_fi[PM_L_POINTS] |
Contains precomputed range bearings.
Definition at line 65 of file polar_match.cpp.
| const PM_TYPE PM_R2D |
Conversion factor for converting radians to degrees.
Definition at line 69 of file polar_match.cpp.
| PM_TYPE pm_si[PM_L_POINTS] |
Contains the sinus of each bearing.
Definition at line 66 of file polar_match.cpp.