PointCloud2 access data

Question

I have a PointCloud2 topic and I need to access the x, y and z of the points. I have found: pcl::PointCloudpcl::PointXYZRGB::ConstPtr

The problem is that I don't know how to use it.

Do you know where can I find some example code describing how to get coordinates in PCL2?

[EDIT]

Now, I am using this code but it is not working properly

void pcl2_to_scan::callback(const sensor_msgs::PointCloud2ConstPtr &pPCL2)
{
        for (uint j=0; j < pPCL2->height * pPCL2->width; j++){
            float x = pPCL2->data[j * pPCL2->point_step + pPCL2->fields[0].offset];
            float y = pPCL2->data[j * pPCL2->point_step + pPCL2->fields[1].offset];
            float z = pPCL2->data[j * pPCL2->point_step + pPCL2->fields[2].offset];
            // Some other operations
       }
}

Thank you.

---

Originally posted by arenillas on ROS Answers with karma: 223 on 2014-08-26

Post score: 1

Answer 1

A good place to start is the API documentation.

For example, the pcl::Poincloud doc shows you that you can get any point by indexing into the cloud with the [] operator, like an array.

The PointXYZRGB description isn't as clear, but it does tell you it's a struct (meaning all members are public), and you'll find you can access the values via point.x, .y, .z, etc.

---

Originally posted by paulbovbel with karma: 4518 on 2014-08-27

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by paulbovbel on 2014-08-27:
of course first you have to dereference (*cloud) the ConstPtr, which is a pointer type

Comment by arenillas on 2014-08-27:
I do not understand what does this mean

Comment by paulbovbel on 2014-08-27:
Anything specific?

Comment by arenillas on 2014-08-27:
this is what I do not understand:

to dereference (*cloud) the ConstPtr

If you want see my code I have edited the question

Comment by paulbovbel on 2014-08-27:
There are two ways to receive pointcloud data in a callback: either as a sensor_msgs or a pcl type. For working with the data, the pcl type provides a better interface since the sensor_msgs type just contains a blob of data.

Comment by paulbovbel on 2014-08-27:
When you subscribe to a message, you get a ConstPtr which means it's a boost shared_pointer to a const piece of data. That is not important, since you can just treat it as a regular pointer, and remember not to try to modify the data.

Comment by paulbovbel on 2014-08-27:
For general C++ help, I would point you to stackoverflow (no pun intended)

Comment by paulbovbel on 2014-08-27:
And here is somewhere you can see how individual points get accessed from a pcl::Pointcloud callback

Comment by kodplayer on 2020-12-25:
The above mentioned website adress docs.pointclouds.org is removed

Answer 2

I've been trying to figure out the same thing. Surprisingly there is no clear cut instruction anywhere. Anyhow, I wanted to convert 2D pixel coordinates (u,v) to X,Y,Z from a point cloud that I got from kinect. I wrote a function to do it, I pass in the point cloud message, u and v coordinates (for a feature in 2D image) and pass a reference to a geometry_msgs point which will get the X,Y,Z values. These X,Y,Z values are in the camera's frame, (X is seen as going from left to right in the image plane, Y is top to bottom and Z pointing into the world).

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
    
/**
    Function to convert 2D pixel point to 3D point by extracting point
    from PointCloud2 corresponding to input pixel coordinate. This function
    can be used to get the X,Y,Z coordinates of a feature using an 
    RGBD camera, e.g., Kinect.
    */
    void pixelTo3DPoint(const sensor_msgs::PointCloud2 pCloud, const int u, const int v, geometry_msgs::Point &p)
    {
      // get width and height of 2D point cloud data
      int width = pCloud.width;
      int height = pCloud.height;
    
      // Convert from u (column / width), v (row/height) to position in array
      // where X,Y,Z data starts
      int arrayPosition = v*pCloud.row_step + u*pCloud.point_step;
    
      // compute position in array where x,y,z data start
      int arrayPosX = arrayPosition + pCloud.fields[0].offset; // X has an offset of 0
      int arrayPosY = arrayPosition + pCloud.fields[1].offset; // Y has an offset of 4
      int arrayPosZ = arrayPosition + pCloud.fields[2].offset; // Z has an offset of 8
    
      float X = 0.0;
      float Y = 0.0;
      float Z = 0.0;
    
      memcpy(&X, &pCloud.data[arrayPosX], sizeof(float));
      memcpy(&Y, &pCloud.data[arrayPosY], sizeof(float));
      memcpy(&Z, &pCloud.data[arrayPosZ], sizeof(float));
    
     // put data into the point p
      p.x = X;
      p.y = Y;
      p.z = Z;
    
    }

---

Originally posted by Saurav Agarwal with karma: 246 on 2016-11-03

This answer was NOT ACCEPTED on the original site

Post score: 7

---

Original comments

Comment by zubair on 2017-04-20:
https://pastebin.com/i71RQcU2 this is my code,, whose part i took from above code,, please have a look , i am not getting x y z values

Comment by Teddy_NTU on 2019-03-07:
Thanks a lot, your code really helps me out.

Comment by Chao Chen on 2020-06-19:
How do we obtain u and v value from unordered pointcloud2 message genereated by the camera?

Why Z value generated stays at 0 all the time? [ INFO] [1592594247.545080677]: X:0.001937 [ INFO] [1592594247.545595176]: Y:4.224004 [ INFO] [1592594247.546480684]: Z:0.000000 [ INFO] [1592594247.609789284]: X:0.002125 [ INFO] [1592594247.610029254]: Y:4.496003 [ INFO] [1592594247.610784074]: Z:0.000000 [ INFO] [1592594247.695069364]: X:0.002029 [ INFO] [1592594247.696253339]: Y:4.368001 [ INFO] [1592594247.696658301]: Z:0.000000 [ INFO] [1592594247.746237034]: X:0.002519 [ INFO] [1592594247.747699666]: Y:4.768003 [ INFO] [1592594247.748495863]: Z:0.000000 [ INFO] [1592594247.823073068]: X:0.000709 [ INFO] [1592594247.823236717]: Y:2.464001 [ INFO] [1592594247.824035601]: Z:0.000000

Comment by flynneva on 2021-04-22:
you should check out the depth image processing package. I think it has what you need!