Make it run with Closedloop Trajectory Tracking Benchmark

ivalab · Dec 13, 2024 · 93c4454 · 93c4454
1 parent 4452a3c
commit 93c4454
Show file tree

Hide file tree

Showing 28 changed files with 1,904 additions and 552 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
diff --git a/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt b/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt
@@ -30,13 +30,7 @@ endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)
 
-find_package(OpenCV 3.0 QUIET)
-if(NOT OpenCV_FOUND)
-   find_package(OpenCV 2.4.3 QUIET)
-   if(NOT OpenCV_FOUND)
-      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
-   endif()
-endif()
+find_package(OpenCV 4 QUIET)
 
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
@@ -55,29 +49,33 @@ ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
 ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
-${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM3.so
+# ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM3.so
+${PROJECT_NAME}
 -lboost_system
+-lGL
 )
 
-# Node for monocular camera
-rosbuild_add_executable(Mono
-src/ros_mono.cc
-)
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(Mono
-${LIBS}
-)
+# # Node for monocular camera
+# rosbuild_add_executable(Mono
+# src/ros_mono.cc
+# )
 
-# Node for monocular camera (Augmented Reality Demo)
-rosbuild_add_executable(MonoAR
-src/AR/ros_mono_ar.cc
-src/AR/ViewerAR.h
-src/AR/ViewerAR.cc
-)
+# target_link_libraries(Mono
+# ${LIBS}
+# )
 
