ROS 2 + OpenCV 实战：基于 RGB-D 相机提取 ORB 特征点并获取真实物理距离

在视觉 SLAM（同步定位与建图）或机器人自主导航中，仅仅提取图像的 2D 特征点是不够的，我们往往还需要知道这些点在三维空间中的真实位置。对于拥有深度传感器的 RGB-D 相机（如 Intel RealSense 或各种仿真环境中的 RGB-D 相机），我们可以直接通过查表的方式获取特征点的真实深度。

今天，我想分享一个在 ROS 2 环境下编写的 C++ 节点实战案例：如何接收并同步 RGB 与 Depth 图像话题，提取 ORB 特征点，并实时获取这些特征点的物理距离。

💡 核心挑战与解决思路

在处理 RGB-D 数据时，最大的痛点通常是时间同步。

彩色图像和深度图像往往是通过两个不同的话题（Topic）发布的。由于系统延迟和传感器采样的差异，它们的时间戳很难完全一致。如果直接在两个独立的回调函数中处理，极易导致彩色图和深度图对不齐，从而产生“鬼影”或测距错误。

为了解决这个问题，我们使用了 ROS 2 的 message_filters，采用**近似时间同步（ApproximateTime）**策略，强制将时间戳最接近的 RGB 和 Depth 帧绑定在一起，送入同一个回调函数中同步处理。

💻 完整源码实现

以下是核心节点 rgbd_orb_node.cpp 的完整代码：

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>

// 引入消息同步核心头文件
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

using std::placeholders::_1;
using std::placeholders::_2;

class RgbdOrbNode : public rclcpp::Node {
public:
    RgbdOrbNode() : Node("rgbd_orb_node") {
        // 配置专门用来接收 Gazebo 传感器数据的 QoS (Best Effort)
        rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
        auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile);

        // 初始化两个订阅者：一个看色彩，一个看深度（注意话题名字要与你的环境对应！）
        rgb_sub_.subscribe(this, "/camera/camera/image_raw", qos.get_rmw_qos_profile());
        depth_sub_.subscribe(this, "/camera/camera/depth/image_raw", qos.get_rmw_qos_profile());

        // 设置“近似时间同步”策略（允许极小的时间戳误差），队列长度设为 10
        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> SyncPolicy;
        sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), rgb_sub_, depth_sub_);
        
        // 当两张图同时到达时，触发统一的回调函数
        sync_->registerCallback(std::bind(&RgbdOrbNode::image_callback, this, _1, _2));

        // 初始化 ORB 特征提取器，最多提取 500 个点
        orb_detector_ = cv::ORB::create(500);
        RCLCPP_INFO(this->get_logger(), "RGB-D 视觉中枢已启动，正在等待双目图像同步...");
    }

