Visualise pointcloud from ros

[Changliu52]

I have a question!

Here is my question: Thank you for sharing the awesome code. I am trying to use it with a mapping robot but struggled to find a way to visualise the pointcloud map from the robot. I wonder if there is any material I can follow to set it up correctly in unity?

Thank you in advance. Chang

[berkayalpcakal]

Hello,

ROS# does not have point cloud subscriber/publisher/processor/visualizer scripts at the moment, but it is possible to adapt your application needs into current ROS# communication framework.

You can directly communicate over point cloud topics, i.e. bring the poincloud2 messages of ROS into Unity, process and visualize them by creating a custom mesh. However, as the pointcloud2 messages are quite big in terms of size, this would create bottleneck in the communication

Another idea would be to communicate over two compressed 2D images, i.e bring compressed rgb images and compressed depth images into Unity, apply back projection techniques to obtain 3D point coordinates, and then visualize them by create a custom mesh. Communicating over compressed image messages rather then poincloud2 messages would result in less bottleneck in the framework.

Please also refer to the issues #79 #152 and #153 .

I guess it would be nice to have a custom material with a custom shader in Unity. You can always search for a simple and high performance shader on web.

I hope it helps.

[bomarali]

I believe you can google the ros-reality package. Alternatively u can write a subscriber for PointCloud2 and use it to animate a particle system.

[stevensu1838]

Hi @berkayalpcakal You mentioned we need to bring compressed rgb images and compressed depth images into Unity I know how to subscribe CompressedImage with the ImageSubscriber by using Messages.Sensor.CompressedImage. Is this the compressed rgb images you were taling about? How about compressed depth images? Do I need to create my own compressed depth images messages and subscriber to have these data in unity？Thanks a lot

[MartinBischoff]

@stevensu1838 yes, you have to create your own compressed depth image subscriber.

[stevensu1838]

Hi Martin, very clear. Will try it hard

[melkishengue]

@stevensu1838 have you been able to visualize pointCloud points in Unity by writing a compressed depth image subscriber ?

[Bradyk27]

Hello everyone. I have written a Pointcloud subscriber, feel free to copy & paste if it helps you. @melkishengue I am currently working on combining depth & color images to create an RGB point cloud, I can post that here as well. The "custom/vertex_color" shader can be found here: http://www.kamend.com/2014/05/rendering-a-point-cloud-inside-unity/

//Adapted from https://answers.ros.org/question/339483/ros-sharp-unity3d-import-pointcloud2/ & https://github.com/siemens/ros-sharp/blob/master/Libraries/RosBridgeClient/PointCloud.cs

using UnityEngine;
using System;
using UnityEditor;

namespace RosSharp.RosBridgeClient
{
    [RequireComponent(typeof(RosConnector))]
    public class PointcloudSubscriber : UnitySubscriber<MessageTypes.Sensor.PointCloud2>
    {
        //Mesh variables
        public RgbPoint3[] Points;
        private Mesh mesh;
        public MeshRenderer meshRenderer;
        Vector3[] vertices;
        //int[] triangles;
        Color[] colors;
        //Subscription variable
        private bool isMessageReceived = false;

        protected override void Start()
        {   
            Debug.Log("Start\n");
            //Start Unity Subscriber
            base.Start();
            //Create empty new mesh for points
            mesh = new Mesh();
            GetComponent<MeshFilter>().mesh = mesh;
            meshRenderer.material = new Material(Shader.Find("Custom/VertexColor"));
        }

        private void Update() //Function to update when new messages are received
        {
            if(isMessageReceived){
                ProcessMessage();
            }
        }

        protected override void ReceiveMessage(MessageTypes.Sensor.PointCloud2 message) //Automatically called when new mesage received
        {
            Debug.Log("ReceiveMessage\n");
            //Processes message, transforms into form Unity can understand
            //NOTE: Pointcloud.cs methods could be implemented here (depth & image)
            long I = message.data.Length / message.point_step;
            RgbPoint3[] Points = new RgbPoint3[I];
            byte[] byteSlice = new byte[message.point_step];
            for (long i = 0; i < I; i++)
            {
                Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
                Points[i] = new RgbPoint3(byteSlice, message.fields);
            }
            vertices = new Vector3[I];
            colors = new Color[I];
            for (var i = 0; i < I; i++)
            {
                vertices[i].x = Points[i].x;
                vertices[i].y = Points[i].z;
                vertices[i].z = Points[i].y;
                colors[i].r = (float)((float)Points[i].rgb[0] / 255.0);
                colors[i].g = (float)((float)Points[i].rgb[1] / 255.0);
                colors[i].b = (float)((float)Points[i].rgb[2] / 255.0);
                colors[i].a = 1.0F; 
                //Debug.Log("Colors: " + colors[i].ToString());
                //Debug.Log("Vertex Colors: " + Points[i].rgb[0] + " " + Points[i].rgb[1] + " " + Points[i].rgb[2] + "\n");
            }
            isMessageReceived = true;
        }

