External polar scan matcher algorithm source code. More...
#include <iostream>#include <math.h>#include <stdlib.h>#include <string.h>#include <assert.h>#include "polar_match.h"
Go to the source code of this file.
Defines | |
| #define | INTERPOLATE_ICP |
| #define | SQ(x) ((x)*(x)) |
Functions | |
| PM_TYPE | norm_a (PM_TYPE a) |
| Normalize angle. | |
| 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, PM_TYPE corr_angle) |
| 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) |
| More quickly calculates an error index expressing the quality of a match. | |
| void | pm_find_far_points (PMScan *ls) |
| Tags point further than a given distance PM_MAX_RANGE. | |
| 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, FILE **fin) |
| 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_median_filter (PMScan *ls) |
| Plots scan ls at robot/laser pose x, y, th. | |
| double | pm_msec (void) |
| Returns thread runtime under Linux in milliseconds. | |
| PM_TYPE | pm_orientation_search (const PMScan *ref, const PM_TYPE *new_r, const int *new_bad) |
| Performs one iteration of orientation alignment of current scan. | |
| 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_scan_project (const PMScan *act, PM_TYPE *new_r, int *new_bad) |
| Plots current and reference scan in the way scans appeared in my thesis. | |
| void | pm_segment_scan (PMScan *ls) |
| Segments scanpoints into groups based on range discontinuities. | |
| void | pm_take_simulated_scan (const PM_TYPE xl, const PM_TYPE yl, const PM_TYPE thl, PMScan *ls, PM_TYPE wallDistLeft=150.0, PM_TYPE wallDistFront=200.0) |
| Takes a scan in a simulated room. | |
| PM_TYPE | pm_translation_estimation (const PMScan *ref, const PM_TYPE *new_r, const int *new_bad, PM_TYPE C, PM_TYPE *dx, PM_TYPE *dy) |
| Estimate the postion of the current scan with respect to a reference scan. | |
| void | pm_unit_test (int matching_alg, bool interactive) |
| Performs unit tests on scan matching. | |
| PM_TYPE | point_line_distance (PM_TYPE x1, PM_TYPE y1, PM_TYPE x2, PM_TYPE y2, PM_TYPE x3, PM_TYPE y3, PM_TYPE *x, PM_TYPE *y) |
| Calculate the distance of a point from a line section. | |
Variables | |
| PM_TYPE | pm_co [PM_L_POINTS] |
| Contains the cosinus of each bearing. | |
| const PM_TYPE | PM_D2R = M_PI/180.0 |
| Conversion factor for converting degrees to radians. | |
| const PM_TYPE | PM_DFI = PM_FOV*PM_D2R/ ( PM_L_POINTS + 1.0 ) |
| PM_TYPE | pm_fi [PM_L_POINTS] |
| Contains precomputed range bearings. | |
| const PM_TYPE | PM_FI_MAX = M_PI/2.0 + PM_FOV*PM_D2R/2.0 |
| const PM_TYPE | PM_FI_MIN = M_PI/2.0 - PM_FOV*PM_D2R/2.0 |
| const PM_TYPE | PM_R2D = 180.0/M_PI |
| 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 source code.
Definition in file polar_match.cpp.
| #define INTERPOLATE_ICP |
| #define SQ | ( | x | ) | ((x)*(x)) |
| PM_TYPE norm_a | ( | PM_TYPE | a | ) | [inline] |
Normalize angle.
Normalize angle to be within [-pi,pi).
TODO: Make more efficient.
| a | The angle to be normalized. |
Definition at line 102 of file polar_match.cpp.
| 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.
| void pm_find_far_points | ( | PMScan * | ls | ) |
Tags point further than a given distance PM_MAX_RANGE.
Far away points get tagged as PM_RANGE.
| ls | The scan searched for far points. |
Definition at line 421 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_median_filter | ( | PMScan * | ls | ) |
Plots scan ls at robot/laser pose x, y, th.
The scan is shifted by PM_LASER_Y.
| ls | The laser scan to be plotted. The scan's pose is ignored. | |
| x | The X coordinate of the pose where the scan should be drawn. | |
| y | The Y coordinate of the pose where the scan should be drawn. | |
| th | The orientation coordinate of the pose where the scan should be drawn. | |
| col | The colour of the scan as "red", "green"... See dr_COLORS for more. | |
| diameter | [cm] The drawn diameter of the measured points. | |
| connect_lines | If true, measured point are connected with lines. Plots scan ls.The scan is shifted by PM_LASER_Y. | |
| ls | The laser scan to be plotted at the scan's pose. | |
| col | The colour of the scan as "red", "green"... See dr_COLORS for more. | |
| diameter | [cm] The drawn diameter of the measured points. | |
| connect_lines | If true, measured points are connected with lines. Filters the laser ranges with a median filter.The job of this median filter is to remove chair and table legs which are likely to change position with time. |
The median filter helps to get rid of spurious data. If the median filter's window is 5, then 3 points need be close to each other to surrive the filtering. Chair legs taking 1 or 2 range readings will be removed.
Do not use this function when fitting lines to laser scans!
Median filter will round up corners.
x,y coordinates of points are not upadted.
| ls | Laser scan to be filtered. |
Definition at line 288 of file polar_match.cpp.
| double pm_msec | ( | void | ) |
Returns thread runtime under Linux in milliseconds.
Definition at line 82 of file polar_match.cpp.
| PM_TYPE pm_orientation_search | ( | const PMScan * | ref, | |
| const PM_TYPE * | new_r, | |||
| const int * | new_bad | |||
| ) |
Performs one iteration of orientation alignment of current scan.
Function estimating the orientation of the current scan represented with range readings new_r tagged with flags new_bad with respect to the reference scan ref.
This function exploits that if the current and reference scan are taken at the same position, an orientation change of the current scan results in a left or right shift of the scan ranges.
This function estimates the orientation by finding that shift which minimizes the difference between the current and ref. scan. The orientation estimate is then refined using interpolation by fitting a parabole to the maximum and its neighbors and finding the maximum.
| ref | The reference scan. | |
| new_r | The interpolated ranges of the current scan. | |
| new_bad | The tags corresponding to the new_r. |
TODO: speed up by unrolling the loop, replace if with multiplication with 0 or 1/ use sse2 instructions...
TODO: Find out when is it useful to check if delta < PM_MAX_ERROR - why only for the UTM...
Definition at line 1828 of file polar_match.cpp.
| 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_scan_project | ( | const PMScan * | act, | |
| PM_TYPE * | new_r, | |||
| int * | new_bad | |||
| ) |
Plots current and reference scan in the way scans appeared in my thesis.
| lsr | Reference scan. | |
| lsa | Current scan. 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.
| xt | The true X coordinate where the solution should converge. | |
| yt | The true Y coordinate where the solution should converge. | |
| tht | Something that was not documented and i don't understand what it means. | |
| iter | number of iterations. | |
| time | final time. Performs scan projection.This function enables the comparisson of two scans. It projects the current (active) scan act into the reference scans ref coordinate frame, using the current scan's pose. As the reference scan is assumed to be at the origin, its coordinates don't need to be passed along. Returns in new_r the interpolated range readinds r at the reference scan's measurement bearings. Returns in new_bad bad flags of the interpolated range readings, where occluded readings are tagged. | |
| act | The current scan. | |
| new_r | Array of the projected range readings (has to have the correct size). | |
| new_bad | Information about the validity of the interpolated range readings is returned here. |
TODO: replace this hack with proper fix where we don't loose points.
TODO: Uncomment this? (or leave it as it is a local scan matching approach anyway)
Definition at line 1686 of file polar_match.cpp.
| void pm_segment_scan | ( | PMScan * | ls | ) |
Segments scanpoints into groups based on range discontinuities.
By segmenting scans into groups of disconnected sets of points, one can prevent falsely interpolating points into the free space between disconnected objects during scan projection.
Segment number 0 is reserved to segments containing only 1 point.
Far away points (r > PM_MAX_RANGE), gaps between groups of points - divide segments. The gap between extrapolated point and current point has to be large as well to prevent corridor walls to be segmented into separate points.
Definition at line 333 of file polar_match.cpp.
| void pm_take_simulated_scan | ( | const PM_TYPE | xl, | |
| const PM_TYPE | yl, | |||
| const PM_TYPE | thl, | |||
| PMScan * | ls, | |||
| PM_TYPE | wallDistLeft = 150.0, |
|||
| PM_TYPE | wallDistFront = 200.0 | |||
| ) |
Takes a scan in a simulated room.
The simulated room is just a rectangle.
| xl | X coordinate of where the scan is taken. | |
| yl | Y coordinate of where the scan is taken. | |
| thl | Orientation with which the scan is taken. | |
| ls | The laser range finder pose is read out from here, and the | |
| wallDistLeft | [cm] The distance of the left and right wall from the centre. Optional. | |
| wallDistFront | [cm] The distance of the front and back wall from the centre. Optional. |
Definition at line 2038 of file polar_match.cpp.
| PM_TYPE pm_translation_estimation | ( | const PMScan * | ref, | |
| const PM_TYPE * | new_r, | |||
| const int * | new_bad, | |||
| PM_TYPE | C, | |||
| PM_TYPE * | dx, | |||
| PM_TYPE * | dy | |||
| ) |
Estimate the postion of the current scan with respect to a reference scan.
| ref | The reference scan. | |
| new_r | The interpolated ranges of the current scan. | |
| new_bad | The tags corresponding to the new_r. | |
| C | Weighting factor for range residuals. | |
| dx | Estimated position increment X coordinate is returned here. | |
| dy | Estimated position increment Y coordinate is returned here. |
Definition at line 1940 of file polar_match.cpp.
| 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 point_line_distance | ( | PM_TYPE | x1, | |
| PM_TYPE | y1, | |||
| PM_TYPE | x2, | |||
| PM_TYPE | y2, | |||
| PM_TYPE | x3, | |||
| PM_TYPE | y3, | |||
| PM_TYPE * | x, | |||
| PM_TYPE * | y | |||
| ) |
Calculate the distance of a point from a line section.
Calculates the distance of the point (x3,y3) from a line defined by (x1,y1) and (x2,y2). Returns the distance to the line or -1 if the projection of (x3,y3) falls outside the line segment defined by (x1,y1) and (x2,y2). The projection of (x3,y3) onto the line is also returned in x,y. This function is used in ICP.
| x1,y1 | The start point of the line section. | |
| x2,y2 | The end point of the line section. | |
| x3,y3 | The point of which distance it sought. | |
| x,y | (x3,y3) projected onto the line segment is returned here. |
Definition at line 1150 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 = M_PI/180.0 |
Conversion factor for converting degrees to radians.
Definition at line 68 of file polar_match.cpp.
Definition at line 72 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.
Definition at line 71 of file polar_match.cpp.
Definition at line 70 of file polar_match.cpp.
| const PM_TYPE PM_R2D = 180.0/M_PI |
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.