private:
    void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg,
                        const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) const {
        try {
            // 1. 转换彩色图像为 BGR8 格式
            cv_bridge::CvImagePtr cv_ptr_rgb = cv_bridge::toCvCopy(rgb_msg, sensor_msgs::image_encodings::BGR8);
            
            // 2. 转换深度图像 (Gazebo 默认的深度格式是 32 位浮点数，单位为米)
            cv_bridge::CvImagePtr cv_ptr_depth = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_32FC1);

            std::vector<cv::KeyPoint> keypoints;
            cv::Mat descriptors;
            
            // 在彩色图上提取 ORB 特征点
            orb_detector_->detectAndCompute(cv_ptr_rgb->image, cv::noArray(), keypoints, descriptors);

            cv::Mat image_with_keypoints = cv_ptr_rgb->image.clone();
            int print_count = 0; // 限制打印频率，防止终端刷屏

            // 3. 遍历提取到的特征点，去深度图里查距离
            for (const auto& kp : keypoints) {
                int u = cvRound(kp.pt.x); // 像素 X 坐标 (列)
                int v = cvRound(kp.pt.y); // 像素 Y 坐标 (行)

                // 安全校验：防止坐标越界导致段错误崩溃
                if (u >= 0 && u < cv_ptr_depth->image.cols && v >= 0 && v < cv_ptr_depth->image.rows) {
                    
                    // 读取对应像素点的深度值（Z 轴距离）
                    // 注意：OpenCV Mat 的访问顺序是 at<类型>(行, 列) 即 (v, u)
                    float depth_val = cv_ptr_depth->image.at<float>(v, u);

                    // 画出绿色的特征点标记
                    cv::circle(image_with_keypoints, kp.pt, 3, cv::Scalar(0, 255, 0), -1);

                    // 过滤掉测距无效的点（例如天空、无穷远和近地噪点）
                    if (depth_val > 0.1 && depth_val < 50.0) {
                        if (print_count < 3) {
                            RCLCPP_INFO(this->get_logger(), "抓取特征点 (像素X:%d, Y:%d) -> 真实物理距离: %.2f 米", u, v, depth_val);
                            print_count++;
                        }
                        
                        // 可视化：直接把距离数字写在画面上 (蓝色字体)
                        std::string distance_text = std::to_string(depth_val).substr(0,4) + "m";
                        cv::putText(image_with_keypoints, distance_text, 
                                    kp.pt, cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 0, 0), 1);
                    }
                }
            }

            // 4. 显示最终合成的图像
            cv::imshow("RGB-D ORB Features", image_with_keypoints);
            cv::waitKey(1); 
            
        } catch (cv_bridge::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge 异常: %s", e.what());
        }
    }

    // 成员变量定义
    message_filters::Subscriber<sensor_msgs::msg::Image> rgb_sub_;
    message_filters::Subscriber<sensor_msgs::msg::Image> depth_sub_;
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> SyncPolicy;
    std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
    cv::Ptr<cv::ORB> orb_detector_;
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RgbdOrbNode>());
    rclcpp::shutdown();
    return 0;
}

🔍 代码避坑指南 (Tips)

在这段代码中，有几个非常值得注意的细节，稍不注意就会踩坑：

• QoS 配置至关重要： 针对 Gazebo 等仿真器发出的高频传感器数据流，普通的 QoS 策略（Reliable）容易导致网络阻塞、丢包或严重延迟。代码中使用了 rmw_qos_profile_sensor_data（即 Best Effort 策略），这对于实时性要求高的图像接收非常有帮助。
• 深度的像素坐标读取： 在 OpenCV 中，获取像素坐标为 $(u,v)$ 的值时，必须使用 image.at<float>(v, u)，也就是先 Y（行）后 X（列）。如果写反了，极易引发指针越界并导致节点崩溃（Segment Fault）。
• 深度数据类型匹配： 在使用 cv_bridge 转换深度图时，编码格式设置为 TYPE_32FC1。因为大部分 3D 传感器（包括 Gazebo 仿真）在 ROS 中的默认深度输出是以“米”为单位的 32 位单精度浮点数。

⚙️ 编译配置补充

为了让上述节点顺利编译，你需要在功能包配置中添加相关依赖：

1. package.xml 添加依赖：

<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>message_filters</depend>

2. CMakeLists.txt 配置：

find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(message_filters REQUIRED)
find_package(OpenCV REQUIRED)

add_executable(rgbd_orb_node src/rgbd_orb_node.cpp)
ament_target_dependencies(rgbd_orb_node 
  rclcpp 
  sensor_msgs 
  cv_bridge 
  message_filters 
  OpenCV
)

install(TARGETS
  rgbd_orb_node
  DESTINATION lib/${PROJECT_NAME}
)

结语

通过将 message_filters 的时间同步与 OpenCV 的 2D 图像处理能力结合，我们可以轻松且稳定地获取环境的三维结构信息。这是构建视觉里程计 (Visual Odometry) 、目标追踪和 3D 避障的坚实第一步。