        private void ProcessMessage() //Clears mesh and loads new vertices
        {
            Debug.Log("ProcessMessage\n");

            mesh.Clear(); //Removed try / catch loop
            mesh.vertices = vertices;
            mesh.colors = colors;

            //Graphs mesh as points. Works with /rtabmap/cloud_map & /voxel_cloud
            int[] indices = new int[vertices.Length];
            for(int i = 0; i < mesh.vertices.Length; i++){
                indices[i] = i;
            }
            mesh.SetIndices(indices, MeshTopology.Points, 0);
            mesh.RecalculateBounds();
            //AssetDatabase.CreateAsset(mesh, "Assets/testMeshColorRTABMap4.asset");
            //AssetDatabase.SaveAssets();
            isMessageReceived = false; //Resets and waits for new message
        }
    }
}

[MartinBischoff]

Hi @Bradyk27 , thank you for sharing your PointCloud2 subscriber class here! Surely it will be of help for others interested in visualizing a PointCloud2.

We decided against including it in ROS# as PointCloud2 messages are relatively huge and entail some latency.

[Bradyk27]

EDIT: I've found one of the issues is that the for loop is never completing--it appears that assigning using Points[u + v * width].x causes a loop freeze. Have you encountered this Dr. Bischoff?

Happy to help @MartinBischoff.

PointCloud2 messages are relatively huge

So I've noticed--quite a pain. I've started work on creating a visualizer script via a depth & rgb image subscriber based on your PointCloud.cs script. It's not working yet, but I'll paste what I have thus far and update over the next few days. If you or anyone else sees anything immediately wrong, please point it out. Excuse the slightly messy comments & redundancy--it's a work in progress.

If you have time and could explain to me the following aspects of your code, I'd thoroughly appreciate it.

• PointCloud.cs: Focal variable? Where might I find that / what is it?
• ImageSubscriber.cs: Applying full (non-compressed) images to a 2D texture to make sure the image is being received. The ImageSubscriber.cs works only with compressed images currently.
• PointCloud.cs: This line: float depth = 0; // depthImage[u,v] I assume setting depth to 0 is simply a default value to prevent errors (in case a depth image is not received). I'm guessing that depthImage[u,v] is equivalent to depthImage.data[u*v]?

//Adapted from https://answers.ros.org/question/339483/ros-sharp-unity3d-import-pointcloud2/ & https://github.com/siemens/ros-sharp/blob/master/Libraries/RosBridgeClient/PointCloud.cs
// Need to see if pointcloud calculation is working
//  Might also be resetting...we shall see
using UnityEngine;
using System;
using UnityEditor;
using RosSharp.RosBridgeClient.MessageTypes.Sensor;

namespace RosSharp.RosBridgeClient
{
    [RequireComponent(typeof(RosConnector))]

    public class PointcloudSubscriberFast : MonoBehaviour
    { 
        //Mesh variables
        public RgbPoint3[] Points;
        private Mesh mesh;
        public MeshRenderer meshRenderer;
        Vector3[] vertices;
        Color[] colors;
        public Image colorImage;
        public Image depthImage;
        public ImageSubscriberFull colorImageSubscriber;
        public ImageSubscriberFull depthImageSubscriber;

        void Start()
        {
            Debug.Log("Start\n");
            mesh = new Mesh();
            GetComponent<MeshFilter>().mesh = mesh;
            meshRenderer.material = new Material(Shader.Find("Custom/VertexColor"));

        }

        void Update() //Need to consider how when and how things are updated
        {
            if(colorImageSubscriber.pointcloudReady && depthImageSubscriber.pointcloudReady && colorImageSubscriber.generationReady && depthImageSubscriber.generationReady)
            {
                GeneratePointcloud();
            }
        }

