企业官网建设流程全解析

一、安装部署ORB-SLAM3

1. 准备工作与基础环境

首先，请确保你的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 tar

2.安装关键依赖库

ORB-SLAM3的运行依赖于多个第三方库，需要逐一正确安装。如果之前二进制安装过以下包，需要先把它们都卸载移除。

（1）Eigen3 (版本 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 install

（2）Pangolin (版本 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_TYPE=Release

make -j$(nproc)

安装：

sudo make install

（3）OpenCV (推荐4.5.4)

Ubuntu 22.04的官方源默认提供，可直接安装：

sudo apt install -y libopencv-dev python3-opencv

3. 编译ORB-SLAM3核心库

独立编译ORB-SLAM3算法库，确保其在不依赖ROS的情况下能正常工作。

（1）克隆源码

在用户主目录下克隆官方仓库：

cd ~ git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git

cd 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} -march=native") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") # Check C++11 or C++0x support include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-DCOMPILEDWITHC11) message(STATUS "Using flag -std=c++11.") elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") add_definitions(-DCOMPILEDWITHC0X) message(STATUS "Using flag -std=c++0x.") else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 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.sh

3. 在ROS2工作空间中集成

将ORB-SLAM3库集成到ROS2 Humble环境中。

（1）创建ROS2工作空间

如果还没有专门的工作空间，请创建一个：

mkdir -p ~/orb_slam3_ws/src

cd ~/orb_slam3_ws/src

（2）克隆ROS2封装代码

在src目录下克隆社区提供的ROS2封装：

git clone https://github.com/benaissa-tayeb37/orb_slam3_ros2.git

（3）配置与编译

返回工作空间根目录，安装ROS2依赖并编译。

cd ~/orb_slam3_ws colcon build --symlink-install

二、可能遇到的问题

1. std::bad_array_new_length

问题：

robot@ll-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)

问题：

robot@ll-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 robot@ll-Lenovo:~/d7lros2/orb_slam3_ws$

解决方法见文章：

解决：ShutdownSaving keyframe trajectory to KeyFrameTrajectory.txt ...double free or corruption

三、深度相机的配置和ORB_SLAM3建图启动

1. 相机配置文件修改

（1）获取深度相机内参

首先，查看在ROS2 封装代码工作空间打开终端，启动深度相机并查看深度相机内参：

终端1：

ros2 launch realsense2_camera rs_launch.py \ align_depth.enable:=true \ depth_module.depth_profile:=640x480x30 \ rgb_camera.color_profile:=640x480x30

终端2：

ros2 topic echo /camera/camera/color/image_raw --once | grep -A 9 "k:"

我的结果为：

wcr@wcr-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. 修改封装代码源码

需要修改的代码如下：

robot@ll-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

文件和路径可以参考上面内容。

（1）rgbd.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_shared<RgbdSlamNode>(&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; }

（2）rgbd-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_shared<message_filters::Subscriber<ImageMsg>>(this, "/camera/color/image_raw"); depth_sub = std::make_shared<message_filters::Subscriber<ImageMsg>>(this, "/camera/depth/image_rect_raw"); // 同步策略：近似时间同步，队列大小 10 syncApproximate = std::make_shared<message_filters::Synchronizer<approximate_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)); }

（3）rgbd-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::ApproximateTime<sensor_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_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > rgb_sub; std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > depth_sub; std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy> > syncApproximate; }; #endif

3. 测试建图定位

电脑连接上D435深度相机后，在终端1中输入：

ros2 launch realsense2_camera rs_launch.py \ align_depth.enable:=true \ depth_module.depth_profile:=640x480x30 \ rgb_camera.color_profile:=640x480x30

完成之前的修改后，返回工作空间目录，进行编译：

colcon build

source install/setup.bash

ros2 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_topic>

• ORBvoc.txt：ORB 特征词典文件路径（通常位于 ORB_SLAM3 源码的Vocabulary/目录下）。

• camera_config.yaml：相机参数配置文件（如你修改后的d435.yaml）。

• --ros-args -r：ROS2 话题重映射语法。

  • 将节点内部订阅的/camera/color/image_raw重映射为你实际发布的 RGB 图像话题。

  • 将节点内部订阅的/camera/depth/image_rect_raw重映射为你实际发布的深度图话题。

可以看到两个窗口均正常启动。