★ FEATURED ARTICLE 一、安装部署ORB-SLAM31. 准备工作与基础环境首先请确保你的Ubuntu 22.04系统已经更新并安装了build-essential,cmake,git等基础编译工具sudo apt update sudo apt upgrade -y sudo apt install -y git cmake gcc g build-essential pkg-config wget curl unzip tar2.安装关键依赖库ORB-SLAM3的运行依赖于多个第三方库需要逐一正确安装。如果之前二进制安装过以下包需要先把它们都卸载移除。1Eigen3 (版本 3.3.9)用于高效的线性代数运算推荐从源码编译安装。打开终端依次输入cd ~克隆代码git clone -b 3.3.9 https://gitlab.com/libeigen/eigen.git创建并进入 build 目录cd eigen mkdir build cd build编译cmake ..安装sudo make install2Pangolin (版本 v0.6)用于可视化需先安装其图形依赖。打开终端依次输入cd ~sudo apt install -y libgl1-mesa-dev libglew-dev libwayland-dev libxkbcommon-dev wayland-protocols libegl1-mesa-dev libpython3-dev克隆代码git clone -b v0.6 https://github.com/stevenlovegrove/Pangolin.git创建并进入 build 目录cd Pangolin mkdir build cd build编译cmake .. -DCMAKE_BUILD_TYPEReleasemake -j$(nproc)安装sudo make install3OpenCV (推荐4.5.4)Ubuntu 22.04的官方源默认提供可直接安装sudo apt install -y libopencv-dev python3-opencv3. 编译ORB-SLAM3核心库独立编译ORB-SLAM3算法库确保其在不依赖ROS的情况下能正常工作。1克隆源码在用户主目录下克隆官方仓库cd ~ git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.gitcd ORB_SLAM3注意检查CMakeLists.txt文件中OpenCV和Eigen3的查找路径是否正确或手动指定路径。cmake_minimum_required(VERSION 2.8) set(CMAKE_CXX_STANDARD 14) project(ORB_SLAM3) IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) ENDIF() MESSAGE(Build type: ${CMAKE_BUILD_TYPE}) set(CMAKE_C_FLAGS ${CMAKE_C_FLAGS} -Wall -O3) set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} -Wall -O3) set(CMAKE_C_FLAGS_RELEASE ${CMAKE_C_FLAGS_RELEASE} -marchnative) set(CMAKE_CXX_FLAGS_RELEASE ${CMAKE_CXX_FLAGS_RELEASE} -marchnative) # Check C11 or C0x support include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG(-stdc11 COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG(-stdc0x COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} -stdc11) add_definitions(-DCOMPILEDWITHC11) message(STATUS Using flag -stdc11.) elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} -stdc0x) add_definitions(-DCOMPILEDWITHC0X) message(STATUS Using flag -stdc0x.) else() message(FATAL_ERROR The compiler ${CMAKE_CXX_COMPILER} has no C11 support. Please use a different C compiler.) 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() MESSAGE(OPENCV VERSION:) MESSAGE(${OpenCV_VERSION}) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) find_package(realsense2) include_directories( ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/include ${PROJECT_SOURCE_DIR}/include/CameraModels ${PROJECT_SOURCE_DIR}/Thirdparty/Sophus ${EIGEN3_INCLUDE_DIR} ${Pangolin_INCLUDE_DIRS} ) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) add_library(${PROJECT_NAME} SHARED src/System.cc src/Tracking.cc src/LocalMapping.cc src/LoopClosing.cc src/ORBextractor.cc src/ORBmatcher.cc src/FrameDrawer.cc src/Converter.cc src/MapPoint.cc src/KeyFrame.cc src/Atlas.cc src/Map.cc src/MapDrawer.cc src/Optimizer.cc src/Frame.cc src/KeyFrameDatabase.cc src/Sim3Solver.cc src/Viewer.cc src/ImuTypes.cc src/G2oTypes.cc src/CameraModels/Pinhole.cpp src/CameraModels/KannalaBrandt8.cpp src/OptimizableTypes.cpp src/MLPnPsolver.cpp src/GeometricTools.cc src/TwoViewReconstruction.cc src/Config.cc src/Settings.cc include/System.h include/Tracking.h include/LocalMapping.h include/LoopClosing.h include/ORBextractor.h include/ORBmatcher.h include/FrameDrawer.h include/Converter.h include/MapPoint.h include/KeyFrame.h include/Atlas.h include/Map.h include/MapDrawer.h include/Optimizer.h include/Frame.h include/KeyFrameDatabase.h include/Sim3Solver.h include/Viewer.h include/ImuTypes.h include/G2oTypes.h include/CameraModels/GeometricCamera.h include/CameraModels/Pinhole.h include/CameraModels/KannalaBrandt8.h include/OptimizableTypes.h include/MLPnPsolver.h include/GeometricTools.h include/TwoViewReconstruction.h include/SerializationUtils.h include/Config.h include/Settings.h) add_subdirectory(Thirdparty/g2o) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so -lboost_serialization -lcrypto ) # If RealSense SDK is found the library is added and its examples compiled if(realsense2_FOUND) include_directories(${PROJECT_NAME} ${realsense_INCLUDE_DIR} ) target_link_libraries(${PROJECT_NAME} ${realsense2_LIBRARY} ) endif() # Build examples # 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}) 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) 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() #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 inertial examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/RGB-D-Inertial) 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() #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}) 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(stereo_realsense_t265_old Examples_old/Stereo/stereo_realsense_t265.cc) target_link_libraries(stereo_realsense_t265_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() #Monocular examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular) add_executable(mono_tum_old Examples_old/Monocular/mono_tum.cc) target_link_libraries(mono_tum_old ${PROJECT_NAME}) add_executable(mono_kitti_old Examples_old/Monocular/mono_kitti.cc) target_link_libraries(mono_kitti_old ${PROJECT_NAME}) add_executable(mono_euroc_old Examples_old/Monocular/mono_euroc.cc) target_link_libraries(mono_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}) 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_realsense_D435i_old Examples_old/Monocular/mono_realsense_D435i.cc) target_link_libraries(mono_realsense_D435i_old ${PROJECT_NAME}) endif() #Monocular inertial examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples_old/Monocular-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(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_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_inertial_realsense_D435i_old Examples_old/Monocular-Inertial/mono_inertial_realsense_D435i.cc) target_link_libraries(mono_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()2执行编译使用官方提供的build.sh脚本进行编译chmod x build.sh./build.sh3. 在ROS2工作空间中集成将ORB-SLAM3库集成到ROS2 Humble环境中。1创建ROS2工作空间如果还没有专门的工作空间请创建一个mkdir -p ~/orb_slam3_ws/srccd ~/orb_slam3_ws/src2克隆ROS2封装代码在src目录下克隆社区提供的ROS2封装git clone https://github.com/benaissa-tayeb37/orb_slam3_ros2.git3配置与编译返回工作空间根目录安装ROS2依赖并编译。cd ~/orb_slam3_ws colcon build --symlink-install二、可能遇到的问题1. std::bad_array_new_length问题robotll-Lenovo:~/d7lros2/orb_slam3_ws$ ros2 run orbslam3 mono \ ~/d7lros2/orb_slam3_ws/src/ORB_SLAM3_ROS2/vocabulary/ORBvoc.txt \ ~/d7lros2/orb_slam3_ws/src/ORB_SLAM3_ROS2/config/monocular/TUM1.yaml ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. This program comes with ABSOLUTELY NO WARRANTY; This is free software, and you are welcome to redistribute it under certain conditions. See LICENSE.txt. Input sensor was set to: Monocular Loading ORB Vocabulary. This could take a while... Vocabulary loaded! Initialization of Atlas from scratch Creation of new map with id: 0 Creation of new map with last KF id: 0 Seq. Name: Camera Parameters: - Camera: Pinhole - Image scale: 1 - fx: 517.306 - fy: 516.469 - cx: 318.643 - cy: 255.314 - k1: 0.262383 - k2: -0.953104 - p1: -0.005358 - p2: 0.002628 - k3: 1.16331 - fps: 30 - color order: RGB (ignored if grayscale) ORB Extractor Parameters: - Number of Features: 1000 - Scale Levels: 8 - Scale Factor: 1.2 - Initial Fast Threshold: 20 - Minimum Fast Threshold: 7 There are 1 cameras in the atlas Camera 0 is pinhole terminate called after throwing an instance of std::bad_array_new_length what(): std::bad_array_new_length [ros2run]: Aborted解决方法见文章解决terminate called after throwing an instance of ‘std::bad_array_new_length‘ what()2. double free or corruption (out)问题robotll-Lenovo:~/d7lros2/orb_slam3_ws$ ros2 run orbslam3 rgbd ~/d7lros2/orb_slam3_ws/src/ORB_SLAM3_ROS2/vocabulary/ORBvoc.txt ~/d7lros2/orb_slam3_ws/src/ORB_SLAM3_ROS2/config/rgb-d/RealSense_D435i.yaml ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. This program comes with ABSOLUTELY NO WARRANTY; This is free software, and you are welcome to redistribute it under certain conditions. See LICENSE.txt. Input sensor was set to: RGB-D Loading ORB Vocabulary. This could take a while... Vocabulary loaded! Initialization of Atlas from scratch Creation of new map with id: 0 Creation of new map with last KF id: 0 Seq. Name: Camera Parameters: - Camera: Pinhole - Image scale: 0.5 - fx: 308.6 - fy: 308.681 - cx: 162.318 - cy: 121.231 - k1: 0 - k2: 0 - p1: 0 - p2: 0 - fps: 30 - color order: RGB (ignored if grayscale) Depth Threshold (Close/Far Points): 2.98185 ORB Extractor Parameters: - Number of Features: 1250 - Scale Levels: 8 - Scale Factor: 1.2 - Initial Fast Threshold: 20 - Minimum Fast Threshold: 7 There are 1 cameras in the atlas Camera 0 is pinhole Shutdown Saving keyframe trajectory to KeyFrameTrajectory.txt ... double free or corruption (out) [ros2run]: Aborted robotll-Lenovo:~/d7lros2/orb_slam3_ws$解决方法见文章解决ShutdownSaving keyframe trajectory to KeyFrameTrajectory.txt ...double free or corruption三、深度相机的配置和ORB_SLAM3建图启动1. 相机配置文件修改1获取深度相机内参首先查看在ROS2 封装代码工作空间打开终端启动深度相机并查看深度相机内参终端1ros2 launch realsense2_camera rs_launch.py \ align_depth.enable:true \ depth_module.depth_profile:640x480x30 \ rgb_camera.color_profile:640x480x30终端2ros2 topic echo /camera/camera/color/image_raw --once | grep -A 9 k:我的结果为wcrwcr-desktop:~$ ros2 topic echo /camera/camera/color/image_raw --once | grep -A 9 k: k: - 615.6708984375 - 0.0 - 330.0564880371094 - 0.0 - 615.7155151367188 - 231.79380798339844 - 0.0 - 0.0 - 1.0这里得到完整的k矩阵现在可以确认我的实际的相机内参为fx 615.6708984375 fy 615.7155151367188 cx 330.0564880371094 cy 231.79380798339844注意你们的要自己按以上方法确定自己的相机参数不能直接复用。2修改配置文件相机参数需要在ORB_SLAM3的 ROS2 封装代码里面设置深度相机参数文件具体位置为your_ws/src/ORB_SLAM3_ROS2/config/rgb-d/d435i.yaml可以另存一个d435i_1.yaml把上面得到的你的相机内参替换里面对应的内容%YAML:1.0 # 使用从 /d435i/color/camera_info 获取的真实内参 Camera.fx: 615.6708984375 Camera.fy: 615.7155151367188 Camera.cx: 330.0564880371094 Camera.cy: 231.79380798339844 # 畸变系数示例请用实际标定值 Camera.k1: -0.073 Camera.k2: 0.021 Camera.p1: 0.0005 Camera.p2: -0.0003 # 正确保留 bf fx * baseline (0.05m) Camera.bf: 30.86 # 深度因子不变 DepthMapFactor: 1000.0 # 其他参数保持不变...2. 修改封装代码源码需要修改的代码如下robotll-Lenovo:~/d7lros2/orb_slam3_ws/src/ORB_SLAM3_ROS2/src/rgbd$ tree . ├── rgbd.cpp ├── rgbd-slam-node.cpp └── rgbd-slam-node.hpp 0 directories, 3 files文件和路径可以参考上面内容。1rgbd.cpp#include iostream #include algorithm #include fstream #include chrono #include memory #include rclcpp/rclcpp.hpp #include rgbd-slam-node.hpp #include System.h int main(int argc, char **argv) { if(argc 3) { std::cerr \nUsage: ros2 run orbslam rgbd path_to_vocabulary path_to_settings std::endl; return 1; } rclcpp::init(argc, argv); // 创建 SLAM 系统开启可视化 bool visualization true; ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::RGBD, visualization); // 创建节点传入 SLAM 系统指针和默认选项 auto node std::make_sharedRgbdSlamNode(SLAM, rclcpp::NodeOptions()); std::cout std::endl; std::cout ORB-SLAM3 RGB-D node started. Waiting for images... std::endl; // 进入 ROS 2 事件循环 rclcpp::spin(node); // 关闭 SLAM 系统并保存轨迹 SLAM.Shutdown(); SLAM.SaveKeyFrameTrajectoryTUM(KeyFrameTrajectory.txt); rclcpp::shutdown(); return 0; }2rgbd-slam-node.cpp#include rgbd-slam-node.hpp #include opencv2/core/core.hpp using std::placeholders::_1; RgbdSlamNode::RgbdSlamNode(ORB_SLAM3::System* pSLAM, const rclcpp::NodeOptions options) : Node(ORB_SLAM3_ROS2, options), m_SLAM(pSLAM) { // 关键修复使用 this 指针创建 Subscriber而非将其包装成 shared_ptr rgb_sub std::make_sharedmessage_filters::SubscriberImageMsg(this, /camera/color/image_raw); depth_sub std::make_sharedmessage_filters::SubscriberImageMsg(this, /camera/depth/image_rect_raw); // 同步策略近似时间同步队列大小 10 syncApproximate std::make_sharedmessage_filters::Synchronizerapproximate_sync_policy( approximate_sync_policy(10), *rgb_sub, *depth_sub); syncApproximate-registerCallback(RgbdSlamNode::GrabRGBD, this); } RgbdSlamNode::~RgbdSlamNode() { // 仅执行必要的清理不再直接调用 SLAM 系统的全局方法 // Shutdown 和轨迹保存应由主函数在节点销毁后统一处理 } void RgbdSlamNode::GrabRGBD(const ImageMsg::SharedPtr msgRGB, const ImageMsg::SharedPtr msgD) { // 将 ROS 图像消息转换为 cv::Mat cv_bridge::CvImageConstPtr cv_ptrRGB; cv_bridge::CvImageConstPtr cv_ptrD; try { cv_ptrRGB cv_bridge::toCvShare(msgRGB); } catch (cv_bridge::Exception e) { RCLCPP_ERROR(this-get_logger(), cv_bridge exception (RGB): %s, e.what()); return; } try { cv_ptrD cv_bridge::toCvShare(msgD); } catch (cv_bridge::Exception e) { RCLCPP_ERROR(this-get_logger(), cv_bridge exception (Depth): %s, e.what()); return; } // 调用 ORB-SLAM3 的 RGB-D 追踪接口 m_SLAM-TrackRGBD(cv_ptrRGB-image, cv_ptrD-image, Utility::StampToSec(msgRGB-header.stamp)); }3rgbd-slam-node.hpp#ifndef __RGBD_SLAM_NODE_HPP__ #define __RGBD_SLAM_NODE_HPP__ #include iostream #include algorithm #include fstream #include chrono #include memory #include rclcpp/rclcpp.hpp #include rclcpp/node_options.hpp #include sensor_msgs/msg/image.hpp #include message_filters/subscriber.h #include message_filters/synchronizer.h #include message_filters/sync_policies/approximate_time.h #include cv_bridge/cv_bridge.h #include System.h #include Frame.h #include Map.h #include Tracking.h #include utility.hpp class RgbdSlamNode : public rclcpp::Node { public: explicit RgbdSlamNode(ORB_SLAM3::System* pSLAM, const rclcpp::NodeOptions options rclcpp::NodeOptions()); ~RgbdSlamNode(); private: using ImageMsg sensor_msgs::msg::Image; typedef message_filters::sync_policies::ApproximateTimesensor_msgs::msg::Image, sensor_msgs::msg::Image approximate_sync_policy; void GrabRGBD(const sensor_msgs::msg::Image::SharedPtr msgRGB, const sensor_msgs::msg::Image::SharedPtr msgD); ORB_SLAM3::System* m_SLAM; cv_bridge::CvImageConstPtr cv_ptrRGB; cv_bridge::CvImageConstPtr cv_ptrD; std::shared_ptrmessage_filters::Subscribersensor_msgs::msg::Image rgb_sub; std::shared_ptrmessage_filters::Subscribersensor_msgs::msg::Image depth_sub; std::shared_ptrmessage_filters::Synchronizerapproximate_sync_policy syncApproximate; }; #endif3. 测试建图定位电脑连接上D435深度相机后在终端1中输入ros2 launch realsense2_camera rs_launch.py \ align_depth.enable:true \ depth_module.depth_profile:640x480x30 \ rgb_camera.color_profile:640x480x30完成之前的修改后返回工作空间目录进行编译colcon buildsource install/setup.bashros2 run orbslam3 rgbd ~/d7lros2/orb_slam3_ws/src/ORB_SLAM3_ROS2/vocabulary/ORBvoc.txt ~/d7lros2/orb_slam3_ws/src/ORB_SLAM3_ROS2/config/rgbd/d435.yaml --ros-args -r /camera/color/image_raw:/camera/camera/color/image_raw -r /camera/depth/image_rect_raw:/camera/camera/aligned_depth_to_color/image_raw注意启动ORB_SLAM要遵循官方的教程ros2 run orbslam3 rgbd \ path_to_vocabulary/ORBvoc.txt \ path_to_config/camera_config.yaml \ --ros-args \ -r /camera/color/image_raw:your_rgb_topic \ -r /camera/depth/image_rect_raw:your_depth_topicORBvoc.txtORB 特征词典文件路径通常位于 ORB_SLAM3 源码的Vocabulary/目录下。camera_config.yaml相机参数配置文件如你修改后的d435.yaml。--ros-args -rROS2 话题重映射语法。将节点内部订阅的/camera/color/image_raw重映射为你实际发布的 RGB 图像话题。将节点内部订阅的/camera/depth/image_rect_raw重映射为你实际发布的深度图话题。可以看到两个窗口均正常启动。
阅读完成 · 觉得有帮助？
