如何使用颜色将 cv::Mat 转换为 pcl::pointcloud

Question

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

代码已更新

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

//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 discussion on PCL ended with me switching to xyzrgba (also mentioned on )。我在转换回来时尝试的代码是：

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?

额外

即使我使用 cvCloudColor.at<cv::Vec3f>(x,y) = cv::Vec3f(0,0,255); 在所有点 OpenCVPointCloudColor 中强制使用红色，然后从 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;
}

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

对于运行这个例子，你的工作目录中应该有一个名为 'test.png' 的文件。

对于硬编码输入文件名并调整到固定分辨率，我深表歉意。

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

如何使用颜色将 cv::Mat 转换为 pcl::pointcloud

How to convert cv::Mat to pcl::pointcloud with color

c++

opencv

point-clouds

ros

point-cloud-library