Ask Your Question
1

PointCloud2 access data

asked 2014-08-26 06:29:02 -0600

updated 2014-08-27 10:11:14 -0600

I have a PointCloud2 topic and I need to access the x, y and z of the points. I have found: pcl::PointCloud<pcl::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.

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
0

answered 2014-08-27 09:37:22 -0600

paulbovbel gravatar image

updated 2014-08-27 10:02:48 -0600

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.

edit flag offensive delete link more

Comments

of course first you have to dereference (*cloud) the ConstPtr, which is a pointer type

paulbovbel gravatar imagepaulbovbel ( 2014-08-27 09:42:07 -0600 )edit

I do not understand what does this mean

arenillas gravatar imagearenillas ( 2014-08-27 10:08:01 -0600 )edit

Anything specific?

paulbovbel gravatar imagepaulbovbel ( 2014-08-27 10:30:44 -0600 )edit

this is what I do not understand:

to dereference (*cloud) the ConstPtr

If you want see my code I have edited the question

arenillas gravatar imagearenillas ( 2014-08-27 10:43:01 -0600 )edit

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.

paulbovbel gravatar imagepaulbovbel ( 2014-08-27 10:55:39 -0600 )edit

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.

paulbovbel gravatar imagepaulbovbel ( 2014-08-27 10:57:57 -0600 )edit

For general C++ help, I would point you to stackoverflow (no pun intended)

paulbovbel gravatar imagepaulbovbel ( 2014-08-27 10:58:42 -0600 )edit

And here is somewhere you can see how individual points get accessed from a pcl::Pointcloud callback

paulbovbel gravatar imagepaulbovbel ( 2014-08-27 11:02:28 -0600 )edit
4

answered 2016-11-03 18:55:32 -0600

Saurav Agarwal gravatar image

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;

    }
edit flag offensive delete link more

Comments

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

zubair gravatar imagezubair ( 2017-04-20 04:33:56 -0600 )edit

Thanks a lot, your code really helps me out.

Teddy_NTU gravatar imageTeddy_NTU ( 2019-03-07 05:13:37 -0600 )edit
0

answered 2014-08-27 04:35:01 -0600

jorge gravatar image

Hi! Here I create "by hand" a sensor_msgs::PointCloud2 with 3 points. Reversing the process you should be able to extract the points coordinates from your pointcloud.

Btw, pcl::PointCloud<pcl::pointxyzrgb> is a PCL class, but I'm using sensor_msgs::PointCloud2 messages (that is what you get from a topic). They are different classes, but pcl_ros gives you functions to convert from one to the other.

Hope this helps. Tell us if you cannot reuse the code for your purposes.

edit flag offensive delete link more

Comments

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

zubair gravatar imagezubair ( 2017-04-20 04:20:12 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2014-08-26 06:29:02 -0600

Seen: 14,293 times

Last updated: Nov 03 '16