From 93c4454592886a02ed9bac18287bffc0c0f5615e Mon Sep 17 00:00:00 2001 From: Yanwei Du Date: Fri, 13 Dec 2024 15:02:36 -0500 Subject: [PATCH] Make it run with Closedloop Trajectory Tracking Benchmark --- CMakeLists.txt | 449 +++++++-------- Examples_old/ROS/ORB_SLAM3/CMakeLists.txt | 74 ++- .../ROS/ORB_SLAM3/launch/ORB3_d435i.launch | 24 + .../launch/ORB3_d435i_inertial.launch | 27 + .../launch/gazebo_ORB3_stereo.launch | 16 + .../launch/gazebo_ORB3_stereo_inertial.launch | 27 + Examples_old/ROS/ORB_SLAM3/src/ros_stereo.cc | 327 ++++++++--- .../ROS/ORB_SLAM3/src/ros_stereo_inertial.cc | 516 ++++++++++-------- .../ORB_SLAM3/src/ros_stereo_inertial_old.cc | 286 ++++++++++ Examples_old/Stereo/EuRoC_feature.yaml | 96 ++++ README.md | 84 +++ Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h | 83 +++ build.sh | 2 +- build_ros.sh | 2 +- export_ros_path.bash | 1 + include/Frame.h | 9 + include/LocalMapping.h | 47 +- include/MacroDefinitions.h | 22 + include/ORBextractor.h | 2 + include/System.h | 8 +- include/Tracking.h | 151 +++++ src/Frame.cc | 4 + src/LocalMapping.cc | 17 +- src/LoopClosing.cc | 13 + src/Optimizer.cc | 2 + src/Sim3Solver.cc | 4 +- src/System.cc | 55 +- src/Tracking.cc | 108 +++- 28 files changed, 1904 insertions(+), 552 deletions(-) create mode 100644 Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i.launch create mode 100644 Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i_inertial.launch create mode 100644 Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo.launch create mode 100644 Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo_inertial.launch create mode 100644 Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial_old.cc create mode 100644 Examples_old/Stereo/EuRoC_feature.yaml create mode 100644 export_ros_path.bash create mode 100644 include/MacroDefinitions.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e74354d..a49cb9a4a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,13 @@ cmake_minimum_required(VERSION 2.8) project(ORB_SLAM3) +option(ENABLE_CLOSED_LOOP "Build Closed-Loop" ON) +if(ENABLE_CLOSED_LOOP) + add_definitions(-DENABLE_CLOSED_LOOP) +endif() + +option(BUILD_ROS "Build Closed-Loop" ON) + IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) ENDIF() @@ -30,17 +37,14 @@ endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 4.4) - if(NOT OpenCV_FOUND) - message(FATAL_ERROR "OpenCV > 4.4 not found.") - endif() +find_package(OpenCV 4 REQUIRED) MESSAGE("OPENCV VERSION:") MESSAGE(${OpenCV_VERSION}) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) -find_package(realsense2) +# find_package(realsense2) include_directories( ${PROJECT_SOURCE_DIR} @@ -123,6 +127,9 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so -lboost_serialization -lcrypto +-lglut +-lGL +-lGLEW ) # If RealSense SDK is found the library is added and its examples compiled @@ -138,253 +145,257 @@ endif() # Build examples -# RGB-D examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) +# # RGB-D examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) -add_executable(rgbd_tum - Examples/RGB-D/rgbd_tum.cc) -target_link_libraries(rgbd_tum ${PROJECT_NAME}) +# add_executable(rgbd_tum +# Examples/RGB-D/rgbd_tum.cc) +# target_link_libraries(rgbd_tum ${PROJECT_NAME}) -if(realsense2_FOUND) - add_executable(rgbd_realsense_D435i - Examples/RGB-D/rgbd_realsense_D435i.cc) - target_link_libraries(rgbd_realsense_D435i ${PROJECT_NAME}) -endif() +# if(realsense2_FOUND) +# add_executable(rgbd_realsense_D435i +# Examples/RGB-D/rgbd_realsense_D435i.cc) +# target_link_libraries(rgbd_realsense_D435i ${PROJECT_NAME}) +# endif() -# RGB-D inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D-Inertial) +# # RGB-D inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D-Inertial) -if(realsense2_FOUND) - add_executable(rgbd_inertial_realsense_D435i - Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) - target_link_libraries(rgbd_inertial_realsense_D435i ${PROJECT_NAME}) -endif() +# if(realsense2_FOUND) +# add_executable(rgbd_inertial_realsense_D435i +# Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) +# target_link_libraries(rgbd_inertial_realsense_D435i ${PROJECT_NAME}) +# endif() #Stereo examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) -add_executable(stereo_kitti - Examples/Stereo/stereo_kitti.cc) -target_link_libraries(stereo_kitti ${PROJECT_NAME}) - -add_executable(stereo_euroc - Examples/Stereo/stereo_euroc.cc) -target_link_libraries(stereo_euroc ${PROJECT_NAME}) - -add_executable(stereo_tum_vi - Examples/Stereo/stereo_tum_vi.cc) -target_link_libraries(stereo_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(stereo_realsense_t265 - Examples/Stereo/stereo_realsense_t265.cc) - target_link_libraries(stereo_realsense_t265 ${PROJECT_NAME}) - - add_executable(stereo_realsense_D435i - Examples/Stereo/stereo_realsense_D435i.cc) - target_link_libraries(stereo_realsense_D435i ${PROJECT_NAME}) -endif() - -#Monocular examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) - -add_executable(mono_tum - Examples/Monocular/mono_tum.cc) -target_link_libraries(mono_tum ${PROJECT_NAME}) - -add_executable(mono_kitti - Examples/Monocular/mono_kitti.cc) -target_link_libraries(mono_kitti ${PROJECT_NAME}) - -add_executable(mono_euroc - Examples/Monocular/mono_euroc.cc) -target_link_libraries(mono_euroc ${PROJECT_NAME}) - -add_executable(mono_tum_vi - Examples/Monocular/mono_tum_vi.cc) -target_link_libraries(mono_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(mono_realsense_t265 - Examples/Monocular/mono_realsense_t265.cc) - target_link_libraries(mono_realsense_t265 ${PROJECT_NAME}) - - add_executable(mono_realsense_D435i - Examples/Monocular/mono_realsense_D435i.cc) - target_link_libraries(mono_realsense_D435i ${PROJECT_NAME}) -endif() - -#Monocular inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular-Inertial) - -add_executable(mono_inertial_euroc - Examples/Monocular-Inertial/mono_inertial_euroc.cc) -target_link_libraries(mono_inertial_euroc ${PROJECT_NAME}) - -add_executable(mono_inertial_tum_vi - Examples/Monocular-Inertial/mono_inertial_tum_vi.cc) -target_link_libraries(mono_inertial_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(mono_inertial_realsense_t265 - Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc) - target_link_libraries(mono_inertial_realsense_t265 ${PROJECT_NAME}) - - add_executable(mono_inertial_realsense_D435i - Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc) - target_link_libraries(mono_inertial_realsense_D435i ${PROJECT_NAME}) -endif() - -#Stereo Inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo-Inertial) - -add_executable(stereo_inertial_euroc - Examples/Stereo-Inertial/stereo_inertial_euroc.cc) -target_link_libraries(stereo_inertial_euroc ${PROJECT_NAME}) - -add_executable(stereo_inertial_tum_vi - Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc) -target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(stereo_inertial_realsense_t265 - Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc) - target_link_libraries(stereo_inertial_realsense_t265 ${PROJECT_NAME}) - - add_executable(stereo_inertial_realsense_D435i - Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) - target_link_libraries(stereo_inertial_realsense_D435i ${PROJECT_NAME}) -endif() - -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Calibration) -if(realsense2_FOUND) - add_executable(recorder_realsense_D435i - Examples/Calibration/recorder_realsense_D435i.cc) - target_link_libraries(recorder_realsense_D435i ${PROJECT_NAME}) - - add_executable(recorder_realsense_T265 - Examples/Calibration/recorder_realsense_T265.cc) - target_link_libraries(recorder_realsense_T265 ${PROJECT_NAME}) -endif() +# add_executable(stereo_kitti +# Examples/Stereo/stereo_kitti.cc) +# target_link_libraries(stereo_kitti ${PROJECT_NAME}) + +# add_executable(stereo_euroc + # Examples/Stereo/stereo_euroc.cc) +# target_link_libraries(stereo_euroc ${PROJECT_NAME}) + +# add_executable(stereo_tum_vi +# Examples/Stereo/stereo_tum_vi.cc) +# target_link_libraries(stereo_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(stereo_realsense_t265 +# Examples/Stereo/stereo_realsense_t265.cc) +# target_link_libraries(stereo_realsense_t265 ${PROJECT_NAME}) + +# add_executable(stereo_realsense_D435i +# Examples/Stereo/stereo_realsense_D435i.cc) +# target_link_libraries(stereo_realsense_D435i ${PROJECT_NAME}) +# endif() + +# #Monocular examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) + +# add_executable(mono_tum +# Examples/Monocular/mono_tum.cc) +# target_link_libraries(mono_tum ${PROJECT_NAME}) + +# add_executable(mono_kitti +# Examples/Monocular/mono_kitti.cc) +# target_link_libraries(mono_kitti ${PROJECT_NAME}) + +# add_executable(mono_euroc +# Examples/Monocular/mono_euroc.cc) +# target_link_libraries(mono_euroc ${PROJECT_NAME}) + +# add_executable(mono_tum_vi +# Examples/Monocular/mono_tum_vi.cc) +# target_link_libraries(mono_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(mono_realsense_t265 +# Examples/Monocular/mono_realsense_t265.cc) +# target_link_libraries(mono_realsense_t265 ${PROJECT_NAME}) + +# add_executable(mono_realsense_D435i +# Examples/Monocular/mono_realsense_D435i.cc) +# target_link_libraries(mono_realsense_D435i ${PROJECT_NAME}) +# endif() + +# #Monocular inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular-Inertial) + +# add_executable(mono_inertial_euroc +# Examples/Monocular-Inertial/mono_inertial_euroc.cc) +# target_link_libraries(mono_inertial_euroc ${PROJECT_NAME}) + +# add_executable(mono_inertial_tum_vi +# Examples/Monocular-Inertial/mono_inertial_tum_vi.cc) +# target_link_libraries(mono_inertial_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(mono_inertial_realsense_t265 +# Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc) +# target_link_libraries(mono_inertial_realsense_t265 ${PROJECT_NAME}) + +# add_executable(mono_inertial_realsense_D435i +# Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc) +# target_link_libraries(mono_inertial_realsense_D435i ${PROJECT_NAME}) +# endif() + +# #Stereo Inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo-Inertial) + +# add_executable(stereo_inertial_euroc +# Examples/Stereo-Inertial/stereo_inertial_euroc.cc) +# target_link_libraries(stereo_inertial_euroc ${PROJECT_NAME}) + +# add_executable(stereo_inertial_tum_vi +# Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc) +# target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(stereo_inertial_realsense_t265 +# Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc) +# target_link_libraries(stereo_inertial_realsense_t265 ${PROJECT_NAME}) + +# add_executable(stereo_inertial_realsense_D435i +# Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) +# target_link_libraries(stereo_inertial_realsense_D435i ${PROJECT_NAME}) +# endif() + +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Calibration) +# if(realsense2_FOUND) +# add_executable(recorder_realsense_D435i +# Examples/Calibration/recorder_realsense_D435i.cc) +# target_link_libraries(recorder_realsense_D435i ${PROJECT_NAME}) + +# add_executable(recorder_realsense_T265 +# Examples/Calibration/recorder_realsense_T265.cc) +# target_link_libraries(recorder_realsense_T265 ${PROJECT_NAME}) +# endif() #Old examples -# RGB-D examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D) - -add_executable(rgbd_tum_old - Examples_old/RGB-D/rgbd_tum.cc) -target_link_libraries(rgbd_tum_old ${PROJECT_NAME}) - -if(realsense2_FOUND) - add_executable(rgbd_realsense_D435i_old - Examples_old/RGB-D/rgbd_realsense_D435i.cc) - target_link_libraries(rgbd_realsense_D435i_old ${PROJECT_NAME}) -endif() - +# # RGB-D examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D) -# RGB-D inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D-Inertial) +# add_executable(rgbd_tum_old +# Examples_old/RGB-D/rgbd_tum.cc) +# target_link_libraries(rgbd_tum_old ${PROJECT_NAME}) -if(realsense2_FOUND) - add_executable(rgbd_inertial_realsense_D435i_old - Examples_old/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) - target_link_libraries(rgbd_inertial_realsense_D435i_old ${PROJECT_NAME}) -endif() +# if(realsense2_FOUND) +# add_executable(rgbd_realsense_D435i_old +# Examples_old/RGB-D/rgbd_realsense_D435i.cc) +# target_link_libraries(rgbd_realsense_D435i_old ${PROJECT_NAME}) +# endif() -#Stereo examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo) - -add_executable(stereo_kitti_old - Examples_old/Stereo/stereo_kitti.cc) -target_link_libraries(stereo_kitti_old ${PROJECT_NAME}) -add_executable(stereo_euroc_old - Examples_old/Stereo/stereo_euroc.cc) -target_link_libraries(stereo_euroc_old ${PROJECT_NAME}) +# # RGB-D inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D-Inertial) -add_executable(stereo_tum_vi_old - Examples_old/Stereo/stereo_tum_vi.cc) -target_link_libraries(stereo_tum_vi_old ${PROJECT_NAME}) +# if(realsense2_FOUND) +# add_executable(rgbd_inertial_realsense_D435i_old +# Examples_old/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc) +# target_link_libraries(rgbd_inertial_realsense_D435i_old ${PROJECT_NAME}) +# endif() -if(realsense2_FOUND) - add_executable(stereo_realsense_t265_old - Examples_old/Stereo/stereo_realsense_t265.cc) - target_link_libraries(stereo_realsense_t265_old ${PROJECT_NAME}) +# #Stereo examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo) - add_executable(stereo_realsense_D435i_old - Examples_old/Stereo/stereo_realsense_D435i.cc) - target_link_libraries(stereo_realsense_D435i_old ${PROJECT_NAME}) -endif() +# add_executable(stereo_kitti_old +# Examples_old/Stereo/stereo_kitti.cc) +# target_link_libraries(stereo_kitti_old ${PROJECT_NAME}) -#Monocular examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular) +# add_executable(stereo_euroc_old +# Examples_old/Stereo/stereo_euroc.cc) +# target_link_libraries(stereo_euroc_old ${PROJECT_NAME}) -add_executable(mono_tum_old - Examples_old/Monocular/mono_tum.cc) -target_link_libraries(mono_tum_old ${PROJECT_NAME}) +# add_executable(stereo_tum_vi_old +# Examples_old/Stereo/stereo_tum_vi.cc) +# target_link_libraries(stereo_tum_vi_old ${PROJECT_NAME}) -add_executable(mono_kitti_old - Examples_old/Monocular/mono_kitti.cc) -target_link_libraries(mono_kitti_old ${PROJECT_NAME}) +# if(realsense2_FOUND) +# add_executable(stereo_realsense_t265_old +# Examples_old/Stereo/stereo_realsense_t265.cc) +# target_link_libraries(stereo_realsense_t265_old ${PROJECT_NAME}) -add_executable(mono_euroc_old - Examples_old/Monocular/mono_euroc.cc) -target_link_libraries(mono_euroc_old ${PROJECT_NAME}) +# add_executable(stereo_realsense_D435i_old +# Examples_old/Stereo/stereo_realsense_D435i.cc) +# target_link_libraries(stereo_realsense_D435i_old ${PROJECT_NAME}) +# endif() -add_executable(mono_tum_vi_old - Examples_old/Monocular/mono_tum_vi.cc) -target_link_libraries(mono_tum_vi_old ${PROJECT_NAME}) +# #Monocular examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular) -if(realsense2_FOUND) - add_executable(mono_realsense_t265_old - Examples_old/Monocular/mono_realsense_t265.cc) - target_link_libraries(mono_realsense_t265_old ${PROJECT_NAME}) +# add_executable(mono_tum_old +# Examples_old/Monocular/mono_tum.cc) +# target_link_libraries(mono_tum_old ${PROJECT_NAME}) - add_executable(mono_realsense_D435i_old - Examples_old/Monocular/mono_realsense_D435i.cc) - target_link_libraries(mono_realsense_D435i_old ${PROJECT_NAME}) -endif() +# add_executable(mono_kitti_old +# Examples_old/Monocular/mono_kitti.cc) +# target_link_libraries(mono_kitti_old ${PROJECT_NAME}) -#Monocular inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular-Inertial) +# add_executable(mono_euroc_old +# Examples_old/Monocular/mono_euroc.cc) +# target_link_libraries(mono_euroc_old ${PROJECT_NAME}) -add_executable(mono_inertial_euroc_old - Examples_old/Monocular-Inertial/mono_inertial_euroc.cc) -target_link_libraries(mono_inertial_euroc_old ${PROJECT_NAME}) +# add_executable(mono_tum_vi_old +# Examples_old/Monocular/mono_tum_vi.cc) +# target_link_libraries(mono_tum_vi_old ${PROJECT_NAME}) -add_executable(mono_inertial_tum_vi_old - Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc) -target_link_libraries(mono_inertial_tum_vi_old ${PROJECT_NAME}) +# if(realsense2_FOUND) +# add_executable(mono_realsense_t265_old +# Examples_old/Monocular/mono_realsense_t265.cc) +# target_link_libraries(mono_realsense_t265_old ${PROJECT_NAME}) -if(realsense2_FOUND) - add_executable(mono_inertial_realsense_t265_old - Examples_old/Monocular-Inertial/mono_inertial_realsense_t265.cc) - target_link_libraries(mono_inertial_realsense_t265_old ${PROJECT_NAME}) +# add_executable(mono_realsense_D435i_old +# Examples_old/Monocular/mono_realsense_D435i.cc) +# target_link_libraries(mono_realsense_D435i_old ${PROJECT_NAME}) +# endif() - add_executable(mono_inertial_realsense_D435i_old - Examples_old/Monocular-Inertial/mono_inertial_realsense_D435i.cc) - target_link_libraries(mono_inertial_realsense_D435i_old ${PROJECT_NAME}) -endif() +# #Monocular inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular-Inertial) -#Stereo Inertial examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo-Inertial) +# add_executable(mono_inertial_euroc_old +# Examples_old/Monocular-Inertial/mono_inertial_euroc.cc) +# target_link_libraries(mono_inertial_euroc_old ${PROJECT_NAME}) -add_executable(stereo_inertial_euroc_old - Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc) -target_link_libraries(stereo_inertial_euroc_old ${PROJECT_NAME}) +# add_executable(mono_inertial_tum_vi_old +# Examples_old/Monocular-Inertial/mono_inertial_tum_vi.cc) +# target_link_libraries(mono_inertial_tum_vi_old ${PROJECT_NAME}) -add_executable(stereo_inertial_tum_vi_old - Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc) -target_link_libraries(stereo_inertial_tum_vi_old ${PROJECT_NAME}) +# if(realsense2_FOUND) +# add_executable(mono_inertial_realsense_t265_old +# Examples_old/Monocular-Inertial/mono_inertial_realsense_t265.cc) +# target_link_libraries(mono_inertial_realsense_t265_old ${PROJECT_NAME}) -if(realsense2_FOUND) - add_executable(stereo_inertial_realsense_t265_old - Examples_old/Stereo-Inertial/stereo_inertial_realsense_t265.cc) - target_link_libraries(stereo_inertial_realsense_t265_old ${PROJECT_NAME}) +# add_executable(mono_inertial_realsense_D435i_old +# Examples_old/Monocular-Inertial/mono_inertial_realsense_D435i.cc) +# target_link_libraries(mono_inertial_realsense_D435i_old ${PROJECT_NAME}) +# endif() - add_executable(stereo_inertial_realsense_D435i_old - Examples_old/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) - target_link_libraries(stereo_inertial_realsense_D435i_old ${PROJECT_NAME}) -endif() +# #Stereo Inertial examples +# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Stereo-Inertial) + +# add_executable(stereo_inertial_euroc_old +# Examples_old/Stereo-Inertial/stereo_inertial_euroc.cc) +# target_link_libraries(stereo_inertial_euroc_old ${PROJECT_NAME}) + +# add_executable(stereo_inertial_tum_vi_old +# Examples_old/Stereo-Inertial/stereo_inertial_tum_vi.cc) +# target_link_libraries(stereo_inertial_tum_vi_old ${PROJECT_NAME}) + +# if(realsense2_FOUND) +# add_executable(stereo_inertial_realsense_t265_old +# Examples_old/Stereo-Inertial/stereo_inertial_realsense_t265.cc) +# target_link_libraries(stereo_inertial_realsense_t265_old ${PROJECT_NAME}) + +# add_executable(stereo_inertial_realsense_D435i_old +# Examples_old/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) +# target_link_libraries(stereo_inertial_realsense_D435i_old ${PROJECT_NAME}) +# endif() + +if (BUILD_ROS) + add_subdirectory(Examples_old/ROS/ORB_SLAM3) +endif() \ No newline at end of file diff --git a/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt b/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt index 12792fe44b..9f1c7dc7be 100644 --- 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 new file mode 100644 index 0000000000..acc1fdfed1 --- /dev/null +++ b/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + diff --git a/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i_inertial.launch b/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i_inertial.launch new file mode 100644 index 0000000000..85414dde11 --- /dev/null +++ b/Examples_old/ROS/ORB_SLAM3/launch/ORB3_d435i_inertial.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + diff --git a/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo.launch b/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo.launch new file mode 100644 index 0000000000..8213b685c3 --- /dev/null +++ b/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + 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 new file mode 100644 index 0000000000..c490ee0562 --- /dev/null +++ b/Examples_old/ROS/ORB_SLAM3/launch/gazebo_ORB3_stereo_inertial.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Examples_old/ROS/ORB_SLAM3/src/ros_stereo.cc b/Examples_old/ROS/ORB_SLAM3/src/ros_stereo.cc index 030fdb6af8..9a87e4e81b 100644 --- a/Examples_old/ROS/ORB_SLAM3/src/ros_stereo.cc +++ b/Examples_old/ROS/ORB_SLAM3/src/ros_stereo.cc @@ -32,6 +32,17 @@ #include"../../../include/System.h" +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "geometry_msgs/TransformStamped.h" +#include "tf/transform_datatypes.h" +#include + +// +//#include "path_smoothing_ros/cubic_spline_interpolator.h" +#include +#include + using namespace std; class ImageGrabber @@ -41,100 +52,153 @@ class ImageGrabber void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight); + void GrabOdom(const nav_msgs::OdometryConstPtr &msg); + ORB_SLAM3::System* mpSLAM; - bool do_rectify; - cv::Mat M1l,M2l,M1r,M2r; -}; -int main(int argc, char **argv) -{ - ros::init(argc, argv, "RGBD"); - ros::start(); + ros::Publisher mpCameraPosePublisher, mpCameraPoseInIMUPublisher; + // ros::Publisher mpDensePathPub; - if(argc != 4) - { - cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary path_to_settings do_rectify" << endl; - ros::shutdown(); - return 1; - } +#ifdef MAP_PUBLISH + size_t mnMapRefreshCounter; + ORB_SLAM2::MapPublisher* mpMapPub; +#endif - // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO,true); +// #ifdef FRAME_WITH_INFO_PUBLISH + ros::Publisher mpFrameWithInfoPublisher; +// #endif - ImageGrabber igb(&SLAM); + // Added by yanwei, save tracking latency + std::vector vTimesTrack; + std::vector > vStampedTimesTrack; - stringstream ss(argv[3]); - ss >> boolalpha >> igb.do_rectify; + void saveStats(const std::string& path_traj) + { + // Tracking time statistics + sort(vTimesTrack.begin(),vTimesTrack.end()); + float totaltime = 0; + int proccIm = vTimesTrack.size(); + for(int ni=0; ni> K_l; - fsSettings["RIGHT.K"] >> K_r; +}; - fsSettings["LEFT.P"] >> P_l; - fsSettings["RIGHT.P"] >> P_r; +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Stereo"); + ros::start(); - fsSettings["LEFT.R"] >> R_l; - fsSettings["RIGHT.R"] >> R_r; + if(argc < 8) + { + cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary path_to_settings num_all_feature do_rectify do_vis cam0_topic cam1_topic path_to_traj" << endl; + ros::shutdown(); + return 1; + } - fsSettings["LEFT.D"] >> D_l; - fsSettings["RIGHT.D"] >> D_r; + const bool do_viz = std::stoi(argv[5]); + std::cout << "viz: " << do_viz << std::endl; - int rows_l = fsSettings["LEFT.height"]; - int cols_l = fsSettings["LEFT.width"]; - int rows_r = fsSettings["RIGHT.height"]; - int cols_r = fsSettings["RIGHT.width"]; + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO, do_viz); + SLAM.mpTracker->updateORBExtractor(stoi(argv[3])); - if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || - rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) - { - cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; - return -1; - } + // realtime trajectory logging + std::string fNameRealTimeTrack = std::string(argv[8]) + "_AllFrameTrajectory.txt"; + std::cout << std::endl << "Saving AllFrame Trajectory to AllFrameTrajectory.txt" << std::endl; + SLAM.mpTracker->SetRealTimeFileStream(fNameRealTimeTrack); - cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l); - cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r); - } + ImageGrabber igb(&SLAM); ros::NodeHandle nh; - message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 1); - message_filters::Subscriber right_sub(nh, "/camera/right/image_raw", 1); + // message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 1); + // message_filters::Subscriber right_sub(nh, "/camera/right/image_raw", 1); + message_filters::Subscriber left_sub(nh, argv[6], 1); + message_filters::Subscriber right_sub(nh, argv[7], 1); typedef message_filters::sync_policies::ApproximateTime sync_pol; - message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); + message_filters::Synchronizer sync(sync_pol(2), left_sub,right_sub); sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2)); - ros::spin(); + igb.mpCameraPosePublisher = nh.advertise("ORB_SLAM/camera_pose", 100); + igb.mpCameraPoseInIMUPublisher = nh.advertise("ORB_SLAM/camera_pose_in_imu", 100); + ros::Subscriber sub = nh.subscribe("/odom_dummy", 100, &ImageGrabber::GrabOdom, &igb); - // Stop all threads - SLAM.Shutdown(); +// #ifdef FRAME_WITH_INFO_PUBLISH + igb.mpFrameWithInfoPublisher = nh.advertise("ORB_SLAM/frame_with_info", 100); +// #endif + while(ros::ok()) + ros::spin(); + // ros::spin(); + + cout << "ros_stereo: done with spin!" << endl; + + // save stats + igb.saveStats(std::string(argv[8])); // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt"); - SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); - SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); + SLAM.SaveKeyFrameTrajectoryTUM(std::string(argv[8]) + "_KeyFrameTrajectory.txt"); + SLAM.SaveTrackingLog(std::string(argv[8]) + "_Log.txt" ); + SLAM.SaveMappingLog(std::string(argv[8]) + "_Log_Mapping.txt"); + // SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); + // SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); + + std::cout << "Finished saving!" << std::endl; ros::shutdown(); + cout << "ros_stereo: done with ros Shutdown!" << endl; + + // Stop all threads + SLAM.Shutdown(); + cout << "ros_stereo: done with SLAM Shutdown!" << endl; + return 0; } void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight) { +#ifdef ENABLE_CLOSED_LOOP + // @NOTE (yanwei) throw the first few garbage images from gazebo + static size_t skip_imgs = 0; + if (skip_imgs < 10) + { + ++skip_imgs; + return; + } +#endif + + const double latency_trans = ros::Time::now().toSec() - msgLeft->header.stamp.toSec(); + // Copy the ros image message to cv::Mat. cv_bridge::CvImageConstPtr cv_ptrLeft; try { - cv_ptrLeft = cv_bridge::toCvShare(msgLeft); + cv_ptrLeft = cv_bridge::toCvShare(msgLeft, "mono8"); } catch (cv_bridge::Exception& e) { @@ -145,7 +209,7 @@ void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const se cv_bridge::CvImageConstPtr cv_ptrRight; try { - cv_ptrRight = cv_bridge::toCvShare(msgRight); + cv_ptrRight = cv_bridge::toCvShare(msgRight, "mono8"); } catch (cv_bridge::Exception& e) { @@ -153,18 +217,151 @@ void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const se return; } - if(do_rectify) + // tracking + Sophus::SE3f pose = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); + + // collect latency + double latency_total = ros::Time::now().toSec() - cv_ptrLeft->header.stamp.toSec(); { - cv::Mat imLeft, imRight; - cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR); - cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR); - mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()); + const double track_latency = latency_total - latency_trans; + vTimesTrack.emplace_back(track_latency); + vStampedTimesTrack.emplace_back(cv_ptrLeft->header.stamp.toSec(), track_latency); } - else + + if (mpSLAM->mpTracker->mState != 2) // OK { - mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); + return; } +// #ifdef ENABLE_CLOSED_LOOP +// ROS_INFO("Pose Tracking Latency: %.03f sec", latency_total - latency_trans); +// #endif + + // prepare for publishing + const Sophus::SE3f Twc = pose.inverse(); + const Eigen::Matrix Rwc = Twc.rotationMatrix(); + const Eigen::Matrix twc = Twc.translation(); + + tf::Matrix3x3 M( + Rwc(0,0), Rwc(0,1), Rwc(0,2), + Rwc(1,0), Rwc(1,1), Rwc(1,2), + Rwc(2,0), Rwc(2,1), Rwc(2,2)); + tf::Vector3 V(twc(0), twc(1), twc(2)); + + tf::Transform tfTcw(M,V); + geometry_msgs::Transform gmTwc; + tf::transformTFToMsg(tfTcw, gmTwc); + + geometry_msgs::Pose camera_pose; + camera_pose.position.x = gmTwc.translation.x; + camera_pose.position.y = gmTwc.translation.y; + camera_pose.position.z = gmTwc.translation.z; + camera_pose.orientation = gmTwc.rotation; + + geometry_msgs::PoseWithCovarianceStamped camera_odom; + camera_odom.header.frame_id = "odom"; + camera_odom.header.stamp = cv_ptrLeft->header.stamp; + camera_odom.pose.pose = camera_pose; + + mpCameraPosePublisher.publish(camera_odom); + +// +// by default, an additional transform is applied to make camera pose and body frame aligned +// which is assumed in msf +#ifdef INIT_WITH_ARUCHO + tf::Matrix3x3 Ric( 0, -1, 0, + 0, 0, -1, + 1, 0, 0); +/* tf::Matrix3x3 Ric( 0, 0, 1, + -1, 0, 0, + 0, -1, 0);*/ + tf::Transform tfTiw ( tf::Matrix3x3( tfTcw.getRotation() ) * Ric, tfTcw.getOrigin() ); +#else + tf::Matrix3x3 Ric( 0, 0, 1, + -1, 0, 0, + 0, -1, 0); + tf::Transform tfTiw ( Ric * tf::Matrix3x3( tfTcw.getRotation() ), Ric * tfTcw.getOrigin() ); +#endif + + geometry_msgs::Transform gmTwi; + tf::transformTFToMsg(tfTiw, gmTwi); + + geometry_msgs::Pose camera_pose_in_imu; + camera_pose_in_imu.position.x = gmTwi.translation.x; + camera_pose_in_imu.position.y = gmTwi.translation.y; + camera_pose_in_imu.position.z = gmTwi.translation.z; + camera_pose_in_imu.orientation = gmTwi.rotation; + + geometry_msgs::PoseWithCovarianceStamped camera_odom_in_imu; + camera_odom_in_imu.header.frame_id = "map"; + camera_odom_in_imu.header.stamp = cv_ptrLeft->header.stamp; + camera_odom_in_imu.pose.pose = camera_pose_in_imu; + + mpCameraPoseInIMUPublisher.publish(camera_odom_in_imu); + +/* + tf::Matrix3x3 Ric( 0, 0, 1, + -1, 0, 0, + 0, -1, 0); + + tf::Matrix3x3 Rbi( 0, -1, 0, + 0, 0, -1, + 1, 0, 0); + + tf::Transform tfTiw ( Ric * tf::Matrix3x3( tfTcw.getRotation() ) * Rbi, Ric * tfTcw.getOrigin() ); + geometry_msgs::Transform gmTwi; + tf::transformTFToMsg(tfTiw, gmTwi); + + geometry_msgs::Pose camera_pose_in_imu; + camera_pose_in_imu.position.x = gmTwi.translation.x; + camera_pose_in_imu.position.y = gmTwi.translation.y; + camera_pose_in_imu.position.z = gmTwi.translation.z; + camera_pose_in_imu.orientation = gmTwi.rotation; + + geometry_msgs::PoseWithCovarianceStamped camera_odom_in_imu; + camera_odom_in_imu.header.frame_id = "odom"; + camera_odom_in_imu.header.stamp = cv_ptrLeft->header.stamp; + camera_odom_in_imu.pose.pose = camera_pose_in_imu; + + mpCameraPoseInIMUPublisher.publish(camera_odom_in_imu); +*/ + +// #ifdef FRAME_WITH_INFO_PUBLISH + if (mpSLAM != NULL && mpSLAM->mpFrameDrawer != NULL) { + cv::Mat fr_info_cv = mpSLAM->mpFrameDrawer->DrawFrame(); + cv_bridge::CvImage out_msg; + out_msg.header = cv_ptrLeft->header; // Same timestamp and tf frame as input image + out_msg.encoding = sensor_msgs::image_encodings::BGR8; // Or whatever + out_msg.image = fr_info_cv; // Your cv::Mat + mpFrameWithInfoPublisher.publish(out_msg.toImageMsg()); + } +// #endif +} + + +void ImageGrabber::GrabOdom(const nav_msgs::Odometry::ConstPtr &msg) { + /* + ROS_INFO("Seq: [%d]", msg->header.seq); + ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", + msg->pose.pose.position.x,msg->pose.pose.position.y, + msg->pose.pose.position.z); ROS_INFO("Orientation-> x: [%f], y: [%f], z: + [%f], w: [%f]", msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); ROS_INFO("Vel-> + Linear: [%f], Angular: [%f]", + msg->twist.twist.linear.x,msg->twist.twist.angular.z); + */ + + // TODO + mpSLAM->mpTracker->motion_model_.append( + {msg->header.stamp.toSec(), + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z, + msg->pose.pose.orientation.w, + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z} + ); } diff --git a/Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc b/Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc index 231ad72c07..884376b3b5 100644 --- a/Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc +++ b/Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc @@ -16,271 +16,361 @@ * If not, see . */ + #include #include #include #include -#include -#include -#include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include"../../../include/System.h" -#include"../include/ImuTypes.h" - -using namespace std; -class ImuGrabber -{ -public: - ImuGrabber(){}; - void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg); +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "geometry_msgs/TransformStamped.h" +#include "tf/transform_datatypes.h" +#include +#include +#include +#include +#include +#include + +// +//#include "path_smoothing_ros/cubic_spline_interpolator.h" +#include +#include - queue imuBuf; - std::mutex mBufMutex; -}; +using namespace std; class ImageGrabber { public: - ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bRect, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe){} + ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM), tfli_(tf_buffer_) {} + + void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight); + void GrabImu(const sensor_msgs::ImuConstPtr& msg); + Eigen::Matrix4f Ros2Eigen(const geometry_msgs::Transform& tf); + Sophus::SE3f PostProcess(const ros::Time& time, const Sophus::SE3f& pose); - void GrabImageLeft(const sensor_msgs::ImageConstPtr& msg); - void GrabImageRight(const sensor_msgs::ImageConstPtr& msg); - cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg); - void SyncWithImu(); + std::mutex imuMutex_; + boost::circular_buffer imus_{100}; + double last_image_timestamp_ = 0.0; - queue imgLeftBuf, imgRightBuf; - std::mutex mBufMutexLeft,mBufMutexRight; - ORB_SLAM3::System* mpSLAM; - ImuGrabber *mpImuGb; - const bool do_rectify; - cv::Mat M1l,M2l,M1r,M2r; + ros::Publisher mpCameraPosePublisher, mpCameraPoseInIMUPublisher; + // ros::Publisher mpDensePathPub; - const bool mbClahe; - cv::Ptr mClahe = cv::createCLAHE(3.0, cv::Size(8, 8)); -}; +#ifdef MAP_PUBLISH + size_t mnMapRefreshCounter; + ORB_SLAM2::MapPublisher* mpMapPub; +#endif +// #ifdef FRAME_WITH_INFO_PUBLISH + ros::Publisher mpFrameWithInfoPublisher; +// #endif + tf2_ros::StaticTransformBroadcaster static_br_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformBroadcaster tfbr_; + tf2_ros::TransformListener tfli_; + bool align_map_with_odom_ = true; -int main(int argc, char **argv) -{ - ros::init(argc, argv, "Stereo_Inertial"); - ros::NodeHandle n("~"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - bool bEqual = false; - if(argc < 4 || argc > 5) - { - cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl; - ros::shutdown(); - return 1; - } - - std::string sbRect(argv[3]); - if(argc==5) - { - std::string sbEqual(argv[4]); - if(sbEqual == "true") - bEqual = true; - } - - // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO,true); - - ImuGrabber imugb; - ImageGrabber igb(&SLAM,&imugb,sbRect == "true",bEqual); - - if(igb.do_rectify) - { - // Load settings related to stereo calibration - cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); - if(!fsSettings.isOpened()) + // Added by yanwei, save tracking latency + std::vector vTimesTrack; + std::vector > vStampedTimesTrack; + + void saveStats(const std::string& path_traj) + { + // Tracking time statistics + sort(vTimesTrack.begin(),vTimesTrack.end()); + float totaltime = 0; + int proccIm = vTimesTrack.size(); + for(int ni=0; ni> K_l; - fsSettings["RIGHT.K"] >> K_r; + // save to stats + { + std::ofstream myfile(path_traj + "_stats.txt"); + myfile << "#dummy processed_img_num mean_time median_time min_time max_time\n"; + myfile << std::setprecision(6) << -1 << " " + << proccIm << " " + << totaltime / proccIm << " " + << vTimesTrack[proccIm/2] << " " + << vTimesTrack.front() << " " + << vTimesTrack.back() << " "; + myfile.close(); + + myfile.open(path_traj + "_Log_Latency.txt"); + myfile << "#timestamp tracking_time\n"; + myfile << std::setprecision(20); + for (const auto& m : vStampedTimesTrack) + { + myfile << m.first << " " << m.second << "\n"; + } + myfile.close(); + } + } - fsSettings["LEFT.P"] >> P_l; - fsSettings["RIGHT.P"] >> P_r; +}; - fsSettings["LEFT.R"] >> R_l; - fsSettings["RIGHT.R"] >> R_r; +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Stereo"); + ros::start(); - fsSettings["LEFT.D"] >> D_l; - fsSettings["RIGHT.D"] >> D_r; + if(argc < 8) + { + cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary path_to_settings num_all_feature do_rectify do_vis cam0_topic cam1_topic path_to_traj" << endl; + ros::shutdown(); + return 1; + } - int rows_l = fsSettings["LEFT.height"]; - int cols_l = fsSettings["LEFT.width"]; - int rows_r = fsSettings["RIGHT.height"]; - int cols_r = fsSettings["RIGHT.width"]; + const bool do_viz = std::stoi(argv[5]); + std::cout << "viz: " << do_viz << std::endl; - if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || - rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) - { - cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; - return -1; - } + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, do_viz); + SLAM.mpTracker->updateORBExtractor(stoi(argv[3])); - cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l); - cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r); - } + // realtime trajectory logging + std::string fNameRealTimeTrack = std::string(argv[8]) + "_AllFrameTrajectory.txt"; + std::cout << std::endl << "Saving AllFrame Trajectory to AllFrameTrajectory.txt" << std::endl; + SLAM.mpTracker->SetRealTimeFileStream(fNameRealTimeTrack); - // Maximum delay, 5 seconds - ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); - ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft,&igb); - ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight,&igb); + ImageGrabber igb(&SLAM); - std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb); + ros::NodeHandle nh; - ros::spin(); + ros::Subscriber sub_imu = nh.subscribe("/imu", 1000, &ImageGrabber::GrabImu, &igb); + // message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 1); + // message_filters::Subscriber right_sub(nh, "/camera/right/image_raw", 1); + message_filters::Subscriber left_sub(nh, argv[6], 1); + message_filters::Subscriber right_sub(nh, argv[7], 1); + typedef message_filters::sync_policies::ApproximateTime sync_pol; + message_filters::Synchronizer sync(sync_pol(2), left_sub,right_sub); + sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2)); - return 0; -} + igb.mpCameraPosePublisher = nh.advertise("ORB_SLAM/camera_pose", 100); + igb.mpCameraPoseInIMUPublisher = nh.advertise("ORB_SLAM/camera_pose_in_imu", 100); +// #ifdef FRAME_WITH_INFO_PUBLISH + igb.mpFrameWithInfoPublisher = nh.advertise("ORB_SLAM/frame_with_info", 100); + nh.param("align_map_with_odom", igb.align_map_with_odom_, "false"); +// #endif + while(ros::ok()) + ros::spin(); + // ros::spin(); + cout << "ros_stereo: done with spin!" << endl; -void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg) -{ - mBufMutexLeft.lock(); - if (!imgLeftBuf.empty()) - imgLeftBuf.pop(); - imgLeftBuf.push(img_msg); - mBufMutexLeft.unlock(); -} + // save stats + igb.saveStats(std::string(argv[8])); -void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg) -{ - mBufMutexRight.lock(); - if (!imgRightBuf.empty()) - imgRightBuf.pop(); - imgRightBuf.push(img_msg); - mBufMutexRight.unlock(); + // Save camera trajectory + SLAM.SaveKeyFrameTrajectoryTUM(std::string(argv[8]) + "_KeyFrameTrajectory.txt"); + SLAM.SaveTrackingLog(std::string(argv[8]) + "_Log.txt" ); + SLAM.SaveMappingLog(std::string(argv[8]) + "_Log_Mapping.txt"); + // SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt"); + // SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt"); + + std::cout << "Finished saving!" << std::endl; + + ros::shutdown(); + + cout << "ros_stereo: done with ros Shutdown!" << endl; + + // Stop all threads + SLAM.Shutdown(); + cout << "ros_stereo: done with SLAM Shutdown!" << endl; + + return 0; } -cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg) +void ImageGrabber::GrabImu(const sensor_msgs::ImuConstPtr &msg) { - // Copy the ros image message to cv::Mat. - cv_bridge::CvImageConstPtr cv_ptr; - try - { - cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - } - - if(cv_ptr->image.type()==0) - { - return cv_ptr->image.clone(); - } - else - { - std::cout << "Error type" << std::endl; - return cv_ptr->image.clone(); - } + std::unique_lock lock(imuMutex_); + imus_.push_back(*msg); } -void ImageGrabber::SyncWithImu() +void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight) { - const double maxTimeDiff = 0.01; - while(1) - { - cv::Mat imLeft, imRight; - double tImLeft = 0, tImRight = 0; - if (!imgLeftBuf.empty()&&!imgRightBuf.empty()&&!mpImuGb->imuBuf.empty()) +#ifdef ENABLE_CLOSED_LOOP + // @NOTE (yanwei) throw the first few garbage images from gazebo + static size_t skip_imgs = 0; + if (skip_imgs < 10) { - tImLeft = imgLeftBuf.front()->header.stamp.toSec(); - tImRight = imgRightBuf.front()->header.stamp.toSec(); - - this->mBufMutexRight.lock(); - while((tImLeft-tImRight)>maxTimeDiff && imgRightBuf.size()>1) - { - imgRightBuf.pop(); - tImRight = imgRightBuf.front()->header.stamp.toSec(); - } - this->mBufMutexRight.unlock(); - - this->mBufMutexLeft.lock(); - while((tImRight-tImLeft)>maxTimeDiff && imgLeftBuf.size()>1) - { - imgLeftBuf.pop(); - tImLeft = imgLeftBuf.front()->header.stamp.toSec(); - } - this->mBufMutexLeft.unlock(); - - if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff) - { - // std::cout << "big time difference" << std::endl; - continue; - } - if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec()) - continue; - - this->mBufMutexLeft.lock(); - imLeft = GetImage(imgLeftBuf.front()); - imgLeftBuf.pop(); - this->mBufMutexLeft.unlock(); - - this->mBufMutexRight.lock(); - imRight = GetImage(imgRightBuf.front()); - imgRightBuf.pop(); - this->mBufMutexRight.unlock(); - - vector vImuMeas; - mpImuGb->mBufMutex.lock(); - if(!mpImuGb->imuBuf.empty()) - { - // Load imu measurements from buffer - vImuMeas.clear(); - while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tImLeft) - { - double t = mpImuGb->imuBuf.front()->header.stamp.toSec(); - cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z); - cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z); - vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t)); - mpImuGb->imuBuf.pop(); + ++skip_imgs; + return; + } +#endif + + const double latency_trans = ros::Time::now().toSec() - msgLeft->header.stamp.toSec(); + + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptrLeft; + try + { + cv_ptrLeft = cv_bridge::toCvShare(msgLeft, "mono8"); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + cv_bridge::CvImageConstPtr cv_ptrRight; + try + { + cv_ptrRight = cv_bridge::toCvShare(msgRight, "mono8"); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // retrieve imus + double current_image_timestamp = cv_ptrLeft->header.stamp.toSec(); + vector vImuMeas; + { + std::unique_lock lock(imuMutex_); + int n_imus = 0; + for (size_t i = 0; i < imus_.size(); ++i) { + const auto& imu = imus_[i]; + double imu_timestamp = imu.header.stamp.toSec(); + // Skip imu msg that is earlier than the previous odom + if (imu_timestamp <= last_image_timestamp_) continue; + if (imu_timestamp > current_image_timestamp) continue; + const cv::Point3f acc(imu.linear_acceleration.x, + imu.linear_acceleration.y, + imu.linear_acceleration.z); + const cv::Point3f gyr(imu.angular_velocity.x, + imu.angular_velocity.y, + imu.angular_velocity.z); + vImuMeas.emplace_back(acc, gyr, imu_timestamp); } - } - mpImuGb->mBufMutex.unlock(); - if(mbClahe) - { - mClahe->apply(imLeft,imLeft); - mClahe->apply(imRight,imRight); - } - - if(do_rectify) - { - cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR); - cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR); - } - - mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas); - - std::chrono::milliseconds tSleep(1); - std::this_thread::sleep_for(tSleep); + // std::cout << "found imus: " << vImuMeas.size() << std::endl; } - } + last_image_timestamp_ = current_image_timestamp; + + // tracking + Sophus::SE3f pose = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec(), vImuMeas); + + // collect latency + double latency_total = ros::Time::now().toSec() - cv_ptrLeft->header.stamp.toSec(); + { + const double track_latency = latency_total - latency_trans; + vTimesTrack.emplace_back(track_latency); + vStampedTimesTrack.emplace_back(cv_ptrLeft->header.stamp.toSec(), track_latency); + } + + if (mpSLAM->mpTracker->mState != 2) // OK + { + std::cout << "track is NOT ready" << std::endl; + return; + } + if (!(mpSLAM->mpLocalMapper->IMU_INIT_1 && + mpSLAM->mpLocalMapper->IMU_INIT_2)) + { + std::cout << "IMU INIT is NOT ready" << std::endl; + return; + } + +// #ifdef ENABLE_CLOSED_LOOP +// ROS_INFO("Pose Tracking Latency: %.03f sec", latency_total - latency_trans); +// #endif + + // Convert the pose from Tcw to Twc. + pose = pose.inverse(); + + // by default, an additional transform is applied to make camera pose and body frame aligned + // In inertial mode, the returned pose is in a frame with z staying vertically. + // https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/409 + { + const Sophus::SE3f Tbi( + (Eigen::Matrix3f() << 0, 1, 0, -1, 0, 0, 0, 0, 1).finished(), Eigen::Vector3f::Zero() + ); + pose = Tbi * pose; + } + + if (align_map_with_odom_) + { + pose = PostProcess(cv_ptrLeft->header.stamp, pose); + } + + const Eigen::Vector3f t = pose.translation(); + const Eigen::Quaternionf q = pose.unit_quaternion(); + + geometry_msgs::Pose camera_pose_in_imu; + camera_pose_in_imu.position.x = t.x(); + camera_pose_in_imu.position.y = t.y(); + camera_pose_in_imu.position.z = t.z(); + camera_pose_in_imu.orientation.x = q.x(); + camera_pose_in_imu.orientation.y = q.y(); + camera_pose_in_imu.orientation.z = q.z(); + camera_pose_in_imu.orientation.w = q.w(); + + geometry_msgs::PoseWithCovarianceStamped camera_odom_in_imu; + camera_odom_in_imu.header.frame_id = "map"; + camera_odom_in_imu.header.stamp = cv_ptrLeft->header.stamp; + camera_odom_in_imu.pose.pose = camera_pose_in_imu; + + mpCameraPoseInIMUPublisher.publish(camera_odom_in_imu); + +// #ifdef FRAME_WITH_INFO_PUBLISH + if (mpSLAM != NULL && mpSLAM->mpFrameDrawer != NULL) { + cv::Mat fr_info_cv = mpSLAM->mpFrameDrawer->DrawFrame(); + cv_bridge::CvImage out_msg; + out_msg.header = cv_ptrLeft->header; // Same timestamp and tf frame as input image + out_msg.encoding = sensor_msgs::image_encodings::BGR8; // Or whatever + out_msg.image = fr_info_cv; // Your cv::Mat + mpFrameWithInfoPublisher.publish(out_msg.toImageMsg()); + } +// #endif } -void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg) + +Sophus::SE3f ImageGrabber::PostProcess(const ros::Time& time, const Sophus::SE3f& pose) { - mBufMutex.lock(); - imuBuf.push(imu_msg); - mBufMutex.unlock(); - return; -} + const Eigen::Matrix4f& Twc = pose.matrix(); + // Define map that aligns with base_frame + static bool is_map_defined = false; + static Eigen::Matrix4f Tmw = Eigen::Matrix4f::Identity(); + if (!is_map_defined) { + geometry_msgs::TransformStamped odom_to_gyro = tf_buffer_.lookupTransform( + "odom", "gyro_link", time, ros::Duration(0.2)); + Tmw = Ros2Eigen(odom_to_gyro.transform) * Twc.inverse(); + is_map_defined = true; + } + // Publish map to odom. + const Eigen::Matrix4f Tmc = Tmw * Twc; + return Sophus::SE3f(Tmc); +} +Eigen::Matrix4f ImageGrabber::Ros2Eigen(const geometry_msgs::Transform& tf) +{ + Eigen::Matrix4f out = Eigen::Matrix4f::Identity(); + out.topLeftCorner(3, 3) = Eigen::Quaternionf( + tf.rotation.w, + tf.rotation.x, + tf.rotation.y, + tf.rotation.z + ).toRotationMatrix(); + out.topRightCorner(3, 1) = Eigen::Vector3f(tf.translation.x, tf.translation.y, tf.translation.z); + return out; +} \ No newline at end of file diff --git a/Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial_old.cc b/Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial_old.cc new file mode 100644 index 0000000000..231ad72c07 --- /dev/null +++ b/Examples_old/ROS/ORB_SLAM3/src/ros_stereo_inertial_old.cc @@ -0,0 +1,286 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. +* +* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public +* License as published by the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even +* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License along with ORB-SLAM3. +* If not, see . +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include"../../../include/System.h" +#include"../include/ImuTypes.h" + +using namespace std; + +class ImuGrabber +{ +public: + ImuGrabber(){}; + void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg); + + queue imuBuf; + std::mutex mBufMutex; +}; + +class ImageGrabber +{ +public: + ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bRect, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe){} + + void GrabImageLeft(const sensor_msgs::ImageConstPtr& msg); + void GrabImageRight(const sensor_msgs::ImageConstPtr& msg); + cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg); + void SyncWithImu(); + + queue imgLeftBuf, imgRightBuf; + std::mutex mBufMutexLeft,mBufMutexRight; + + ORB_SLAM3::System* mpSLAM; + ImuGrabber *mpImuGb; + + const bool do_rectify; + cv::Mat M1l,M2l,M1r,M2r; + + const bool mbClahe; + cv::Ptr mClahe = cv::createCLAHE(3.0, cv::Size(8, 8)); +}; + + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Stereo_Inertial"); + ros::NodeHandle n("~"); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); + bool bEqual = false; + if(argc < 4 || argc > 5) + { + cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl; + ros::shutdown(); + return 1; + } + + std::string sbRect(argv[3]); + if(argc==5) + { + std::string sbEqual(argv[4]); + if(sbEqual == "true") + bEqual = true; + } + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO,true); + + ImuGrabber imugb; + ImageGrabber igb(&SLAM,&imugb,sbRect == "true",bEqual); + + if(igb.do_rectify) + { + // Load settings related to stereo calibration + cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); + if(!fsSettings.isOpened()) + { + cerr << "ERROR: Wrong path to settings" << endl; + return -1; + } + + cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; + fsSettings["LEFT.K"] >> K_l; + fsSettings["RIGHT.K"] >> K_r; + + fsSettings["LEFT.P"] >> P_l; + fsSettings["RIGHT.P"] >> P_r; + + fsSettings["LEFT.R"] >> R_l; + fsSettings["RIGHT.R"] >> R_r; + + fsSettings["LEFT.D"] >> D_l; + fsSettings["RIGHT.D"] >> D_r; + + int rows_l = fsSettings["LEFT.height"]; + int cols_l = fsSettings["LEFT.width"]; + int rows_r = fsSettings["RIGHT.height"]; + int cols_r = fsSettings["RIGHT.width"]; + + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || + rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) + { + cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; + return -1; + } + + cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l); + cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r); + } + + // Maximum delay, 5 seconds + ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); + ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft,&igb); + ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight,&igb); + + std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb); + + ros::spin(); + + return 0; +} + + + +void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg) +{ + mBufMutexLeft.lock(); + if (!imgLeftBuf.empty()) + imgLeftBuf.pop(); + imgLeftBuf.push(img_msg); + mBufMutexLeft.unlock(); +} + +void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg) +{ + mBufMutexRight.lock(); + if (!imgRightBuf.empty()) + imgRightBuf.pop(); + imgRightBuf.push(img_msg); + mBufMutexRight.unlock(); +} + +cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg) +{ + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + } + + if(cv_ptr->image.type()==0) + { + return cv_ptr->image.clone(); + } + else + { + std::cout << "Error type" << std::endl; + return cv_ptr->image.clone(); + } +} + +void ImageGrabber::SyncWithImu() +{ + const double maxTimeDiff = 0.01; + while(1) + { + cv::Mat imLeft, imRight; + double tImLeft = 0, tImRight = 0; + if (!imgLeftBuf.empty()&&!imgRightBuf.empty()&&!mpImuGb->imuBuf.empty()) + { + tImLeft = imgLeftBuf.front()->header.stamp.toSec(); + tImRight = imgRightBuf.front()->header.stamp.toSec(); + + this->mBufMutexRight.lock(); + while((tImLeft-tImRight)>maxTimeDiff && imgRightBuf.size()>1) + { + imgRightBuf.pop(); + tImRight = imgRightBuf.front()->header.stamp.toSec(); + } + this->mBufMutexRight.unlock(); + + this->mBufMutexLeft.lock(); + while((tImRight-tImLeft)>maxTimeDiff && imgLeftBuf.size()>1) + { + imgLeftBuf.pop(); + tImLeft = imgLeftBuf.front()->header.stamp.toSec(); + } + this->mBufMutexLeft.unlock(); + + if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff) + { + // std::cout << "big time difference" << std::endl; + continue; + } + if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec()) + continue; + + this->mBufMutexLeft.lock(); + imLeft = GetImage(imgLeftBuf.front()); + imgLeftBuf.pop(); + this->mBufMutexLeft.unlock(); + + this->mBufMutexRight.lock(); + imRight = GetImage(imgRightBuf.front()); + imgRightBuf.pop(); + this->mBufMutexRight.unlock(); + + vector vImuMeas; + mpImuGb->mBufMutex.lock(); + if(!mpImuGb->imuBuf.empty()) + { + // Load imu measurements from buffer + vImuMeas.clear(); + while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tImLeft) + { + double t = mpImuGb->imuBuf.front()->header.stamp.toSec(); + cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z); + cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z); + vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t)); + mpImuGb->imuBuf.pop(); + } + } + mpImuGb->mBufMutex.unlock(); + if(mbClahe) + { + mClahe->apply(imLeft,imLeft); + mClahe->apply(imRight,imRight); + } + + if(do_rectify) + { + cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR); + cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR); + } + + mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas); + + std::chrono::milliseconds tSleep(1); + std::this_thread::sleep_for(tSleep); + } + } +} + +void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg) +{ + mBufMutex.lock(); + imuBuf.push(imu_msg); + mBufMutex.unlock(); + return; +} + + diff --git a/Examples_old/Stereo/EuRoC_feature.yaml b/Examples_old/Stereo/EuRoC_feature.yaml new file mode 100644 index 0000000000..531833b511 --- /dev/null +++ b/Examples_old/Stereo/EuRoC_feature.yaml @@ -0,0 +1,96 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# System config +#-------------------------------------------------------------------------------------------- + +# When the variables are commented, the system doesn't load a previous session or not store the current one + +# If the LoadFile doesn't exist, the system give a message and create a new Atlas from scratch +#System.LoadAtlasFromFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo" + +# The store file is created from the current session, if a file with the same name exists it is deleted +#System.SaveAtlasToFile: "Session_MH01_MH02_MH03_Stereo60_Pseudo" + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Camera calibration and distortion parameters (OpenCV) +Camera1.fx: 458.654 +Camera1.fy: 457.296 +Camera1.cx: 367.215 +Camera1.cy: 248.375 + +Camera1.k1: -0.28340811 +Camera1.k2: 0.07395907 +Camera1.p1: 0.00019359 +Camera1.p2: 1.76187114e-05 + +Camera2.fx: 457.587 +Camera2.fy: 456.134 +Camera2.cx: 379.999 +Camera2.cy: 255.238 + +Camera2.k1: -0.28368365 +Camera2.k2: 0.07451284 +Camera2.p1: -0.00010473 +Camera2.p2: -3.55590700e-05 + +Camera.width: 752 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 20 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +Stereo.ThDepth: 60.0 +Stereo.T_c1_c2: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [0.999997256477797,-0.002317135723275,-0.000343393120620,0.110074137800478, + 0.002312067192432,0.999898048507103,-0.014090668452683,-0.000156612054392, + 0.000376008102320,0.014089835846691,0.999900662638081,0.000889382785432, + 0,0,0,1.000000000000000] + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1200 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500.0 +Viewer.imageViewScale: 1.0 + diff --git a/README.md b/README.md index 0ca3a9f2d3..0ec05f83f7 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,87 @@ +## Ubuntu 20.04 & ROS Noetic + +## Build Pangolin + +- Pangolin (visualization) + + Dowload and install instructions can be found at: https://github.com/stevenlovegrove/Pangolin. + + +## Build with ROS +```bash + +cd ~/closedloop_ws/src/ # ros workspace + +git clone https://github.com/ivalab/ORB_SLAM3.git + +cd ORB_SLAM3 + +# Build support. +./buid.sh + + +# Build ROS. + +# !!! ALWAYS REMEMBER TO EXPORT ROS PATH !!! +source export_ros_path.bash # Plz configure your own path before sourcing + +cd build + +# Default build ROS with closedloop test enabled and map-atlas disabled. +cmake .. +make -j + +# They can be turned off by passing corresponding flags, e.g. +cmake .. -DBUILD_ROS=OFF -DENABLE_CLOSED_LOOP=OFF + +``` +### Build non-ROS only (no closed-loop supported) +```bash +cd ~/catkin_ws/src/ORB_SLAM3 + +./build.sh +``` + +### Run +```bash +roscore + +# !!! ALWAYS REMEMBER TO EXPORT ROS PATH !!! +source export_ros_path.bash + +cd script + +python Run_EuRoC_Stereo.py + +``` + +### Evaluation +```bash +python Evaluate_EuRoC_Stereo.py + +python Collect_EuRoC_Stereo.py +``` + +--- +--- + +## ROS Kinect and Ubuntu 16.04 (DEPRECATED) + +``` +cd ydu_workspace/catkin_ws/src/ORB_SLAM3 + +export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/yipuzhao/ydu_workspace/catkin_ws/src/ORB_SLAM3/Examples_old/ROS + +./build_ros.sh + +roscore + +rosrun ORB_SLAM3 Stereo Vocabulary/ORBvoc.txt Examples_old/Stereo/EuRoC.yaml true + +rosbag play --pause MH_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu +``` +--- +--- # ORB-SLAM3 ### V1.0, December 22th, 2021 diff --git a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h index 01959344ed..963eb99113 100644 --- a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h +++ b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h @@ -246,6 +246,18 @@ class TemplatedVocabulary */ void saveToTextFile(const std::string &filename) const; + /** + * Loads the vocabulary from a binary file + * @param filename + */ + bool loadFromBinaryFile(const std::string &filename); + + /** + * Saves the vocabulary into a binary file + * @param filename + */ + void saveToBinaryFile(const std::string &filename) const; + /** * Saves the vocabulary into a file * @param filename @@ -1461,6 +1473,77 @@ void TemplatedVocabulary::save(const std::string &filename) const // -------------------------------------------------------------------------- +template +bool TemplatedVocabulary::loadFromBinaryFile(const std::string &filename) { + fstream f; + f.open(filename.c_str(), ios_base::in|ios::binary); + unsigned int nb_nodes, size_node; + f.read((char*)&nb_nodes, sizeof(nb_nodes)); + f.read((char*)&size_node, sizeof(size_node)); + f.read((char*)&m_k, sizeof(m_k)); + f.read((char*)&m_L, sizeof(m_L)); + f.read((char*)&m_scoring, sizeof(m_scoring)); + f.read((char*)&m_weighting, sizeof(m_weighting)); + createScoringObject(); + + m_words.clear(); + m_words.reserve(pow((double)m_k, (double)m_L + 1)); + m_nodes.clear(); + m_nodes.resize(nb_nodes+1); + m_nodes[0].id = 0; + char buf[size_node]; int nid = 1; + while (!f.eof()) { + f.read(buf, size_node); + m_nodes[nid].id = nid; + // FIXME + const int* ptr=(int*)buf; + m_nodes[nid].parent = *ptr; + //m_nodes[nid].parent = *(const int*)buf; + m_nodes[m_nodes[nid].parent].children.push_back(nid); + m_nodes[nid].descriptor = cv::Mat(1, F::L, CV_8U); + memcpy(m_nodes[nid].descriptor.data, buf+4, F::L); + m_nodes[nid].weight = *(float*)(buf+4+F::L); + if (buf[8+F::L]) { // is leaf + int wid = m_words.size(); + m_words.resize(wid+1); + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } + else + m_nodes[nid].children.reserve(m_k); + nid+=1; + } + f.close(); + return true; +} + + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::saveToBinaryFile(const std::string &filename) const { + fstream f; + f.open(filename.c_str(), ios_base::out|ios::binary); + unsigned int nb_nodes = m_nodes.size(); + float _weight; + unsigned int size_node = sizeof(m_nodes[0].parent) + F::L*sizeof(char) + sizeof(_weight) + sizeof(bool); + f.write((char*)&nb_nodes, sizeof(nb_nodes)); + f.write((char*)&size_node, sizeof(size_node)); + f.write((char*)&m_k, sizeof(m_k)); + f.write((char*)&m_L, sizeof(m_L)); + f.write((char*)&m_scoring, sizeof(m_scoring)); + f.write((char*)&m_weighting, sizeof(m_weighting)); + for(size_t i=1; i void TemplatedVocabulary::load(const std::string &filename) { diff --git a/build.sh b/build.sh index 96d1c0941c..b1cae6ea3f 100755 --- a/build.sh +++ b/build.sh @@ -21,7 +21,7 @@ echo "Configuring and building Thirdparty/Sophus ..." mkdir build cd build -cmake .. -DCMAKE_BUILD_TYPE=Release +cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF make -j cd ../../../ diff --git a/build_ros.sh b/build_ros.sh index 1f13d2155f..351273ca57 100755 --- a/build_ros.sh +++ b/build_ros.sh @@ -1,6 +1,6 @@ echo "Building ROS nodes" -cd Examples/ROS/ORB_SLAM3 +cd Examples_old/ROS/ORB_SLAM3 mkdir build cd build cmake .. -DROS_BUILD_TYPE=Release diff --git a/export_ros_path.bash b/export_ros_path.bash new file mode 100644 index 0000000000..0540ee7e66 --- /dev/null +++ b/export_ros_path.bash @@ -0,0 +1 @@ +export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${HOME}/closedloop_ws/src/ORB_SLAM3/Examples_old/ROS \ No newline at end of file diff --git a/include/Frame.h b/include/Frame.h index 572073f403..c16ff13be1 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -32,6 +32,7 @@ #include "Converter.h" #include "Settings.h" +#include "Timer.h" #include #include @@ -367,6 +368,14 @@ class Frame } Sophus::SE3 T_test; +public: + struct TimeLog + { + double feature_extraction = 0.0; + double stereo_matching = 0.0; + }; + TimeLog logCurrentFrame_; + slam_utility::stats::TicTocTimer timer_; }; }// namespace ORB_SLAM diff --git a/include/LocalMapping.h b/include/LocalMapping.h index dd04a3d47f..cebd926f34 100644 --- a/include/LocalMapping.h +++ b/include/LocalMapping.h @@ -194,7 +194,52 @@ class LocalMapping //DEBUG ofstream f_lm; - +public: + struct TimeLog + { + double timestamp = 0.0; + double local_ba = 0.0; + int num_fixed_kfs = 0; + int num_opt_kfs = 0; + int num_points = 0; + int num_edges = 0; + + /** + * @brief Set the Zero object + * + */ + void setZero() + { + timestamp = 0.0; + local_ba = 0.0; + num_fixed_kfs = 0; + num_opt_kfs = 0; + num_points = 0; + num_edges = 0; + } + + friend std::ostream& operator<<(std::ostream& os, const TimeLog& l) + { + os << std::setprecision(10); + os << l.timestamp << " " + << l.local_ba << " " + << l.num_fixed_kfs << " " + << l.num_opt_kfs << " " + << l.num_points; + // << l.num_edges; + return os; + } + + static std::string header() + { + return "# timestamp local_ba num_fixed_kfs num_opt_kfs num_points " + "num_edges"; + } + }; + TimeLog logCurrentFrame_; + std::vector mFrameTimeLog_; + slam_utility::stats::TicTocTimer timer_; + bool IMU_INIT_1 = false, IMU_INIT_2 = false; }; } //namespace ORB_SLAM diff --git a/include/MacroDefinitions.h b/include/MacroDefinitions.h new file mode 100644 index 0000000000..2486af7024 --- /dev/null +++ b/include/MacroDefinitions.h @@ -0,0 +1,22 @@ +/** + * @file MacroDefinitions.h + * @author Yanwei Du (yanwei.du@gatech.edu) + * @brief None + * @version 0.1 + * @date 01-28-2023 + * @copyright Copyright (c) 2023 + */ + +#ifndef ORB_SLAM3_MACRO_DEFINITIONS_H_ +#define ORB_SLAM3_MACRO_DEFINITIONS_H_ + +//////////////////////////////////////////////////////////////////////////////// + +#define DISABLE_ATLAS + +// #define ENABLE_CLOSED_LOOP + +// #define DISABLE_RELOC +// #define DISABLE_LOOP_CLOSURE + +#endif // ORB_SLAM3_MACRO_DEFINITIONS_H_ \ No newline at end of file diff --git a/include/ORBextractor.h b/include/ORBextractor.h index bb7aace3eb..dc30895acc 100644 --- a/include/ORBextractor.h +++ b/include/ORBextractor.h @@ -63,6 +63,8 @@ class ORBextractor float inline GetScaleFactor(){ return scaleFactor;} + float GetInitThres() {return iniThFAST;} + float GetMinThres() {return minThFAST;} std::vector inline GetScaleFactors(){ return mvScaleFactor; diff --git a/include/System.h b/include/System.h index 872c86e734..d757ddd2b3 100644 --- a/include/System.h +++ b/include/System.h @@ -167,6 +167,9 @@ class System // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php void SaveTrajectoryKITTI(const string &filename); + void SaveTrackingLog(const string &filename); + void SaveMappingLog(const string &filename); + // TODO: Save/Load functions // SaveMap(const string &filename); // LoadMap(const string &filename); @@ -215,19 +218,22 @@ class System // Tracker. It receives a frame and computes the associated camera pose. // It also decides when to insert a new keyframe, create some new MapPoints and // performs relocalization if tracking fails. +public: Tracking* mpTracker; // Local Mapper. It manages the local map and performs local bundle adjustment. LocalMapping* mpLocalMapper; +private: // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs // a pose graph optimization and full bundle adjustment (in a new thread) afterwards. LoopClosing* mpLoopCloser; // The viewer draws the map and the current camera pose. It uses Pangolin. Viewer* mpViewer; - +public: FrameDrawer* mpFrameDrawer; +private: MapDrawer* mpMapDrawer; // System threads: Local Mapping, Loop Closing, Viewer. diff --git a/include/Tracking.h b/include/Tracking.h index 863c98e4f2..9286cac6b2 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -36,6 +36,8 @@ #include "System.h" #include "ImuTypes.h" #include "Settings.h" +#include "MacroDefinitions.h" +#include "Timer.h" #include "GeometricCamera.h" @@ -53,6 +55,94 @@ class LoopClosing; class System; class Settings; + +class OdometryLog +{ +public: + + OdometryLog(double time_stamp_, double tx_, double ty_, double tz_, + double qw_, double qx_, double qy_, double qz_) : + time_stamp(time_stamp_), tx(tx_), ty(ty_), tz(tz_), qw(qw_), qx(qx_), qy(qy_), qz(qz_), + Twc(Eigen::Quaternionf(qw, qx, qy, qz), Eigen::Vector3f(tx, ty, tz)), + Tcw(Twc.inverse()) + { + } + + double time_stamp; + double tx, ty, tz; + double qw, qx, qy, qz; + + Sophus::SE3f Tcw; + Sophus::SE3f Twc; + +}; + +struct OdomLogComparator { + bool operator()(double const& t0, OdometryLog const& m1) const { + return t0 < m1.time_stamp; + } + + bool operator()(OdometryLog const& m0, double const& t1) const { + return m0.time_stamp < t1; + } +}; +class MotionModel +{ +public: + MotionModel() + { + buffer_.reserve(10000); + // Initialize extrisnics. + Tc2b.setQuaternion(Eigen::Quaternion(-0.500, 0.500, -0.500, 0.500)); + Tc2b.translation() = Eigen::Vector3f(0.094, -0.074, 0.280); + Tb2c = Tc2b.inverse(); + } + + void append(const OdometryLog& odom) + { + std::unique_lock lock(mutex_); + buffer_.emplace_back(odom); + } + + void reset() + { + buffer_.clear(); + } + + bool predict(const double time_prev, const double time_curr, Sophus::SE3f & T_se) { + std::unique_lock lock(mutex_); + int n = buffer_.size(), i; + //cout << "mvOdomBuf.size() = " << n ; + + auto lower = std::lower_bound( buffer_.begin(), buffer_.end(), time_prev, OdomLogComparator() ); + auto upper = std::upper_bound( buffer_.begin(), buffer_.end(), time_curr, OdomLogComparator() ); + + if (lower == buffer_.end()){ + buffer_.clear(); + return false; + } + const auto T_st = lower->Twc; + + if (upper == buffer_.end()){ + buffer_.clear(); + return false; + } + const auto T_ed = upper->Twc; + + // relative transform between i_st & i_ed + // cv::Mat Ttmp = (Tb2c * T_ed * T_st.inv() * Tc2b); + T_se = (Tb2c * T_ed.inverse() * T_st * Tc2b); + + //mvOdomBuf.clear(); + buffer_.erase(buffer_.begin(), upper); + return true; + } + +private: + std::mutex mutex_; + std::vector buffer_; + Sophus::SE3f Tb2c, Tc2b; +}; class Tracking { @@ -107,6 +197,8 @@ class Tracking void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, Map* pMap); float GetImageScale(); + void updateORBExtractor(int feature_num); + void SetRealTimeFileStream(string fNameRealTimeTrack); #ifdef REGISTER_LOOP void RequestStop(); @@ -366,8 +458,67 @@ class Tracking std::mutex mMutexStop; #endif + std::ofstream f_realTimeTrack; + public: cv::Mat mImRight; + + struct TimeLog + { + double timestamp = 0.0; + double feature_extraction = 0.0; + double stereo_matching = 0.0; + double create_frame = 0.0; + double track_motion = 0.0; + double track_keyframe = 0.0; + double track_map = 0.0; + double update_motion = 0.0; + double post_processing = 0.0; + + /** + * @brief Set the Zero object + * + */ + void setZero() + { + timestamp = 0.0; + feature_extraction = 0.0; + stereo_matching = 0.0; + create_frame = 0.0; + track_motion = 0.0; + track_keyframe = 0.0; + track_map = 0.0; + update_motion = 0.0; + post_processing = 0.0; + } + + friend std::ostream& operator<<(std::ostream& os, const TimeLog& l) + { + os << std::setprecision(10); + os << l.timestamp << " " + << l.feature_extraction << " " + << l.stereo_matching << " " + << l.create_frame << " " + << l.track_motion << " " + << l.track_keyframe << " " + << l.track_map << " " + << l.update_motion << " " + << l.post_processing; + return os; + } + + static std::string header() + { + return "# timestamp feature_extraction stereo_matching " + "create_frame track_motion track_keyframe track_map " + "update_motion post_processing"; + } + }; + TimeLog logCurrentFrame_; + std::vector mFrameTimeLog_; + slam_utility::stats::TicTocTimer timer_; + + MotionModel motion_model_; }; } //namespace ORB_SLAM diff --git a/src/Frame.cc b/src/Frame.cc index a04c312ce0..80d60f1dc1 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -119,10 +119,12 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif + timer_.tic(); thread threadLeft(&Frame::ExtractORB,this,0,imLeft,0,0); thread threadRight(&Frame::ExtractORB,this,1,imRight,0,0); threadLeft.join(); threadRight.join(); + logCurrentFrame_.feature_extraction = timer_.toc(); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -138,7 +140,9 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now(); #endif + timer_.tic(); ComputeStereoMatches(); + logCurrentFrame_.stereo_matching = timer_.toc(); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now(); diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 53df3324fa..c8478e858d 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -125,7 +125,9 @@ void LocalMapping::Run() { if(mpAtlas->KeyFramesInMap()>2) { - + logCurrentFrame_.setZero(); + logCurrentFrame_.timestamp = mpCurrentKeyFrame->mTimeStamp; + timer_.tic(); if(mbInertial && mpCurrentKeyFrame->GetMap()->isImuInitialized()) { float dist = (mpCurrentKeyFrame->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->GetCameraCenter()).norm() + @@ -154,7 +156,12 @@ void LocalMapping::Run() Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA); b_doneLBA = true; } - + logCurrentFrame_.local_ba = timer_.toc(); + logCurrentFrame_.num_fixed_kfs = num_FixedKF_BA; + logCurrentFrame_.num_opt_kfs = num_OptKF_BA; + logCurrentFrame_.num_points = num_MPs_BA; + logCurrentFrame_.num_edges = num_edges_BA; + mFrameTimeLog_.emplace_back(logCurrentFrame_); } #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndLBA = std::chrono::steady_clock::now(); @@ -212,6 +219,7 @@ void LocalMapping::Run() InitializeIMU(1.f, 1e5, true); cout << "end VIBA 1" << endl; + IMU_INIT_1 = true; } } else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){ @@ -224,6 +232,7 @@ void LocalMapping::Run() InitializeIMU(0.f, 0.f, true); cout << "end VIBA 2" << endl; + IMU_INIT_2 = true; } } @@ -1116,6 +1125,8 @@ void LocalMapping::ResetIfRequested() mbNotBA2 = true; mbNotBA1 = true; mbBadImu=false; + IMU_INIT_1 = false; + IMU_INIT_2 = false; mIdxInit=0; @@ -1133,6 +1144,8 @@ void LocalMapping::ResetIfRequested() mbNotBA2 = true; mbNotBA1 = true; mbBadImu=false; + IMU_INIT_1 = false; + IMU_INIT_2 = false; mbResetRequested = false; mbResetRequestedActiveMap = false; diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 319afb551b..51d1d28185 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -74,6 +74,13 @@ LoopClosing::LoopClosing(Atlas *pAtlas, KeyFrameDatabase *pDB, ORBVocabulary *pV mstrFolderSubTraj = "SubTrajectories/"; mnNumCorrection = 0; mnCorrectionGBA = 0; + +#ifdef DISABLE_LOOP_CLOSURE + std::cout << "Main: loop closure disabled!" << std::endl; +#else + std::cout << "Main: loop closure enabled!" << std::endl; +#endif + } void LoopClosing::SetTracker(Tracking *pTracker) @@ -94,6 +101,10 @@ void LoopClosing::Run() while(1) { +#ifdef DISABLE_LOOP_CLOSURE + // do nothing + +#else //NEW LOOP AND MERGE DETECTION ALGORITHM //---------------------------- @@ -296,6 +307,8 @@ void LoopClosing::Run() mpLastCurrentKF = mpCurrentKF; } +#endif + ResetIfRequested(); if(CheckFinish()){ diff --git a/src/Optimizer.cc b/src/Optimizer.cc index b785be3829..6c723f2f8f 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -1158,6 +1158,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap } } } + num_MPs = lLocalMapPoints.size(); // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes list lFixedCameras; @@ -2431,6 +2432,7 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& } } } + num_MPs = lLocalMapPoints.size(); // Fixed Keyframe: First frame previous KF to optimization window) list lFixedKeyFrames; diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc index b8d3abfb14..9769c18914 100644 --- a/src/Sim3Solver.cc +++ b/src/Sim3Solver.cc @@ -364,7 +364,9 @@ void Sim3Solver::ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2) // Rotation angle. sin is the norm of the imaginary part, cos is the real part double ang=atan2(vec.norm(),evec(0,maxIndex)); - vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half + if(vec.norm()!=0){ + vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half + } mR12i = Sophus::SO3f::exp(vec).matrix(); // Step 5: Rotate set 2 diff --git a/src/System.cc b/src/System.cc index 60d9c5185a..0d8c754759 100644 --- a/src/System.cc +++ b/src/System.cc @@ -33,6 +33,11 @@ #include #include +bool has_suffix(const std::string &str, const std::string &suffix) { + std::size_t index = str.find(suffix, str.size() - suffix.size()); + return (index != std::string::npos); +} + namespace ORB_SLAM3 { @@ -115,7 +120,15 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; mpVocabulary = new ORBVocabulary(); - bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + bool bVocLoad = false; + if (has_suffix(strVocFile, ".txt")) + { + bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + } + else + { + bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile); + } if(!bVocLoad) { cerr << "Wrong path to vocabulary. " << endl; @@ -137,7 +150,15 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; mpVocabulary = new ORBVocabulary(); - bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + bool bVocLoad = false; + if (has_suffix(strVocFile, ".txt")) + { + bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + } + else + { + bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile); + } if(!bVocLoad) { cerr << "Wrong path to vocabulary. " << endl; @@ -239,6 +260,9 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS // Fix verbosity Verbose::SetTh(Verbose::VERBOSITY_QUIET); +#ifdef ENABLE_CLOSED_LOOP + std::cout << "System: Closed-Loop mode enabled!" << std::endl; +#endif } Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp, const vector& vImuMeas, string filename) @@ -1545,5 +1569,32 @@ string System::CalculateCheckSum(string filename, int type) return checksum; } + +void System::SaveTrackingLog(const string &filename) +{ + cout << endl << "Saving tracking log to " << filename << " ..." << endl; + std::ofstream myfile(filename); + myfile << Tracking::TimeLog::header() << "\n"; + myfile << std::fixed; + for (const auto& log : mpTracker->mFrameTimeLog_) + { + myfile << log << "\n"; + } + myfile.close(); +} + +void System::SaveMappingLog(const string &filename) +{ + cout << endl << "Saving mapping log to " << filename << " ..." << endl; + std::ofstream myfile(filename); + myfile << LocalMapping::TimeLog::header() << "\n"; + myfile << std::fixed; + for (const auto& log : mpLocalMapper->mFrameTimeLog_) + { + myfile << log << "\n"; + } + myfile.close(); +} + } //namespace ORB_SLAM diff --git a/src/Tracking.cc b/src/Tracking.cc index 5191451a81..3b062bc7fc 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -128,6 +128,20 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, vdNewKF_ms.clear(); vdTrackTotal_ms.clear(); #endif + + +#ifdef DISABLE_RELOC + std::cout << "Tracking: relocalization disabled!" << std::endl; +#else + std::cout << "Tracking: relocalization enabled!" << std::endl; +#endif + +#ifdef DISABLE_ATLAS + std::cout << "Tracking: MapAtlas disabled!" << std::endl; +#else + std::cout << "Tracking: MapAtlas enabled!" << std::endl; +#endif + } #ifdef REGISTER_TIMES @@ -1454,7 +1468,9 @@ bool Tracking::GetStepByStep() Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp, string filename) { //cout << "GrabImageStereo" << endl; - + logCurrentFrame_.setZero(); + logCurrentFrame_.timestamp = timestamp; + timer_.tic(); mImGray = imRectLeft; cv::Mat imGrayRight = imRectRight; mImRight = imRectRight; @@ -1503,6 +1519,9 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat mCurrentFrame.mNameFile = filename; mCurrentFrame.mnDataset = mnNumDataset; + logCurrentFrame_.create_frame = timer_.toc(); + logCurrentFrame_.feature_extraction = mCurrentFrame.logCurrentFrame_.feature_extraction; + logCurrentFrame_.stereo_matching = mCurrentFrame.logCurrentFrame_.stereo_matching; #ifdef REGISTER_TIMES vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); @@ -1945,14 +1964,22 @@ void Tracking::Track() if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnIdstd::numeric_limits::max() && !bOK) +#else if(mCurrentFrame.mTimeStamp-mTimeStampLost>3.0f && !bOK) +#endif { mState = LOST; Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL); @@ -2124,8 +2155,9 @@ void Tracking::Track() { if(bOK) { + timer_.tic(); bOK = TrackLocalMap(); - + logCurrentFrame_.track_map = timer_.toc(); } if(!bOK) cout << "Fail to track local map!" << endl; @@ -2204,6 +2236,7 @@ void Tracking::Track() if(bOK || mState==RECENTLY_LOST) { + timer_.tic(); // Update motion model if(mLastFrame.isSet() && mCurrentFrame.isSet()) { @@ -2214,6 +2247,8 @@ void Tracking::Track() else { mbVelocity = false; } + logCurrentFrame_.update_motion = timer_.toc(); + timer_.tic(); if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose()); @@ -2265,6 +2300,7 @@ void Tracking::Track() if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i]) mCurrentFrame.mvpMapPoints[i]=static_cast(NULL); } + logCurrentFrame_.post_processing = timer_.toc(); } // Reset if the camera get lost soon after initialization @@ -2297,7 +2333,7 @@ void Tracking::Track() - if(mState==OK || mState==RECENTLY_LOST) + if(mState==OK || mState==RECENTLY_LOST || mState==LOST) { // Store frame pose information to retrieve the complete camera trajectory afterwards. if(mCurrentFrame.isSet()) @@ -2306,7 +2342,19 @@ void Tracking::Track() mlRelativeFramePoses.push_back(Tcr_); mlpReferences.push_back(mCurrentFrame.mpReferenceKF); mlFrameTimes.push_back(mCurrentFrame.mTimeStamp); - mlbLost.push_back(mState==LOST); + mlbLost.push_back(mState==LOST || mState==RECENTLY_LOST); + + // realtime trajectory logging + { + const auto Twc = mCurrentFrame.GetPose().inverse(); + const auto t = Twc.translation(); + const auto q = Twc.unit_quaternion(); + f_realTimeTrack + << setprecision(6) << mCurrentFrame.mTimeStamp << setprecision(7) << " " + << t.x() << " " << t.y() << " " + << t.z() << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; + } } else { @@ -2314,10 +2362,11 @@ void Tracking::Track() mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); mlpReferences.push_back(mlpReferences.back()); mlFrameTimes.push_back(mlFrameTimes.back()); - mlbLost.push_back(mState==LOST); + mlbLost.push_back(mState==LOST ||mState==RECENTLY_LOST); } } + mFrameTimeLog_.push_back(logCurrentFrame_); #ifdef REGISTER_LOOP if (Stop()) { @@ -2334,7 +2383,7 @@ void Tracking::Track() void Tracking::StereoInitialization() { - if(mCurrentFrame.N>500) + if(mCurrentFrame.N>100) { if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) { @@ -2867,7 +2916,17 @@ bool Tracking::TrackWithMotionModel() } else { - mCurrentFrame.SetPose(mVelocity * mLastFrame.GetPose()); + // Hard-code to use odom motion model. + Sophus::SE3f tmp_vel; + if (motion_model_.predict(mLastFrame.mTimeStamp, mCurrentFrame.mTimeStamp, tmp_vel)) + { + mCurrentFrame.SetPose(tmp_vel * mLastFrame.GetPose()); + + } + else + { + mCurrentFrame.SetPose(mVelocity * mLastFrame.GetPose()); + } } @@ -3608,6 +3667,9 @@ void Tracking::UpdateLocalKeyFrames() bool Tracking::Relocalization() { +#ifdef DISABLE_RELOC + // do nothing +#else Verbose::PrintMess("Starting relocalization", Verbose::VERBOSITY_NORMAL); // Compute Bag of Words Vector mCurrentFrame.ComputeBoW(); @@ -3773,7 +3835,7 @@ bool Tracking::Relocalization() cout << "Relocalized!!" << endl; return true; } - +#endif } void Tracking::Reset(bool bLocMap) @@ -4123,4 +4185,34 @@ void Tracking::Release() } #endif +void Tracking::updateORBExtractor(int feature_num) { + + assert(mpORBextractorLeft != NULL); + + // take the budget number of input arg as feature extraction constr + float fScaleFactor = mpORBextractorLeft->GetScaleFactor(); + int nLevels = mpORBextractorLeft->GetLevels(); + int fIniThFAST = mpORBextractorLeft->GetInitThres(); + int fMinThFAST = mpORBextractorLeft->GetMinThres(); + + // + delete mpORBextractorLeft; + mpORBextractorLeft = new ORBextractor(feature_num,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + + if(mSensor==System::STEREO) { + assert(mpORBextractorRight != NULL); + // + delete mpORBextractorRight; + mpORBextractorRight = new ORBextractor(feature_num,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); + } + std::cout << "mpTracker constraint adjusted to " << feature_num << std::endl; +} + +void Tracking::SetRealTimeFileStream(string fNameRealTimeTrack) +{ + f_realTimeTrack.open(fNameRealTimeTrack.c_str()); + f_realTimeTrack << fixed; + f_realTimeTrack << "#TimeStamp Tx Ty Tz Qx Qy Qz Qw" << std::endl; +} + } //namespace ORB_SLAM