Reading float from Float32 data from kinect pointcloud2

Question

Hi everyone,

I am trying to get the xyz data from a pointcloud2 msg in bagfile created with kinect. So far I seem to be getting quite far, but at this point I'm a bit uncertain.

The Pointfield in the pointcloud2 shows that the data should consist of three Float32 (std_msgs::Float32) values for x,y & z. However the pointstep parameter shows 16 bytes are being used per datapoint. Are the first 12 bytes used for the x,y,z Float32 values and are the last 4 bytes overhead, or is this built up differently?

Result for information parameters:

width = 320 height = 240 row step = 5120 point step = 16

Fields[0] --> name: x offset: 0 datatype: 7 count: 1

Fields[1] --> name: y offset: 4 datatype: 7 count: 1

Fields[2] --> name: z offset: 8 datatype: 7 count: 1

Little code snippet:

typedef sensor_msgs::PointCloud2 PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef std_msgs::Float32 Float32;

//get the pointcloud loaded from view_it
PointCloud cloud_t;
PointCloudPtr cloud = view_it->instantiate<PointCloud> ();
cloud_t = *cloud;

//get pointer to first element of dataline 20 and typecast it to Float32
unsigned char *dataptr = &cloud_t.data[20];
entryptr = (Float32 *) dataptr;

//print the value of x (if its in first 4 bytes)
std::cout << "data x float is " << (*entryptr).data <<std::endl;

---

Originally posted by Marco on ROS Answers with karma: 77 on 2011-08-18

Post score: 1

Answer 1

Lucky for you, code to do this already exists in the pcl_ros package. Then, rather than having your callbacks take sensor_msgs::PointCloud2 objects (or, really, ConstPtrs to sensor_msgs::PointCloud2 objects), your callbacks take in pcl::PointCloud<pcl::PointXYZRGB>::ConstPtrs; the conversion to the PCL point cloud is completely transparent, and you don't have to think about interpreting the ROS message yourself.

---

Originally posted by Mac with karma: 4119 on 2011-08-18

This answer was ACCEPTED on the original site

Post score: 3

---

Original comments

Comment by Marco on 2011-08-18:
So I should use void pcl::fromROSMsg and then adressing the xyz values in the pcl::PointCloud< PointT > should be easy? PS1: I'm not using the RGB channel but the depth/points so no RGB information in my pointcloud2. PS2: as I'm this far I would like to know the format of the data-field. Anyone?

Comment by Mac on 2012-08-09:
You don't need fromROSMsg at all; all you need to do is include pcl_ros/point_cloud.h, and then treat PointCloud like it's a message type, and everything will Just Work.

Answer 2

Using functions in pcl_ros, I started from the sensor_msgs::PointCloud2 cloud_t in code above, I now use from the pcl (with some extra includes) the code below to convert from sensor_msgs::PointCloud2 to pcl::PointCloud< pcl::PointXYZ > , and this seems to work (outputs are within reasonable range or NaN):

pcl::PointCloud< pcl::PointXYZ > PointCloudXYZ;

pcl::fromROSMsg(cloud_t,PointCloudXYZ);
std::cout << "width is " << PointCloudXYZ.width << std::endl;
std::cout << "height is " << PointCloudXYZ.height << std::endl;
int cloudsize = (PointCloudXYZ.width) * (PointCloudXYZ.height);
for (int i=0; i< cloudsize; i++){
std::cout << "(x,y,z) = " << PointCloudXYZ.points[i] << std::endl;
}

---

Originally posted by Marco with karma: 77 on 2011-08-18

This answer was NOT ACCEPTED on the original site

Post score: 2