code release

.gitignore

@@ -0,0 +1,2 @@
+data/
+.vscode/

CMakeLists.txt

@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(cocolic)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -Wno-sign-compare -Wno-unused -Wno-comment -pthread")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -msse4.2")
+
+find_package(OpenMP)
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  nav_msgs
+  visualization_msgs
+  eigen_conversions
+  cv_bridge
+  roslib
+  rosbag
+  tf
+  message_generation
+  image_transport
+)
+
+# FIND_PACKAGE(Boost REQUIRED COMPONENTS thread)
+FIND_PACKAGE(Boost REQUIRED COMPONENTS filesystem iostreams program_options system serialization thread)
+if(Boost_FOUND)
+	INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
+	LINK_DIRECTORIES(${Boost_LIBRARY_DIRS})
+endif()
+
+find_package(Eigen3 REQUIRED)
+find_package(Ceres REQUIRED)
+# find_package(Sophus REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(PCL 1.13.0 REQUIRED)
+
+# find_package(yaml-cpp REQUIRED)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp>=0.5)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  feature_cloud.msg
+  imu_array.msg
+  pose_array.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs 
+  sensor_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS src
+  CATKIN_DEPENDS std_msgs
+)
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  src
+  ${catkin_INCLUDE_DIRS}
+  ${YAML_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  # ${Sophus_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+  ${PCL_INCLUDE_DIRS}
+  src/camera
+  src/camera/loam/include
+  src/camera/tools/
+  src/camera/rgb_map
+)
+list(APPEND thirdparty_libraries
+        ${YAML_CPP_LIBRARIES}
+        ${Boost_LIBRARIES}
+        ${catkin_LIBRARIES}
+        ${OpenCV_LIBS}
+        ${CERES_LIBRARIES}
+        ${PCL_LIBRARIES}
+        ${Boost_LIBRARIES}
+        ${Boost_FILESYSTEM_LIBRARY}
+        ${Boost_SERIALIZATION_LIBRARY} # serialization
+)
+
+add_library(spline_lib src/spline/trajectory.cpp)
+target_link_libraries(spline_lib ${thirdparty_libraries})
+
+file(GLOB lidar_odometry_files
+    "src/lidar/lidar_handler.cpp"
+    "src/lidar/velodyne_feature_extraction.cpp",
+    "src/lidar/livox_feature_extraction.cpp"
+)
+add_library(lidar_lib ${lidar_odometry_files})
+target_link_libraries(lidar_lib spline_lib ${thirdparty_libraries})
+
+file(GLOB r3live_files
+    "src/camera/loam/*.cpp"
+    "src/camera/optical_flow/*.cpp"
+    "src/camera/rgb_map/*.cpp"
+    "src/camera/*.cpp"
+)
+add_library(r3live_lib ${r3live_files})
+target_link_libraries(r3live_lib ${thirdparty_libraries})
+
+add_executable(odometry_node 
+   src/odometry_node.cpp
+   src/imu/imu_state_estimator.cpp
+   src/imu/imu_initializer.cpp
+   src/odom/msg_manager.cpp
+   src/odom/trajectory_manager.cpp
+   src/odom/trajectory_estimator.cpp
+   src/odom/odometry_manager.cpp
+   src/odom/factor/analytic_diff/marginalization_factor.cpp
+   src/utils/parameter_struct.cpp
+)
+
+target_link_libraries(odometry_node 
+   spline_lib 
+   lidar_lib 
+   r3live_lib
+   ${thirdparty_libraries}
+  #  fmt::fmt
+)

README.md

@@ -1,25 +1,111 @@
 # Coco-LIC
 
-*News📝: Occupied by so many tasks over the past few months😭, we now ultimately have time to clean up the code and the code will be released in April!*
+**Coco-LIC: Continuous-Time Tightly-Coupled LiDAR-Inertial-Camera Odometry using Non-Uniform B-spline**
 
----
+<p>
+<img src="figure/r3live.gif" width="30%" alt="r3live" />
+<img src="figure/fastlivo.gif" width="30%" alt="fastlivo" />
+<img src="figure/lvisam.gif" width="30%" alt="lvisam" />
+</p>
 