        private void GeneratePointcloud()
        {   
            colorImageSubscriber.generationReady = false;
            depthImageSubscriber.generationReady = false;
            Debug.Log("Started Generation");
            try{
                float focal = 1.0f;
                colorImage = colorImageSubscriber.image;
                depthImage = depthImageSubscriber.image;
                uint width = depthImage.width;
                uint height = depthImage.height;
                float invFocal = 1.0f / focal; //Remove focal variable

                Points = new RgbPoint3[width * height];
                for (uint v = 0; v < height; v++)
                {
                    for (uint u = 0; u < width; u++) //why the hell is u not incrementing? Something to do with how "generate pointcloud" variable???
                    {
                        Debug.Log("W: " + width);
                        //Debug.Log("H: " + height);
                        Debug.Log("U: " + u);
                        //Debug.Log("V: " + v);
                        //Debug.Log("W*H: " + (width*height));
                        //Debug.Log("U*V: " + ((u+1)*(v+1)));
                        //double percent_complete = ((double)(u*v) / (double)(width*height)) * 100.0f;
                        //Debug.Log("Percent Complete: " + percent_complete.ToString("0.000000000000###"));

                        float depth = 0; //depthImage.data[u*v]; //Gotta figure out what this is and how to set it.
                        if (depth == 0)
                        {
                            Debug.Log("Depth 0");
                            Points[u + v * width].x = float.NaN;
                            Points[u + v * width].y = float.NaN;
                            Points[u + v * width].z = float.NaN;
                            Points[u + v * width].rgb = new int[] { 0, 0, 0 };
                        }
                        else
                        {
                            Debug.Log("Depth not 0");
                            Points[u + v * width].z = depth * invFocal;
                            Points[u + v * width].x = u * depth * invFocal;
                            Points[u + v * width].y = v * depth * invFocal;
                            Points[u + v * width].rgb = new int[] { 0, 0, 0 };// rgbImage[u, v]; // Gotta figure out what this is and how to set it as well
                        }
                    }
                }

                Debug.Log("Pointcloud being generated!");
                vertices = new Vector3[Points.Length];
                colors = new Color[Points.Length];
                for (var i = 0; i < Points.Length; i++)
                {
                    vertices[i].x = Points[i].x;
                    vertices[i].y = Points[i].z;
                    vertices[i].z = Points[i].y;
                    colors[i].r = (float)((float)Points[i].rgb[0] / 255.0);
                    colors[i].g = (float)((float)Points[i].rgb[1] / 255.0);
                    colors[i].b = (float)((float)Points[i].rgb[2] / 255.0);
                    colors[i].a = 1.0F; 

                    Debug.Log("Vertexes: " + Points[i].x + " " + Points[i].y + " " + Points[i].z + "\n");
                    Debug.Log("Vertex Colors: " + Points[i].rgb[0] + " " + Points[i].rgb[1] + " " + Points[i].rgb[2] + "\n");
                }

                mesh.Clear();
                mesh.vertices = vertices;
                mesh.colors = colors;

                //Graphs mesh as points
                int[] indices = new int[vertices.Length];
                for(int i = 0; i < mesh.vertices.Length; i++){
                    indices[i] = i;
                }
                mesh.SetIndices(indices, MeshTopology.Points, 0);
                mesh.RecalculateBounds();
                //AssetDatabase.CreateAsset(mesh, "Assets/testMeshColorRTABMap4.asset");
                //AssetDatabase.SaveAssets();
            }
            catch(Exception) {
                return;
            }

            colorImageSubscriber.generationReady = true;
            depthImageSubscriber.generationReady = true;
            colorImageSubscriber.pointcloudReady = false;
            depthImageSubscriber.pointcloudReady = false;

        }

    }   
}

And the accompanying modified image subscriber:

using UnityEngine;
using RosSharp.RosBridgeClient.MessageTypes.Sensor;

namespace RosSharp.RosBridgeClient
{
    [RequireComponent(typeof(RosConnector))]
    public class ImageSubscriberFull : UnitySubscriber<MessageTypes.Sensor.Image> //Better way to do this, probably be some version of ID
    {
        public Image image;
        private bool isMessageReceived;

        public MeshRenderer meshRenderer;
        private Texture2D texture2D;
        private byte[] imageData;

        public bool pointcloudReady;
        public bool generationReady;

        protected override void Start()
        {
            Debug.Log("Start\n");
            base.Start();

            texture2D = new Texture2D(1, 1);
            meshRenderer.material = new Material(Shader.Find("Standard"));
            generationReady = true;
        }
        private void Update()
        {
            if (isMessageReceived)
                ProcessMessage();
        }

        protected override void ReceiveMessage(MessageTypes.Sensor.Image Image)
        {   
            Debug.Log("ReceiveMessage\n");
            image = Image;
            isMessageReceived = true;

            imageData = Image.data;
        }

        private void ProcessMessage()
        {
            Debug.Log("ProcessMessage\n");
            isMessageReceived = false;

            texture2D.LoadRawTextureData(imageData);
            texture2D.Apply();
            meshRenderer.material.SetTexture("_MainTex", texture2D);
            pointcloudReady = true;
        }

    }
}