ScanMatching

This driver implements segment-based EKF localization:

"Mobile Robot Localization and Map Building: A Multisensor Fusion Approach" J.A. Castellanos and J.D. Tardós Kluwer Academic Publishers, Boston, 1999 ISBN 0-7923-7789-3

Compile-time dependencies
  • none
Provides
  • interface_position2d
    • The pose estimation
  • interface_localize
    • The full data about the estimation
  • interface_opaque
    • Publishes a 9-tuple of doubles which is the covariance matrix.
    • Under the key "covariance"
Requires
  • interface_position2d : odometry source of pose and velocity information
  • interface_laser : Pose-stamped laser scans (subtype PLAYER_LASER_DATA_SCANPOSE)
  • interface_map : Vector map with segments modelling the environment
  • interface_graphics2d : optional. If provided, it will be used to display debug data
Configuration requests
  • Position requests are forwarded to the odometry position2 interface.
    • They are expected to be in the ekfvloc ref. frame, and will be automatically converted to odometry reference.
  • Velocity requests are forwarded to the odometry position2 interface
Configuration file options
  • max_laser_range (float)
    • Default: 7.9 m
    • Maximum laser range.
  • laser_noise (tuple (doubles): [m rad])
    • Default: [0.004 0.045]
    • Noise in laser range/bearing readings; default is for a SICK LMS200
  • odom_noise (tuple (doubles): [m m rad])
    • Default: [0.4 0.2 0.2]
    • Noise in motion model
    • This is, in terms of covariance, 2*sigma
  • robot_pose (tuple (doubles): [m m rad])
    • Default: [0.0 0.0 0.0]
    • Initial pose estimation
  • robot_pose_initial_error (tuple (doubles): [m m rad])
    • Default: [1.0 1.0 0.2]
    • Initial error the initial robot pose. It EX = 1.0, robot is at X +/- (E/2)
  • mapfile (string)
    • Default: none
    • Parameter that allows the loading of a mapfile instead of using a map interface. Mapfile formap is #segments [x0 y0 x1 y1]...
    • When not using this but a requires, use alwayson 1 in the vmapfile driver
    • if you get errors initializing this driver
  • truth_model (string)
    • Default : none
    • Named model in stage for ground truth. Used for debugging.
    • When using this, also use a "simulation" requires
  • FINE TUNING OPTIONS (units (default))
    • Note that player unit conversions are applied, so these units are only informative for the defaults
  • max_region_empty_angle (rad (0.035 ~ 2deg))
    • Angular distance to split regions
  • max_region_empty_distance (m (0.1))
    • Euclidean distance to split regions
  • min_region_length (m (0.2))
    • Minimum distance between region endpoints
  • min_points_in_region (int (4))
    • Minimum laser returns in a region
  • split_confidence (percent (95.0))
    • chi-2 confidence test for segment splitting
  • check_residual (boolean_int (0))
    • perform residual testing for segment splitting
  • max_ang_ebe (rad (0.0))
    • ???
  • min_split_segment_distance (m (0.0))
    • minimum dist between new segment endpoints for segment splitting
  • min_odom_distance_delta (m (0.0))
  • min_odom_angle_delta (rad (0.0))
    • minimum change in odometry before performing update
  • backoff_period (s (0.05))
    • minimum time between updates; if lasers arrive faster they'll be dropped
  • DEBUG OPTIONS
    • send_debug (int (0))
    • port for custom socket connection to an external debugging GUI
    • If != 0 it will be used.
Example
# Using degrees
driver
(
  name "ekfvmap"
  provides ["position2d:1" "localize:0" "covariance:::0"]
  requires ["position2d:0" "laser:0" "map:0"]

  max_laser_range 31.9
  laser_noise [0.004 0.5]
  odom_noise  [0.4 0.2 10]
  robot_pose  [-49 -39 0]
  robot_pose_initial_error [1.0 1.0 20]
  # mapfile "upc.vec"
  # Use either "requires" or "mapfile"

  # truth_model "robot"
)
Author
J.A. Castellanos, J.D. Tardós (underlying algorithm), Mayte Lázaro (code), A. Mosteo (player integration)