scan_tools: csm | laser_ortho_projector | laser_scan_matcher | laser_scan_sparsifier | laser_scan_splitter | ncd_parser | polar_scan_matcher | scan_to_cloud_converter

Package LinksDependencies (10)
Used by (2)

Package Summary

An incremental laser scan matcher, using Andrea Censi's Canonical Scan Matcher implementation. More about CSM: http://www.cds.caltech.edu/~andrea/research/sw/csm.html

Contents

  1. Package Summary
  2. Details
  3. Input
    1. Supported message types
    2. Scan prediction
  4. Example
  5. Nodes & Nodelets
    1. laser_scan_matcher_node / laser_scan_matcher_nodelet
      1. Subscribed Topics
      2. Published Topics
      3. Parameters
      4. Required tf Transforms
      5. Provided tf Transforms
  6. References
  7. Bug Reports & Feature Requests

laser_scan_matcher.png

Details

The laser_scan_matcher package is an incremental laser scan registration tool. The package allows to scan match between consecutive sensor_msgs/LaserScan messages, and publish the estimated position of the laser as a geometry_msgs/Pose2D or a tf transform.

The package can be used without any odometry estimation provided by other sensors. Thus, it can serve as a stand-alone odometry estimator. Alternatively, you can provide several types of odometry input to improve the registration speed and accuracy.

Input

Supported message types

The laser_scan_matcher can operate using sensor_msgs/LaserScan messages or sensor_msgs/PointCloud2 messages. When using sensor_msgs/PointCloud2, make sure they have no nan values.

Scan prediction

While the laser_scan_matcher can operate by just using scan data, we can speed up the scan registration process by providing a guess for the current position of the sensor every time a new scan message arrives. When no guess is available, a reasonable (and widely-used) assumption is that the sensor didn't move (zero-velocity model). Below is a list of inputs that laser_scan_matcher accepts:

  1. IMU :An estimation for the change of the orientation angle (delta-theta) of the robot in the form of a sensor_msgs/IMU message. We are assume that the yaw component of the IMU message corresponds to the orientation of the robot. Thus, we don't really need a full 6DoF IMU sensor - a cheap 1-axis gyro will work as well, as well as its output is packed as an IMU message. The required topic is /imu

  2. Wheel odometry: An estimation for the change of x-, y-, and orientation angle of the robot from an odometric sensor such as wheel encoders. The required topic is /odom

  3. Velocity tracking: An estimation for the change of x-, y-, and orientation angle of the robot using alpha-beta [2] tracking. The velocity of the robot is continuously estimated by differentiating the pose and applying an alpha-beta filter. A constant velocity model is assumed for the prediction. No additional topics required.

  4. Zero-velocity model: Don't use any prediction, ie, assume that the sensor stayed in the same place.

We can use combinations of the above such as IMU together with wheel odometry or IMU together with alpha beta tracking. When several prediction modes are enabled, the priority is IMU > Odometry > Alpha-Beta Velocity > Zero Velocity.

What should I use in practice?

IMU and (to some extent) wheel odometry inputs significantly improve convergence speed for rotational motion. The addition of an IMU input is thus highly recommended.

Alpha-beta tracking can lead to a significant speed up when the performance of the scan matcher is stable, but might result in weird behavior for highly dynamic environments or environments with poor features. We recommend enabling it and determining empirically if it is useful for your environment.

Example

You can run the laser_scan_matcher on a pre-recorded bag file that comes with the package. First, make sure you have the scan_tools stack downloaded and installed by following the instructions here.

Next, make sure you have the necessary tools installed: 

   1 rosmake rviz rosbag tf

Finally, run the demo: 

   1 roslaunch laser_scan_matcher demo.launch

You should see a result similar to the video below. The video shows tracking the position of a Hokuyo laser as it is being carried freely around a room. The pose is determined entirely by the scan matcher - no additional odometry is provided.

Nodes & Nodelets

Two drivers are available: laser_scan_matcher_nodelet and laser_scan_matcher_node. Their parameters and topics are identical.

laser_scan_matcher_node / laser_scan_matcher_nodelet

The laser_scan_matcher_node takes in sensor_msgs/LaserScan messages, performs scan registration, and outputs an estimate for the displacement of the robot.

Subscribed Topics

scan (sensor_msgs/LaserScan)
  • Scans from a laser range-finder
cloud (sensor_msgs/PointCloud2)
  • Scans in point cloud form
imu (sensor_msgs/Imu)
  • Imu messages, used for theta prediction. Only used if use_imu is set to true.
odom (nav_msgs/Odometry)
  • Odometry messages, used for x-, y-, and theta prediction. Only used if use_odom is set to true.

Published Topics

pose2D (geometry_msgs/Pose2D)
  • The pose of the base frame, in some fixed (world) frame.

Parameters

~fixed_frame (string, default: "world")
  • the fixed frame
~base_frame (string, default: "base_link")
  • the base frame of the robot
~use_imu (bool, default: true)
  • Whether to use an imu for the theta prediction of the scan registration.
