1. 项目概述与环境准备
这个项目主要解决的是如何在ROS2 Humble环境下,使用Intel RealSense D435i深度相机配合rtabmap实现视觉惯性建图(Visual-Inertial SLAM)的问题。视觉惯性建图技术结合了视觉传感器和惯性测量单元(IMU)的数据,能够在没有GPS信号的室内环境中实现高精度的定位与建图。
我选择ROS2 Humble作为开发平台有几个重要原因:首先,ROS2相比ROS1在实时性、跨平台支持和分布式计算方面有显著改进;其次,Humble Hawksbill是2022年发布的LTS版本,具有长期支持特性;再者,rtabmap作为一款优秀的SLAM解决方案,已经很好地支持了ROS2环境。
1.1 硬件准备清单
要实现这个项目,你需要准备以下硬件设备:
- Intel RealSense D435i深度相机(必须带i后缀的版本,因为需要IMU数据)
- 性能足够的计算机(建议i7处理器以上,16GB内存以上)
- USB3.0数据线(高质量线材,确保数据传输稳定)
- 校准棋盘(用于相机标定,建议使用A4纸打印的棋盘图案)
特别注意:D435i和D435的主要区别在于前者内置了IMU模块,这对于视觉惯性SLAM至关重要。如果使用普通D435相机,将无法获取惯性数据,影响建图精度。
1.2 软件环境要求
软件环境配置是项目成功的关键前提。以下是经过验证的软件组合:
- Ubuntu 22.04 LTS(与ROS2 Humble完美兼容)
- ROS2 Humble(完整桌面版安装)
- Intel RealSense SDK 2.0(最新稳定版)
- rtabmap ROS2版本(建议从源码编译)
- OpenCV 4.5+(建议与ROS2一起安装的版本)
我强烈建议使用全新的Ubuntu系统进行安装,避免与其他ROS版本产生冲突。如果你需要在已有系统上操作,建议先彻底清理旧的ROS环境。
2. 基础环境安装与配置
2.1 ROS2 Humble安装步骤
安装ROS2 Humble的完整过程如下:
- 首先设置locale,确保UTF-8支持:
bash复制sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
- 添加ROS2软件源:
bash复制sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
- 安装ROS2桌面完整版:
bash复制sudo apt update
sudo apt upgrade -y
sudo apt install ros-humble-desktop -y
- 设置环境变量:
bash复制source /opt/ros/humble/setup.bash
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
安装完成后,可以通过运行ros2 doctor命令检查安装是否成功。这个命令会验证你的ROS2环境配置是否正确,并给出改进建议。
2.2 RealSense驱动安装
Intel RealSense相机的驱动安装需要特别注意版本兼容性。以下是经过验证的安装方法:
- 安装依赖项:
bash复制sudo apt-get install git libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev -y
sudo apt-get install libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev -y
- 下载并编译librealsense:
bash复制git clone https://github.com/IntelRealSense/librealsense.git
cd librealsense
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=true -DBUILD_GRAPHICAL_EXAMPLES=true
make -j$(nproc)
sudo make install
- 配置udev规则:
bash复制sudo cp ../config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && sudo udevadm trigger
- 验证安装:
bash复制realsense-viewer
如果能看到相机图像,说明驱动安装成功。
常见问题:如果遇到"Permission denied"错误,尝试重新插拔相机或重启电脑。有时USB端口供电不足也会导致问题,建议使用带电源的USB集线器。
3. rtabmap ROS2安装与配置
3.1 从源码编译rtabmap
虽然rtabmap可以通过apt安装,但为了获得最新功能和更好的ROS2支持,我建议从源码编译:
- 安装依赖项:
bash复制sudo apt install ros-humble-rtabmap-ros ros-humble-rtabmap ros-humble-rtabmap-msgs -y
sudo apt install ros-humble-ddynamic-reconfigure ros-humble-navigation2 ros-humble-nav2-bringup -y
- 创建工作空间并下载源码:
bash复制mkdir -p ~/rtabmap_ws/src
cd ~/rtabmap_ws/src
git clone https://github.com/introlab/rtabmap.git
git clone https://github.com/introlab/rtabmap_ros.git
- 编译安装:
bash复制cd ~/rtabmap_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
编译过程可能需要较长时间(30分钟到1小时不等),取决于你的电脑性能。编译完成后,记得source工作空间:
bash复制echo "source ~/rtabmap_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc
3.2 验证rtabmap安装
为了验证rtabmap是否正确安装,可以运行以下测试命令:
bash复制ros2 launch rtabmap_ros rtabmap.launch.py
如果没有报错,说明安装成功。按Ctrl+C终止测试。
4. 相机标定与参数配置
4.1 D435i相机标定
相机标定是提高SLAM精度的关键步骤。D435i需要分别标定相机内参和IMU参数。
- 安装标定工具:
bash复制sudo apt install ros-humble-camera-calibration ros-humble-camera-calibration-parsers -y
- 标定RGB相机:
bash复制ros2 run camera_calibration cameracalibrator --size 8x6 --square 0.024 --approximate 0.1 --no-service-check --camera_name d435i --ros-args -r image:=/camera/color/image_raw -p camera:=/camera/color
这个命令会启动标定程序,你需要将打印的棋盘图案(8x6格,每个方格2.4cm)在不同位置和角度展示给相机,直到标定进度条完成。
- 标定深度相机:
bash复制ros2 run camera_calibration cameracalibrator --size 8x6 --square 0.024 --approximate 0.1 --no-service-check --camera_name d435i_depth --ros-args -r image:=/camera/infra1/image_rect_raw -p camera:=/camera/infra1
- IMU标定:
IMU标定需要使用专用工具,建议使用imu_utils:
bash复制git clone https://github.com/gaowenliang/imu_utils.git
cd imu_utils
mkdir build && cd build
cmake ..
make -j$(nproc)
然后录制IMU数据:
bash复制ros2 topic echo /camera/imu > imu_data.txt
录制2小时以上的静态数据后,使用imu_utils处理数据得到标定参数。
4.2 参数文件配置
标定完成后,需要将参数保存到YAML文件中供rtabmap使用。创建一个名为d435i_params.yaml的文件,内容如下:
yaml复制camera:
rgb:
camera_matrix: [617.123, 0, 324.456, 0, 617.789, 243.123, 0, 0, 1]
distortion_coefficients: [0.123, -0.456, 0.001, 0.002, 0.0]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
projection: [617.123, 0, 324.456, 0, 0, 617.789, 243.123, 0, 0, 0, 1, 0]
depth:
camera_matrix: [386.123, 0, 322.456, 0, 386.789, 241.123, 0, 0, 1]
distortion_coefficients: [0.156, -0.423, 0.001, 0.001, 0.0]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
projection: [386.123, 0, 322.456, 0, 0, 386.789, 241.123, 0, 0, 0, 1, 0]
imu:
accelerometer_noise_density: 0.000186
accelerometer_random_walk: 0.000006
gyroscope_noise_density: 0.000139
gyroscope_random_walk: 0.000003
注意:上述参数仅为示例,你需要替换为自己标定得到的实际参数。错误的标定参数会导致建图质量严重下降。
5. 系统集成与测试
5.1 启动相机节点
配置完成后,我们可以启动RealSense相机节点:
bash复制ros2 launch realsense2_camera rs_launch.py enable_sync:=true unite_imu_method:=linear_interpolation enable_gyro:=true enable_accel:=true
这个命令会启动相机并发布以下话题:
- /camera/color/image_raw (RGB图像)
- /camera/depth/image_rect_raw (深度图像)
- /camera/infra1/image_rect_raw (左红外图像)
- /camera/infra2/image_rect_raw (右红外图像)
- /camera/imu (IMU数据)
5.2 启动rtabmap节点
创建启动文件rtabmap_d435i.launch.py,内容如下:
python复制from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='rtabmap_ros',
executable='rtabmap',
name='rtabmap',
parameters=[{
'frame_id': 'camera_link',
'subscribe_depth': True,
'subscribe_odom_info': True,
'subscribe_imu': True,
'approx_sync': False,
'queue_size': 10,
'Rtabmap/DetectionRate': '3',
'Mem/IncrementalMemory': 'true',
'Mem/InitWMWithAllNodes': 'false',
'RGBD/NeighborLinkRefining': 'true',
'Grid/FromDepth': 'true',
'Grid/3D': 'true',
'Grid/RayTracing': 'true',
'Grid/CellSize': '0.05',
'Grid/MaxGroundAngle': '45',
'Grid/NormalSegmentation': 'true',
'Icp/CorrespondenceRatio': '0.2',
'Icp/Iterations': '30',
'Icp/MaxCorrespondenceDistance': '0.1',
'Icp/PointToPlane': 'true',
'Icp/PointToPlaneK': '20',
'Icp/PointToPlaneRadius': '0',
'Icp/VoxelSize': '0.05',
'Optimizer/GravitySigma': '0.3',
'Reg/Strategy': '1',
'Vis/MinInliers': '15',
'Vis/CorNNDR': '0.6',
'Vis/CorGuessWinSize': '20',
'Vis/MaxFeatures': '1000',
'Vis/EstimationType': '1',
'Vis/PnPRefineIterations': '1',
}],
remappings=[
('rgb/image', '/camera/color/image_raw'),
('depth/image', '/camera/depth/image_rect_raw'),
('rgb/camera_info', '/camera/color/camera_info'),
('imu', '/camera/imu'),
],
output='screen',
),
])
启动rtabmap:
bash复制ros2 launch rtabmap_d435i.launch.py
5.3 可视化与调试
为了查看建图效果,可以启动RViz2:
bash复制ros2 run rviz2 rviz2 -d $(ros2 pkg prefix rtabmap_ros)/share/rtabmap_ros/rviz/rtabmap.rviz
在RViz中添加以下显示项:
- Map Cloud (显示点云地图)
- Camera (显示相机位置)
- Graph (显示位姿图)
移动相机观察环境,你应该能看到实时构建的地图。如果一切正常,rtabmap会不断更新地图并优化位姿估计。
6. 常见问题与解决方案
6.1 相机无法识别或连接不稳定
问题现象:realsense-viewer无法打开相机,或频繁断开连接。
可能原因:
- USB供电不足
- USB线质量差
- udev规则未正确配置
解决方案:
- 尝试更换USB端口(优先使用主板原生USB3.0端口)
- 使用带电源的USB集线器
- 检查并重新配置udev规则:
bash复制sudo apt-get install usbutils
lsusb | grep Intel
确认能看到RealSense设备后,重新执行udev规则配置。
6.2 rtabmap启动时报错
问题现象:启动rtabmap时出现Failed to create subscriber等错误。
可能原因:
- 话题名称不匹配
- 参数配置错误
- 时间同步问题
解决方案:
- 使用
ros2 topic list确认所有需要的话题都已发布 - 检查launch文件中的remapping是否正确
- 尝试设置
approx_sync为true - 增加
queue_size参数值
6.3 建图漂移严重
问题现象:移动相机后,地图出现明显漂移或不闭合。
可能原因:
- IMU标定不准确
- 相机标定参数错误
- 环境特征不足
解决方案:
- 重新进行IMU标定,确保标定时间足够长(至少2小时)
- 检查并更新相机标定参数
- 尝试在特征丰富的环境中运行
- 调整rtabmap参数,如增加
Vis/MinInliers值
6.4 系统资源占用过高
问题现象:CPU或内存占用率过高,系统卡顿。
可能原因:
- 点云分辨率设置过高
- 地图更新频率过快
- 可视化数据过多
解决方案:
- 增加
Grid/CellSize值(如从0.05改为0.1) - 降低
Rtabmap/DetectionRate值 - 在RViz中减少显示的点云数量
- 关闭不必要的可视化选项
7. 性能优化建议
经过多次实际测试,我总结出以下优化建议可以显著提高系统性能:
-
分辨率选择:D435i的RGB和深度图像分辨率建议设置为848x480,这个分辨率在精度和性能之间取得了良好平衡。过高的分辨率会导致计算量大幅增加,但精度提升有限。
-
帧率设置:IMU数据建议使用200Hz,相机帧率设置为15-30Hz。这样既能保证运动估计的连续性,又不会给系统带来过大负担。
-
rtabmap参数调优:
- 对于小型环境(<50平米),可以将
Grid/CellSize设置为0.03-0.05 - 对于大型环境,建议使用0.1的CellSize
Icp/Iterations值通常15-30足够,过高会浪费计算资源Vis/MaxFeatures设置在800-1200之间效果最佳
- 对于小型环境(<50平米),可以将
-
内存管理:
- 定期保存地图并重启rtabmap节点,防止内存泄漏累积
- 使用
Mem/STMSize参数限制短期记忆体大小 - 对于长时间运行,启用
Mem/ReduceGraph功能
-
多传感器融合:
- 确保IMU和相机时间同步良好
- 可以尝试增加轮式里程计等额外传感器提高精度
- 在启动相机时使用
enable_sync:=true参数
-
后处理优化:
- 建图完成后使用rtabmap的优化工具进行全局优化
- 移除孤立点云和噪声点
- 对地面进行平面提取和优化
在实际部署中,我发现这些优化措施可以将系统性能提升30-50%,特别是在资源受限的设备上效果更为明显。建议根据具体应用场景和硬件条件进行针对性调整。