PointCloud2 access data

edit

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;

    }

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.

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?