first commit open source

coolaogege · Dec 16, 2022 · 71c31c6 · 71c31c6
commit 71c31c6
Show file tree

Hide file tree

Showing 145 changed files with 28,335 additions and 0 deletions.
diff --git a/.github/stale.yml b/.github/stale.yml
@@ -0,0 +1,17 @@
+# Number of days of inactivity before an issue becomes stale
+daysUntilStale: 14
+# Number of days of inactivity before a stale issue is closed
+daysUntilClose: 1
+# Issues with these labels will never be considered stale
+exemptLabels:
+  - pinned
+  - security
+# Label to use when marking an issue as stale
+staleLabel: stale
+# Comment to post when marking an issue as stale. Set to `false` to disable
+markComment: >
+  This issue has been automatically marked as stale because it has not had
+  recent activity. It will be closed if no further activity occurs. Thank you
+  for your contributions.
+# Comment to post when closing a stale issue. Set to `false` to disable
+closeComment: false
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -0,0 +1,136 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lvi_sam)
+
+################## 编译开关 compile switch##############
+# -DIF_OFFICIAL=1: use origin official LVI-SAM code
+# -DIF_OFFICIAL=0: use modified code of this repo
+add_definitions(-DIF_OFFICIAL=0)
+
+
+######################
+### Cmake flags
+######################
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
+
+######################
+### Packages
+######################
+find_package(catkin REQUIRED COMPONENTS
+    tf
+    roscpp
+    rospy
+    roslib
+    # msg
+    std_msgs
+    sensor_msgs
+    geometry_msgs
+    nav_msgs
+    # cv
+    cv_bridge
+    # pcl
+    pcl_conversions
+    # msg generation
+    message_generation
+)
+
+find_package(OpenMP REQUIRED)
+find_package(PCL REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Ceres REQUIRED)
+find_package(GTSAM REQUIRED QUIET)
+find_package(Boost REQUIRED COMPONENTS filesystem program_options system timer)
+
+
+######################
+### Message generation
+######################
+add_message_files(
+  DIRECTORY msg
+  FILES
+  cloud_info.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+  std_msgs
+  nav_msgs
+  sensor_msgs
+)
+
+######################
+### Catkin
+######################
+catkin_package(
+    DEPENDS PCL GTSAM
+)
+
+include_directories(
+    ${catkin_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${CERES_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+    ${GTSAM_INCLUDE_DIR}
+)
+
+link_directories(
+    ${PCL_LIBRARY_DIRS}
+    ${OpenCV_LIBRARY_DIRS}
+    ${GTSAM_LIBRARY_DIRS}
+)
+
+######################
+### visual odometry
+######################
+file(GLOB visual_feature_files
+    "src/visual_odometry/visual_feature/*.cpp"
+    "src/visual_odometry/visual_feature/camera_models/*.cc"
+)
+file(GLOB visual_odometry_files
+    "src/visual_odometry/visual_estimator/*.cpp"
+    "src/visual_odometry/visual_estimator/factor/*.cpp"
+    "src/visual_odometry/visual_estimator/initial/*.cpp"
+    "src/visual_odometry/visual_estimator/utility/*.cpp"
+)
+file(GLOB visual_loop_files
+    "src/visual_odometry/visual_loop/*.cpp"
+    "src/visual_odometry/visual_loop/utility/*.cpp"
+    "src/visual_odometry/visual_loop/ThirdParty/*.cpp"
+    "src/visual_odometry/visual_loop/ThirdParty/DBoW/*.cpp"
+    "src/visual_odometry/visual_loop/ThirdParty/DUtils/*.cpp"
+    "src/visual_odometry/visual_loop/ThirdParty/DVision/*.cpp"
+    "src/visual_odometry/visual_feature/camera_models/*.cc"
+)
+# Visual Feature Tracker
+add_executable(${PROJECT_NAME}_visual_feature ${visual_feature_files})
+target_link_libraries(${PROJECT_NAME}_visual_feature ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
+# Visual Odometry
+add_executable(${PROJECT_NAME}_visual_odometry ${visual_odometry_files})
+target_link_libraries(${PROJECT_NAME}_visual_odometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 
+# Visual Lopp
+add_executable(${PROJECT_NAME}_visual_loop ${visual_loop_files})
+target_link_libraries(${PROJECT_NAME}_visual_loop ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 
+
+######################
+### lidar odometry
+######################
+
+# IMU Preintegration
+add_executable(${PROJECT_NAME}_imuPreintegration src/lidar_odometry/imuPreintegration.cpp)
+target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam Boost::timer)
+# Range Image Projection
+add_executable(${PROJECT_NAME}_imageProjection src/lidar_odometry/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/lidar_odometry/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/lidar_odometry/mapOptmization.cpp)
+add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS})
+target_link_libraries(${PROJECT_NAME}_mapOptmization PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam Boost::timer)
diff --git a/LICENSE b/LICENSE
@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2021, Tixiao Shan
+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.
diff --git a/README.md b/README.md
@@ -0,0 +1,205 @@
+# LVI-SAM-Easyused
+
+This repository contains modified code of [LVI-SAM](https://github.com/TixiaoShan/LVI-SAM) for easier using. 
+
+---
+
+
+
+## Compile
+
+You can use the following commands to download and compile the package.
+
+```shell
+mkdir -p ~/catkin_ws/src
+cd ~/catkin_ws/src
+git clone https://github.com/Cc19245/LVI-SAM-Easyused
+cd ..
+catkin_make
+```
+
+**Note**：If you want to use the no-modified code (origin LVI-SAM official code), you can change the defination in `CMakeLists.txt` and compile again.
+
+```cmake
+################## 编译开关 compile switch##############
+# -DIF_OFFICIAL=1: use origin official LVI-SAM code
+# -DIF_OFFICIAL=0: use modified code of this repo
+add_definitions(-DIF_OFFICIAL=0)
+```
+
+---
+
+
+
+## Params config
+
+1. `params_camera.yaml`: set the VIO params, especially for $T\_imu\_camera$. It's same as VINS-Mono.
+
+```yaml
+###################### extrinsic between IMU and Camera  ###########################
+###################### T_IMU_Camera, Camera -> IMU       ###########################  
+# R_imu_camera
+extrinsicRotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 0,    0,    -1, 
+               -1,     0,    0, 
+                0,     1,    0]
+# t_imu_camera
+extrinsicTranslation: !!opencv-matrix
+   rows: 3
+   cols: 1
+   dt: d
+   data: [0.006422381632411965, 0.019939800449065116, 0.03364235163589248]
+```
+
+2. `params_lidar.yaml`: set the LIO params, especially for $T\_imu\_lidar$. 
+
+```yaml
+  ###################### extrinsic between IMU and LiDAR  ###########################
+  ###################### T_IMU_LiDAR, LiDAR -> IMU       ############################
+  # t_imu_lidar
+  extrinsicTranslation: [0.0,   0.0,   0.0]    
+  # R_imu_lidar
+  extrinsicRotation: [-1,   0,    0, 
+                                         0,    1,    0, 
+                                         0,    0,   -1]
+```
+
+**Besides**, due to the special IMU (the Euler angle coordinate system is different from the angular velocity and angular velocity coordinate system) of official dataset , you also need to set which axis the IMU rotates around counterclockwise to get a positive output. For official sensor equipment, it is set as follows.
+
+```yaml
+  ## 对绝大多数IMU来说，下面三个值分别是"+z", "+y", "+x" (for most of IMUs, the following config is "+z", "+y", "+x")
+  # 绕着哪个轴逆时针转动，输出yaw角度为正(which axis the IMU rotates around counterclockwise to get a positive yaw angle)
+  yawAxis: "-z"  
+  # 绕着哪个轴逆时针转动，输出pitch角度为正(which axis the IMU rotates counterclockwise to get a positive pitch angle)
+  pitchAxis: "+x"    
+  # 绕着哪个轴逆时针转动，输出roll角度为正(which axis the IMU rotates around counterclockwise to get a positive roll angle)
+  rollAxis: "+y"    
+```
+
+<p align='center'>
+    <img src="./doc/official-equipment.png" alt="drawing" width="800"/>
+</p>
+
+**However**, for most of the IMUs, the Euler angle coordinate system is same as the angular velocity and angular velocity coordinate system. So the above parameters should be set as follows.
+
+```yaml
+  ## 对绝大多数IMU来说，下面三个值分别是"+z", "+y", "+x" (for most of IMUs, the following config is "+z", "+y", "+x")
+  # 绕着哪个轴逆时针转动，输出yaw角度为正(which axis the IMU rotates around counterclockwise to get a positive yaw angle)
+  yawAxis: "+z"  
+  # 绕着哪个轴逆时针转动，输出pitch角度为正(which axis the IMU rotates counterclockwise to get a positive pitch angle)
+  pitchAxis: "+y"    
+  # 绕着哪个轴逆时针转动，输出roll角度为正(which axis the IMU rotates around counterclockwise to get a positive roll angle)
+  rollAxis: "+x"    
+```
+
+---
+
+
+
+## Run the package on different datasets
+
+1. [Official dataset](https://drive.google.com/drive/folders/1q2NZnsgNmezFemoxhHnrDnp1JV_bqrgV)
+
+   - Run the launch file:
+
+     ```
+     roslaunch lvi_sam run.launch
+     ```
+
+     **Note**: If you want to test the origin official LVI-SAM code (e.g. set `add_definitions(-DIF_OFFICIAL=1)` in CMakeLists.txt to compile), you should run launch file as following.
+
+     ```
+     roslaunch lvi_sam run_official.launch
+     ```
+
+   - Play existing bag files, e.g. handheld.bag:
+
+     ```
+     rosbag play handheld.bag 
+     ```
+
+   - Results of origin official code (up fig) and our's modified code (down fig) on handheld.bag:
+
+     <p align='center'>
+         <img src="./doc/handheld-official.png" alt="drawing" width="600"/>
+     </p>
+
+     <p align='center'>
+         <img src="./doc/handheld.png" alt="drawing" width="600"/>
+     </p>
+
+2. [M2DGR dataset](https://github.com/SJTU-ViSYS/M2DGR)
+
+   - Run the launch file:
+
+     ```
+     roslaunch lvi_sam M2DGR.launch
+     ```
+
+   - Play existing bag files, e.g. gate_01.bag:
+
+     ```
+     rosbag play gate_01.bag 
+     ```
+
+   - Results of our's modified code on gate_01.bag:
+
+     <p align='center'>
+         <img src="./doc/gate_01.png" alt="drawing" width="600"/>
+     </p>
+
+3. [My test dataset](https://1drv.ms/u/s!AqYajE_ft9lwg0paJQu_DRzU-GQ5?e=A95yfn)
+
+   - Run the launch file:
+
+     ```
+     roslaunch lvi_sam backbag.launch
+     ```
+
+   - Play existing bag files, e.g. backbag.bag:
+
+     ```
+     rosbag play backbag.bag 
+     ```
+
+   - Results of our's modified code on backbag.bag:
+
+     <p align='center'>
+         <img src="./doc/backbag.png" alt="drawing" width="600"/>
+     </p>
+
+   - Results of our's modified code on 0117-1525.bag (Due to privacy issues, this data package is not open source):
+
+     ```
+     roslaunch lvi_sam ljj.launch
+     rosbag play 0117-1525.bag 
+     ```
+
+     <p align='center'>    <img src="./doc/ljj.png" alt="drawing" width="600"/></p>
+
+---
+
+
+
+## TODO
+
+  - [ ] More test on different dataset, e.g. KITTI, KAIST. **However**, these datasets' lidar data have no **ring** information. So LVI-SAM can't run directly. If you want to run on these datasets, you need to modifidy the code to add this information refer to [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM).
+
+---
+
+
+
+## Notes
+
+If you want to know what changes I made and why they make sense, you can refer to my blog: [LVI-SAM坐标系外参分析与代码修改，以适配各种数据集](https://blog.csdn.net/qq_42731705/article/details/128344179).
+
+---
+
+
+
+## Acknowledgement
+
+- Origin official [LVI-SAM](https://github.com/TixiaoShan/LVI-SAM).