EKF_SLAM

This package contains all the mapping software needed for EKF SLAM.

Running the mapping package:

• there are two launch files that start everything needed to build a map. You should only have to launch one launch file to begin:

  roslaunch ig_mapping build_map_P2P.launch

  or

  roslaunch ig_mapping ig_build_map_Pt2Pl.launch

• After launching the launch file, play a ros bag making sure to include the clock parameter. It is also useful to slow down the bag playback to make sure your computer has plenty of time to process the data:

  rosbag play -r 0.3 /path/to/bag/file.bag --clock

  As a rule of thumb, you should be processing no more than one scan per second. Therefore if you are subscribing to scans at 3Hz, you should playback the bag at a rate of 0.3Hz, as shown above.

• All the parameters you need to change are most likely in the yaml files in the config folder. There are also some parameters in the include launch files that you can play around with as needed.

Main nodes:

(1) ig_broadcast_gps_map_transform (2) ig_debug (3) ig_combine_scans (4) ig_build_map (DEPRECIATED - replaced with 5) (5) ig_build_map_ekf (DEPRECIATED - replaced with 6) (6) ig_build_two_stage (7) ig_build_map_Pt2Pl (8) ig_extract_pose

Filter nodes:

(1) ig_cropbox (2) ig_dror (3) ig_refine_map (4) ig_voxel_grid

Node details:

(1) ig_broadcast_gps_map_transform:

  This was a quick node that publishes a gps to map transform with only
  GPS data. This was done before the EKF and scan registration was
  implemented. This is probably not useful anymore, but kept for debugging
  purposes. This was never fully validated so there might be errors.

(2) ig_debug:

  Just a simple node to output cloud details. Alternatively, you can echo
  cloud topics and specify which parts of the message you want to echo.

  e.g.: rostopic echo /cloud_msg/header/frame_id

(3) ig_combine_scans:

  This package takes the tf_static data published by the
  ig_calibration_publisher, subscribes to the velodyne topics and outputs a
  combined scan in the horizontal lidar frame. It also applies a cropbox
  filter from PCL to remove points on the robot. This can all be done in
  post-processing of the bags.

  Suscribed Topics:
    /vvlp/velodyne_points
    /hvlp/velodyne_points
    /tf_static

  Published Topics:
    /combined/velodyne_points
    /cropped/velodyne_points

  Launch Files:
    combine_scans.launch

  Notes on Running:
    - this launch file does not launch the calibration publisher, this needs to be
      done ahead of time!

(4) ig_build_map (DEPRECIATED - replaced with 5):

  This was the first iteration of the map building that only takes the ekf
  transforms, feeds that to icp and then builds a map based on the final
  icp output. It was replaced with (5)

(5) ig_build_map_ekf (DEPRECIATED - replaced with 6):

  This replaced (4) by incorporating the icp transforms into an odometry
  message which can then be fed into the ekf. This significantly improved
  mapping by allowing us to build large maps because we no longer have major
  initiation problems. This was replaced by (6).

(6) ig_build_two_stage:

  Same process as (5), but using a two stage mapping process. It builds a
  "registration map" which uses the entire scans, and this map is only used
  for refistration. Simultaneously, a second map is being built which is
  only used as the final map. It is not used for registration, so we crop
  this map to used only the close scan measurements which eliminated the
  issue of having bad scan points at greater distances due to minor error
  in orientation.

  Suscribed Topics:
    /ig/combineScans/velodyne_points (DEFAULT)
    /tf
    /tf_static

  Published Topics:
    /ig/buildMap/finalMap (DEFAULT)
    /ig/buildMap/regMap (DEFAULT)
    /ig/icpOdom (DEFAULT)

(7) ig_build_map_Pt2Pl:

  Same as (6) but uses ICP point to plane instead of the default Point to
  Point. This tends to work better with scans taken closer together. Close
  scans incrementally registered with traditional ICP tend to have ring
  snapping and drift. Point to Plant seems to fix these problems.

(8) ig_extract_pose:

  This node subscribes to the camera data and tf, and every time an image is
  received, this nodes outputs the timestamp and transform to a text file to
  be used in the point cloud colouring/post-processing.

Filter details:

(1) ig_cropbox:

  This is a PCL implementation of a cropbox filter that removes all points
  inside or outside the specified box. The point of this is to preprocess
  the scans to remove unwanted parts of the scan. For example running
  remove_ground.launch will use the cropbox filter to remove the ground
  points on each scan.

(2) ig_dror:

  This is a custom variation to PCL's Radius Outlier Filter based on our
  CRV paper "De-noising of Lidar Point Clouds Corrupted by Snowfall". This
  is used to pre-process the scans to remove noisy points or tree/vegetation
  points if the threshold is increased enough. This cannot be used on full
  maps, just the scans. THE CUSTOM PCL THAT CONTAINS THIS CODE IS NOT IMPLEMENTED IN BEAM YET.

(3) ig_voxel_grid:

  Standard PCL voxel grid filter implementation.

(4) ig_refine_map:

  This is used to filter the final maps to reduce the point size. It is a
  special implementation of the voxel grid filter. In this node, the voxel
  grid filter is used three different times. Each of the 3 times, one axis
  uses a large voxel size to filter out surfaces in that plane and this is
  repeated for each of the 3 axes/planes. Then the 3 resulting point clouds
  are added together. This refines the surfaces while reducing the overall
  point cloud size significantly.

Last updated: March 21, 2019, by: Nick Charron