mbicp

ScanMatching. More...

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

• position2d

Requires

• position2d : source of pose and velocity information
• 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)