Closed dvargasfr closed 5 years ago
Hi @dvargasfr ,
a SensorPointCloud2 intro is a pure ROS topic again. Please place questions like this better on ROS Answers in the future...
There are also many examples, mainly in python, explaining in detail how a PointCloud2 is (de-)composed, e.g. here.
I used these lines of code to unpack a PointCloud2:
public static Point[] convertBinaryBlob(SensorPointCloud2 sensorPointCloud2)
{
int I = sensorPointCloud2.data.Length / sensorPointCloud2.point_step;
Point[] pointArray = new Point[I];
byte[] byteSlice = new byte[sensorPointCloud2.point_step];
for (int i = 0; i < I; i++)
{
Array.Copy(sensorPointCloud2.data, i * sensorPointCloud2.point_step, byteSlice, 0, sensorPointCloud2.point_step);
pointArray[i] = new Point(byteSlice);
}
return pointArray;
}
public class Point
{
public float x;
public float y;
public float z;
public int[] rgb;
public Point(byte[] bytes)
{
byte[] slice = new byte[4];
Array.Copy(bytes, 0, slice, 0, 4);
x = getValue(slice);
Array.Copy(bytes, 4, slice, 0, 4);
y = getValue(slice);
Array.Copy(bytes, 8, slice, 0, 4);
z = getValue(slice);
Array.Copy(bytes, 16, slice, 0, 4);
rgb = getRGB(slice);
}
public override string ToString()
{
return "xyz=(" + x.ToString() + ", " + y.ToString() + ", " + z.ToString() + ")"
+" rgb=(" + rgb[0].ToString() + ", " + rgb[1].ToString() + ", " + rgb[2].ToString() + ")";
}
public static float getValue(byte[] bytes)
{
if (!BitConverter.IsLittleEndian)
Array.Reverse(bytes);
float result = BitConverter.ToSingle(bytes, 0);
return result;
}
public static int[] getRGB(byte[] bytes)
{
int[] rgb = new int[3];
rgb[0]= Convert.ToInt16(bytes[0]);
rgb[1] = Convert.ToInt16(bytes[1]);
rgb[2] = Convert.ToInt16(bytes[2]);
return rgb;
}
}
For better performance, rather than point clouds, I'd suggest to communicate (depth and color) compressed images or even streams and compose a 3d point cloud in Unity yourself.
Please don't forget to close this issue if it answers your question.
Thanks @MartinBischoff for your time and help. Adapting the code above to mine I could correctly unpack PointCloud2. Just a detail, Array.Copy(bytes, 16, slice, 0, 4);
should be Array.Copy(bytes, 12, slice, 0, 4);
, isn't it??
As you suggest, I´ll try to compose a 3D point cloud in Unity by myself. This solved my question, so issue closed. Thanks again.
@dvargasfr Hey buddy, have you made any progress on your project? I am on the same topic. How for are your through?
Dear @stevensu1838 ,
Hi!
I am trying to visualize RGB-D images from Asus Xtion Pro on Unity using ROS#. Main problem is to understand and correctly process data structure of PointCloud2 messages. An example of a message:
Data is uint array, and point step is 16, so every 16 uints of data correspond to XYZ coordinates of a pointcloud point (each coordinate represented by 4 uints). First step is do is to convert these uint data to hexadecimal. Due to
is_bigendian=False
data reversing is needed. As shown above, last four uints of each point are zeros, so I am reversing each uint[4] data array corresponding to XYZ coordinates, not uint[16] data array corresponding to each point. And last step consist of convert that hexa string to float and get float values for every XYZ points.But after that I am not getting reasonable values to display a pointcloud corresponding to the real one. I'd like to know if the methon I followed is correct, if there is any simpler way, or if anyone have some function code or clue which could guide me.
Thanks a lot!