Как преобразовать cv :: Mat в pcl :: pointcloud с цветом

Question

Я пытаюсь преобразовать pointcloud с информацией 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); 

Отображение снимка выше обеспечили мне, что цвета извлекаются правильно (изображение сравнивается на глаз с необработанного изображения). Затем я хочу, чтобы преобразовать его обратно в PointCloud:

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?

Extra

Даже если я заставляю красный цвет во всех точках OpenCVPointCloudColor, используя cvCloudColor.at<cv::Vec3f>(x,y) = cv::Vec3f(0,0,255); затем чтение из OpenCVPointCloudColor все еще создает неверную информацию о цвете в ЗКС облаке.

Answer 1

Я почти уверен, что в ваших кодах есть ошибка, связанная с этими функциями. Я попробовал простой пример, следуя вашей парадигмы, и она прекрасно работает (PCL 1,8 построены из источников, OpenCV 3,1 построены из источников, г ++ 5.x или 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 слишком старая или даже сломанная.