Refactor improve depth cam perf

Runtime/Tx/DepthCameraTx.cs

@@ -1,10 +1,11 @@
 using System;
-using System.Threading;
 using ProBridge.Tx;
 using sensor_msgs.msg;
 using TurboJpegWrapper;
+using Unity.Collections;
 using UnityEngine;
 using UnityEngine.UI;
+using UnitySensors.Data.PointCloud;
 using UnitySensors.Sensor.Camera;
 
 [AddComponentMenu("ProBridge/Tx/sensor_msgs/Depth Camera")]
@@ -20,15 +21,23 @@ public class DepthCameraTx : ProBridgeTxStamped<CompressedImage>
     public RawImage _rawImage;
     [Range(1, 100)] public int CompressionQuality = 90;
     public Shader depthShader;
+    public bool getPointCloud = false;
 
+    public PointCloud<PointXYZ> pointCloud
+    {
+        get => _cameraSensor.pointCloud;
+    }
 
     private DepthCameraSensor _cameraSensor;
-
     private bool sensorReady = false;
 
+    // Reuse the compressor to avoid per-frame allocations.
+    private TJCompressor _compressor;
 
     protected override void AfterEnable()
     {
+        _compressor = new TJCompressor();
+
         _cameraSensor = renderCamera.gameObject.AddComponent<DepthCameraSensor>();
         _cameraSensor.mat = new Material(depthShader);
         _cameraSensor._camera = renderCamera;
@@ -40,12 +49,20 @@ protected override void AfterEnable()
         _cameraSensor._frequency_inv = sendRate;
         _cameraSensor._resolution.x = textureWidth;
         _cameraSensor._resolution.y = textureHeight;
+        _cameraSensor.getPointCloud = getPointCloud;
         _cameraSensor.Init();
     }
 
     protected override void AfterDisable()
     {
-        _cameraSensor.DisposeSensor();
+        if (_cameraSensor != null)
+            _cameraSensor.DisposeSensor();
+
+        if (_compressor != null)
+        {
+            _compressor.Dispose();
+            _compressor = null;
+        }
     }
 
     private void OnSensorUpdated()
@@ -56,17 +73,31 @@ private void OnSensorUpdated()
     protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts)
     {
         if (!sensorReady)
-        {
             throw new Exception("Sensor is not ready");
-        }
+
+        var tex = _cameraSensor.texture0;
+        if (tex == null)
+            throw new Exception("Depth sensor texture is not a Texture2D.");
 
         if (_rawImage != null)
-            _rawImage.texture = _cameraSensor.texture0;
+            _rawImage.texture = tex;
 
         sensorReady = false;
 
+        NativeArray<byte> raw = tex.GetRawTextureData<byte>();
+        byte[] rgbaBytes = raw.ToArray();
+
+        var jpg = _compressor.Compress(
+            rgbaBytes, 0,
+            tex.width, tex.height,
+            TJPixelFormat.RGBA,
+            TJSubsamplingOption.Gray,
+            CompressionQuality,
+            TJFlags.FastDct | TJFlags.BottomUp
+        );
+
         data.format = "jpeg";
-        data.data = _cameraSensor.texture0.EncodeToJPG(CompressionQuality);
+        data.data = jpg;
 
         return base.GetMsg(ts);
     }

Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs

@@ -12,20 +12,16 @@
 using UnitySensors.Data.PointCloud;
 using UnitySensors.Interface.Sensor;
 using UnitySensors.Utils.Noise;
