基于PCL实现RGB-D图像转换为点云

  原理： RGB 和 depth图已经对齐了，也就是 depth 图中某个位置的深度值在 RGB图中同样的位置处就是它对应的颜色。假设相机内参矩阵为：

则RGB-D图像转换为点云代码如下：

for (int m = 0; m < depth.rows; m++)
    for (int n = 0; n < depth.cols; n++)
    {
        // 获取深度图中(m,n)处的值
        ushort d = depth.ptr<ushort>(m)[n];
        // d 可能没有值，若如此，跳过此点
        if (d == 0)
            continue;
        // d 存在值，则向点云增加一个点
        PointT p;
        // 计算这个点的空间坐标
        p.z = double(d) / camera.scale;
        p.x = (n - camera.cx) * p.z / camera.fx;
        p.y = (m - camera.cy) * p.z / camera.fy;

        // 从rgb图像中获取它的颜色
        p.b = rgb.ptr<uchar>(m)[n * 3];
        p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
        p.r = rgb.ptr<uchar>(m)[n * 3 + 2];

        // 把p加入到点云中
        cloud->points.push_back(p);
    }

注意：OpenCV里 RGB 图像里存储颜色顺序为BGR，所以从 RGB中获取颜色时顺序也是 BGR而不是RGB。