如何将cv::Mat转换为带有颜色的pcl::点云

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

本文关键字：有颜色 点云 pcl 转换 cv Mat      更新时间：2023-10-16

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

代码更新

因此，我使用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的值会把所有的东西都染成红色吗？但如果我从彩色图像中选择输出，那么云的颜色就错了？

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

更新

在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？

额外

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

我几乎可以肯定，您的代码中有一个关于这些函数的错误。我尝试了一个简单的例子，遵循你的范式，它运行得很好(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"的文件。

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

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

相关文章：