ScanMatching

This driver implements the metric-based ICP scan-matching algorithm.

J. Minguez, L. Montesano, and F. Lamiraux, "Metric-based iterative closest point scan matching for sensor displacement estimation," IEEE Transactions on Robotics, vol. 22, no. 5, pp. 1047-1054, 2006.

Compile-time dependencies
  • none
Provides
  • interface_position2d
Requires
  • interface_position2d : source of pose and velocity information
  • interface_laser : Pose-stamped laser scans (subtype PLAYER_LASER_DATA_SCANPOSE)
Configuration requests
  • none
Configuration file options
  • max_laser_range (float)
    • Default: 7.9 m
    • Maximum laser range.
  • laserPose_x (float)
    • Default: 0.16 m
    • Offset of the laser on the edge x (in the robot's system of reference).
  • laserPose_y (float)
    • Default: 0.0 m
    • Offset of the laser on the edge y (in the robot's system of reference).
  • laserPose_th (float)
    • Default: 0.0 rad
    • Offset of the laser on th (in the robot's system of reference).
  • radial_window (float)
    • Default: 0.3 m
    • Maximum distance difference between points of different scans. Points with greater Br cannot be correspondent (eliminate spurius asoc.).
  • angular_window (float)
    • Default: 0.523333333 rad
    • Maximum angle diference between points of different scans. Points with greater Bw cannot be correspondent (eliminate spurius asoc.).
  • L (float)
    • Default: 3.00
    • Value of the metric. When L tends to infinity you are using the standart ICP. When L tends to 0 you use the metric (more importance to rotation), when L tends to infinity you are using Euclidian metric.
  • laserStep (integer)
    • Default: 1
    • Selects points of each scan with an step laserStep. When laserStep=1 uses all the points of the scans When laserStep=2 uses one each two ... This is an speed up parameter.
  • MaxDistInter (float)
    • Default: 0.5 m
    • Maximum distance to interpolate between points in the ref scan. Consecutive points with less Euclidean distance than MaxDistInter are considered to be a segment.
  • filter (float)
    • Default: 0.95
    • In [0,1] sets the % of asociations NOT considered spurious. E.g. if filter=0.9 you use 90% of the associations. The associations are ordered by distance and the (1-filter) with greater distance are not used. This type of filtering is called "trimmed-ICP".
  • ProjectionFilter (int)
    • Default: 1
    • Eliminate the points that cannot be seen given the two scans (see Lu&Millios 97). It works well for angles < 45 deg. 1 : activates the filter. 0 : desactivates the filter.
  • AsocError (float)
    • Default: 0.1
    • In [0,1]. Sets the % of minimun associations to run the algorithm. One way to check if the algorithm diverges is to supervise if the number of associatios goes below a thresold. When the number of associations is below AsocError, the main function will return error in associations step.
  • MaxIter (int)
    • Default: 50
    • Sets the maximum number of iterations for the algorithm to exit. The more iterations, the more chance you give the algorithm to be more accurate.
  • errorRatio (float)
    • Default: 0.0001 m
    • In [0,1] sets the maximum error ratio between iterations to exit. In iteration K, let be errorK the residual of the minimization. Error_th=(errorK-1/errorK). When error_th tends to 1 more precise is the solution of the scan matching.
  • IterSmoothConv (int)
    • Default: 2
    • Number of consecutive iterations that satisfity the error criteria (the two above criteria) (error_th) OR (errorx_out && errory_out && errt_out). With this parameter >1 avoids random solutions and estabilices the algorithm.
  • errx_out (float)
    • Default: 0.0001 m
    • Minimum error in x of the asociations to exit. In each iteration, the error is the residual of the minimization in each component. The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK tends to 0 the more precise is the solution of the scan matching
  • erry_out (float)
    • Default: 0.0001 m
    • Minimum error in x of the asociations to exit. In each iteration, the error is the residual of the minimization in each component. The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK tends to 0 the more precise is the solution of the scan matching
  • errt_out (float)
    • Default: 0.0001 m
    • Minimum error in x of the asociations to exit. In each iteration, the error is the residual of the minimization in each component. The condition is (errorKx<errx_out && errorKx<erry_out && errorKx<errt_out). When errorK tends to 0 the more precise is the solution of the scan matching
Example
driver
(
  name "mbicp"
  provides ["position2d:1"]
  requires ["position2d:0" "laser:1"]

  max_laser_range               7.9
  laserPose_x                   0.16
  laserPose_y                   0
  laserPose_th                  0

  radial_window                 0.3
  angular_window                0.523333333

  L                             3.00
  laserStep                     1
  MaxDistInter                  0.5
  filter                        0.95
  ProjectionFilter              1
  AsocError                     0.1
  MaxIter                       50

  errorRatio                    0.0001
  errx_out                      0.0001
  erry_out                      0.0001
  errt_out                      0.0001
  IterSmoothConv                2
)
Author
Javier Minguez (underlying algorithm)