-Coco-LIC: Continuous-Time Tightly-Coupled LiDAR-Inertial-Camera Odometry using Non-Uniform B-spline
+The following are three main characters of 🥥 Coco-LIC [[`Paper`](https://arxiv.org/pdf/2309.09808)] [[`Video`](https://www.youtube.com/watch?v=M-vlxK4DWno)] :
 
-![factor-graph](figure/factor-graph.jpg) 
++ **dynamically** place control points to unlock the real power of the continuous-time trajectory
++ tightly fuse LiDAR-Inertial-Camera data in a **short sliding window** based on a factor graph
++ support **multimodal multiple** LiDARs and achieve great performance in **degenerated** cases 
 
-Coco-LIC is a continuous-time tightly-coupled LiDAR-Inertial-Camera Odometry system based on non-uniform B-Splines. In contrast to uniform B-spline-based methods, it offers significant advantages in terms of efficiency and accuracy. This is accomplished by dynamically and adaptively placing control points, taking into account the varying dynamics of the motion. A novel way to enable efficient fusion of LiDAR-Inertial-Camera data within a short sliding window is also proposed for continuous-time framework.
+## Prerequisites
 
-The source code will be available after the publication of the related paper.
++ ROS（tested with noetic）
++ Eigen 3.3.7
++ Ceres 2.0.0
++ OpenCV 4
++ PCL >= 1.13
++ [livox_ros_driver](https://github.com/Livox-SDK/livox_ros_driver)
++ yaml-cpp
 
-## Comparison of Uniform and Non-Uniform Placement 
+## Install
 
-1 control point per 0.1 seconds **vs** adaptively adjusting control points per 0.1 seconds.
+```shell
+mkdir -p ~/catkin_ws/src
+cd ~/catkin_ws/src
+git clone https://github.com/Livox-SDK/livox_ros_driver.git
+cd ~/catkin_ws && catkin_make
+cd ~/catkin_ws/src
+git clone https://github.com/APRIL-ZJU/Coco-LIC.git
+cd ~/catkin_ws && catkin_make
+source ~/catkin_ws/devel/setup.bash
+cd ~/catkin_ws/src/Coco-LIC && mkdir data
+```
 
-![uni-vs-nonuni](figure/uni-vs-nonuni.png) 
+## Run
 
-## Evaluation on challenging degenerate dataset
++ Download [R3LIVE dataset](https://github.com/ziv-lin/r3live_dataset) or [FAST-LIVO dataset](https://connecthkuhk-my.sharepoint.com/personal/zhengcr_connect_hku_hk/_layouts/15/onedrive.aspx?id=%2Fpersonal%2Fzhengcr%5Fconnect%5Fhku%5Fhk%2FDocuments%2FFAST%2DLIVO%2DDatasets&ga=1) or [NTU-VIRAL dataset](https://ntu-aris.github.io/ntu_viral_dataset/) or [LVI-SAM dataset](https://drive.google.com/drive/folders/1q2NZnsgNmezFemoxhHnrDnp1JV_bqrgV).
 
-The odometry and mapping result of Coco-LIC on the sequence degenerate_seq_00 of [R3LIVE Dataset](https://github.com/ziv-lin/r3live_dataset), where severe LiDAR degeneration happens when Livox Avia faces the ground for a while. Coco-LIC overcomes the degradation and succeeds in returning to the origin.
++ Configure parameters in the `config/ct_odometry_xxx.yaml` file.
 
-![degenerate](figure/degenerate.png) 
+  - `log_path`: the path to log
+  - `config_path`: the path of `config` folder 
+  - `bag_path`: the file path of rosbag  
+
++ Run on R3LIVE dataset for example.
+
+  ```shell
+  roslaunch cocolic odometry.launch config_path:=config/ct_odometry_r3live.yaml
+  ```
+
+  The estimated trajectory is saved in the folder `./src/Coco-LIC/data`.
+
+## Supplymentary1 - non-uniform verification
+
+1 control point per 0.1 seconds 🥊 adaptively placing control points per 0.1 seconds.
+
+<img src="figure/uni-vs-nonuni.png" width="60%" height="60%" /> 
+
+The different colors of the trajectory correspond to different densities of control points.
+
+ <img src="figure/color-traj.png" width="60%" height="60%" />
+
+## Supplymentary2 - comparison on NTU-VIRAL
+
+We additionally compare Coco-LIC with our previous work [CLIC](https://github.com/APRIL-ZJU/clic) on NTU-VIRAL dataset, employing 1 LiDAR. 
+
+The best results are marked in bold. It can be seen that Coco-LIC stably outperforms CLIC.
+
+<img src="figure/cocovsclic.png" width="60%" height="60%" /> 
+
+## TODO List
+
+- [ ] serves as the front-end of incremental 3D Gaussian Splatting（[Gaussian-LIC](https://arxiv.org/pdf/2404.06926)）
+- [ ] optimize the code architecture (rosbag play mode) and support ikd-tree for acceleration
+
+## Citation
+
+If you find our work helpful, please consider citing 🌟:
+
+```bibtex
+@article{lang2023coco,
+  title={Coco-LIC: continuous-time tightly-coupled LiDAR-inertial-camera odometry using non-uniform B-spline},
+  author={Lang, Xiaolei and Chen, Chao and Tang, Kai and Ma, Yukai and Lv, Jiajun and Liu, Yong and Zuo, Xingxing},
+  journal={IEEE Robotics and Automation Letters},
+  year={2023},
+  publisher={IEEE}
+}
+```
+```bibtex
+@article{lv2023continuous,
+  title={Continuous-time fixed-lag smoothing for lidar-inertial-camera slam},
+  author={Lv, Jiajun and Lang, Xiaolei and Xu, Jinhong and Wang, Mengmeng and Liu, Yong and Zuo, Xingxing},
+  journal={IEEE/ASME Transactions on Mechatronics},
+  year={2023},
+  publisher={IEEE}
+}
+```
+
+## Acknowledgement
+
+Thanks for [Basalt](https://gitlab.com/VladyslavUsenko/basalt-headers), [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM/tree/a246c960e3fca52b989abf888c8cf1fae25b7c25), [Open-VINS](https://github.com/rpng/open_vins), [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono), [R3LIVE](https://github.com/hku-mars/r3live) and [FAST-LIVO](https://github.com/hku-mars/FAST-LIVO).
+
+## LICENSE
+
+The code is released under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt).