Merge pull request IntelRealSense#1293 from zivsha/unity2

[Unity] Textured Point Cloud
zokin · Mar 19, 2018 · c7fcfd9 · c7fcfd9
2 parents 65e269c + 2cd2c43
commit c7fcfd9
Show file tree

Hide file tree

Showing 5 changed files with 163 additions and 99 deletions.
diff --git a/wrappers/csharp/Intel.RealSense/Frame.cs b/wrappers/csharp/Intel.RealSense/Frame.cs
@@ -205,7 +205,11 @@ public struct Vertex
             public float y;
             public float z;
         }
-
+        public struct TextureCoordinate
+        {
+            public float u;
+            public float v;
+        }
         public Points(IntPtr ptr) : base(ptr)
         {
         }
@@ -229,7 +233,6 @@ private IntPtr VertexData
             }
         }
 
-
         /// <summary>
         /// Copy frame data to Vertex array
         /// </summary>
@@ -248,6 +251,35 @@ public void CopyTo(Vertex[] array)
                 handle.Free();
             }
         }
+
+        private IntPtr TextureData
+        {
+            get
+            {
+                object error;
+                return NativeMethods.rs2_get_frame_texture_coordinates(m_instance.Handle, out error);
+            }
+        }
+        /// <summary>
+        /// Copy frame data to TextureCoordinate array
+        /// </summary>
+        /// <param name="textureArray"></param>
+        public void CopyTo(TextureCoordinate[] textureArray)
+        {
+            if (textureArray == null)
+                throw new ArgumentNullException("textureArray");
+
+            var handle = GCHandle.Alloc(textureArray, GCHandleType.Pinned);
+            try
+            {
+                var size = Count * Marshal.SizeOf(typeof(TextureCoordinate));
+                NativeMethods.memcpy(handle.AddrOfPinnedObject(), TextureData, size);
+            }
+            finally
+            {
+                handle.Free();
+            }
+        }
     }
 
 

diff --git a/wrappers/csharp/Intel.RealSense/FrameQueue.cs b/wrappers/csharp/Intel.RealSense/FrameQueue.cs
@@ -20,7 +20,12 @@ public FrameQueue(int capacity = 10)
         public bool PollForFrame(out Frame frame)
         {
             object error;
-            return NativeMethods.rs2_poll_for_frame(m_instance.Handle, out frame, out error) > 0;
+            if(NativeMethods.rs2_poll_for_frame(m_instance.Handle, out frame, out error) > 0)
+            {
+                frame = FrameSet.CreateFrame(frame.m_instance.Handle);
+                return true;
+            }
+            return false;
         }
 
         public Frame WaitForFrame()
@@ -39,6 +44,12 @@ public FrameSet WaitForFrames()
             return frame;
         }
 
+        public void Enqueue(Frame f)
+        {
+            object error;
+            NativeMethods.rs2_frame_add_ref(f.m_instance.Handle, out error);
+            NativeMethods.rs2_enqueue_frame(f.m_instance.Handle, m_instance.Handle);
+        }
         #region IDisposable Support
         private bool disposedValue = false; // To detect redundant calls
 

diff --git a/wrappers/csharp/Intel.RealSense/Processing.cs b/wrappers/csharp/Intel.RealSense/Processing.cs
@@ -186,6 +186,13 @@ public Points Calculate(Frame original)
             NativeMethods.rs2_process_frame(m_instance.Handle, original.m_instance.Handle, out error);
             return queue.WaitForFrame() as Points;
         }
+        public void MapTexture(VideoFrame texture)
+        {
+            object error;
+            Options[Option.TextureSource].Value = Convert.ToSingle(texture.Profile.UniqueID);
+            NativeMethods.rs2_frame_add_ref(texture.m_instance.Handle, out error);
+            NativeMethods.rs2_process_frame(m_instance.Handle, texture.m_instance.Handle, out error);
+        }
     }
 
 }
diff --git a/wrappers/unity/Assets/RealSenseSDK2.0/Scenes/RealSenseTextures.unity b/wrappers/unity/Assets/RealSenseSDK2.0/Scenes/RealSenseTextures.unity
@@ -285,6 +285,21 @@ Prefab:
       propertyPath: textureBinding.m_PersistentCalls.m_Calls.Array.data[0].m_Target
       value: 
       objectReference: {fileID: 312912468}
