Pointcloud value at a given pixel

asked 2021-01-18 09:54:09 -0600

Schloern93 gravatar image


I need some help in understanding a PointCloud message. I have a /image_raw image through my D435 camera. With this image, I make an object detection and give me back the pixels of the objects. This works so far without problems. Furthermore, I have through my D435 a sorted PointCloud2 message. Now I want to determine the corresponding point in the PointCloud message based on the given pixels of the objects so that I can determine the depth of the object. I receive the Point Cloud message with the following function.

void callback_depth_image_d435 (const sensor_msgs::msg::PointCloud2::SharedPtr point_cloud2_msgs)

      pcl::PointCloud<pcl::PointXYZ> point_cloud;

      // convert point clouds2 msgs data to points
      pcl::fromROSMsg(*point_cloud2_msgs, point_cloud);  

      point_cloud2_points.resize(point_cloud.points.size()); // size 307200

      // get image hight and width
      image_hight = point_cloud2_msgs->height;
      image_width = point_cloud2_msgs->width;
      // get all point Cloud points from the D435 camera
      for(unsigned long i = 0; i < point_cloud.points.size(); i++)
        point_cloud2_points.at(i).x = point_cloud.points.at(i).x;
        point_cloud2_points.at(i).y = point_cloud.points.at(i).y;
        point_cloud2_points.at(i).z = point_cloud.points.at(i).z;

What exactly is the structure of the "pcl::PointCloud<pcl::pointxyz> point_cloud" that I receive from the function

pcl::fromROSMsg(*point_cloud2_msgs, point_cloud);

I expected the first point of the Point Cloud message correspond to the pixel in the upper left corner. The point at location 480 corresponds to the pixel at the bottom left corner. (Image 480h * 640w) Point 481 should correspond to the first pixel in the second row and so on. I have already tried different variants of how the points are arranged to the pixels. Unfortunately without success Can someone explain to me or send me a link to documentation where the structure of the array is explained.

Thanks for the help

edit retag flag offensive close merge delete