Reading float from Float32 data from kinect pointcloud2

asked 2011-08-18 02:31:08 -0600

Marco
75 ●9 ●10 ●17

updated 2016-10-24 09:01:52 -0600

ngrennan
1 ●1 ●1

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;

edit retag flag offensive close merge delete

add a comment

3

answered 2011-08-18 05:16:07 -0600

Mac

4119 ●27 ●62 ●83 http://www.cs.duke.edu/~mac

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.

edit flag offensive delete link

Comments

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?

Marco ( 2011-08-18 20:23:22 -0600 )edit

You don't need fromROSMsg at all; all you need to do is include pcl_ros/point_cloud.h, and then treat PointCloud<T> like it's a message type, and everything will Just Work.

Mac ( 2012-08-09 16:52:18 -0600 )edit

add a comment

2

answered 2011-08-18 21:24:46 -0600

Marco
75 ●9 ●10 ●17

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;
}

edit flag offensive delete link

add a comment

Reading float from Float32 data from kinect pointcloud2

2 Answers

Comments

Question Tools

Stats

Related questions

Reading float from Float32 data from kinect pointcloud2 edit

2 Answers

Comments

Question Tools

Stats

Related questions

Reading float from Float32 data from kinect pointcloud2