diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index c36fb4a..6563cf8 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")] @@ -20,15 +21,23 @@ 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; + // 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; @@ -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 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..4219f38 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,33 @@ 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 bool getPointCloud = false; + public override void Init() { @@ -59,10 +75,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 +89,6 @@ public override void Init() private void SetupDirections() { - _pointsNum = _resolution.x * _resolution.y; _directions = new NativeArray(_pointsNum, Allocator.Persistent); @@ -91,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++) { @@ -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(0); - _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) @@ -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(); + var data = request.GetData(); _texture.LoadRawTextureData(data); _texture.Apply(); result = true; @@ -189,4 +208,4 @@ public void DisposeSensor() _rt.Release(); } } -} +} \ No newline at end of file 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