GAC-Mapping

Ground and Aerial Collaborative Mapping in Urban Environments

Version 1.1.0

GAC-Mapping (GACM for short) is a multi-session SLAM to solve the mapping and localization of ground-aerial heterogeneous robot clusters in urban scenes. In GACM, a Visual-LiDAR ego-motion estimation module that considers point, line and planar constraints can provide robust odometry information. Thumbnail images representing obstacle outlines are generated and descriptors are extracted using a neural network to help perform data association between separate runs. Map segments and the robot poses are organized together and are updated during a pose graph optimization procedure. We provide guidance and examples how to run GACM on sample data and GrAco dataset (website or github).

Install

Docker Setup

Make sure to install:

• Docker

Then, create the ros workspace with the src folder, and then navigate the terminal to that src folder. Creating the workspace outside the docker helps you keep your files and changes within the workspace even if you delete the un-committed docker container. Then, clone this repository into the src folder.

After that, navigate to the docker directory. Log in to the user that you want the docker file to create in the container. Then, edit the enter_container.sh script with the following paths:

• DATA_DIR=: The directory where the HERCULES dataset is located
• WS_DIR=: The directory of the ROS workspace

Now, run the following commands:

build_container.sh
run_container.sh

The rest of this README assumes that you are inside the Docker container. For easier debugging and use, its highly recommended to install the VSCode Docker extension, which allows you to start/stop the container and additionally attach VSCode to the container by right-clicking on the container and selecting Attach Visual Studio Code. If that isn't possible, you can re-enter the container running the following command:

enter_container.sh

Build

Run the following command in the root of the ROS workspace to build the package:

source /opt/ros/melodic/setup.bash
catkin_make

Run GACM on dataset

We provide some sample data (including two ground sequences and one aerial sequence). In addition, we also provide configuration files for the GrAco Dataset. You can also download the it for testing.

Operation process

• Open a terminal, run:

source /opt/ros/melodic/setup.bash
roscore

• Open a new terminal, run Patch-NetVLAD to extract feature.

  conda activate patchnetvlad
  source ~/gac_mapping_ws/devel/setup.bash
  cd ~/gac_mapping_ws/src/GAC-Mapping/Patch-NetVLAD
  python feature_extract_ros.py
  # use only cpu
  python feature_extract_ros.py --nocuda

  GACM uses the pretrained model provided by Patch-NetVLAD. The script will automatically download the corresponding model file, or you can download it manually. Please refer to the README file of Patch-NetVLAD for the download address.

  After testing, when using a 512-dimensional descriptor, the average thumbnail processing time is 0.007 seconds under GPU (NVIDIA TITAN X), and 0.085 seconds under CPU (i7-10710U).

• Open a new terminal. Make sure to edit graco.launch to use the right configs (based on the order you play your rosbags in).

  source ~/gac_mapping_ws/devel/setup.bash
  roslaunch gacm graco.launch

  After the algorithm runs, you can choose whether to load the data file from gacm_output\data\testSavemap or not (see Output file for details). If you choose not to load, then you can play rosbag directly.

  If you choose to load, you need to wait for all data to be loaded. After loading, you can choose whether to perform pose graph optimization. The optimization looks for new inter-robot loops based on the global coordinates of each submap. If you think the mapping effect is good enough, or there are enough inter-robot loops, you don’t need to optimize it. Then you can play rosbag to run the algorithm.

  source ~/gac_mapping_ws/devel/setup.bash
  cd ~/data/GrAco_dataset/V1.0/data/ground-06
  rosbag play ground-06.bag

  source ~/gac_mapping_ws/devel/setup.bash
  cd ~/data/GrAco_dataset/V1.0/data/aerial-08
  rosbag play aerial-08-25m.bag

  After rosbag finishes, wait a moment (about 10 seconds), the algorithm will save data files and ask you whether to perform pose graph optimization. However, if it's the end of the first sequence (robot id is 0), the pose graph optimization will not be performed by default. After optimization, the algorithm will save the odometry file. When all files are saved, you can run the next rosbag or exit directly.

  

  Finally, we recommend playing ground rosbags first, then air ones.

Sample Data

1. download data: car1.bag car2.bag uav.bag

2. Run the feature_extract_ros.py script and gacm.launch file according to the operation process section.

3. Play car1.bag, car2.bag and uav.bag file in sequence and and follow the operation process section.

4. After all rosbags are played, you can see the map shown in the picture below on rviz. The trajectories and point cloud maps of different robots have different colors.

   

5. Also, you can see that there are yellow lines connecting some parts of the trajectory, which means that loop closures are not only detected but relative poses are obtained.

   

GrAco Dataset

Dataset website: https://sites.google.com/view/graco-dataset

