First commit

CMakeLists.txt

@@ -0,0 +1,91 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lio_sam)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -std=c++11 -O3 -pthread")
+
+find_package(catkin REQUIRED COMPONENTS
+  tf
+  roscpp
+  rospy
+  cv_bridge
+  # pcl library
+  pcl_conversions
+  # msgs
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  nav_msgs
+  message_generation
+)
+
+find_package(OpenMP)
+find_package(PCL REQUIRED QUIET)
+find_package(OpenCV REQUIRED QUIET)
+find_package(GTSAM REQUIRED QUIET)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  cloud_info.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+  std_msgs
+  nav_msgs
+  sensor_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  DEPENDS PCL GTSAM
+
+  CATKIN_DEPENDS 
+  std_msgs
+  nav_msgs
+  geometry_msgs
+  sensor_msgs
+  message_runtime 
+  message_generation
+)
+
+# include directories
+include_directories(
+	include
+	${catkin_INCLUDE_DIRS}
+	${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+	${GTSAM_INCLUDE_DIR}
+)
+
+# link directories
+link_directories(
+	include
+	${PCL_LIBRARY_DIRS}
+  ${OpenCV_LIBRARY_DIRS}
+  ${GTSAM_LIBRARY_DIRS}
+)
+
+###########
+## Build ##
+###########
+
+# Range Image Projection
+add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp)
+add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+
+# Feature Association
+add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp)
+add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+
+# Mapping Optimization
+add_executable(${PROJECT_NAME}_mapOptmization src/mapOptmization.cpp)
+add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(${PROJECT_NAME}_mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
+
+# IMU Preintegration
+add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp)
+target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)

LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2018, Robust Field Autonomy Lab
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,136 @@
+# LIO-SAM
+
+**A real-time lidar-inertial odometry package. We strongly recommend the users read this document thoroughly and test the package with the provided dataset first.**
+
+<p align='center'>
+    <img src="./config/doc/demo.gif" alt="drawing" width="800"/>
+</p>
+
+## Menu
+
+[**System architecture**](#system-architecture)
+
+[**Dependency**](#dependency)
+
+[**Install**](#install)
+
+[**Prepare lidar data**](#prepare-lidar-data) (must read)
+
+[**Prepare IMU data**](#prepare-imu-data) (must read)
+
+[**Sample data**](#sample-data)
+
+[**Run the package**](#run-the-package)
+
+[**Paper**](#paper)
+
+[**Acknowledgement**](#acknowledgement)
+
+## System architecture
+
+<p align='center'>
+    <img src="./config/doc/system.png" alt="drawing" width="800"/>
+</p>
+
+We design a system that maintains two graphs and runs up to 10x faster than real-time. 
+  - The factor graph in "mapOptimization.cpp" optimizes lidar odometry factor and GPS factor. This factor graph is maintained consistently throughout the whole test. 
+  - The factor graph in "imuPreintegration.cpp" optimizes IMU and lidar odometry factor and estimates IMU bias. This factor graph is reset periodically and guarantees real-time odometry estimation at IMU frequency.
+
+## Dependency
+
+- [ROS](http://wiki.ros.org/ROS/Installation) (tested with Kinetic and Melodic)
+  ```
+  sudo apt-get install -y ros-kinetic-navigation
+  sudo apt-get install -y ros-kinetic-robot-localization
+  sudo apt-get install -y ros-kinetic-robot-state-publisher
+  ```
+- [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library)
+  ```
+  wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip
+  cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/
+  cd ~/Downloads/gtsam-4.0.2/
+  mkdir build && cd build
+  cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..
+  sudo make install -j8
+  ```
+
+## Install
+
+Use the following commands to download and compile the package.
+
+```
+cd ~/catkin_ws/src
+git clone https://github.com/TixiaoShan/LIO-SAM.git
+cd ..
+catkin_make
+```
+
+## Prepare lidar data
+
+The user needs to prepare the point cloud data in the correct format for cloud deskewing, which is mainly done in "imageProjection.cpp". The two requirements are:
+  - **Provide point time stamp**. LIO-SAM uses IMU data to perform point cloud deskew. Thus, the relative point time in a scan needs to be known. The up-to-data Velodyne ROS driver should output this information directly. Here, we assume the point time channel is called "time." The definition of the point type is located at the top of the "imageProjection.cpp." "deskewPoint()" function utilizes this relative time to obtain the transformation of this point relative to the beginning of the scan. When the lidar rotates at 10Hz, the timestamp of a point should vary between 0 and 0.1 seconds. If you are using other lidar sensors, you may need to change the name of this time channel and make sure that it is the relative time in a scan.
+  - **Provide point ring number**. LIO-SAM uses this information to organize the point correctly in a matrix. The ring number indicates which channel of the sensor that this point belongs to. The definition of the point type is located at the top of "imageProjection.cpp." The up-to-data Velodyne ROS driver should output this information directly. Again, if you are using other lidar sensors, you may need to rename this information.
+
+## Prepare IMU data
+
+  - **IMU requirement**. Like the original LOAM implementation, LIO-SAM only works with a 9-axis IMU, which gives roll, pitch, and yaw estimation. The roll and pitch estimation is mainly used to initialize the system at the correct attitude. The yaw estimation initializes the system at the right heading when using GPS data. Theoretically, an initialization procedure like VINS-Mono will enable LIO-SAM to work with a 6-axis IMU. The performance of the system largely depends on the quality of the IMU measurements. The higher the IMU data rate, the better the system accuracy. We use Microstrain 3DM-GX5-25, which outputs data at 500Hz. We recommend using an IMU that gives at least a 200Hz output rate. Note that the internal IMU of Ouster lidar is not usable due to high vibration.
+
+  - **IMU alignment**. LIO-SAM transforms IMU raw data from the IMU frame to the Lidar frame, which follows the ROS REP-105 convention (x - forward, y - left, z - upward). To make the system function properly, the correct extrinsic transformation needs to be provided in "params.yaml" file. Using our setup as an example, we need to set the readings of x-z acceleration and gyro negative to transform the IMU data in the lidar frame, which is indicated by "extrinsicRot" in "params.yaml." The transformation of attitude readings is similar. We rotate the attitude measurements by -90 degrees around "lidar-z" axis and get the corresponding readings in the lidar frame. This transformation is indicated by "extrinsicRPY" in "params.yaml."
+
+  - **IMU debug**. It's strongly recommended that the user uncomment the debug lines in "imuHandler()" of "imageProjection.cpp" and test the output of the transformed IMU data. The user can rotate the sensor suite to check whether the readings correspond to the sensor's movement.
+
+<p align='center'>
+    <img src="./config/doc/imu-transform.png" alt="drawing" width="800"/>
+</p>
+
+## Sample data
+
+  * Download some sample datasets to test the functionality of the package. The datasets below is configured to run using the default settings:
+    - [**Walk dataset**](https://drive.google.com/file/d/1HN5fYPXEHbDq0E5JtbQPkCHIHUoTFFnN/view?usp=sharing)
+    - [**Garden dataset**](https://drive.google.com/file/d/1q6yuVhyJbkUBhut9yhfox2WdV4VZ9BZX/view?usp=sharing)
+
+  * The datasets below need the parameters to be configured. In these datasets, the point cloud topic is "points_raw." The IMU topic is "imu_correct," which gives the IMU data in ROS REP105 standard. Because no IMU transformation is needed for this dataset, the following configurations need to be changed to run this dataset successfully:
+    - The "imuTopic" parameter in "config/params.yaml" needs to be set to "imu_correct".
+    - The "extrinsicRot" and "extrinsicRPY" in "config/params.yaml" needs to be set as identity matrices.
+      - [**Campus dataset**](https://drive.google.com/file/d/1q4Sf7s2veVc7bs08Qeha3stOiwsytopL/view?usp=sharing)
+
+
+
+## Run the package
+
+1. Run the launch file:
+```
+roslaunch lio_sam run.launch
+```
+
+2. Play existing bag files:
+```
+rosbag play your-bag.bag -r 3
+```
+
+## Paper 
+
+Thank you for citing [LIO-SAM](./config/doc/paper.pdf) if you use any of this code: 
+```
+@inproceedings{liosam2020shan,
+  title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping},
+  author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus Daniela},
+  journal={arXiv preprint arXiv:2007.xxxxx}
+  year={2020}
+}
+```
+
+Part of the code is adapted from [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM)
+```
+@inproceedings{legoloam2018shan,
+  title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain},
+  author={Shan, Tixiao and Englot, Brendan},
+  booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+  pages={4758-4765},
+  year={2018},
+  organization={IEEE}
+}
+```
+
+## Acknowledgement
+LIO-SAM is based on LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time).