pcl :: Pointcloud to cv :: Mat深度图像

Question

我正在将此深度图像转换为pcl :: pointcloud。

使用以下内容：

PointCloud::Ptr PointcloudUtils::RGBDtoPCL(cv::Mat depth_image, Eigen::Matrix3f& _intrinsics)
{
    PointCloud::Ptr pointcloud(new PointCloud);

    float fx = _intrinsics(0, 0);
    float fy = _intrinsics(1, 1);
    float cx = _intrinsics(0, 2);
    float cy = _intrinsics(1, 2);

    float factor = 1;

    depth_image.convertTo(depth_image, CV_32F); // convert the image data to float type 

    if (!depth_image.data) {
        std::cerr << "No depth data!!!" << std::endl;
        exit(EXIT_FAILURE);
    }

    pointcloud->width = depth_image.cols; //Dimensions must be initialized to use 2-D indexing 
    pointcloud->height = depth_image.rows;
    pointcloud->resize(pointcloud->width*pointcloud->height);

#pragma omp parallel for
    for (int v = 0; v < depth_image.rows; v += 4)
    {
        for (int u = 0; u < depth_image.cols; u += 4)
        {
            float Z = depth_image.at<float>(v, u) / factor;

            PointT p;
            p.z = Z;
            p.x = (u - cx) * Z / fx;
            p.y = (v - cy) * Z / fy;

            p.z = p.z / 1000;
            p.x = p.x / 1000;
            p.y = p.y / 1000;

            pointcloud->points.push_back(p);

        }
    }

    return pointcloud;

}

这很好用，我在云上运行了一些处理，现在我需要将pointcloud转换回cv :: Mat深度图像。我找不到一个这样的例子，我很难找到它。与上述功能相反的是什么？

如何将pcl :: pointcloud转换回cv :: mat？

谢谢。

Answer 1

这是未经测试的代码，因为我的机器上没有点云。 根据您自己的转换代码，我假设您的图片是单个频道图片。

void PCL2Mat(PointCloud::Ptr pointcloud, cv::Mat& depth_image, int original_width, int original_height)
{
    if (!depth_image.empty())
        depth_image.release();

    depth_image.create(original_height, original_width, CV_32F);

    int count = 0;

    #pragma omp parallel for
    for (int v = 0; v < depth_image.rows; ++v)
    {
        for (int u = 0; u < depth_image.cols; ++u)
        {
            depth_image.at<float>(v, u) = pointcloud->points.at(count++).z * 1000;

        }
    }

    depth_image.convertTo(depth_image,CV_8U);

}

Answer 2

我不了解OpenCV方法，但是如果你做了一些使你的点云非结构化的事情你的过程可能是这样的

% rescale the points by 1000
p.x = p.x * 1000; p.y = p.y * 1000; p.z = p.z * 1000;

% project points on image plane and correct center point + factor
 image_p.x = ( p.x * fx / p.z  -cf ) * factor;
 image_p.y = ( p.y * fy / p.z  -cy ) * factor;

现在，根据您对点云所做的操作，点可能无法精确映射到图像矩阵像素中心点（或某些应用程序的左上角），或者您可能缺少点 - ＆gt; NaN / 0值像素。你如何处理这取决于你，但最简单的方法是将image_p.x和image_p.y作为整数进行投射，确保它们具有图像边界并设置

depth_image.at<float>(image_p.y, image_p.x) = p.Z;`