-target_link_libraries(MonoAR
-${LIBS}
-)
+# # Node for monocular camera (Augmented Reality Demo)
+# rosbuild_add_executable(MonoAR
+# src/AR/ros_mono_ar.cc
+# src/AR/ViewerAR.h
+# src/AR/ViewerAR.cc
+# )
+
+# target_link_libraries(MonoAR
+# ${LIBS}
+# )
 
 # Node for stereo camera
 rosbuild_add_executable(Stereo
@@ -88,23 +86,23 @@ target_link_libraries(Stereo
 ${LIBS}
 )
 
-# Node for RGB-D camera
-rosbuild_add_executable(RGBD
-src/ros_rgbd.cc
-)
+# # Node for RGB-D camera
+# rosbuild_add_executable(RGBD
+# src/ros_rgbd.cc
+# )
 
-target_link_libraries(RGBD
-${LIBS}
-)
+# target_link_libraries(RGBD
+# ${LIBS}
+# )
 
-# Node for monocular-inertial camera
-rosbuild_add_executable(Mono_Inertial
-src/ros_mono_inertial.cc
-)
+# # Node for monocular-inertial camera
+# rosbuild_add_executable(Mono_Inertial
+# src/ros_mono_inertial.cc
+# )
 
-target_link_libraries(Mono_Inertial
-${LIBS}
-)
+# target_link_libraries(Mono_Inertial
+# ${LIBS}
+# )
 
 # Node for stereo-inertial camera
 rosbuild_add_executable(Stereo_Inertial

diff --git a/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i.launch b/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i.launch
@@ -0,0 +1,24 @@
+<launch>
+
+<arg name="path_slam_config" default = "/home/yanwei/closedloop_ws/src/ORB_Data/"/>
+<arg name="path_track_logging" default = "/tmp/orb3"/>
+<arg name="num_all_feature" default = "400" />
+<arg name="do_rectify" default = "false" />
+<arg name="do_vis" default = "0" />
+<arg name="slam_pose_topic" default="/visual/pose"/>
+
+
+<!-- call baselin ORB-SLAM for vision-based state estimation  -->
+<node pkg="ORB_SLAM3" type="Stereo" name="visual_slam" 
+    args="$(arg path_slam_config)/ORBvoc.bin 
+        $(arg path_slam_config)/TSRB_yaml/d435i_orb3.yaml 
+        $(arg num_all_feature) 
+        $(arg do_rectify) 
+        $(arg do_vis) 
+        /camera/infra1/image_rect_raw 
+        /camera/infra2/image_rect_raw 
+        $(arg path_track_logging)" output="screen">
+    <remap from="ORB_SLAM/camera_pose_in_imu" to="$(arg slam_pose_topic)"/>
+</node>
+
+</launch>
diff --git a/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i_inertial.launch b/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i_inertial.launch
@@ -0,0 +1,27 @@
+<launch>
+
+<arg name="path_slam_config" default = "/home/yanwei/closedloop_ws/src/ORB_Data/"/>
+<arg name="path_track_logging" default = "/tmp/orb3_imu"/>
+<arg name="num_all_feature" default = "400" />
+<arg name="do_rectify" default = "false" />
+<arg name="do_vis" default = "0" />
+<arg name="slam_pose_topic" default="/visual/pose"/>
+<arg name="imu_topic" default="/camera/imu"/>
+
+
+<!-- call baselin ORB-SLAM for vision-based state estimation  -->
+<node pkg="ORB_SLAM3" type="Stereo_Inertial" name="visual_slam" 
+    args="$(arg path_slam_config)/ORBvoc.bin 
+        $(arg path_slam_config)/TSRB_yaml/d435i_orb3_inertial.yaml 
+        $(arg num_all_feature) 
+        $(arg do_rectify) 
+        $(arg do_vis) 
+        /camera/infra1/image_rect_raw 
+        /camera/infra2/image_rect_raw 
+        $(arg path_track_logging)" output="screen">
+    <remap from="ORB_SLAM/camera_pose_in_imu" to="$(arg slam_pose_topic)"/>
+    <remap from="/imu" to="$(arg imu_topic)"/>
+</node>
+
+</launch>
+
diff --git a/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo.launch b/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo.launch
@@ -0,0 +1,16 @@
+<launch>
+
+<arg name="path_slam_config" default = "/home/yipuzhao/ros_workspace/package_dir/ORB_Data/"/>
+<arg name="path_track_logging" default = "/mnt/DATA/tmp/ClosedNav/debug"/>
+<arg name="num_all_feature" default = "1600" />
+<arg name="do_rectify" default = "true" />
+<arg name="do_vis" default = "0" />
+<arg name="slam_pose_topic" default="/visual/pose"/>
+
+
+<!-- call baselin ORB-SLAM for vision-based state estimation  -->
+<node pkg="ORB_SLAM3" type="Stereo" name="visual_slam" args="$(arg path_slam_config)/ORBvoc.bin $(arg path_slam_config)/Gazebo_yaml/pinhole_stereo_orb3.yaml $(arg num_all_feature) $(arg do_rectify) $(arg do_vis) /multisense_sl/camera/left/image_raw /multisense_sl/camera/right/image_raw $(arg path_track_logging)" output="screen">
+    <remap from="ORB_SLAM/camera_pose_in_imu" to="$(arg slam_pose_topic)"/>
+</node>
+
+</launch>
diff --git a/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo_inertial.launch b/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo_inertial.launch
@@ -0,0 +1,27 @@
+<launch>
+
+<arg name="path_slam_config" default = "/home/yanwei/closedloop_ws/src/ORB_Data/"/>
+<arg name="path_track_logging" default = "/mnt/DATA/tmp/ClosedNav/debug"/>
+<arg name="num_all_feature" default = "400" />
+<arg name="do_rectify" default = "false" />
+<arg name="do_vis" default = "0" />
+<arg name="slam_pose_topic" default="/visual/pose"/>
+<arg name="imu_topic" default="/imu0"/>
+
+
+<!-- call baselin ORB-SLAM for vision-based state estimation  -->
+<node pkg="ORB_SLAM3" type="Stereo_Inertial" name="visual_slam" 
+    args="$(arg path_slam_config)/ORBvoc.bin 
+        $(arg path_slam_config)/Gazebo_yaml/pinhole_stereo_orb3_inertial.yaml 
+        $(arg num_all_feature) 
+        $(arg do_rectify) 
+        $(arg do_vis) 
+        /multisense_sl/camera/left/image_raw 
+        /multisense_sl/camera/right/image_raw 
+        $(arg path_track_logging)" output="screen">
+    <remap from="ORB_SLAM/camera_pose_in_imu" to="$(arg slam_pose_topic)"/>
+    <remap from="/imu" to="$(arg imu_topic)"/>
+    <param name="align_map_with_odom" value="true" />
+</node>
+
+</launch>