+    - target: {fileID: 198902074929829518, guid: c88c4d8330157094da830413654dd969,
+        type: 2}
+      propertyPath: randomSeed
+      value: -239482275
+      objectReference: {fileID: 0}
+    - target: {fileID: 198902074929829518, guid: c88c4d8330157094da830413654dd969,
+        type: 2}
+      propertyPath: EmissionModule.enabled
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 198902074929829518, guid: c88c4d8330157094da830413654dd969,
+        type: 2}
+      propertyPath: ShapeModule.enabled
+      value: 0
+      objectReference: {fileID: 0}
     m_RemovedComponents: []
   m_ParentPrefab: {fileID: 100100000, guid: c88c4d8330157094da830413654dd969, type: 2}
   m_IsPrefabParent: 0

diff --git a/wrappers/unity/Assets/RealSenseSDK2.0/Scripts/PointCloudGenerator.cs b/wrappers/unity/Assets/RealSenseSDK2.0/Scripts/PointCloudGenerator.cs
@@ -10,134 +10,133 @@
 
 public class PointCloudGenerator : MonoBehaviour
 {
-    private int streamWidth;
-    private int streamHeight;
-    private int totalImageSize;
-    private int particleSize;
-    private int particleCount;
-    private ParticleSystem.Particle[] particles;
+    private ParticleSystem.Particle[] particles = new ParticleSystem.Particle[0];
     private PointCloud pc = new PointCloud();
-    public UnityEngine.Gradient gradient;
+    FrameQueue pointsQueue = new FrameQueue(1);
+    Points.Vertex[] vertices;
+    Points.TextureCoordinate[] textureCoordinate;
+    private byte[] lastColorImage;
+    Texture2D colorTexture;
+    private int colorFrameWidth;
+    private int colorFrameHeight;
+
+    public bool mirrored;
     public float pointsSize = 0.01f;
     public int skipParticles = 2;
     public ParticleSystem pointCloudParticles;
 
     // Use this for initialization
     void Start()
     {
-        Application.runInBackground = true;
-        particleCount = 0;
+        //RealSenseDevice.Instance.onNewSample += OnFrame;
+        RealSenseDevice.Instance.onNewSampleSet += OnFrames;
 
-        if (RealSenseDevice.Instance.ActiveProfile != null)
-        {
-            OnStartStreaming(RealSenseDevice.Instance.ActiveProfile);
-        }
-        else
-        {
-            RealSenseDevice.Instance.OnStart += OnStartStreaming;
-        }
-        particles = new ParticleSystem.Particle[particleSize];
     }
-
-    private void OnStartStreaming(PipelineProfile activeProfile)
+    object l = new object();
+    private void OnFrames(FrameSet frames)
     {
-        if(InitializeStream(activeProfile))
+        if(frames.DepthFrame == null)
         {
-            RealSenseDevice.Instance.onNewSample += OnFrame;
+            Debug.Log("No depth frame in frameset, can't create point cloud");
+            return;
         }
-    }
 
-    private void OnFrame(Frame frame)
-    {
-        if (frame.Profile.Stream == Intel.RealSense.Stream.Depth)
+        if (!UpdateParticleParams(frames.DepthFrame.Width, frames.DepthFrame.Height))
         {
-            var depthFrame = frame as DepthFrame;
-            if (depthFrame == null)
-            {
-                Debug.Log("Frame is not a depth frame");
-                return;
-            }
-
-            UpdateParticleParams(depthFrame.Width, depthFrame.Height, depthFrame.Profile.Format);
-
-            var points = pc.Calculate(frame);
+            Debug.Log("Unable to craete point cloud");
+            return;
+        }
 
-            Points.Vertex[] vertices = new Points.Vertex[points.Count];
-            points.CopyTo(vertices);
-            for (int index = 0; index < vertices.Length; index += skipParticles)
+        using (var points = pc.Calculate(frames.DepthFrame))
+        {
+            if (frames.ColorFrame != null)
             {
-                var v = vertices[index];
-                if (v.z > 0)
-                {
-                    particles[index].position = new Vector3(v.x, v.y, v.z);
-                    particles[index].startSize = pointsSize;
-                    particles[index].startColor = gradient.Evaluate(v.z);
-                }
-                else
+                if (frames.ColorFrame.BitsPerPixel == 24)
                 {
-                    particles[index].position = new Vector3(0, 0, 0);
-                    particles[index].startSize = (float)0.0;
-                    particles[index].startColor = new Color32(0, 0, 0, 0);
+                    pc.MapTexture(frames.ColorFrame);
+                    colorFrameWidth = frames.ColorFrame.Width;
+                    colorFrameHeight = frames.ColorFrame.Height;
+                    var newSize = frames.ColorFrame.Stride * colorFrameHeight;
+                    lock (l)
+                    {
+                        if (lastColorImage == null || lastColorImage.Length != newSize)
+                            lastColorImage = new byte[newSize];
+
+                        frames.ColorFrame.CopyTo(lastColorImage);
+                    }
                 }
             }
-        }
-        else if(frame.Profile.Stream == Intel.RealSense.Stream.Color)
-        {
-            //pc.MapTexture(frame);
+            pointsQueue.Enqueue(points);
         }
     }
 
-    private void UpdateParticleParams(int width, int height, Format format)
+    private bool UpdateParticleParams(int width, int height)
     {
-        streamWidth = width;
-        streamHeight = height;
-
-        if (format != Format.Z16)
+        var numParticles = (width * height);
+        if (particles.Length != numParticles)
         {
-            Debug.Log("Unsupported format");
-            return;
+            particles = new ParticleSystem.Particle[numParticles];
         }
 
-        const int bpp = 2;
-
-        if (totalImageSize != streamWidth * streamHeight * bpp)
-        {
-            totalImageSize = streamWidth * streamHeight * bpp;
-            particleSize = totalImageSize / skipParticles;
-            particles = new ParticleSystem.Particle[particleSize];
-        }
-
-        if (particleSize != totalImageSize / skipParticles)
-        {
-            particleSize = totalImageSize / skipParticles;
-            particles = new ParticleSystem.Particle[particleSize];
-        }
-        particleCount = particleSize;
+        return true;
     }
 
     void Update()
     {
-        //TODO: Lock & copy particles?
-        pointCloudParticles.SetParticles(particles, particleCount);
-    }
-
-    private bool InitializeStream(PipelineProfile activeProfile)
-    {
-        var depthStream = activeProfile.Streams.FirstOrDefault(s => s.Stream == Intel.RealSense.Stream.Depth);
-        if (depthStream == null)
+        Frame frame;
+        if (pointsQueue.PollForFrame(out frame))
         {
-            Debug.Log("No Depth stream available");
-            return false;
-        }
-        var depthProfile = depthStream as VideoStreamProfile;
-        //depthIntrinsic = depthProfile.GetIntrinsics();
-        streamWidth = depthProfile.Width;
-        streamHeight = depthProfile.Height;
-        return true;
-    }
+            using (Points points = frame as Points)
+            {
+                if (points == null)
+                    throw new Exception("Frame in queue is not a points frame");
 
-    private float Remap(float value, float from1, float to1, float from2, float to2)
-    {
-        return (value - from1) / (to1 - from1) * (to2 - from2) + from2;
+                vertices = vertices ?? new Points.Vertex[points.Count];
+                points.CopyTo(vertices);
+
+                lock (l)
+                {
+                    if (textureCoordinate == null || textureCoordinate.Length != points.Count)
+                        textureCoordinate = new Points.TextureCoordinate[points.Count];
+
+                    points.CopyTo(textureCoordinate);
+
+                    if (lastColorImage != null)
+                    {
+                        if (colorTexture == null || colorTexture.width != colorFrameWidth || colorTexture.height != colorFrameHeight)
+                        {
+                            colorTexture = new Texture2D(colorFrameWidth, colorFrameHeight, TextureFormat.RGB24, false, true)
+                            {
+                                wrapMode = TextureWrapMode.Clamp,
+                                filterMode = FilterMode.Point
+                            };
+                        }
+
+                        colorTexture.LoadRawTextureData(lastColorImage);
+                        colorTexture.Apply();
+                    }
+                }
+                Debug.Assert(vertices.Length == particles.Length);
+                int mirror = mirrored ? -1 : 1;
+                for (int index = 0; index < vertices.Length; index += skipParticles)
+                {
+                    var v = vertices[index];
+                    if (v.z > 0)
+                    {
+                        particles[index].position = new Vector3(v.x * mirror, v.y, v.z);
+                        particles[index].startSize = pointsSize;
+                        particles[index].startColor = colorTexture.GetPixelBilinear(textureCoordinate[index].u, textureCoordinate[index].v);
+                    }
+                    else //Required since we reuse the array
+                    {
+                        particles[index].position = Vector3.zero;
+                        particles[index].startSize = 0;
+                        particles[index].startColor = Color.black;
+                    }
+                }
+            }
+        }
+        //Either way, update particles
+        pointCloudParticles.SetParticles(particles, particles.Length);
     }
 }