4. 传感器基础：深度相机（RGB-D）点云数据获取与处理

说到机器人自主避障，深度相机是个绕不开的传感器。我个人觉得，RGB-D相机就像是给机器人装上了一双「带尺子的眼睛」——不光能看见颜色，还能知道每个像素离自己有多远。

这一章，咱们就聊聊怎么把深度相机的原始数据变成机器人能用的点云。嗯，这里要注意，不是所有深度数据都能直接用，中间有不少坑要踩。

4.1 深度相机的工作原理

市面上主流的RGB-D相机，说白了就三种技术路线：

• 结构光法：像Kinect v1那样，投射红外光斑，通过变形计算深度。我早期做项目用过，室内效果不错，一到室外就废了——太阳光里的红外成分会直接淹没信号。
• 飞行时间法（ToF）：Kinect v2、Azure Kinect都用这个。发射光脉冲，测量反射时间。速度快，但多路径反射会引入噪声。
• 双目立体视觉：靠两个摄像头视差算深度。Intel RealSense系列就是典型。不依赖主动光源，但需要纹理特征，白墙面前直接「瞎」掉。

我个人习惯用ToF相机做避障，因为它在室内外都能用，而且帧率够高。不过你要记住一点：没有完美的深度传感器，只有合适的应用场景。

4.2 ROS中的深度数据流

在ROS里，深度相机发布的数据主要有两种格式：

你可能会问：「直接用点云不好吗？干嘛还要深度图？」

嗯，这里有个性能问题。点云数据量很大，一张640×480的深度图转成点云，就是30万个三维点。如果你做实时避障，每帧都处理完整点云，CPU直接拉满。我建议的策略是：需要精细建模时用点云，快速避障时用深度图投影。

4.3 点云获取实战

好，咱们直接上手。下面是一个ROS节点，订阅深度图并转成点云：

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

class DepthToPointcloud {
public:
    DepthToPointcloud() {
        // 订阅深度图像话题
        sub_ = nh_.subscribe("/camera/depth/image_rect_raw", 1, 
                             &DepthToPointcloud::depthCallback, this);
        // 发布点云话题
        pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/my_cloud", 1);
        
        // 获取相机内参（这里假设已知，实际应从camera_info话题读取）
        fx_ = 525.0;  // 焦距x
        fy_ = 525.0;  // 焦距y
        cx_ = 319.5;  // 光心x
        cy_ = 239.5;  // 光心y
    }

    void depthCallback(const sensor_msgs::ImageConstPtr& msg) {
        // 将ROS图像转为OpenCV格式
        cv_bridge::CvImagePtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
        } catch (cv_bridge::Exception& e) {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }

        // 创建PCL点云对象
        pcl::PointCloud<pcl::PointXYZRGB> cloud;
        cloud.width = msg->width;
        cloud.height = msg->height;
        cloud.is_dense = false;
        cloud.points.resize(cloud.width * cloud.height);

        // 遍历每个像素，计算三维坐标
        for (int v = 0; v < msg->height; v++) {
            for (int u = 0; u < msg->width; u++) {
                float z = cv_ptr->image.at<float>(v, u);
                
                // 跳过无效深度值（NaN或0）
                if (std::isnan(z) || z <= 0.001) {
                    cloud.points[v * msg->width + u].x = std::numeric_limits<float>::quiet_NaN();
                    cloud.points[v * msg->width + u].y = std::numeric_limits<float>::quiet_NaN();
                    cloud.points[v * msg->width + u].z = std::numeric_limits<float>::quiet_NaN();
                    continue;
                }

                // 根据针孔相机模型反投影
                float x = (u - cx_) * z / fx_;
                float y = (v - cy_) * z / fy_;
                
                cloud.points[v * msg->width + u].x = x;
                cloud.points[v * msg->width + u].y = y;
                cloud.points[v * msg->width + u].z = z;
                
                // 颜色可以后续从RGB图像获取，这里先设为白色
                cloud.points[v * msg->width + u].r = 255;
                cloud.points[v * msg->width + u].g = 255;
                cloud.points[v * msg->width + u].b = 255;
            }
        }

        // 发布点云
        sensor_msgs::PointCloud2 output;
        pcl::toROSMsg(cloud, output);
        output.header = msg->header;
        pub_.publish(output);
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    ros::Publisher pub_;
    float fx_, fy_, cx_, cy_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "depth_to_pointcloud");
    DepthToPointcloud converter;
    ros::spin();
    return 0;
}

4.4 点云预处理——别让噪声毁了你的避障

原始点云直接拿来用？不行的。你想想看，深度相机在边缘处、反光表面、远距离都会产生大量噪声。我一般会做三步处理：

1. 直通滤波：只保留感兴趣的距离范围。比如避障时，10米外的点直接扔掉，省计算量。
2. 体素滤波：用立方体网格对点云降采样。我常用0.02m的体素大小，既能保留结构，又能把点数降到原来的十分之一。
3. 统计滤波：去除离群点。每个点检查周围k个邻居的平均距离，偏离太远的直接干掉。

下面是用PCL做体素滤波的代码片段：

#include <pcl/filters/voxel_grid.h>

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud(cloud_input);
sor.setLeafSize(0.02f, 0.02f, 0.02f);  // 2cm体素
sor.filter(*cloud_filtered);

std::cout << "滤波前点数: " << cloud_input->points.size() << std::endl;
std::cout << "滤波后点数: " << cloud_filtered->points.size() << std::endl;

4.5 从点云到障碍物——核心思路

拿到干净的点云后，怎么判断有没有障碍物？我常用的方法有两种：

• 栅格化投影：把点云投影到地面平面，统计每个栅格内的点云高度差。如果某个栅格内最高点和最低点差距超过阈值（比如20cm），就认为有障碍物。
• 欧几里得聚类：用PCL的 EuclideanClusterExtraction 把空间上靠近的点聚成一坨，每坨就是一个障碍物候选。

我个人更推荐栅格化方法做避障，因为它计算量小、实时性好。聚类方法适合做物体识别，但实时避障时容易把相邻物体误判成一个。

4.6 本章知识体系

下面这张图帮你理清深度相机数据处理的完整链路：

这张图把整个流程串起来了。从传感器原始数据，到ROS消息，再到点云生成、预处理，最后做障碍物检测。每一步都有坑，但每一步也都有成熟的解决方案。

4.7 本章小结

深度相机是机器人感知的重要工具，但用好它需要理解原理、熟悉ROS接口、掌握点云处理技巧。我个人觉得，初学者最容易犯的错误就是拿到点云直接往上堆算法，忽略了预处理的重要性。

记住三个关键点：

• 深度图转点云时，相机内参必须准确
• 点云预处理不是锦上添花，是雪中送炭
• 避障场景下，栅格化方法比聚类更实用

下一章咱们会聊点云数据如何用于路径规划，到时候这些预处理后的点云就能派上大用场了。

可可精品资料库