Conversion from PointCloud to Mat

1
votes

Let's say I initialize a point-cloud. I want to store its RGB channels in opencv's Mat data-type. How can I do that?

pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);   //Create a new cloud
pcl::io::loadPCDFile<pcl::PointXYZRGBA> ("cloud.pcd", *cloud);
3
c++opencvimage-processingcomputer-visionpoint-cloud-library

3 Answers

0
votes

I know how to convert from Mat(3D Image) to XYZRGB. I think you can figure out the other way. Here Q is disparity to depth Matrix.

 pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
 double px, py, pz;
 uchar pr, pg, pb;

 for (int i = 0; i < img_rgb.rows; i++)
 {
     uchar* rgb_ptr = img_rgb.ptr<uchar>(i);
     uchar* disp_ptr = img_disparity.ptr<uchar>(i);
     double* recons_ptr = recons3D.ptr<double>(i);
     for (int j = 0; j < img_rgb.cols; j++)
     {
         //Get 3D coordinates

          uchar d = disp_ptr[j];
          if ( d == 0 ) continue; //Discard bad pixels
          double pw = -1.0 * static_cast<double>(d) * Q32 + Q33; 
          px = static_cast<double>(j) + Q03;
          py = static_cast<double>(i) + Q13;
          pz = Q23;

          // Normalize the points
          px = px/pw;
          py = py/pw;
          pz = pz/pw;

          //Get RGB info
          pb = rgb_ptr[3*j];
          pg = rgb_ptr[3*j+1];
          pr = rgb_ptr[3*j+2];

          //Insert info into point cloud structure
          pcl::PointXYZRGB point;
          point.x = px;
          point.y = py;
          point.z = pz;
          uint32_t rgb = (static_cast<uint32_t>(pr) << 16 |
          static_cast<uint32_t>(pg) << 8 | static_cast<uint32_t>(pb));
          point.rgb = *reinterpret_cast<float*>(&rgb);
          point_cloud_ptr->points.push_back (point);
    }
}

point_cloud_ptr->width = (int) point_cloud_ptr->points.size();
point_cloud_ptr->height = 1;
0
votes

Do I understand it right, that you are only interested in the RGB-values of the point-cloud and don't care about its XYZ-values?

Then you can do: 

pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>); 
//Create a new cloud
pcl::io::loadPCDFile<pcl::PointXYZRGBA> ("cloud.pcd", *cloud);

cv::Mat result; 

if (cloud->isOrganized()) {
    result = cv::Mat(cloud->height, cloud->width, CV_8UC3);

    if (!cloud->empty()) {

        for (int h=0; h<result.rows; h++) {
            for (int w=0; w<result.cols; w++) {
                pcl::PointXYZRGBA point = cloud->at(w, h);

                Eigen::Vector3i rgb = point.getRGBVector3i();

                result.at<cv::Vec3b>(h,w)[0] = rgb[2];
                result.at<cv::Vec3b>(h,w)[1] = rgb[1];
                result.at<cv::Vec3b>(h,w)[2] = rgb[0];
            }
        }
    }
}

I think it's enough to show the basic idea.

BUT this only works, if your point-cloud is organized:

An organized point cloud dataset is the name given to point clouds that resemble an organized image (or matrix) like structure, where the data is split into rows and columns. Examples of such point clouds include data coming from stereo cameras or Time Of Flight cameras. The advantages of a organized dataset is that by knowing the relationship between adjacent points (e.g. pixels), nearest neighbor operations are much more efficient, thus speeding up the computation and lowering the costs of certain algorithms in PCL. (Source)

0
votes

I have the same problem and I succeed to solve it!

You should firstly transform the coordinate so that your 'ground plane' is the X-O-Y plane. The core api is pcl::getTransformationFromTwoUnitVectorsAndOrigin

You can have a look at my question:

good luck!