Ubuntu22.04 + ROS2 Humble + ORB-SLAM3 下从环境配置到深度相机d435实时SLAM

本文详细介绍了ORB-SLAM3在Ubuntu22.04系统上的安装部署过程。首先需要安装基础编译工具和关键依赖库（Eigen3、Pangolin、OpenCV），然后编译ORB-SLAM3核心库并集成到ROS2Humble工作空间。文章还列举了可能遇到的常见错误（如std::bad_array_new_length和doublefree问题）及其解决方法。整个安装过程包括环境准备、依赖安装、源码

MIXLLRED

707人浏览 · 2026-04-10 14:38:17

MIXLLRED · 2026-04-10 14:38:17 发布

一、安装部署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 重映射为你实际发布的深度图话题。

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

全球具身智能开发者社区

立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。

更多推荐

开源聚势·具身启智，杭州这场沙龙给出中国具身智能产业化新答案

全球具身智能开发者社区

YoMo安全机制详解：TLS v1.3如何保护你的AI代理通信

在当今AI驱动的分布式系统中，安全通信已成为不可忽视的核心需求。YoMo作为Serverless AI Agent Framework，采用TLS v1.3加密协议构建了强大的安全防护机制，确保AI代理在地理分布式边缘计算环境中的通信安全。本文将深入解析YoMo的TLS实现原理、配置方法及最佳实践，帮助开发者构建安全可靠的AI应用。## 为什么TLS v1.3是AI代理通信的理想选择TLS

全球具身智能开发者社区

InternScenes开源数据集

数据集介绍 InternScenes 是上海人工智能实验室发布的大规模、可模拟室内场景数据集，论文收录于 NeurIPS 2025。具身人工智能的发展高度依赖于具有场景多样性和逼真布局的大规模、可模拟3D场景数据集。然而，现有数据集通常存在以下不足：数据规模或多样性有限、布局经过"净化"处理导致小物体缺失，以及严重的物体碰撞问题。为解决上述问题，InternScenes 整合