-
 using Random = Unity.Mathematics.Random;
 
 namespace UnitySensors.Sensor.Camera
 {
     [AddComponentMenu("")]
     public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInterface<PointXYZ>
     {
-        [SerializeField]
-        public float _minRange = 0.05f;
-        [SerializeField]
-        public float _maxRange = 100.0f;
-        [SerializeField]
-        public float _gaussianNoiseSigma = 0.0f;
+        [SerializeField] public float _minRange = 0.05f;
+        [SerializeField] public float _maxRange = 100.0f;
+        [SerializeField] public float _gaussianNoiseSigma = 0.0f;
 
 
         public UnityEngine.Camera _camera;
@@ -45,24 +41,44 @@ public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInt
         private PointCloud<PointXYZ> _pointCloud;
         private int _pointsNum;
 
-        public override UnityEngine.Camera m_camera { get => _camera; }
-        public Texture2D texture0 { get => _texture; }
-        public Texture2D texture1 { get => _texture; }
-        public PointCloud<PointXYZ> pointCloud { get => _pointCloud; }
-        public int pointsNum { get => _pointsNum; }
-
-
+        public override UnityEngine.Camera m_camera
+        {
+            get => _camera;
+        }
+
+        public Texture2D texture0
+        {
+            get => _texture;
+        }
+
+        public Texture2D texture1
+        {
+            get => _texture;
+        }
+
+        public PointCloud<PointXYZ> pointCloud
+        {
+            get => _pointCloud;
+        }
+
+        public int pointsNum
+        {
+            get => _pointsNum;
+        }
+
+        public bool getPointCloud = false;
+
 
         public override void Init()
         {
             _camera.fieldOfView = _fov;
             _camera.nearClipPlane = _minRange;
             _camera.farClipPlane = _maxRange;
 
-            _rt = new RenderTexture(_resolution.x, _resolution.y, 32, RenderTextureFormat.ARGBFloat);
+            _rt = new RenderTexture(_resolution.x, _resolution.y, 32, RenderTextureFormat.ARGB32);
             _camera.targetTexture = _rt;
 
-            _texture = new Texture2D(_resolution.x, _resolution.y, TextureFormat.RGBAFloat, false);
+            _texture = new Texture2D(_resolution.x, _resolution.y, TextureFormat.ARGB32, false);
 
             float f = m_camera.farClipPlane;
             mat.SetFloat("_F", f);
@@ -73,7 +89,6 @@ public override void Init()
 
         private void SetupDirections()
         {
-
             _pointsNum = _resolution.x * _resolution.y;
 
             _directions = new NativeArray<float3>(_pointsNum, Allocator.Persistent);
@@ -91,14 +106,14 @@ private void SetupDirections()
 
         private void SetupJob()
         {
-
+            if (!getPointCloud) return;
             _pointCloud = new PointCloud<PointXYZ>()
             {
                 points = new NativeArray<PointXYZ>(_pointsNum, Allocator.Persistent)
             };
 
             _noises = new NativeArray<float>(pointsNum, Allocator.Persistent);
-            if(_gaussianNoiseSigma==0f)
+            if (_gaussianNoiseSigma == 0f)
             {
                 for (int i = 0; i < pointsNum; i++)
                 {
@@ -117,7 +132,7 @@ private void SetupJob()
 
             _textureToPointsJob = new ITextureToPointsJob()
             {
-                near= m_camera.nearClipPlane,
+                near = m_camera.nearClipPlane,
                 far = m_camera.farClipPlane,
                 directions = _directions,
                 noises = _noises,
@@ -129,18 +144,21 @@ public override void UpdateSensor()
         {
             if (!LoadTexture()) return;
 
-            JobHandle jobHandle = default;
-
-            if (_gaussianNoiseSigma != 0f)
+            if (getPointCloud)
             {
-                jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1);
-            }
+                JobHandle jobHandle = default;
 
-            _textureToPointsJob.depthPixels = _texture.GetPixelData<Color>(0);
-            _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle);
+                if (_gaussianNoiseSigma != 0f)
+                {
+                    jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1);
+                }
 
-            JobHandle.ScheduleBatchedJobs();
-            _jobHandle.Complete();
+                _textureToPointsJob.depthPixels = _texture.GetPixelData<Color32>(0);
+                _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle);
+
+                JobHandle.ScheduleBatchedJobs();
+                _jobHandle.Complete();
+            }
 
 
             if (onSensorUpdated != null)
@@ -150,13 +168,14 @@ public override void UpdateSensor()
         private bool LoadTexture()
         {
             bool result = false;
-            AsyncGPUReadback.Request(_rt, 0, request => {
+            AsyncGPUReadback.Request(_rt, 0, request =>
+            {
                 if (request.hasError)
                 {
                 }
                 else
                 {
-                    var data = request.GetData<Color>();
+                    var data = request.GetData<Color32>();
                     _texture.LoadRawTextureData(data);
                     _texture.Apply();
                     result = true;
@@ -189,4 +208,4 @@ public void DisposeSensor()
             _rt.Release();
         }
     }
-}
+}

Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs

@@ -2,7 +2,6 @@
 // SPDX-License-Identifier: Apache-2.0
 
 using UnityEngine;
-
 using Unity.Burst;
 using Unity.Collections;
 using Unity.Jobs;
@@ -17,21 +16,20 @@ public struct ITextureToPointsJob : IJobParallelFor
         public float near;
         public float far;
 
-        [ReadOnly]
-        public NativeArray<float3> directions;
+        [ReadOnly] public NativeArray<float3> directions;
 
-        [ReadOnly]
-        public NativeArray<Color> depthPixels;
-        [ReadOnly]
-        public NativeArray<float> noises;
+        [ReadOnly] public NativeArray<Color32> depthPixels;
+        [ReadOnly] public NativeArray<float> noises;
 
         public NativeArray<PointXYZ> points;
 
         public void Execute(int index)
         {
             float distance = (1.0f - Mathf.Clamp01(depthPixels.AsReadOnly()[index].r)) * far;
             float distance_noised = distance + noises[index];
-            distance = (near < distance && distance < far && near < distance_noised && distance_noised < far) ? distance_noised : 0;
+            distance = (near < distance && distance < far && near < distance_noised && distance_noised < far)
+                ? distance_noised
+                : 0;
 
             PointXYZ point = new PointXYZ()
             {
@@ -40,4 +38,4 @@ public void Execute(int index)
             points[index] = point;
         }
     }
-}
+}