Ask Your Question
1

PointCloud2 access data

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

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

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

4 Answers

Sort by ยป oldest newest most voted
0

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

paulbovbel gravatar image

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

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 image paulbovbel  ( 2014-08-27 09:42:07 -0500 )edit

I do not understand what does this mean

arenillas gravatar image arenillas  ( 2014-08-27 10:08:01 -0500 )edit

Anything specific?

paulbovbel gravatar image paulbovbel  ( 2014-08-27 10:30:44 -0500 )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 image arenillas  ( 2014-08-27 10:43:01 -0500 )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 image paulbovbel  ( 2014-08-27 10:55:39 -0500 )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 image paulbovbel  ( 2014-08-27 10:57:57 -0500 )edit

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

paulbovbel gravatar image paulbovbel  ( 2014-08-27 10:58:42 -0500 )edit

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

paulbovbel gravatar image paulbovbel  ( 2014-08-27 11:02:28 -0500 )edit
6

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

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 image zubair  ( 2017-04-20 04:33:56 -0500 )edit

Thanks a lot, your code really helps me out.

Teddy_NTU gravatar image Teddy_NTU  ( 2019-03-07 05:13:37 -0500 )edit

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?

Chao Chen gravatar image Chao Chen  ( 2020-06-19 14:31:24 -0500 )edit

you should check out the depth image processing package. I think it has what you need!

flynneva gravatar image flynneva  ( 2021-04-22 16:40:35 -0500 )edit
0

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

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 image zubair  ( 2017-04-20 04:20:12 -0500 )edit
0

answered 2020-06-19 14:32:38 -0500

Chao Chen gravatar image

updated 2020-06-19 14:39:00 -0500

void callback(const sensor_msgs::PointCloud2ConstPtr& msg){

int width = msg->width;
int height = msg->height;
int v = 1;
int u = 1;
int arrayPosition = v * msg->row_step + u * msg->point_step;
int arrayPosX = arrayPosition + msg->fields[0].offset; // X has an offset of 0
int arrayPosY = arrayPosition + msg->fields[1].offset; // Y has an offset of 4
int arrayPosZ = arrayPosition + msg->fields[2].offset; // Z has an offset of 8

double X = 0.0;
double Y = 0.0;
double Z = 0.0;

memcpy(&X, &msg->data[arrayPosX], sizeof(double));
memcpy(&Y, &msg->data[arrayPosY], sizeof(double));
memcpy(&Z, &msg->data[arrayPosZ], sizeof(double));

ROS_INFO("X:%lf",X);
ROS_INFO("Y:%lf",Y);
ROS_INFO("Z:%lf",Z);

}

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?

edit flag offensive delete link more

Comments

i'd recommend taking a look at these examples of sending a pointcloud in cpp as well as sending a pointcloud in python

flynneva gravatar image flynneva  ( 2021-04-22 16:46:13 -0500 )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 -0500

Seen: 17,472 times

Last updated: Jun 19 '20