RGB-D image from ROS on Unity

[dvargasfr]

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:

header: 
  seq: 9421
  stamp: 
    secs: 1544456358
    nsecs: 530346386
  frame_id: "camera_depth_optical_frame"
height: 480
width: 640
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
is_bigendian: False
point_step: 16
row_step: 10240
data: [29, 82, 110, 192, 214, 116, 50, 192, 148, 24, 216, 64, 0, 0, 0, 0, 31, 144, 109, 192, 214, 116, 50, 192, 148, 24, 216, 64, 0, 0, 0, 0, ... ]
is_dense: False

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!

[MartinBischoff]

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.

[dvargasfr]

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.

[stevensu1838]

@dvargasfr Hey buddy, have you made any progress on your project? I am on the same topic. How for are your through?

[MartinBischoff]

Dear @stevensu1838 ,

• we are not a Q&A desk for your coding projects.
• Your questions have nothing to do with ROS#.
• This is a public issue board, and not a place to put personal messages.
• This issue is closed since moths already.
  Thank you for your understanding.