loosely_lio

.gitignore

@@ -0,0 +1,2 @@
+.vscode
+log

CMakeLists.txt

@@ -0,0 +1,111 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(icp_odometry)
+
+SET(CMAKE_BUILD_TYPE "Release")
+SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")
+add_compile_options(-std=c++17)
+add_definitions(-std=c++17)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  pcl_ros
+  tf
+  eigen_conversions
+)
+
+find_package( TBB REQUIRED)
+find_package(Sophus REQUIRED)
+include_directories(${Sophus_INCLUDE_DIRS})
+find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIRS})
+include_directories(/usr/local/include/eigen3)
+# //?：什么鬼bug https://blog.csdn.net/qq_39779233/article/details/127973918
+set(Sophus_LIBRARIES libSophus.so)
+
+# GNSS
+find_package (GeographicLib REQUIRED)
+include_directories(${GeographicLib_INCLUDE_DIRS})
+
+find_package(Glog)
+
+find_package(PCL 1.9 REQUIRED QUIET)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+# yaml-cpp
+find_package(yaml-cpp REQUIRED)
+include_directories(${yaml-cpp_INCLUDE_DIRS})
+
+catkin_package(
+)
+
+include(cmake/geographic.cmake)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+# 新的包含写法
+add_library(${PROJECT_NAME}.utils
+          src/utils/cloud_publisher.cpp
+          src/utils/cloud_subscriber.cpp
+          src/utils/gnss_data.cpp
+          src/utils/gnss_subscriber.cpp
+          src/utils/imu_subscriber.cpp
+          src/utils/odometry_publisher.cpp
+)
+target_link_libraries(${PROJECT_NAME}.utils ${PCL_LIBRARIES})
+
+add_library(${PROJECT_NAME}.ndt_lo
+          src/modules/ndt_lo/direct_ndt_lo.cpp
+          src/modules/ndt_lo/ndt_3d.cpp
+)
+file(GLOB_RECURSE ALL_MODULES "*src/modules/*.cpp")
+
+# ndt_lo_node
+add_executable(ndt_3d_odometry_node src/nodes/ndt_3d_odometry_node.cpp 
+# ${ALL_SRCS}
+${ALL_MODULES}
+)
+target_link_libraries(ndt_3d_odometry_node 
+              ${PROJECT_NAME}.utils
+              ${catkin_LIBRARIES}
+              ${Sophus_LIBRARIES}
+              ${GeographicLib_LIBRARIES}
+              glog::glog
+              TBB::tbb
+              yaml-cpp
+              )
+
+# loosely_lio_node
+add_executable(loosely_lio_node 
+              src/nodes/loosely_lio_node.cpp 
+              src/modules/ESKF/loosely_lio.cpp
+              src/modules/ESKF/static_imu_init.cpp
+)
+target_link_libraries(loosely_lio_node 
+              ${PROJECT_NAME}.utils
+              ${PROJECT_NAME}.ndt_lo
+              ${catkin_LIBRARIES}
+              ${Sophus_LIBRARIES}
+              ${GeographicLib_LIBRARIES}
+              glog::glog
+              TBB::tbb
+              ${yaml-cpp_LIBRARIES}
+              yaml-cpp
+              )
+
+# add_executable(gnss_data_node src/nodes/gnss_data_node.cpp 
+#               src/utils/gnss_data.cpp
+#               src/utils/gnss_subscriber.cpp
+#               src/utils/odometry_publisher.cpp
+#               src/utils/imu_subscriber.cpp
+# )
+# target_link_libraries(gnss_data_node 
+#             ${catkin_LIBRARIES}
+#             ${GeographicLib_LIBRARIES}
+#             glog::glog
+#             )

README.md

@@ -0,0 +1,57 @@
+# SLAM Box
+
+## 1. 纯ndt里程计
+基于手写的多线程ndt匹配算法实现简单的里程计
+<table>
+  <thead>
+    <tr>
+      <th colspan="6">On KITTI Datasets</th>
+    </tr>
+  </thead>
+  <!-- <tbody>
+    <tr align="center">
+      <td> 00 </td><td> 01 </td><td> 02 </td><td> 03 </td><td> 05 </td><td> 04 </td>
+    </tr>
+  </tbody> -->
+  <tbody>
+    <tr>
+      <td> 05 </td>
+      <td> <p align="center"><img src="data/ndt_lo/05/map.png" alt="animated" height="200" /></p> </td>
+      <td> <p align="center"><img src="data/ndt_lo/05/pose.png" alt="animated" height="200" /></p> </td>
+      <td> <p align="center"><img src="data/ndt_lo/05/ape_pose.png" alt="animated" height="200" /></p> </td>
+    </tr>
+  </tbody>
+</table>
+
+## 2. 基于ESKF的松耦合里程计
+
+<table>
+  <thead>
+    <tr>
+      <th colspan="6">Test Results On Datasets</th>
+    </tr>
+  </thead>
+  <!-- <tbody>
+    <tr align="center">
+      <td> 00 </td><td> 01 </td><td> 02 </td><td> 03 </td><td> 05 </td><td> 04 </td>
+    </tr>
+  </tbody> -->
+  <tbody>
+    <tr>
+      <th rowspan="3" scope="rowgroup">ulhk</th>
+      <th scope="row">ndt_lo</th>
+      <td> <p align="center"><img src="data/ndt_lo/ulhk/global_map.png" alt="animated" height="400" /></p> </td>
+      <td> <p align="center"><img src="data/ndt_lo/ulhk/drift.png" alt="animated" height="350" /></p> </td>
+    </tr>
+    <tr>
+      <th scope="row">loosely_lio</th>
+      <td> <p align="center"><img src="data/loose_lio/ulhk/global_map.png" alt="animated" height="400" /></p> </td>
+      <td> <p align="center"><img src="data/loose_lio/ulhk/drift.png" alt="animated" height="400" /></p> </td>
+    </tr>
+    <tr>
+      <th scope="row">dlo</th>
+      <td> <p align="center"><img src="data/dlo/ulhk.png" alt="animated" height="400" /></p> </td>
+      <td> <p align="center"><img src="data/dlo/little_drift.png" alt="animated" height="350" /></p> </td>
+    </tr>
+  </tbody>
+</table>

cmake/geographic.cmake

@@ -0,0 +1,3 @@
+find_package (GeographicLib REQUIRED)
+include_directories(${GeographicLib_INCLUDE_DIRS})
+list(APPEND ALL_TARGET_LIBRARIES ${GeographicLib_LIBRARIES})

config/velodyne_ulhk.yaml

@@ -0,0 +1,15 @@
+preprocess:
+  lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
+  scan_line: 32
+  time_scale: 1e3             # 兼容不同数据集的时间单位
+
+mapping:
+  extrinsic_T: [ 0, 0, -0.28 ]
+  extrinsic_R: [ 2.67949e-08,-1,  0,
+                 1,2.67949e-08,  0,
+                 0,          0,  1, ]
+
+point_filter_num: 10
+max_iteration: 3
+imu_has_gravity: false
+with_ui: false