[Feat/Refactor] MID360-Fisheye Support + Core Module Refactor (Params/Storage/Image) for FAST-LIVO2 ROS2 Humble

.gitignore

@@ -1,2 +1 @@
-Log/*
 build/*

CMakeLists.txt

@@ -1,6 +1,9 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.10)
 project(fast_livo)
 
+find_package(fmt REQUIRED)
+find_package(Boost REQUIRED COMPONENTS filesystem system)
+
 set(CMAKE_BUILD_TYPE "Release")
 message(STATUS "Build Type: ${CMAKE_BUILD_TYPE}")
 
@@ -75,73 +78,139 @@ else()
   message(STATUS "mimalloc not found, proceeding without it")
 endif()
 
-# Find catkin and required dependencies
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  nav_msgs
-  sensor_msgs
-  roscpp
-  rospy
-  std_msgs
-  pcl_ros
-  tf
-  message_generation
-  eigen_conversions
-  vikit_common
-  vikit_ros
-  cv_bridge
-  image_transport
-)
-
+# Find ament and required dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(livox_ros_driver2 REQUIRED)
+find_package(vikit_common REQUIRED)
+find_package(vikit_ros REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(Sophus REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
 
-set(Sophus_LIBRARIES libSophus.so)
-
-# Define the catkin package
-catkin_package(
-  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime cv_bridge vikit_common vikit_ros image_transport
-  DEPENDS EIGEN3 PCL OpenCV Sophus
-)
-
 # Include directories for dependencies
 include_directories(
-  ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
   ${Sophus_INCLUDE_DIRS}
+  ${vikit_common_INCLUDE_DIRS}
+  ${vikit_ros_INCLUDE_DIRS}
   include
 )
 
+set(dependencies
+  rclcpp
+  rclpy
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  visualization_msgs
+  cv_bridge
+  vikit_common
+  vikit_ros
+  image_transport
+  pcl_ros
+  pcl_conversions
+  tf2_ros
+  livox_ros_driver2
+)
+
+set(COMMON_DEPENDENCIES OpenMP::OpenMP_CXX)
+
+# link_directories(${COMMON_DEPENDENCIES}
+#   ${vikit_common_LIBRARIES}/libvikit_common.so
+#   ${vikit_ros_LIBRARIES}/libvikit_ros.so
+# )
+
 # Add libraries
 add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp)
 add_library(lio src/voxel_map.cpp)
 add_library(pre src/preprocess.cpp)
 add_library(imu_proc src/IMU_Processing.cpp)
 add_library(laser_mapping src/LIVMapper.cpp)
+add_library(utils src/utils.cpp)
+
+ament_target_dependencies(vio ${dependencies} )
+ament_target_dependencies(lio ${dependencies})
+ament_target_dependencies(pre ${dependencies})
+ament_target_dependencies(imu_proc ${dependencies})
+ament_target_dependencies(laser_mapping ${dependencies})
+
+# linking libraries or executables to public dependencies
+target_link_libraries(laser_mapping 
+    ${CMAKE_SOURCE_DIR}/../../install/vikit_common/lib/libvikit_common.so 
+    ${CMAKE_SOURCE_DIR}/../../install/vikit_ros/lib/libvikit_ros.so
+    ${COMMON_DEPENDENCIES}
+)
+target_link_libraries(vio ${COMMON_DEPENDENCIES})
+target_link_libraries(lio utils ${COMMON_DEPENDENCIES})
+target_link_libraries(pre ${COMMON_DEPENDENCIES})
+target_link_libraries(imu_proc ${COMMON_DEPENDENCIES})
 
 # Add the main executable
 add_executable(fastlivo_mapping src/main.cpp)
 
+ament_target_dependencies(fastlivo_mapping ${dependencies})
+
 # Link libraries to the executable
 target_link_libraries(fastlivo_mapping
   laser_mapping
   vio
   lio
   pre
   imu_proc
-  ${catkin_LIBRARIES}
   ${PCL_LIBRARIES}
   ${OpenCV_LIBRARIES}
   ${Sophus_LIBRARIES}
   ${Boost_LIBRARIES}
+  fmt::fmt
+  Boost::filesystem
 )
 
 # Link mimalloc if found
 if(mimalloc_FOUND)
   target_link_libraries(fastlivo_mapping mimalloc)
-endif()
+endif()
+
+# Install the executable
+install(TARGETS
+  fastlivo_mapping
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY config launch rviz_cfg
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Export dependencies
+ament_export_dependencies(
+  rclcpp
+  rclpy
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  pcl_ros
+  pcl_conversions
+  tf2_ros
+  livox_ros_driver2
+  Eigen3
+  PCL
+  OpenCV 
+  Sophus
+)
+
+ament_package()

Log/Colmap/sparse/0/cameras.txt

@@ -1,3 +1,3 @@
 # Camera list with one line of data per camera:
 #   CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]
-1 PINHOLE 640 512 588.143715 588.107927 296.059369 254.543215
+1 EQUIDISTANT 640 512 646.784720 646.657750 313.456795 261.399612

README.md

@@ -1,7 +1,14 @@
-# FAST-LIVO2
+# FAST-LIVO2 ROS2 HUMBLE
 
 ## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry
 
+### :star: Acknowledgments
+
+This project extends the following open-source contributions:
+
+- **Original work**: [FAST-LIVO2](https://github.com/hku-mars/FAST-LIVO2) by Chunran Zheng (HKU MARS Lab).
+- **ROS2 Humble port**: [FAST-LIVO2/tree/humble](https://github.com/Robotic-Developer-Road/FAST-LIVO2/tree/humble) by Robotic-Developer-Road.
+
 ### 📢 News
 
 - 🔓 **2025-01-23**: Code released!  
@@ -37,14 +44,17 @@ We open-source our handheld device, including CAD files, synchronization scheme,
 ### 1.4 Our associate dataset: FAST-LIVO2-Dataset
 Our associate dataset [**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z) used for evaluation is also available online.
 
+### MARS-LVIG dataset
+[**MARS-LVIG dataset**](https://mars.hku.hk/dataset.html)：A multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion.
+
 ### 1.5 Our LiDAR-camera calibration method
-The [**FAST-Calib**](https://github.com/hku-mars/FAST-Calib) toolkit is recommended. Its output extrinsic parameters can be directly filled into the YAML file. 
+The [**FAST-Calib**](https://github.com/hku-mars/FAST-Calib) toolkit is recommended. Its output extrinsic parameters can be directly filled into the YAML file.
 
 ## 2. Prerequisited
 
 ### 2.1 Ubuntu and ROS
 
-Ubuntu 18.04~20.04.  [ROS Installation](http://wiki.ros.org/ROS/Installation).
+Ubuntu 22.04.  [ROS Installation](http://wiki.ros.org/ROS/Installation).
 
 ### 2.2 PCL && Eigen && OpenCV
 
@@ -56,48 +66,113 @@ OpenCV>=4.2, Follow [Opencv Installation](http://opencv.org/).
 
 ### 2.3 Sophus
 
-Sophus Installation for the non-templated/double-only version.
-
+#### Binary installation
 ```bash
-git clone https://github.com/strasdat/Sophus.git
+cd ~
+git clone https://github.com/strasdat/Sophus.git -b 1.22.10
 cd Sophus
-git checkout a621ff
 mkdir build && cd build && cmake ..
 make
 sudo make install
 ```
 
 ### 2.4 Vikit
 
-Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder.
+Vikit provides essential camera models, math utilities, and interpolation functions. As an ament package for ROS2, download its source into your colcon workspace's src folder. Additionally, I've added OpenCV fisheye distortion correction to the equidistant camera model in vikit_common.
 
 ```bash
 # Different from the one used in fast-livo1
-cd catkin_ws/src
-git clone https://github.com/xuankuzcr/rpg_vikit.git 
+cd ~
+git clone https://github.com/Rhymer-Lcy/rpg_vikit_ros2_fisheye.git
+mkdir -p ~/fast/src/
+cp -r ./rpg_vikit_ros2_fisheye/{vikit_common,vikit_ros} ~/fast_ws/src/
+```
+
+Thanks to the following repositories for the code reference:
+
+- [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit)
+- [integralrobotics/rpg_vikit](https://github.com/integralrobotics/rpg_vikit)
+
+### 2.5 **livox_ros_driver2**
+
+Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2).
+
+```bash
+cd ~/fast_ws/src/
+git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
 ```
 
+why not use `livox_ros_driver`? Because it is not compatible with ROS2 directly. actually i am not think there s any difference between [livox ros driver](https://github.com/Livox-SDK/livox_ros_driver.git) and [livox ros driver2](https://github.com/Livox-SDK/livox_ros_driver2.git) 's `CustomMsg`, the latter 's ros2 version is sufficient.
+
 ## 3. Build
 
-Clone the repository and catkin_make:
+Clone the repository and colcon build:
 
 ```
-cd ~/catkin_ws/src
-git clone https://github.com/hku-mars/FAST-LIVO2
-cd ../
-catkin_make
-source ~/catkin_ws/devel/setup.bash
+git clone https://github.com/Rhymer-Lcy/FAST-LIVO2-ROS2-MID360-Fisheye.git
+cd livox_ros_driver2
+./build.sh humble
+source ~/fast_ws/install/setup.bash
 ```
 
 ## 4. Run our examples
 
 Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)). 
 
+### convert rosbag
+
+convert ROS1 rosbag to ROS2 rosbag
+```bash
+pip install rosbags
+rosbags-convert --src Retail_Street.bag --dst Retail_Street
 ```
-roslaunch fast_livo mapping_avia.launch
-rosbag play YOUR_DOWNLOADED.bag
+- [gitlab rosbags](https://gitlab.com/ternaris/rosbags)
+- [pypi rosbags](https://pypi.org/project/rosbags/)
+
+### change the msg type on rosbag
+
+Such as dataset `Retail_Street.db3`, because we use `livox_ros2_driver2`'s `CustomMsg`, we need to change the msg type in the rosbag file. 
+1. use `rosbags-convert` to convert rosbag from ROS1 to ROS2.
+2. change the msg type of msg type in **metadata.yaml** as follows:
+
+**metadata.yaml**
+```diff
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  custom_data: {}
+  duration:
+    nanoseconds: 135470252209
+  files:
+  - duration:
+      nanoseconds: 135470252209
+    message_count: 30157
+    path: Retail_Street.db3
+    ..............
+    topic_metadata:
+      name: /livox/lidar
+      offered_qos_profiles: ''
+      serialization_format: cdr
+-     type: livox_ros_driver/msg/CustomMsg
++     type: livox_ros_driver2/msg/CustomMsg
+      type_description_hash: RIHS01_94041b4794f52c1d81def2989107fc898a62dacb7a39d5dbe80d4b55e538bf6d
+    ...............
+.....
 ```
 
+### Run the demo
+
+Do not forget to `source` your ROS2 workspace before running the following command.
+
+```bash
+ros2 launch fast_livo mapping_aviz.launch.py use_rviz:=True use_sim_time:=True
+ros2 bag play -p Retail_Street  # Use space bar to play/pause
+```
+
+```bash
+ros2 launch fast_livo mapping_aviz_metacamedu.launch.py use_rviz:=True use_sim_time:=True  # Configuration for MID360-Fisheye dataset
+ros2 bag play -p $BAG_PATH  # Use space bar to play/pause (self-collected MID360-Fisheye dataset)
+```
 
 ## 5. License