pcl :: Pointcloud zu cv :: Mat Tiefe Bild

Question

Ich konvertiere diese Tiefe Bild zu einem pcl :: Pointcloud.

mit dem folgende:

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; 

} 

diesem großen Werk, habe ich einige Verarbeitung auf der Wolke läuft, und jetzt brauche ich die Punktewolke zurück zu einem cv konvertieren :: Mat Tiefenbild . Ich kann dafür kein Beispiel finden und habe Schwierigkeiten, mich darum zu kümmern. Was ist das Gegenteil der obigen Funktion?

Wie kann ich eine pcl :: pointcloud zurück in eine cv :: mat konvertieren?

Vielen Dank.

Answer 1

Dies ist nicht getesteter Code, da ich keine Punktwolke auf meinem Rechner habe. Von Ihrem eigenen Umwandlungscode nehme ich an, dass Ihr Bild ein Einkanalbild ist.

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

Ich weiß nicht, über OpenCV Methoden, aber falls Sie etwas tun, das macht Ihre Punktwolke unstrukturiert Ihr Prozess so etwas wie dieses Jetzt

% 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; 

sein könnte, je nachdem, was Sie getan haben, mit Die Punktwolke Die Punkte können nicht genau auf Bildmatrixpixelmittelpunkte (oder die obere linke Ecke für einige Anwendungen) abgebildet werden, oder es fehlen möglicherweise Punkte -> NaN/0-Wertpixel. Wie verarbeiten Sie, dass bis zu Ihnen, aber der einfachste Weg wäre image_p.x und image_p.y als ganze Zahlen zu werfen, stellen Sie sicher, dass sie withing Bildgrenzen und setzen

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