(or download data from github: https://github.com/SYSU-RoboticsLab/GrAco)

Notice: If you plan to run ground-05 (duration over 400 seconds), you need to set the submap_length parameter in the configuration file to 70 or more.

• Aerial-Aerial

  play order: Aerial-07 -> Aerial-05

  Change graco.launch configration file order

  <arg name="config_path" default = "$(find gacm)/config/graco_uav.yaml" />
  <arg name="config_path1" default = "$(find gacm)/config/graco_uav.yaml" />

  run feature_extract_ros.py and graco.launch.

  

• Ground-Aerial

  play order: Ground-06 -> Aerial-07

  Change graco.launch configration file order

  <arg name="config_path" default = "$(find gacm)/config/graco_ugv.yaml" />
  <arg name="config_path1" default = "$(find gacm)/config/graco_uav.yaml" />

  run feature_extract_ros.py and graco.launch.

  

4. Run GACM on your own device

Prepare device

GACM uses Visual-LiDAR odometry as a front end, which means your device should contain LiDAR and camera. In addition, the front-end LiDAR part of GACM will generate a depth image based on the LiDAR point cloud to assist the visual part to restore the depth of the feature. So GACM also has certain requirements for the synchronization and placement of sensors.

• Do your best to keep in sync: Both the LiDAR and the camera in our device use hardware triggers to ensure time stamp synchronization. If your device synchronization is not good, you can reduce the weight of the visual part by reducing the value of visual_weight_max (in the configuration file).
• Face the same direction: Because the point cloud is used to help image features recover depth, the overlapping of the field of view between the camera and LiDAR should be as much as possible. Our LiDAR uses Velodyne VLP16 (it has a vertical field of view of only 30 degrees). So if you have a mechanical LiDAR with a larger field of view, the effect of depth recovery is definitely better. However, the current algorithm does not support solid LiDAR (Livox).
• Rotation is important: For the extrinsic parameter of LiDAR and camera, we found that the error on the rotation has a greater impact on the accuracy of the odometry than the translation. For calibration, you can try adding an imu. Then complete the calibration of LiDAR-Imu and Camera-Imu respectively, get their respective extrinsic parameters. Finally, the extrinsic parameter of LiDAR-Camera are indirectly obtained by calculation LiDAR-Imu and Camera-Imu.

Prepare file

1. Create your own device configuration file

   You can modify the configuration file we provide to adapt to your device. The configuration file is under theGAC-Mapping/gacm/config/folder.

   If your camera z-axis is above the horizontal plane, you can refer to the car.yaml or graco_ugv.yaml file.

   If your camera z-axis is below the horizontal plane (such as the way the camera is mounted on the drone in the GrAco dataset), you can refer to the uav.yaml or graco_uav.yaml.

   

2. Create your own roslaunch file

   GACM is multi-session SLAM, which supports running up to 5 different sequences. As shown below, you should modify the config file path for each sequence. When playing rosbag, be sure to play in the order of the configuration file.

   <arg name="config_path" default = "$(find gacm)/config/car.yaml" />
   <arg name="config_path1" default = "$(find gacm)/config/car.yaml" />
   <arg name="config_path2" default = "$(find gacm)/config/uav.yaml" />
   <arg name="config_path3" default = "$(find gacm)/config/uav.yaml" />
   <arg name="config_path4" default = "$(find gacm)/config/uav.yaml" />

   Also, you can change the pointcloud and image topic name by remap.

   <node pkg="gacm" type="featureTrackingNode" name="featureTrackingNode"    		output="screen">
   	...
       <!-- change your topic name -->
       <remap from="/image_raw" to="/camera_left/image_raw" />		
       <remap from="/velodyne_points" to="/velodyne/points" />
   </node>

Output file

Data file

GACM will save all the data stored in the current algorithm at the end of each sequence of data (rosbag play ends). This allows GACM to start from the end of the previous sequence without re-running all sequences. These data files will be stored in the gacm_output/data/testSavemap folder in the user's home directory (such as /home/sysu-rapid).

The data files includes loop closure information for each sequence, pose graph node and edge information, pcd files of corner points, planar points and thumbnail point clouds of submap, and point cloud maps before and after fusion.

Odometry file

When a sequence ends, GACM automatically saves the odometry file for each sequence in tum format (timestamp[s] x y z q_x q_y q_z q_w). But you should modify the save path of the file in the configuration file! Otherwise the file will be saved in the gacm_output/data/testSavemap folder.

Thumbnail file

You can view the thumbnails generated by the algorithm in real time under the gacm_ouput/pictures/submap_img folder.

Known issues

1. In order to reduce the memory usage when the code is running, each sequence can only generate up to 100 submap. The number of lidar frames contained in each submap can be set (submap_length parameter). Assume that the period of each frame of lidar point cloud is 0.1 second, parameter submap_length is 50. Each sequence can only run for a maximum of 500 seconds (0.1 x 50 x 100 = 500). However, after testing, the running time of a sequence should not exceed 400 seconds, otherwise it may affect the real-time performance of the algorithm.
2. The inter-robot loop detection of the algorithm is unstable. After testing, for the same submap, the descriptors generated by Patch-NetVLAD may be different, which will affect loop closure detection. In addition, the point cloud registration results of NDT+GICP will also be greatly affected by the size of submap point cloud ( submap_length, surf_threshold), matching parameters (ndt_resolution_match), and the initial values. Therefore, even if the correct loop closure is detected, the correct relative pose may not be obtained.

5. Acknowledgements

We use ceres solver for non-linear optimization and a generic camera model, Patch-NetVLAD for loop dectect and skipmap_ros to manage submap. In addition, this algorithm refers to the A-LOAM, LeGO-LOAM and VINS-mono. We really appreciate these open source projects!

6. License

The source code is released under GPLv3 license.