Graph_SLAM

Graph SLAM

This wiki explains how to use the graph slam package located at: https://github.com/nickcharron/ig_graph_slam.git

ig_graph_slam was built specifically for inspector gadget data, however, it has been designed to be a generic graph slam solution. It has been tested on IG data and Autonomoose data. You should be able to use any platform with this code by only changing parameters in the config file.

Dependencies

The following software is currently needed to run ig_graph_slam:

• kindr
• gtsam
• geographiclib
• libwave

Install scripts for the above dependencies can be found in: https://github.com/BEAMRobotics/beam_install_scripts

Inputs

• rosbag path: string

  the rosbag must have the following topics:

  • localization scan topic: sensor_msgs/PointCloud2
  • mapping scan topic: sensor_msgs/PointCloud2

  And, at least one of the following:

  • odometry topic: nav_msgs/Odometry
  • gps/imu topic: INSPVAX or sensor_msgs/NavSatFix + sensor_msgs/Imu

• T_LIDAR_GPS: 4x4 array (only if using GPS data)

• T_LMAP_LLOC: 4x4 array ( static transform from localization scan frame to mapping scan frame)

If you are only performing the mapping, using previous localization results, you will also need to input:

• poses_file path: string

Outputs

• params.txt: file containing all the params used to run the program
• init_traj.txt: file containing the initial pose estimates collected from the bag
• gtsam_graph.dot: graph file showing the gtsam graph used to solve the localization
• opt_traj.txt: file containing the final optimized poses
• map.pcd: final output point cloud using the mapping scan topic (see inputs)
• loc_map.pcd: the localization map generated using the localization scan topic (see inputs)

Running ig_graph_slam

The executable will build in the build directory of your workspace. Example run command:

./catkin_ws/build/ig_graph_slam/ig_graph_slam_node

There are two important things to consider before running this program: (1) bag pre-processing, and (2) config file editing.

1. bag pre-processing:

Each bag needs to be pre-processed to ensure we can initialze the pose graph well enough to run the optimizer. If we don't have a 'decent' guess of the robot trajectory, the mapping will not work. The poses can either be initialized using odometry messages or gps/imu messages. The later is only recommended if you have a high quality GPS/IMU system such as the Novatel Span. Using regular GPS + IMU will usually not give accurate enough results to properly initialize the graph. The other alternative is to produce odometry messages using any other slam software. The two options available inside beam_robotics are:

a. EKF SLAM: https://github.com/BEAMRobotics/ig_online_mapping (wiki: https://github.com/BEAMRobotics/beam_robotics/wiki/EKF_SLAM)

b. LOAM: https://github.com/BEAMRobotics/loam_velodyne (wiki: https://github.com/BEAMRobotics/beam_robotics/wiki/LOAM)

Note: There is a shell script (collect_loam_gs_bag.sh) that you can run to pre-process a bag with LOAM automatically.

2. config file:

The config file is located at ./config/ig_graph_slam_config.yaml. You will need to go through and update these parameters every time you want to run ig_graph_slam. There is already detailed information in this file that explains what all the params do. Here I will go through the common things you should be changing each time you run it.

• bag_file_path: you should always set this before running the code. Bags are not saved in this repo so you will need to save it on your machine and set the appropriate path.
• init_method: this controls how you initialize your pose map. This part is crucial. Make sure you have accurate odometry or GPS/IMU data and set their topics accordingly.
• use_prev_poses: if you want to only run the map building, not the whole localization, set this param to true and make sure you give a path to the opt_traj.txt file that you have already generated with ig_graph_slam. This is useful if you want to play around with the mapping parameters and not the localization. Or if you want to completely remove the filtering of scans for the mapping, you will have to run localization and mapping separately since it normally filters the incoming scans before saving them in their containers.
• trajectory_sampling_distance: this sets how often we extract poses in our graph. Extracting poses too close together will cause drift error. Extracting poses too far away will not give sufficient number of measurement constrains. Usually a good range is from 0.5m to 3m.
• trajectory_rotation_change: this sets how often poses are extracted based on rotation. Meaning, if it rotated more than a specific angle, it will extract a pose (regardless of how much is translated). This seems to worsen the results since ICP alone isn't good at estimating orientation.
• output_path: set where you want to output the data

scripts

• collect_load_gs_bag.sh: this script asks for a bag file path + name, then asks for a playback rate and start time for the bag. Once those are inputted, it runs LOAM over the entire bag and saves a new bag with the topics needed to run graph slam.

• install_deps.sh: deprecated. DO NOT USE

• joy_mapping_controller.py: run this to control the mapping automatically with a joystick. You can start and stop a scan then automatically run graph slam to produce a map.

• start_graph_slam.sh: called by joy_mapping_controller

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