Release v1.0.0

CMakeLists.txt

@@ -0,0 +1,119 @@
+###########################################################
+#                                                         #
+# Copyright (c)                                           #
+#                                                         #
+# The Verifiable & Control-Theoretic Robotics (VECTR) Lab #
+# University of California, Los Angeles                   #
+#                                                         #
+# Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez   #
+# Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu         #
+#                                                         #
+###########################################################
+
+cmake_minimum_required(VERSION 3.12.4)
+project(direct_lidar_inertial_odometry)
+
+add_compile_options(-std=c++14)
+set(CMAKE_BUILD_TYPE "Release")
+
+find_package( PCL REQUIRED )
+include_directories(${PCL_INCLUDE_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+link_directories(${PCL_LIBRARY_DIRS})
+
+find_package( Eigen3 REQUIRED )
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+include(FindOpenMP)
+if(OPENMP_FOUND)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+  set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+else(OPENMP_FOUND)
+  message("ERROR: OpenMP could not be found.")
+endif(OPENMP_FOUND)
+
+set(CMAKE_THREAD_PREFER_PTHREAD TRUE)
+set(THREADS_PREFER_PTHREAD_FLAG TRUE)
+find_package(Threads REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  nav_msgs
+  pcl_ros
+  message_generation
+)
+
+add_service_files(
+  DIRECTORY srv
+  FILES
+  save_pcd.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+  std_msgs
+  geometry_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    roscpp
+    std_msgs
+    sensor_msgs
+    geometry_msgs
+    pcl_ros
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+    nano_gicp
+    nanoflann
+)
+
+include_directories(include)
+include_directories(SYSTEM ${catkin_INCLUDE_DIRS})
+
+# Not all machines have <cpuid.h> available
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
+file(WRITE ${CMAKE_BINARY_DIR}/test_cpuid.cpp "#include <cpuid.h>")
+try_compile(HAS_CPUID ${CMAKE_BINARY_DIR} ${CMAKE_BINARY_DIR}/test_cpuid.cpp)
+file(REMOVE ${CMAKE_BINARY_DIR}/test_cpuid.cpp)
+if(HAS_CPUID)
+  add_compile_definitions(HAS_CPUID)
+endif()
+
+# NanoFLANN
+add_library(nanoflann STATIC
+  src/nano_gicp/nanoflann.cc
+)
+target_link_libraries(nanoflann ${PCL_LIBRARIES})
+
+# NanoGICP
+add_library(nano_gicp STATIC
+  src/nano_gicp/lsq_registration.cc
+  src/nano_gicp/nano_gicp.cc
+)
+target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann)
+
+# Odometry Node
+add_executable(dlio_odom_node src/dlio/odom_node.cc src/dlio/odom.cc)
+add_dependencies(dlio_odom_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_compile_options(dlio_odom_node PRIVATE ${OpenMP_FLAGS})
+target_link_libraries(dlio_odom_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads nano_gicp)
+
+# Mapping Node
+add_executable (dlio_map_node src/dlio/map_node.cc src/dlio/map.cc)
+add_dependencies(dlio_map_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS})
+target_link_libraries(dlio_map_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads)
+
+# Binaries
+install( TARGETS dlio_odom_node dlio_map_node
+         DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )
+install( DIRECTORY cfg launch
+         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} )

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) Kenny J. Chen, Ryan Nemiroff, and Brett T. Lopez
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -1,2 +1,107 @@
-# direct_lidar_inertial_odometry
-[IEEE ICRA'23] A new lightweight LiDAR-inertial odometry algorithm with a novel coarse-to-fine approach in constructing continuous-time trajectories for precise motion correction.
+# Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction
+
+#### [[Paper](https://arxiv.org/abs/2203.03749)] [[Video](https://www.youtube.com/watch?v=4-oXjG8ow10)] [[Presentation](https://www.youtube.com/watch?v=Hmiw66KZ1tU)]
+
+DLIO is a new lightweight LiDAR-inertial odometry algorithm with a novel coarse-to-fine approach in constructing continuous-time trajectories for precise motion correction. It features several algorithmic improvements over its predecessor, [DLO](https://github.com/vectr-ucla/direct_lidar_odometry), and was presented at the IEEE International Conference on Robotics and Automation (ICRA) in London, UK in 2023.
+
+<br>
+<p align='center'>
+    <img src="./doc/img/dlio.png" alt="drawing" width="720"/>
+</p>
+
+## Instructions
+
+### Sensor Setup
+DLIO has been extensively tested using a variety of sensor configurations. DLIO requires only a point cloud input of type `sensor_msgs::PointCloud2` and a 6-axis IMU input of type `sensor_msgs::Imu`.
+
+For best performance, extrinsic calibration between the LiDAR/IMU sensors and the robot's center-of-gravity should be inputted into `cfg/dlio.yaml`. If the exact values of these are unavailable, a rough LiDAR-to-IMU extrinsics can also be used (note however that performance will be degraded).
+
+IMU intrinsics are also necessary for best performance, and there are several open-source calibration tools to get these values. These values should also go into `cfg/dlio.yaml`. In practice however, if you are just testing this work, using the default ideal values and performing the initial calibration procedure should be fine.
+
+Also note that the LiDAR and IMU sensors _need_ to be properly time-synchronized, otherwise DLIO will not work. We recommend using a LiDAR with an integrated IMU (such as an Ouster) for simplicity of extrinsics and synchronization.
+
+### Dependencies
+The following has been verified to be compatible, although other configurations may work too:
+
+- Ubuntu 20.04
+- ROS Noetic (`roscpp`, `std_msgs`, `sensor_msgs`, `geometry_msgs`, `nav_msgs`, `pcl_ros`)
+- C++ 14
+- CMake >= `3.12.4`
+- OpenMP >= `4.5`
+- Point Cloud Library >= `1.10.0`
+- Eigen >= `3.3.7`
+
+```sh
+sudo apt install libomp-dev libpcl-dev libeigen3-dev
+```
+
+DLIO currently only supports ROS1, but we welcome any contributions by the community to add ROS2 support!
+
+### Compiling
+Compile using the [`catkin_tools`](https://catkin-tools.readthedocs.io/en/latest/) package via:
+
+```sh
+mkdir ws && cd ws && mkdir src && catkin init && cd src
+git clone https://github.com/vectr-ucla/direct_lidar_inertial_odometry.git
+catkin build
+```
+
+### Execution
+After compiling, source the workspace and execute via:
+
+```sh
+roslaunch direct_lidar_inertial_odometry dlio.launch \
+  rviz:={true, false} \
+  pointcloud_topic:=/robot/lidar \
+  imu_topic:=/robot/imu
+```
+
+Be sure to change the topic names to your corresponding topics. Alternatively, edit the launch file directly if desired. If successful, you should see the following output in your terminal:
+<br>
+<p align='center'>
+    <img src="./doc/img/terminal.png" alt="drawing" width="480"/>
+</p>
+
+### Services
+To save DLIO's generated map into `.pcd` format, call the following service:
+
+```sh
+rosservice call /robot/dlio_map/save_pcd LEAF_SIZE SAVE_PATH
+```
+
+### Test Data
+For your convenience, we provide test data [here](https://drive.google.com/file/d/1Sp_Mph4rekXKY2euxYxv6SD6WIzB-wVU/view?usp=sharing) (1.2GB, 1m 13s) of an aggressive motion to test our motion correction scheme. Try this data with both deskewing on and off!
+
+<br>
+<p align='center'>
+    <img src="./doc/gif/aggressive.gif" alt="drawing" width="720"/>
+</p>
+
+## Citation
+If you found this work useful, please cite our manuscript:
+
+```bibtex
+@article{chen2022dlio,
+  title={Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction},
+  author={Chen, Kenny and Nemiroff, Ryan and Lopez, Brett T},
+  journal={IEEE International Conference on Robotics and Automation (ICRA)},
+  year={2023}
+}
+```
+
+## Acknowledgements
+
+We thank the authors of the [FastGICP](https://github.com/SMRT-AIST/fast_gicp) and [NanoFLANN](https://github.com/jlblancoc/nanoflann) open-source packages:
+
+- Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration,” in _IEEE International Conference on Robotics and Automation (ICRA)_, IEEE, 2021, pp. 11 054–11 059.
+- Jose Luis Blanco and Pranjal Kumar Rai, “NanoFLANN: a C++ Header-Only Fork of FLANN, A Library for Nearest Neighbor (NN) with KD-Trees,” https://github.com/jlblancoc/nanoflann, 2014.
+
+We would also like to thank Helene Levy and David Thorne for their help with data collection.
+
+## License
+This work is licensed under the terms of the MIT license.
+
+<br>
+<p align='center'>
+    <img src="./doc/img/ucla.png" alt="drawing" width="720"/>
+</p>

cfg/dlio.yaml

@@ -0,0 +1,45 @@
+###########################################################
+#                                                         #
+# Copyright (c)                                           #
+#                                                         #
+# The Verifiable & Control-Theoretic Robotics (VECTR) Lab #
+# University of California, Los Angeles                   #
+#                                                         #
+# Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez   #
+# Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu         #
+#                                                         #
+###########################################################
+
+dlio:
+
+  version: 1.0.0
+
+  adaptive: true
+
+  pointcloud:
+    deskew: true
+    voxelize: true
+
+  imu:
+    calibration: true
+    intrinsics:
+      accel:
+        bias: [ 0.0, 0.0, 0.0 ]
+        sm:   [ 1.,  0.,  0.,
+                0.,  1.,  0.,
+                0.,  0.,  1. ]
+      gyro:
+        bias: [ 0.0, 0.0, 0.0 ]
+
+  extrinsics:
+    baselink2imu:
+      t: [ 0.006253, -0.011775, 0.007645 ]
+      R: [ 1.,  0.,  0.,
+           0.,  1.,  0.,
+           0.,  0.,  1. ]
+    baselink2lidar:
+      t: [ 0.,  0.,  0. ]
+      R: [ 1.,  0.,  0.,
+           0.,  1.,  0.,
+           0.,  0.,  1. ]
+

cfg/params.yaml

@@ -0,0 +1,72 @@
+###########################################################
+#                                                         #
+# Copyright (c)                                           #
+#                                                         #
+# The Verifiable & Control-Theoretic Robotics (VECTR) Lab #
+# University of California, Los Angeles                   #
+#                                                         #
+# Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez   #
+# Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu         #
+#                                                         #
+###########################################################
+
+dlio:
+
+  frames:
+    odom: odom
+    baselink: base_link
+    lidar: lidar
+    imu: imu
+
+  map:
+    waitUntilMove: true
+    dense:
+      filtered: true
+    sparse:
+      frequency: 1.0
+      leafSize: 0.25
+
+  odom:
+
+    gravity: 9.80665
+
+    imu:
+      approximateGravity: false
+      calibration:
+        gyro: true
+        accel: true
+        time: 3
+      bufferSize: 5000
+
+    preprocessing:
+      cropBoxFilter:
+        size: 1.0
+      voxelFilter:
+        res: 0.25
+
+    keyframe:
+      threshD: 1.0
+      threshR: 45.0
+
+    submap:
+      keyframe:
+        knn: 10
+        kcv: 10
+        kcc: 10
+    gicp:
+      minNumPoints: 64
+      kCorrespondences: 16
+      maxCorrespondenceDistance: 0.5
+      maxIterations: 32
+      transformationEpsilon: 0.01
+      rotationEpsilon: 0.01
+      initLambdaFactor: 1e-9
+
+    geo:
+      Kp: 4.5
+      Kv: 11.25
+      Kq: 4.0
+      Kab: 2.25
+      Kgb: 1.0
+      abias_max: 5.0
+      gbias_max: 0.5