~use_odom (bool, default: true)
  • Whether to use wheel odometry for the x-, y-, and theta prediction of the scan registration.
~use_alpha_beta (bool, default: false)
  • Whether to use a constant-velocity model via alpha-beta tracking for the x-, y-, and theta prediction of the scan registration.
~alpha (double, default: 1.0)
  • The alpha parameter for the alpha-beta filter
~beta (double, default: 0.8)
  • The beta parameter for the alpha-beta filter
~use_cloud_input (bool, default: false) ~cloud_range_min (double, default: 0.1) ~cloud_range_max (double, default: 50.0) ~publish_tf (bool, default: true)
  • whether to publish scan matcher's estimation for the position of the base frame in the world frame as a transform.
~publish_pose (bool, default: true)
  • whether to publish scan matcher's estimation for the position of the base frame in the world frame as a geometry_msgs/Pose2D
~max_angular_correction_deg (double, default: 45.0)
  • Maximum angular displacement between scans, in degrees
~max_linear_correction (double, default: 0.50)
  • Maximum translation between scans (m)
~max_iterations (int, default: 10)
  • Maximum ICP cycle iterations
~epsilon_xy (double, default: 0.000001)
  • A threshold for stopping (m)
~epsilon_theta (double, default: 0.000001)
  • A threshold for stopping (rad)
~max_correspondence_dist (double, default: 0.3)
  • Maximum distance for a correspondence to be valid
~sigma (double, default: 0.010)
  • Noise in the scan (m) (Not sure if changing this has any effect in the current implementation)
~use_corr_tricks (int, default: 1)
  • If 1, use smart tricks for finding correspondences (see paper).
~restart (int, default: 0)
  • Restart: If 1, restart if error is over threshold
~restart_threshold_mean_error (double, default: 0.01)
  • Restart: Threshold for restarting
~restart_dt (double, default: 1.0)
  • Restart: displacement for restarting. (m)
~restart_dtheta (double, default: 0.1)
  • Restart: displacement for restarting. (rad)
~clustering_threshold (double, default: 0.25)
  • Max distance for staying in the same clustering
~orientation_neighbourhood (int, default: 10)
  • Number of neighbour rays used to estimate the orientation
~use_point_to_line_distance (int, default: 1)
  • If 0, it's vanilla ICP
~do_alpha_test (int, default: 0)
  • If 1, discard correspondences based on the angles
~do_alpha_test_thresholdDeg (double, default: 20.0)
  • Discard correspondences based on the angles - threshold angle, in degrees
~outliers_maxPerc (double, default: 0.90)
  • Percentage of correspondences to consider: if 0.90, always discard the top 10% of correspondences with more error
~outliers_adaptive_order (double, default: 0.7)
  • Parameters describing a simple adaptive algorithm for discarding. 1) Order the errors. 2) Choose the percentile according to outliers_adaptive_order (if it is 0.7, get the 70% percentile). 3) Define an adaptive threshold multiplying outliers_adaptive_mult with the value of the error at the chosen percentile. 4) Discard correspondences over the threshold. This is useful to be conservative; yet remove the biggest errors.
~outliers_adaptive_mul (double, default: 2.0)
  • Parameters describing a simple adaptive algorithm for discarding. 1) Order the errors. 2) Choose the percentile according to outliers_adaptive_order (if it is 0.7, get the 70% percentile). 3) Define an adaptive threshold multiplying outliers_adaptive_mult with the value of the error at the chosen percentile. 4) Discard correspondences over the threshold. This is useful to be conservative; yet remove the biggest errors.
~do_visibility_test (int, default: 0)
  • If you already have a guess of the solution, you can compute the polar angle of the points of one scan in the new position. If the polar angle is not a monotone function of the readings index, it means that the surface is not visible in the next position. If it is not visible, then we don't use it for matching.
~outliers_remove_doubles (int, default: 1)
  • If 1, no two points in laser_sens can have the same correspondence
~do_compute_covariance (int, default: 0) ~debug_verify_trick (int, default: 0)
  • If 1, checks that find_correspondences_tricks gives the right answer
~use_ml_weights (int, default: 0)
  • If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence. (Changing this has no effect in the current implementation)
~use_sigma_weights (int, default: 0)
  • If 1, the field 'readings_sigma' in the second scan is used to weight the correspondence by 1/sigma^2 (Not sure if changing this has any effect in the current implementation)

Required tf Transforms

base_link → laser
  • the pose of the laser in the base frame.

Provided tf Transforms

world → base_link
  • the pose of the robot base in the world frame. Only provided when publish_tf is enabled.

References

[1] A. Censi, "An ICP variant using a point-to-line metric" Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2008

[2] Alpha beta filter. http://en.wikipedia.org/wiki/Alpha_beta_filter

Bug Reports & Feature Requests

We appreciate the time and effort spent submitting bug reports.

Please use our Trac to report bugs or request features.

Wiki: laser_scan_matcher (last edited 2011-06-28 14:30:53 by IvanDryanovski)

Except where otherwise noted, the ROS wiki is licensed under Creative Commons Attribution 3.0.