initial commit

CMakeLists.txt

@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(coin_lio)
+
+SET(CMAKE_BUILD_TYPE "Release")
+
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
+
+add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions -fdiagnostics-color=always")
+set(CMAKE_EXPORT_COMPILE_COMMANDS "1")
+
+message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
+  include(ProcessorCount)
+  ProcessorCount(N)
+  message("Processer number:  ${N}")
+  if(N GREATER 4)
+    add_definitions(-DMP_EN)
+    add_definitions(-DMP_PROC_NUM=3)
+    message("core for MP: 3")
+  elseif(N GREATER 3)
+    add_definitions(-DMP_EN)
+    add_definitions(-DMP_PROC_NUM=2)
+    message("core for MP: 2")
+  else()
+    add_definitions(-DMP_PROC_NUM=1)
+  endif()
+else()
+  add_definitions(-DMP_PROC_NUM=1)
+endif()
+
+find_package(OpenMP QUIET)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}   ${OpenMP_C_FLAGS}")
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  roscpp
+  rospy
+  std_msgs
+  pcl_ros
+  tf
+  message_generation
+  eigen_conversions
+  cv_bridge
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL 1.8 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+message(Eigen: ${EIGEN3_INCLUDE_DIR})
+
+include_directories(
+	${catkin_INCLUDE_DIRS} 
+  ${EIGEN3_INCLUDE_DIR}
+  ${PCL_INCLUDE_DIRS}
+  ${PYTHON_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  include)
+
+add_library(${PROJECT_NAME}
+  include/ikd-Tree/ikd_Tree.cpp
+  src/preprocess.cpp
+  src/image_processing.cpp
+  src/imu_processing.cpp
+  src/feature_manager.cpp
+  src/timing.cpp
+  src/use_ikfom.cpp
+  src/projector.cpp
+)
+
+add_message_files(
+  FILES
+  Pose6D.msg
+)
+
+generate_messages(
+ DEPENDENCIES
+ geometry_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp std_msgs message_runtime
+  DEPENDS EIGEN3 PCL
+  INCLUDE_DIRS
+)
+
+add_executable(coin_lio_mapping src/laserMapping.cpp)
+target_link_libraries(coin_lio_mapping ${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+target_include_directories(${PROJECT_NAME} PRIVATE ${PYTHON_INCLUDE_DIRS})
+
+add_executable(coin_lio_calibration src/calibration.cpp)
+target_link_libraries(coin_lio_calibration ${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

README.md

@@ -0,0 +1,106 @@
+<p align="center">
+  <img width=125 src="doc/coin_transparent.gif">
+</p>
+
+# Complementary Intensity-Augmented LiDAR Inertial Odometry
+Patrick Pfreundschuh, Helen Oleynikova, Cesar Cadena, Roland Siegwart, and Olov Andersson. "COIN-LIO: Complementary Intensity-Augmented LiDAR Inertial Odometry" accepted for presentation at ICRA 2024. \[[ArXiv](https://arxiv.org/abs/2310.01235)\]
+<p align="center">
+  <img width='100%' src="doc/coin_tracking.gif">
+</p>
+
+<details>
+<summary>Abstract</summary>
+<br>
+We present COIN-LIO, a LiDAR Inertial Odometry pipeline that tightly couples information from LiDAR intensity with geometry-based point cloud registration. The focus of our work is to improve the robustness of LiDAR-inertial odometry in geometrically degenerate scenarios, like tunnels or flat fields. We project LiDAR intensity returns into an intensity image, and propose an image processing pipeline that produces filtered images with improved brightness consistency within the image as well as across different scenes. To effectively leverage intensity as an additional modality, we present a novel feature selection scheme that detects uninformative directions in the point cloud registration and explicitly selects patches with complementary image information. Photometric error minimization in the image patches is then fused with inertial measurements and point-to-plane registration in an iterated Extended Kalman Filter. The proposed approach improves accuracy and robustness on a public dataset. We additionally publish a new dataset, that captures five real-world environments in challenging, geometrically degenerate scenes. By using the additional photometric information, our approach shows drastically improved robustness against geometric degeneracy in environments where all compared baseline approaches fail.
+</details>
+
+Please cite our work if you are using COIN-LIO in your research.
+  ```bibtex
+  @article{pfreundschuh2023coin,
+  title={COIN-LIO: Complementary Intensity-Augmented LiDAR Inertial Odometry},
+  author={Pfreundschuh, Patrick and Oleynikova, Helen and Cadena, Cesar and Siegwart, Roland and Andersson, Olov},
+  journal={arXiv preprint arXiv:2310.01235},
+  year={2023}
+  }
+  ```
+
+# Setup
+## Installation
+This package was developed on Ubuntu 20.04 using ROS Noetic. Other versions should also work but have not been tested and we do not guarantee support.
+
+1. If not done yet, please [install ROS](https://wiki.ros.org/noetic/Installation), [install the proposed system dependencies](https://wiki.ros.org/noetic/Installation/Ubuntu#Dependencies_for_building_packages).
+  Install some additional system dependencies:
+    ```bash
+    sudo apt-get install python3-catkin-tools libgoogle-glog-dev
+    ```
+2. Then create a catkin workspace:
+    ```bash
+    mkdir -p ~/catkin_ws/src
+    cd ~/catkin_ws
+    catkin init
+    catkin config --extend /opt/ros/$ROS_DISTRO
+    catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+    catkin config --merge-devel
+    ```
+3. Clone COIN-LIO into your workspace:
+    ```bash
+    cd ~/catkin_ws/src
+    git clone [email protected]:ethz-asl/coin-lio.git
+    cd COIN-LIO
+    git submodule init
+    git submodule update --recursive
+    ```
+4. Build COIN-LIO:
+    ```bash
+    catkin build coin_lio
+    ```
+
+## Alternative Installation: Docker
+To instead use docker, check out the repository locally, navigate to it, and:
+    ```bash
+    cd docker/
+    ./run_docker.sh -b
+    ```
+Which will build a docker image with a copy of the code checked out inside.
+Your `~/data` folder will be mounted to `/root/data` within the docker, so you
+can download datasets and follow the rest of the tutorial below. On future runs,
+you can simply use `./run_docker.sh` (without `-b`) to not re-build the image.
+
+## Running ENWIDE Dataset Sequences
+The ENWIDE dataset sequences can be downloaded [here](https://projects.asl.ethz.ch/datasets/enwide).
+Run a sequence:
+  ```bash
+  roslaunch coin_lio mapping_enwide.launch bag_file:=<example_bag_path.bag>
+  ```
+## Running Newer College Dataset Sequences
+The Newer College Dataset sequences can be downloaded [here](https://drive.google.com/drive/u/0/folders/1uR476FzjN3PfAiCknVKtuZi3_QfVvSdA).
+Run a sequence:
+  ```bash
+  roslaunch coin_lio mapping_newer_college.launch bag_file:=<example_bag_path.bag>
+  ```
+## Running COIN-LIO on your own data:
+**Note on LiDAR type:** COIN-LIO currently only supports data from Ouster LiDARs, as we use the calibration in the metadata file for the image projection model. Implementing different sensors is theoretically possible but requires a proper implementation of a projection model that works for the specific sensor. Contributions are welcome.
+
+### Sensor Calibration
+* **LiDAR:**
+Since different Ouster sensors have different image projection parameters, we need to run a calibration tool to evaluate the column shift which is required to correct the image projection model. This procedure is only required once per sensor.
+  ```bash
+  roslaunch coin_lio calibrate.launch bag_file:=<bag_path.bag> metadata_file:=<metadata_path.bag> point_topic:=<pointcloud_topic>
+  ```
+  The evaluated column shift parameter will be printed at the end of the procedure.
+* **IMU:**
+If you are not using the built-in IMU in the Ouster LiDAR, you need to adapt the extrinsic calibration between IMU and LiDAR accordingly in the [parameter file]().
+### Run COIN-LIO with your own data
+Launch with settings for your data:
+  ```bash
+  roslaunch coin_lio mapping.launch metadata_file:=<metadata_path.bag> column_shift:=<parameter from calibration> point_topic:=<pointcloud_topic> imu_topic:=<imu_topic>
+  ```
+Play your data:
+  ```bash
+  rosbag play <bag_path.bag>
+  ```
+
+## Acknowledgements
+COIN-LIO builds on top of [FAST-LIO2](https://github.com/hku-mars/FAST_LIO) for the point-to-plane registration. We thank the authors for open-sourcing their outstanding work.
+Our dashboard was inspired by [DLIO](https://github.com/vectr-ucla/direct_lidar_inertial_odometry).\
+We used [ascii-image-converter](https://github.com/TheZoraiz/ascii-image-converter) for our ascii animation.