c++ - 如何将 cv::Mat 转换为带颜色的 pcl::pointcloud

Question

我正在尝试将带有 rgb 信息的点云从 pcl 格式转换为 cv::Mat，然后再转换回 pcl。我找到了 convert mat to pointcloud在 stackoverflow 上。

代码已更新

因此，我使用在 stackoverflow 上找到的代码作为起点。现在有以下内容:

//input pcl::PointCloud<pcl::PointXYZRGB> point_cloud_ptr
cv::Mat OpenCVPointCloud(3, point_cloud_ptr.points.size(), CV_64FC1);
OpenCVPointCloudColor = cv::Mat(480, 640, CV_8UC3);
for(int y=0;y<OpenCVPointCloudColor.rows;y++)
{
    for(int x=0;x<OpenCVPointCloudColor.cols;x++)
    {
        OpenCVPointCloud.at<double>(0,y*OpenCVPointCloudColor.cols+x) = point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).x;
        OpenCVPointCloud.at<double>(1,y*OpenCVPointCloudColor.cols+x) = point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).y;
        OpenCVPointCloud.at<double>(2,y*OpenCVPointCloudColor.cols+x) = point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).z;

        cv::Vec3b color = cv::Vec3b(point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).b,point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).g,point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).r);

        OpenCVPointCloudColor.at<cv::Vec3b>(cv::Point(x,y)) = color;
    }
}

cv::imshow("view 2", OpenCVPointCloudColor);
cv::waitKey(30);

显示上面的图像确保我正确提取了颜色(通过肉眼将图像与原始图像进行比较)。然后我想将它转换回点云:

pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

for(int y=0;y<OpenCVPointCloudColor.rows;y++)
{
    for(int x=0;x<OpenCVPointCloudColor.cols;x++)
    {
        pcl::PointXYZRGB point;
        point.x = OpencVPointCloud.at<double>(0,y*OpenCVPointCloudColor.cols+x);
        point.y = OpencVPointCloud.at<double>(1,y*OpenCVPointCloudColor.cols+x);
        point.z = OpencVPointCloud.at<double>(2,y*OpenCVPointCloudColor.cols+x);

            cv::Vec3b color = OpenCVPointCloudColor.at<cv::Vec3b>(cv::Point(x,y));
        //Try 1 not working
        //uint8_t r = (color[2]); 
        //uint8_t g = (color[1]);
        //uint8_t b = (color[0]);

        //Try 2 not working
        //point.r = color[2];
        //point.g = color[1];
        //point.b = color[0];

        //Try 3 not working
        //uint8_t r = (color.val[2]);
        //uint8_t g = (color.val[1]);
        //uint8_t b = (color.val[0]);

        //test working WHY DO THE ABOVE CODES NOT WORK :/
        uint8_t r = 255; 
        uint8_t g = 0;
        uint8_t b = 0;
        int32_t rgb = (r << 16) | (g << 8) | b;
        point.rgb = *reinterpret_cast<float*>(&rgb);

        point_cloud_ptr -> points.push_back(point);
    }
} 

谁能解释为什么明确指定 255,0,0 的值会把所有东西都染成红色。但是如果我选择彩色图像的输出，云的颜色是错误的吗？

偶然发现this ，除了云类型不同之外，我看不出我的代码有什么不同？

更新

读书this关于 PCL 的讨论以我切换到 xyzrgba 结束(也在 stackoverflow 中提到)。我在转换回来时尝试的代码是:

point.b = OpenCVPointCloudColor.at<cv::Vec3b>(x,y)[0];
point.g = OpenCVPointCloudColor.at<cv::Vec3b>(x,y)[1];
point.r = OpenCVPointCloudColor.at<cv::Vec3b>(x,y)[2];
point.a = 255;

生成的颜色云与 XYZRGB 中创建的颜色云不同，但仍然是错误的:/WTF？

额外

即使我在所有点中强制使用红色 OpenCVPointCloudColor使用 cvCloudColor.at<cv::Vec3f>(x,y) = cv::Vec3f(0,0,255);然后从 OpenCVPointCloudColor 读取仍然在 pcl 云中创建错误的颜色信息。

Answer 1

我几乎可以肯定您的代码中围绕这些函数的某处存在错误。我按照你的范例尝试了简单的例子，它完美地工作(从源代码构建的 PCL 1.8，从源代码构建的 OpenCV 3.1，g++ 5.x 或 g++ 6.x，Ubuntu 16.10):

#include <pcl/visualization/cloud_viewer.h>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

void draw_cloud(
    const std::string &text,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
    pcl::visualization::CloudViewer viewer(text);
    viewer.showCloud(cloud);
    while (!viewer.wasStopped())
    {
    }
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr img_to_cloud(
        const cv::Mat& image,
        const cv::Mat &coords)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());

    for (int y=0;y<image.rows;y++)
    {
        for (int x=0;x<image.cols;x++)
        {
            pcl::PointXYZRGB point;
            point.x = coords.at<double>(0,y*image.cols+x);
            point.y = coords.at<double>(1,y*image.cols+x);
            point.z = coords.at<double>(2,y*image.cols+x);

            cv::Vec3b color = image.at<cv::Vec3b>(cv::Point(x,y));
            uint8_t r = (color[2]);
            uint8_t g = (color[1]);
            uint8_t b = (color[0]);

            int32_t rgb = (r << 16) | (g << 8) | b;
            point.rgb = *reinterpret_cast<float*>(&rgb);

            cloud->points.push_back(point);
        }
    }
    return cloud;
}

void cloud_to_img(
        const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
        cv::Mat &coords,
        cv::Mat &image)
{
    coords = cv::Mat(3, cloud->points.size(), CV_64FC1);
    image = cv::Mat(480, 640, CV_8UC3);
    for(int y=0;y<image.rows;y++)
    {
        for(int x=0;x<image.cols;x++)
        {
            coords.at<double>(0,y*image.cols+x) = cloud->points.at(y*image.cols+x).x;
            coords.at<double>(1,y*image.cols+x) = cloud->points.at(y*image.cols+x).y;
            coords.at<double>(2,y*image.cols+x) = cloud->points.at(y*image.cols+x).z;

            cv::Vec3b color = cv::Vec3b(
                    cloud->points.at(y*image.cols+x).b,
                    cloud->points.at(y*image.cols+x).g,
                    cloud->points.at(y*image.cols+x).r);

            image.at<cv::Vec3b>(cv::Point(x,y)) = color;
        }
    }
}

int main(int argc, char *argv[])
{
    cv::Mat image = cv::imread("test.png");
    cv::resize(image, image, cv::Size(640, 480));
    cv::imshow("initial", image);

    cv::Mat coords(3, image.cols * image.rows, CV_64FC1);
    for (int col = 0; col < coords.cols; ++col)
    {
        coords.at<double>(0, col) = col % image.cols;
        coords.at<double>(1, col) = col / image.cols;
        coords.at<double>(2, col) = 10;
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = img_to_cloud(image, coords);
    draw_cloud("points", cloud);

    cloud_to_img(cloud, coords, image);
    cv::imshow("returned", image);

    cv::waitKey();
    return 0;
}

“初始”和“返回”图像完全相同。在 PCL 的查看器中，我将我的图像视为点云，当然，z = 10，因为我对其进行了硬编码。您可能应该使用鼠标滚轮来缩小并查看整个图像。

要运行此示例，您的工作目录中应该有名为“test.png”的文件。

我很抱歉硬编码输入文件名并调整到固定分辨率。

尝试一下，如果它在您的系统中有效，请尝试在您的代码中查找错误。如果它不起作用，则可能是您的 PCL 版本太旧甚至损坏。