From 2d1b8d6ed74c759cf9c6a95f8ee9948d4fccaf57 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Thu, 14 Aug 2025 16:28:30 +0300 Subject: [PATCH 1/3] Use TurboJpeg for encoding --- Runtime/Tx/DepthCameraTx.cs | 39 +++++++++++--- .../Camera/DepthCamera/DepthCameraSensor.cs | 54 ++++++++++++------- .../Camera/DepthCamera/ITextureToPointsJob.cs | 16 +++--- 3 files changed, 73 insertions(+), 36 deletions(-) diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index c36fb4a..ff31efd 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/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")] @@ -23,12 +24,15 @@ public class DepthCameraTx : ProBridgeTxStamped 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(); _cameraSensor.mat = new Material(depthShader); _cameraSensor._camera = renderCamera; @@ -45,7 +49,14 @@ protected override void AfterEnable() protected override void AfterDisable() { - _cameraSensor.DisposeSensor(); + if (_cameraSensor != null) + _cameraSensor.DisposeSensor(); + + if (_compressor != null) + { + _compressor.Dispose(); + _compressor = null; + } } private void OnSensorUpdated() @@ -56,17 +67,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 raw = tex.GetRawTextureData(); + 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); } diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs index 99fb2c8..5a52650 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs @@ -12,7 +12,6 @@ using UnitySensors.Data.PointCloud; using UnitySensors.Interface.Sensor; using UnitySensors.Utils.Noise; - using Random = Unity.Mathematics.Random; namespace UnitySensors.Sensor.Camera @@ -20,12 +19,9 @@ namespace UnitySensors.Sensor.Camera [AddComponentMenu("")] public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInterface { - [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,13 +41,31 @@ public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInt private PointCloud _pointCloud; private int _pointsNum; - public override UnityEngine.Camera m_camera { get => _camera; } - public Texture2D texture0 { get => _texture; } - public Texture2D texture1 { get => _texture; } - public PointCloud 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 pointCloud + { + get => _pointCloud; + } + + public int pointsNum + { + get => _pointsNum; + } + public override void Init() { @@ -59,10 +73,10 @@ public override void Init() _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 +87,6 @@ public override void Init() private void SetupDirections() { - _pointsNum = _resolution.x * _resolution.y; _directions = new NativeArray(_pointsNum, Allocator.Persistent); @@ -136,11 +149,11 @@ public override void UpdateSensor() jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); } - _textureToPointsJob.depthPixels = _texture.GetPixelData(0); _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); JobHandle.ScheduleBatchedJobs(); _jobHandle.Complete(); + _textureToPointsJob.depthPixels = _texture.GetPixelData(0); if (onSensorUpdated != null) @@ -150,13 +163,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(); + var data = request.GetData(); _texture.LoadRawTextureData(data); _texture.Apply(); result = true; diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs index 6e0a24a..5e562d1 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs +++ b/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,13 +16,10 @@ public struct ITextureToPointsJob : IJobParallelFor public float near; public float far; - [ReadOnly] - public NativeArray directions; + [ReadOnly] public NativeArray directions; - [ReadOnly] - public NativeArray depthPixels; - [ReadOnly] - public NativeArray noises; + [ReadOnly] public NativeArray depthPixels; + [ReadOnly] public NativeArray noises; public NativeArray points; @@ -31,7 +27,9 @@ 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; } } -} +} \ No newline at end of file From 2dc2a3476f46f856215277234b9c10591196d7b5 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Thu, 14 Aug 2025 16:29:04 +0300 Subject: [PATCH 2/3] Add manaul toggle for getting point cloud --- Runtime/Tx/DepthCameraTx.cs | 5 ++++ .../Camera/DepthCamera/DepthCameraSensor.cs | 29 +++++++++++-------- 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index ff31efd..45dc098 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/Runtime/Tx/DepthCameraTx.cs @@ -21,7 +21,12 @@ public class DepthCameraTx : ProBridgeTxStamped public RawImage _rawImage; [Range(1, 100)] public int CompressionQuality = 90; public Shader depthShader; + public bool getPointCloud = false; + public PointCloud pointCloud + { + get => _cameraSensor.pointCloud; + } private DepthCameraSensor _cameraSensor; private bool sensorReady = false; diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs index 5a52650..4219f38 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs @@ -66,6 +66,8 @@ public int pointsNum get => _pointsNum; } + public bool getPointCloud = false; + public override void Init() { @@ -104,14 +106,14 @@ private void SetupDirections() private void SetupJob() { - + if (!getPointCloud) return; _pointCloud = new PointCloud() { points = new NativeArray(_pointsNum, Allocator.Persistent) }; _noises = new NativeArray(pointsNum, Allocator.Persistent); - if(_gaussianNoiseSigma==0f) + if (_gaussianNoiseSigma == 0f) { for (int i = 0; i < pointsNum; i++) { @@ -130,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, @@ -142,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; - _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); + if (_gaussianNoiseSigma != 0f) + { + jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); + } - JobHandle.ScheduleBatchedJobs(); - _jobHandle.Complete(); _textureToPointsJob.depthPixels = _texture.GetPixelData(0); + _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); + + JobHandle.ScheduleBatchedJobs(); + _jobHandle.Complete(); + } if (onSensorUpdated != null) @@ -203,4 +208,4 @@ public void DisposeSensor() _rt.Release(); } } -} +} \ No newline at end of file From de7cdc16f49c56d525edeae09fe5a81e9bbe86a7 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Thu, 14 Aug 2025 16:35:37 +0300 Subject: [PATCH 3/3] Send the get pointcloud flag to sensor --- Runtime/Tx/DepthCameraTx.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index 45dc098..6563cf8 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/Runtime/Tx/DepthCameraTx.cs @@ -49,6 +49,7 @@ protected override void AfterEnable() _cameraSensor._frequency_inv = sendRate; _cameraSensor._resolution.x = textureWidth; _cameraSensor._resolution.y = textureHeight; + _cameraSensor.getPointCloud = getPointCloud; _cameraSensor.Init(); }