From 6e51ae1cddcbe7676ccfd4f59f1665d2f71b836e Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Thu, 9 Oct 2025 09:57:42 +0300 Subject: [PATCH 1/8] Initial update of depth sensor to 16 bit --- Runtime/Tx/DepthCameraTx.cs | 78 +++++++++++-------- .../Camera/DepthCamera/DepthCameraSensor.cs | 15 ++-- .../Camera/DepthCamera/ITextureToPointsJob.cs | 2 +- 3 files changed, 54 insertions(+), 41 deletions(-) diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index 60437cb..86a481d 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/Runtime/Tx/DepthCameraTx.cs @@ -1,15 +1,12 @@ using System; 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")] -public class DepthCameraTx : ProBridgeTxStamped +public class DepthCameraTx : ProBridgeTxStamped { public Camera renderCamera; public float _minRange = 0.05f; @@ -32,11 +29,11 @@ public PointCloud pointCloud private bool sensorReady = false; // Reuse the compressor to avoid per-frame allocations. - private TJCompressor _compressor; + //private TJCompressor _compressor; protected override void AfterEnable() { - _compressor = new TJCompressor(); + //_compressor = new TJCompressor(); _cameraSensor = renderCamera.gameObject.AddComponent(); _cameraSensor.mat = new Material(depthShader); @@ -58,11 +55,11 @@ protected override void AfterDisable() if (_cameraSensor != null) _cameraSensor.DisposeSensor(); - if (_compressor != null) - { - _compressor.Dispose(); - _compressor = null; - } + //if (_compressor != null) + //{ + // _compressor.Dispose(); + // _compressor = null; + //} } private void OnSensorUpdated() @@ -72,33 +69,52 @@ private void OnSensorUpdated() protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) { - if (!sensorReady) - throw new Exception("Sensor is not ready"); + if (!sensorReady) return null; + sensorReady = false; - var tex = _cameraSensor.texture0; - if (tex == null) - throw new Exception("Depth sensor texture is not a Texture2D."); + var tex = _cameraSensor.texture0; // TextureFormat.RGBAFloat + if (tex == null) return null; - if (_rawImage != null) - _rawImage.texture = tex; + int W = tex.width, H = tex.height, N = W * H; - sensorReady = false; + // --- Read depth in METERS as float per pixel --- + float[] src; // one float per pixel, row-major (Unity origin = bottom-left) + + if (tex.format == TextureFormat.RFloat) + { + // Single-channel 32F texture + src = tex.GetRawTextureData().ToArray(); + } + else + { + // RGBAFloat / RGBAHalf path: take R channel as meters + var px = tex.GetPixelData(0); + if (!px.IsCreated || px.Length != N) return null; + src = new float[N]; + for (int i = 0; i < N; i++) src[i] = px[i].r; + } - NativeArray raw = tex.GetRawTextureData(); - byte[] rgbaBytes = raw.ToArray(); + // --- FLIP VERTICALLY: Unity (bottom-left) -> ROS/OpenCV (top-left) --- + var flipped = new float[N]; + for (int y = 0; y < H; y++) + { + int srcY = H - 1 - y; // take rows from bottom to top + Array.Copy(src, srcY * W, flipped, y * W, W); + } - var jpg = _compressor.Compress( - rgbaBytes, 0, - tex.width, tex.height, - TJPixelFormats.TJPF_RGBA, - TJSubsamplingOptions.TJSAMP_GRAY, - CompressionQuality, - TJFlags.FASTDCT | TJFlags.BOTTOMUP - ); + // --- Pack to bytes and publish as 32FC1 --- + var bytes = new byte[N * sizeof(float)]; + Buffer.BlockCopy(flipped, 0, bytes, 0, bytes.Length); - data.format = "jpeg"; - data.data = jpg; + data.height = (uint)H; + data.width = (uint)W; + data.encoding = "32FC1"; // meters + data.is_bigendian = 0; + data.step = (uint)(W * 4); + data.data = bytes; + data.header.frame_id = "depth_camera"; return base.GetMsg(ts); } + } \ No newline at end of file diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs index 4219f38..09e1d51 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs @@ -75,10 +75,10 @@ public override void Init() _camera.nearClipPlane = _minRange; _camera.farClipPlane = _maxRange; - _rt = new RenderTexture(_resolution.x, _resolution.y, 32, RenderTextureFormat.ARGB32); + _rt = new RenderTexture(_resolution.x, _resolution.y, 0, RenderTextureFormat.ARGBFloat); _camera.targetTexture = _rt; - _texture = new Texture2D(_resolution.x, _resolution.y, TextureFormat.ARGB32, false); + _texture = new Texture2D(_resolution.x, _resolution.y, TextureFormat.RGBAFloat, false); float f = m_camera.farClipPlane; mat.SetFloat("_F", f); @@ -153,7 +153,7 @@ public override void UpdateSensor() jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); } - _textureToPointsJob.depthPixels = _texture.GetPixelData(0); + _textureToPointsJob.depthPixels = _texture.GetPixelData(0); _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); JobHandle.ScheduleBatchedJobs(); @@ -170,14 +170,11 @@ private bool LoadTexture() bool result = false; AsyncGPUReadback.Request(_rt, 0, request => { - if (request.hasError) + if (!request.hasError) { - } - else - { - var data = request.GetData(); + var data = request.GetData(); _texture.LoadRawTextureData(data); - _texture.Apply(); + _texture.Apply(false, false); 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 5e562d1..97f101c 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs @@ -18,7 +18,7 @@ public struct ITextureToPointsJob : IJobParallelFor [ReadOnly] public NativeArray directions; - [ReadOnly] public NativeArray depthPixels; + [ReadOnly] public NativeArray depthPixels; [ReadOnly] public NativeArray noises; public NativeArray points; From 30fe27e04ea1b1dd085afcdbd24fb1ce64e8b06f Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Thu, 9 Oct 2025 15:39:11 +0300 Subject: [PATCH 2/8] Lower 32bit output to 16bit --- Runtime/Tx/DepthCameraTx.cs | 110 +++++++++++++----- .../Camera/DepthCamera/DepthCameraSensor.cs | 62 +++++++--- 2 files changed, 126 insertions(+), 46 deletions(-) diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index 86a481d..06d6519 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/Runtime/Tx/DepthCameraTx.cs @@ -72,49 +72,105 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) if (!sensorReady) return null; sensorReady = false; - var tex = _cameraSensor.texture0; // TextureFormat.RGBAFloat - if (tex == null) return null; + int W = _cameraSensor.Width; + int H = _cameraSensor.Height; + if (W <= 0 || H <= 0) return null; - int W = tex.width, H = tex.height, N = W * H; + // Packed R16 UNorm from the sensor (length = W*H*2) + ReadOnlySpan src = _cameraSensor.LatestR16Bytes(); + if (src.Length != W * H * 2) return null; - // --- Read depth in METERS as float per pixel --- - float[] src; // one float per pixel, row-major (Unity origin = bottom-left) + float farM = _cameraSensor.m_camera.farClipPlane; - if (tex.format == TextureFormat.RFloat) - { - // Single-channel 32F texture - src = tex.GetRawTextureData().ToArray(); - } - else - { - // RGBAFloat / RGBAHalf path: take R channel as meters - var px = tex.GetPixelData(0); - if (!px.IsCreated || px.Length != N) return null; - src = new float[N]; - for (int i = 0; i < N; i++) src[i] = px[i].r; - } + // Reuse these as fields if you want to avoid GC each frame + ushort[] dstU16 = new ushort[W * H]; - // --- FLIP VERTICALLY: Unity (bottom-left) -> ROS/OpenCV (top-left) --- - var flipped = new float[N]; + // Convert: R16 UNorm -> distanceNorm [0..1] -> depth meters -> mm (uint16) + // Also flip vertically (Unity bottom-left -> ROS top-left) + int rowBytes = W * 2; for (int y = 0; y < H; y++) { - int srcY = H - 1 - y; // take rows from bottom to top - Array.Copy(src, srcY * W, flipped, y * W, W); + int srcY = H - 1 - y; // flip + int srcRow = srcY * rowBytes; + int dstRow = y * W; + + for (int x = 0; x < W; x++) + { + // read uint16 little-endian + int b0 = src[srcRow + (x << 1) + 0]; + int b1 = src[srcRow + (x << 1) + 1]; + int u16 = (b1 << 8) | b0; + + float distanceNorm = u16 * (1.0f / 65535.0f); // 0..1 + float depthM = (1.0f - distanceNorm) * farM; // meters (undo shader) + int mm = (int)Mathf.Round(depthM * 1000.0f); // to mm + + if (mm < 0) mm = 0; + else if (mm > 65535) mm = 65535; + + dstU16[dstRow + x] = (ushort)mm; + } } - // --- Pack to bytes and publish as 32FC1 --- - var bytes = new byte[N * sizeof(float)]; - Buffer.BlockCopy(flipped, 0, bytes, 0, bytes.Length); + // Pack to bytes + byte[] bytes = new byte[W * H * 2]; + Buffer.BlockCopy(dstU16, 0, bytes, 0, bytes.Length); data.height = (uint)H; data.width = (uint)W; - data.encoding = "32FC1"; // meters + data.encoding = "16UC1"; data.is_bigendian = 0; - data.step = (uint)(W * 4); + data.step = (uint)(W * 2); data.data = bytes; data.header.frame_id = "depth_camera"; return base.GetMsg(ts); + // if (!sensorReady) return null; + // sensorReady = false; + + // var tex = _cameraSensor.texture0; // TextureFormat.RGBAFloat + // if (tex == null) return null; + + // int W = tex.width, H = tex.height, N = W * H; + + // // --- Read depth in METERS as float per pixel --- + // float[] src; // one float per pixel, row-major (Unity origin = bottom-left) + + // if (tex.format == TextureFormat.RFloat) + // { + // // Single-channel 32F texture + // src = tex.GetRawTextureData().ToArray(); + // } + // else + // { + // // RGBAFloat / RGBAHalf path: take R channel as meters + // var px = tex.GetPixelData(0); + // if (!px.IsCreated || px.Length != N) return null; + // src = new float[N]; + // for (int i = 0; i < N; i++) src[i] = px[i].r; + // } + + // // --- FLIP VERTICALLY: Unity (bottom-left) -> ROS/OpenCV (top-left) --- + // var flipped = new float[N]; + // for (int y = 0; y < H; y++) + // { + // int srcY = H - 1 - y; // take rows from bottom to top + // Array.Copy(src, srcY * W, flipped, y * W, W); + // } + + // // --- Pack to bytes and publish as 32FC1 --- + // var bytes = new byte[N * sizeof(float)]; + // Buffer.BlockCopy(flipped, 0, bytes, 0, bytes.Length); + + // data.height = (uint)H; + // data.width = (uint)W; + // data.encoding = "32FC1"; // meters + // data.is_bigendian = 0; + // data.step = (uint)(W * 4); + // data.data = bytes; + // data.header.frame_id = "depth_camera"; + + // return base.GetMsg(ts); } } \ No newline at end of file diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs index 09e1d51..a54f3a4 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs @@ -68,21 +68,37 @@ public int pointsNum public bool getPointCloud = false; - + // fields to reuse buffers (avoid GC) + private byte[] _r16Bytes; // w*h*2 + private int _w, _h; public override void Init() { _camera.fieldOfView = _fov; _camera.nearClipPlane = _minRange; _camera.farClipPlane = _maxRange; - _rt = new RenderTexture(_resolution.x, _resolution.y, 0, RenderTextureFormat.ARGBFloat); - _camera.targetTexture = _rt; + //_rt = new RenderTexture(_resolution.x, _resolution.y, 0, RenderTextureFormat.ARGBFloat); + //_camera.targetTexture = _rt; - _texture = new Texture2D(_resolution.x, _resolution.y, TextureFormat.RGBAFloat, false); + //_texture = new Texture2D(_resolution.x, _resolution.y, TextureFormat.RGBAFloat, false); float f = m_camera.farClipPlane; mat.SetFloat("_F", f); + var desc = new RenderTextureDescriptor(_resolution.x, _resolution.y) + { + graphicsFormat = UnityEngine.Experimental.Rendering.GraphicsFormat.R16_UNorm, + depthBufferBits = 0, + msaaSamples = 1, + sRGB = false + }; + _rt = new RenderTexture(desc); + _camera.targetTexture = _rt; + _camera.depthTextureMode |= DepthTextureMode.Depth; + + _w = _resolution.x; _h = _resolution.y; + _r16Bytes = new byte[_w * _h * 2]; // staged buffer + SetupDirections(); SetupJob(); } @@ -144,21 +160,21 @@ public override void UpdateSensor() { if (!LoadTexture()) return; - if (getPointCloud) - { - JobHandle jobHandle = default; + //if (getPointCloud) + //{ + // JobHandle jobHandle = default; - if (_gaussianNoiseSigma != 0f) - { - jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); - } + // if (_gaussianNoiseSigma != 0f) + // { + // jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); + // } - _textureToPointsJob.depthPixels = _texture.GetPixelData(0); - _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); + // _textureToPointsJob.depthPixels = _texture.GetPixelData(0); + // _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); - JobHandle.ScheduleBatchedJobs(); - _jobHandle.Complete(); - } + // JobHandle.ScheduleBatchedJobs(); + // _jobHandle.Complete(); + //} if (onSensorUpdated != null) @@ -172,9 +188,12 @@ private bool LoadTexture() { if (!request.hasError) { - var data = request.GetData(); - _texture.LoadRawTextureData(data); - _texture.Apply(false, false); + //var data = request.GetData(); + //_texture.LoadRawTextureData(data); + //_texture.Apply(false, false); + var data = request.GetData(); // tightly packed R16 + // copy into our reusable managed buffer (NativeArray becomes invalid later) + data.CopyTo(_r16Bytes); result = true; } }); @@ -204,5 +223,10 @@ public void DisposeSensor() _directions.Dispose(); _rt.Release(); } + + // Expose the latest packed R16 bytes to the publisher + public ReadOnlySpan LatestR16Bytes() => _r16Bytes; + public int Width => _w; + public int Height => _h; } } \ No newline at end of file From 77b2ca57c8a48d38ff8b498f3b40e82c93b8620e Mon Sep 17 00:00:00 2001 From: batouly <71108921+batouly@users.noreply.github.com> Date: Tue, 14 Oct 2025 14:04:38 +0300 Subject: [PATCH 3/8] Create DepthPointCloudTx.cs --- Runtime/Tx/DepthPointCloudTx.cs | 115 +++++++++++++++++++++++++++ Runtime/Tx/DepthPointCloudTx.cs.meta | 2 + 2 files changed, 117 insertions(+) create mode 100644 Runtime/Tx/DepthPointCloudTx.cs create mode 100644 Runtime/Tx/DepthPointCloudTx.cs.meta diff --git a/Runtime/Tx/DepthPointCloudTx.cs b/Runtime/Tx/DepthPointCloudTx.cs new file mode 100644 index 0000000..d7f4392 --- /dev/null +++ b/Runtime/Tx/DepthPointCloudTx.cs @@ -0,0 +1,115 @@ +using System; +using ProBridge.Tx; +using sensor_msgs.msg; +using UnityEngine; +using UnitySensors.Sensor.Camera; +using UnitySensors.Data.PointCloud; + +[AddComponentMenu("ProBridge/Tx/sensor_msgs/PointCloud2 (Depth)")] +public class DepthPointCloudTx : ProBridgeTxStamped +{ + public DepthCameraTx depthTx; + public bool skipNaN = false; // true => drop NaN points (changes width/height semantics) + + private bool sensorReady; + private DepthCameraSensor sensor; + protected override void AfterEnable() + { + } + + protected override void AfterDisable() + { + if (sensor != null) + sensor.onSensorUpdated -= OnSensorUpdated; + } + + private void OnSensorUpdated() => sensorReady = true; + + protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) + { + if (sensor == null && depthTx) + { + sensor = depthTx._cameraSensor; + sensor.onSensorUpdated += OnSensorUpdated; + return null; + } + if (!sensor.Initialized) + { + Debug.Log("sensor not initialized!"); + return null; + } + + if (!sensorReady) + return null; + + sensorReady = false; + + if (sensor == null || !sensor.getPointCloud) + return null; + + PointCloud pc = sensor.pointCloud; + if (pc.points.Length == 0) + { + Debug.Log("empty pcd!"); + return null; } + + int W = sensor.Width; + int H = sensor.Height; + int N = pc.points.Length; + + // Build PointCloud2 fields: x,y,z float32 + var fields = new PointField[3]; + fields[0] = new PointField { name = "x", offset = 0, datatype = PointField.FLOAT32, count = 1 }; + fields[1] = new PointField { name = "y", offset = 4, datatype = PointField.FLOAT32, count = 1 }; + fields[2] = new PointField { name = "z", offset = 8, datatype = PointField.FLOAT32, count = 1 }; + + int pointStep = 12; // 3 * float32 + byte[] bytes; + + if (!skipNaN) + { + bytes = new byte[N * pointStep]; + var tmp = new float[3]; + for (int i = 0; i < N; i++) + { + var p = pc.points[i].position; + tmp[0] = p.x; tmp[1] = p.y; tmp[2] = p.z; + Buffer.BlockCopy(tmp, 0, bytes, i * pointStep, pointStep); + } + + data.height = (uint)H; + data.width = (uint)W; + data.is_dense = false; // NaNs allowed + } + else + { + // compact: drop NaNs + var list = new System.Collections.Generic.List(N * pointStep); + var tmp = new float[3]; + + for (int i = 0; i < N; i++) + { + var p = pc.points[i].position; + if (float.IsNaN(p.x) || float.IsNaN(p.y) || float.IsNaN(p.z)) continue; + tmp[0] = p.x; tmp[1] = p.y; tmp[2] = p.z; + + var row = new byte[pointStep]; + Buffer.BlockCopy(tmp, 0, row, 0, pointStep); + list.AddRange(row); + } + bytes = list.ToArray(); + + data.height = 1; + data.width = (uint)(bytes.Length / pointStep); + data.is_dense = true; + } + + data.fields = fields; + data.is_bigendian = false; + data.point_step = (uint)pointStep; + data.row_step = (uint)(pointStep * (int)data.width); + data.data = bytes; + + return base.GetMsg(ts); + } +} diff --git a/Runtime/Tx/DepthPointCloudTx.cs.meta b/Runtime/Tx/DepthPointCloudTx.cs.meta new file mode 100644 index 0000000..1b37805 --- /dev/null +++ b/Runtime/Tx/DepthPointCloudTx.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 93fb9c1a14552f84d9010ce53d977018 \ No newline at end of file From d859f4ebd4091222908280de8f9c89e6b93bd4a6 Mon Sep 17 00:00:00 2001 From: batouly <71108921+batouly@users.noreply.github.com> Date: Tue, 14 Oct 2025 14:05:36 +0300 Subject: [PATCH 4/8] Create DepthMetersToPointsJob for 16bit depth image --- .../DepthCamera/DepthMetersToPointsJob.cs | 30 +++++++++++++++++++ .../DepthMetersToPointsJob.cs.meta | 11 +++++++ 2 files changed, 41 insertions(+) create mode 100644 Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs create mode 100644 Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs.meta diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs new file mode 100644 index 0000000..2e45f3f --- /dev/null +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs @@ -0,0 +1,30 @@ + +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using UnitySensors.Data.PointCloud; + +[BurstCompile] +public struct DepthMetersToPointsJob : IJobParallelFor +{ + public float near, far; + [ReadOnly] public NativeArray directions; + [ReadOnly] public NativeArray depthMeters; + [ReadOnly] public NativeArray noises; + public bool hasNoises; + + public NativeArray points; + + public void Execute(int i) + { + float d = depthMeters[i]; + if (hasNoises) d += noises[i]; + + bool valid = math.isfinite(d) & (d > near) & (d < far); + points[i] = new PointXYZ + { + position = valid ? directions[i] * d : new float3(float.NaN, float.NaN, float.NaN) + }; + } +} diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs.meta b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs.meta new file mode 100644 index 0000000..6a1bb4e --- /dev/null +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthMetersToPointsJob.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a17839a5ca716584ba0e7999e0bb23c9 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From dda81ab39c13ac9718895ecbfe52acea63ca70de Mon Sep 17 00:00:00 2001 From: batouly <71108921+batouly@users.noreply.github.com> Date: Tue, 14 Oct 2025 14:08:28 +0300 Subject: [PATCH 5/8] Add back the pointcloud code --- Runtime/Tx/DepthCameraTx.cs | 27 +++++++- .../Camera/DepthCamera/DepthCameraSensor.cs | 63 +++++++++++++++++-- 2 files changed, 83 insertions(+), 7 deletions(-) diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index 06d6519..6f6b7de 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/Runtime/Tx/DepthCameraTx.cs @@ -25,11 +25,16 @@ public PointCloud pointCloud get => _cameraSensor.pointCloud; } - private DepthCameraSensor _cameraSensor; + public DepthCameraSensor _cameraSensor { get; private set; } private bool sensorReady = false; // Reuse the compressor to avoid per-frame allocations. //private TJCompressor _compressor; +<<<<<<< Updated upstream +======= + private int H; + private int W; +>>>>>>> Stashed changes protected override void AfterEnable() { @@ -48,6 +53,15 @@ protected override void AfterEnable() _cameraSensor._resolution.y = textureHeight; _cameraSensor.getPointCloud = getPointCloud; _cameraSensor.Init(); + + W = _cameraSensor.Width; + H = _cameraSensor.Height; + + data.height = (uint)H; + data.width = (uint)W; + data.encoding = "16UC1"; + data.is_bigendian = 0; + data.step = (uint)(W * 2); } protected override void AfterDisable() @@ -72,8 +86,12 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) if (!sensorReady) return null; sensorReady = false; +<<<<<<< Updated upstream int W = _cameraSensor.Width; int H = _cameraSensor.Height; +======= + +>>>>>>> Stashed changes if (W <= 0 || H <= 0) return null; // Packed R16 UNorm from the sensor (length = W*H*2) @@ -116,6 +134,7 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) byte[] bytes = new byte[W * H * 2]; Buffer.BlockCopy(dstU16, 0, bytes, 0, bytes.Length); +<<<<<<< Updated upstream data.height = (uint)H; data.width = (uint)W; data.encoding = "16UC1"; @@ -171,6 +190,12 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) // data.header.frame_id = "depth_camera"; // return base.GetMsg(ts); +======= + data.data = bytes; + + return base.GetMsg(ts); + +>>>>>>> Stashed changes } } \ No newline at end of file diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs index a54f3a4..b277b76 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs @@ -13,6 +13,7 @@ using UnitySensors.Interface.Sensor; using UnitySensors.Utils.Noise; using Random = Unity.Mathematics.Random; +using UnityEngine.EventSystems; namespace UnitySensors.Sensor.Camera { @@ -33,7 +34,7 @@ public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInt private JobHandle _jobHandle; private IUpdateGaussianNoisesJob _updateGaussianNoisesJob; - private ITextureToPointsJob _textureToPointsJob; + private DepthMetersToPointsJob _depthMetersToPointsJob; private NativeArray _noises; private NativeArray _directions; @@ -41,6 +42,8 @@ public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInt private PointCloud _pointCloud; private int _pointsNum; + private NativeArray _depthMeters; + public override UnityEngine.Camera m_camera { get => _camera; @@ -77,11 +80,14 @@ public override void Init() _camera.nearClipPlane = _minRange; _camera.farClipPlane = _maxRange; +<<<<<<< Updated upstream //_rt = new RenderTexture(_resolution.x, _resolution.y, 0, RenderTextureFormat.ARGBFloat); //_camera.targetTexture = _rt; //_texture = new Texture2D(_resolution.x, _resolution.y, TextureFormat.RGBAFloat, false); +======= +>>>>>>> Stashed changes float f = m_camera.farClipPlane; mat.SetFloat("_F", f); @@ -101,6 +107,7 @@ public override void Init() SetupDirections(); SetupJob(); + Initialized = true; } private void SetupDirections() @@ -127,7 +134,7 @@ private void SetupJob() { points = new NativeArray(_pointsNum, Allocator.Persistent) }; - + _depthMeters = new NativeArray(_pointsNum, Allocator.Persistent); _noises = new NativeArray(pointsNum, Allocator.Persistent); if (_gaussianNoiseSigma == 0f) { @@ -146,12 +153,14 @@ private void SetupJob() }; } - _textureToPointsJob = new ITextureToPointsJob() + _depthMetersToPointsJob = new DepthMetersToPointsJob() { - near = m_camera.nearClipPlane, - far = m_camera.farClipPlane, + near = _camera.nearClipPlane, + far = _camera.farClipPlane, directions = _directions, + depthMeters = _depthMeters, noises = _noises, + hasNoises = (_gaussianNoiseSigma != 0f), points = _pointCloud.points }; } @@ -160,6 +169,7 @@ public override void UpdateSensor() { if (!LoadTexture()) return; +<<<<<<< Updated upstream //if (getPointCloud) //{ // JobHandle jobHandle = default; @@ -175,6 +185,36 @@ public override void UpdateSensor() // JobHandle.ScheduleBatchedJobs(); // _jobHandle.Complete(); //} +======= + if (getPointCloud) + { + // Convert R16 -> meters (NO vertical flip here) + int W = _w, H = _h; + float farM = _camera.farClipPlane; + + int idx = 0; + for (int y = 0; y < H; y++) + { + int rowOff = y * W * 2; + for (int x = 0; x < W; x++, idx++) + { + int off = rowOff + (x << 1); + int u16 = (_r16Bytes[off + 1] << 8) | _r16Bytes[off]; // little-endian + float norm = u16 * (1.0f / 65535.0f); + float dMeters = (1.0f - norm) * farM; // undo shader's normalization + _depthMeters[idx] = dMeters; + } + } + + JobHandle jh = default; + if (_gaussianNoiseSigma != 0f) + jh = _updateGaussianNoisesJob.Schedule(_pointsNum, 128); + + _jobHandle = _depthMetersToPointsJob.Schedule(_pointsNum, 128, jh); + JobHandle.ScheduleBatchedJobs(); + _jobHandle.Complete(); + } +>>>>>>> Stashed changes if (onSensorUpdated != null) @@ -184,13 +224,17 @@ public override void UpdateSensor() private bool LoadTexture() { bool result = false; - AsyncGPUReadback.Request(_rt, 0, request => + AsyncGPUReadback.Request(_rt, 0, TextureFormat.R16, request => { if (!request.hasError) +<<<<<<< Updated upstream { //var data = request.GetData(); //_texture.LoadRawTextureData(data); //_texture.Apply(false, false); +======= + { +>>>>>>> Stashed changes var data = request.GetData(); // tightly packed R16 // copy into our reusable managed buffer (NativeArray becomes invalid later) data.CopyTo(_r16Bytes); @@ -222,11 +266,18 @@ public void DisposeSensor() _noises.Dispose(); _directions.Dispose(); _rt.Release(); + + if (_depthMeters.IsCreated) _depthMeters.Dispose(); } // Expose the latest packed R16 bytes to the publisher public ReadOnlySpan LatestR16Bytes() => _r16Bytes; public int Width => _w; public int Height => _h; +<<<<<<< Updated upstream +======= + + public bool Initialized { get; private set; } +>>>>>>> Stashed changes } } \ No newline at end of file From 816279ea41677ba751af706fa32646559eaa6566 Mon Sep 17 00:00:00 2001 From: batouly <71108921+batouly@users.noreply.github.com> Date: Mon, 20 Oct 2025 11:21:04 +0300 Subject: [PATCH 6/8] Create depth16bit sensor and publisher --- Runtime/Tx/Depth16bitCameraTx.cs | 124 +++++++++ Runtime/Tx/Depth16bitCameraTx.cs.meta | 2 + .../DepthCamera/Depth16bitCameraSensor.cs | 242 ++++++++++++++++++ .../Depth16bitCameraSensor.cs.meta | 2 + 4 files changed, 370 insertions(+) create mode 100644 Runtime/Tx/Depth16bitCameraTx.cs create mode 100644 Runtime/Tx/Depth16bitCameraTx.cs.meta create mode 100644 Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs create mode 100644 Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs.meta diff --git a/Runtime/Tx/Depth16bitCameraTx.cs b/Runtime/Tx/Depth16bitCameraTx.cs new file mode 100644 index 0000000..00a21cb --- /dev/null +++ b/Runtime/Tx/Depth16bitCameraTx.cs @@ -0,0 +1,124 @@ +using System; +using ProBridge.Tx; +using UnityEngine; +using UnityEngine.UI; +using UnitySensors.Data.PointCloud; +using UnitySensors.Sensor.Camera; + +[AddComponentMenu("ProBridge/Tx/sensor_msgs/Depth16bit Camera")] +public class Depth16bitCameraTx : ProBridgeTxStamped +{ + public Camera renderCamera; + public float _minRange = 0.05f; + public float _maxRange = 100.0f; + public float _gaussianNoiseSigma = 0.0f; + public int fov = 30; + public int textureWidth = 1024; + public int textureHeight = 1024; + public RawImage _rawImage; + [Range(1, 100)] public int CompressionQuality = 90; + public Shader depthShader; + public bool getPointCloud = false; + + public PointCloud pointCloud + { + get => _cameraSensor.pointCloud; + } + + public Depth16bitCameraSensor _cameraSensor { get; private set; } + private bool sensorReady = false; + + private int H; + private int W; + + protected override void AfterEnable() + { + + _cameraSensor = renderCamera.gameObject.AddComponent(); + _cameraSensor.mat = new Material(depthShader); + _cameraSensor._camera = renderCamera; + _cameraSensor.onSensorUpdated += OnSensorUpdated; + _cameraSensor._minRange = _minRange; + _cameraSensor._maxRange = _maxRange; + _cameraSensor._gaussianNoiseSigma = _gaussianNoiseSigma; + _cameraSensor._fov = fov; + _cameraSensor._frequency_inv = sendRate; + _cameraSensor._resolution.x = textureWidth; + _cameraSensor._resolution.y = textureHeight; + _cameraSensor.getPointCloud = getPointCloud; + _cameraSensor.Init(); + + W = _cameraSensor.Width; + H = _cameraSensor.Height; + + data.height = (uint)H; + data.width = (uint)W; + data.encoding = "16UC1"; + data.is_bigendian = 0; + data.step = (uint)(W * 2); + } + + protected override void AfterDisable() + { + if (_cameraSensor != null) + _cameraSensor.DisposeSensor(); + } + + private void OnSensorUpdated() + { + sensorReady = true; + } + + protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) + { + if (!sensorReady) return null; + sensorReady = false; + + if (W <= 0 || H <= 0) return null; + + // Packed R16 UNorm from the sensor (length = W*H*2) + ReadOnlySpan src = _cameraSensor.LatestR16Bytes(); + if (src.Length != W * H * 2) return null; + + float farM = _cameraSensor.m_camera.farClipPlane; + + // Reuse these as fields if you want to avoid GC each frame + ushort[] dstU16 = new ushort[W * H]; + + // Convert: R16 UNorm -> distanceNorm [0..1] -> depth meters -> mm (uint16) + // Also flip vertically (Unity bottom-left -> ROS top-left) + int rowBytes = W * 2; + for (int y = 0; y < H; y++) + { + int srcY = H - 1 - y; // flip + int srcRow = srcY * rowBytes; + int dstRow = y * W; + + for (int x = 0; x < W; x++) + { + // read uint16 little-endian + int b0 = src[srcRow + (x << 1) + 0]; + int b1 = src[srcRow + (x << 1) + 1]; + int u16 = (b1 << 8) | b0; + + float distanceNorm = u16 * (1.0f / 65535.0f); // 0..1 + float depthM = (1.0f - distanceNorm) * farM; // meters (undo shader) + int mm = (int)Mathf.Round(depthM * 1000.0f); // to mm + + if (mm < 0) mm = 0; + else if (mm > 65535) mm = 65535; + + dstU16[dstRow + x] = (ushort)mm; + } + } + + // Pack to bytes + byte[] bytes = new byte[W * H * 2]; + Buffer.BlockCopy(dstU16, 0, bytes, 0, bytes.Length); + + data.data = bytes; + + return base.GetMsg(ts); + } + +} \ No newline at end of file diff --git a/Runtime/Tx/Depth16bitCameraTx.cs.meta b/Runtime/Tx/Depth16bitCameraTx.cs.meta new file mode 100644 index 0000000..d96aa36 --- /dev/null +++ b/Runtime/Tx/Depth16bitCameraTx.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 126030a133f16ae478ab7a97a9f6158e \ No newline at end of file diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs new file mode 100644 index 0000000..c18ccdf --- /dev/null +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs @@ -0,0 +1,242 @@ +// Copyright [2020-2024] Ryodo Tanaka (groadpg@gmail.com) and Akiro Harada +// SPDX-License-Identifier: Apache-2.0 + +using System; +using UnityEngine; +using UnityEngine.Rendering; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine.Serialization; +using UnityEngine.UI; +using UnitySensors.Data.PointCloud; +using UnitySensors.Interface.Sensor; +using UnitySensors.Utils.Noise; +using Random = Unity.Mathematics.Random; +using UnityEngine.EventSystems; + +namespace UnitySensors.Sensor.Camera +{ + [AddComponentMenu("")] + public class Depth16bitCameraSensor : CameraSensor, ITextureInterface, IPointCloudInterface + { + [SerializeField] public float _minRange = 0.05f; + [SerializeField] public float _maxRange = 100.0f; + [SerializeField] public float _gaussianNoiseSigma = 0.0f; + + + public UnityEngine.Camera _camera; + private RenderTexture _rt = null; + private Texture2D _texture; + + public Material mat; + + private JobHandle _jobHandle; + + private IUpdateGaussianNoisesJob _updateGaussianNoisesJob; + private DepthMetersToPointsJob _depthMetersToPointsJob; + + private NativeArray _noises; + private NativeArray _directions; + + private PointCloud _pointCloud; + private int _pointsNum; + + private NativeArray _depthMeters; + + 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; + + // fields to reuse buffers (avoid GC) + private byte[] _r16Bytes; // w*h*2 + private int _w, _h; + public override void Init() + { + _camera.fieldOfView = _fov; + _camera.nearClipPlane = _minRange; + _camera.farClipPlane = _maxRange; + + float f = m_camera.farClipPlane; + mat.SetFloat("_F", f); + + var desc = new RenderTextureDescriptor(_resolution.x, _resolution.y) + { + graphicsFormat = UnityEngine.Experimental.Rendering.GraphicsFormat.R16_UNorm, + depthBufferBits = 0, + msaaSamples = 1, + sRGB = false + }; + _rt = new RenderTexture(desc); + _camera.targetTexture = _rt; + _camera.depthTextureMode |= DepthTextureMode.Depth; + + _w = _resolution.x; _h = _resolution.y; + _r16Bytes = new byte[_w * _h * 2]; // staged buffer + + SetupDirections(); + SetupJob(); + Initialized = true; + } + + private void SetupDirections() + { + _pointsNum = _resolution.x * _resolution.y; + + _directions = new NativeArray(_pointsNum, Allocator.Persistent); + + float z = _resolution.y * 0.5f / Mathf.Tan(m_camera.fieldOfView * 0.5f * Mathf.Deg2Rad); + for (int y = 0; y < _resolution.y; y++) + { + for (int x = 0; x < _resolution.x; x++) + { + Vector3 vec = new Vector3(-_resolution.x / 2 + x, -_resolution.y / 2 + y, z); + _directions[y * _resolution.x + x] = vec.normalized; + } + } + } + + private void SetupJob() + { + if (!getPointCloud) return; + _pointCloud = new PointCloud() + { + points = new NativeArray(_pointsNum, Allocator.Persistent) + }; + _depthMeters = new NativeArray(_pointsNum, Allocator.Persistent); + _noises = new NativeArray(pointsNum, Allocator.Persistent); + if (_gaussianNoiseSigma == 0f) + { + for (int i = 0; i < pointsNum; i++) + { + _noises[i] = 0f; + } + } + else + { + _updateGaussianNoisesJob = new IUpdateGaussianNoisesJob() + { + sigma = _gaussianNoiseSigma, + random = new Random((uint)Environment.TickCount), + noises = _noises + }; + } + + _depthMetersToPointsJob = new DepthMetersToPointsJob() + { + near = _camera.nearClipPlane, + far = _camera.farClipPlane, + directions = _directions, + depthMeters = _depthMeters, + noises = _noises, + hasNoises = (_gaussianNoiseSigma != 0f), + points = _pointCloud.points + }; + } + + public override void UpdateSensor() + { + if (!LoadTexture()) return; + + if (getPointCloud) + { + // Convert R16 -> meters (NO vertical flip here) + int W = _w, H = _h; + float farM = _camera.farClipPlane; + + int idx = 0; + for (int y = 0; y < H; y++) + { + int rowOff = y * W * 2; + for (int x = 0; x < W; x++, idx++) + { + int off = rowOff + (x << 1); + int u16 = (_r16Bytes[off + 1] << 8) | _r16Bytes[off]; // little-endian + float norm = u16 * (1.0f / 65535.0f); + float dMeters = (1.0f - norm) * farM; // undo shader's normalization + _depthMeters[idx] = dMeters; + } + } + + JobHandle jh = default; + if (_gaussianNoiseSigma != 0f) + jh = _updateGaussianNoisesJob.Schedule(_pointsNum, 128); + + _jobHandle = _depthMetersToPointsJob.Schedule(_pointsNum, 128, jh); + JobHandle.ScheduleBatchedJobs(); + _jobHandle.Complete(); + } + + if (onSensorUpdated != null) + onSensorUpdated.Invoke(); + } + + private bool LoadTexture() + { + bool result = false; + AsyncGPUReadback.Request(_rt, 0, TextureFormat.R16, request => + { + if (!request.hasError) + { + var data = request.GetData(); // tightly packed R16 + // copy into our reusable managed buffer (NativeArray becomes invalid later) + data.CopyTo(_r16Bytes); + result = true; + } + }); + AsyncGPUReadback.WaitAllRequests(); + return result; + } + + protected override void OnSensorDestroy() + { + } + + private void OnRenderImage(RenderTexture source, RenderTexture dest) + { + Graphics.Blit(source, dest, mat); + } + + public void DisposeSensor() + { + _jobHandle.Complete(); + _pointCloud.Dispose(); + _noises.Dispose(); + _directions.Dispose(); + _rt.Release(); + + if (_depthMeters.IsCreated) _depthMeters.Dispose(); + } + + // Expose the latest packed R16 bytes to the publisher + public ReadOnlySpan LatestR16Bytes() => _r16Bytes; + public int Width => _w; + public int Height => _h; + + public bool Initialized { get; private set; } + + } +} \ No newline at end of file diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs.meta b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs.meta new file mode 100644 index 0000000..9c05c3a --- /dev/null +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/Depth16bitCameraSensor.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 3abecd6017586ac4ba57b7fb26df196b \ No newline at end of file From 0fd69d57f53aa53e17ff20cc961d5bc2c2859631 Mon Sep 17 00:00:00 2001 From: batouly <71108921+batouly@users.noreply.github.com> Date: Mon, 20 Oct 2025 11:23:47 +0300 Subject: [PATCH 7/8] Change back depth publisher and sensor to 8bit --- Runtime/Tx/DepthCameraTx.cs | 157 ++++-------------- .../Camera/DepthCamera/DepthCameraSensor.cs | 116 +++---------- .../Camera/DepthCamera/ITextureToPointsJob.cs | 26 +-- 3 files changed, 64 insertions(+), 235 deletions(-) diff --git a/Runtime/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index 6f6b7de..8e0b45d 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/Runtime/Tx/DepthCameraTx.cs @@ -1,12 +1,15 @@ using System; 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")] -public class DepthCameraTx : ProBridgeTxStamped +public class DepthCameraTx : ProBridgeTxStamped { public Camera renderCamera; public float _minRange = 0.05f; @@ -29,16 +32,11 @@ public PointCloud pointCloud private bool sensorReady = false; // Reuse the compressor to avoid per-frame allocations. - //private TJCompressor _compressor; -<<<<<<< Updated upstream -======= - private int H; - private int W; ->>>>>>> Stashed changes + private TJCompressor _compressor; protected override void AfterEnable() { - //_compressor = new TJCompressor(); + _compressor = new TJCompressor(); _cameraSensor = renderCamera.gameObject.AddComponent(); _cameraSensor.mat = new Material(depthShader); @@ -53,15 +51,6 @@ protected override void AfterEnable() _cameraSensor._resolution.y = textureHeight; _cameraSensor.getPointCloud = getPointCloud; _cameraSensor.Init(); - - W = _cameraSensor.Width; - H = _cameraSensor.Height; - - data.height = (uint)H; - data.width = (uint)W; - data.encoding = "16UC1"; - data.is_bigendian = 0; - data.step = (uint)(W * 2); } protected override void AfterDisable() @@ -69,11 +58,11 @@ protected override void AfterDisable() if (_cameraSensor != null) _cameraSensor.DisposeSensor(); - //if (_compressor != null) - //{ - // _compressor.Dispose(); - // _compressor = null; - //} + if (_compressor != null) + { + _compressor.Dispose(); + _compressor = null; + } } private void OnSensorUpdated() @@ -83,119 +72,31 @@ private void OnSensorUpdated() protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) { - if (!sensorReady) return null; - sensorReady = false; - -<<<<<<< Updated upstream - int W = _cameraSensor.Width; - int H = _cameraSensor.Height; -======= - ->>>>>>> Stashed changes - if (W <= 0 || H <= 0) return null; + if (!sensorReady) + throw new Exception("Sensor is not ready"); - // Packed R16 UNorm from the sensor (length = W*H*2) - ReadOnlySpan src = _cameraSensor.LatestR16Bytes(); - if (src.Length != W * H * 2) return null; - - float farM = _cameraSensor.m_camera.farClipPlane; - - // Reuse these as fields if you want to avoid GC each frame - ushort[] dstU16 = new ushort[W * H]; + sensorReady = false; - // Convert: R16 UNorm -> distanceNorm [0..1] -> depth meters -> mm (uint16) - // Also flip vertically (Unity bottom-left -> ROS top-left) - int rowBytes = W * 2; - for (int y = 0; y < H; y++) - { - int srcY = H - 1 - y; // flip - int srcRow = srcY * rowBytes; - int dstRow = y * W; - - for (int x = 0; x < W; x++) - { - // read uint16 little-endian - int b0 = src[srcRow + (x << 1) + 0]; - int b1 = src[srcRow + (x << 1) + 1]; - int u16 = (b1 << 8) | b0; - - float distanceNorm = u16 * (1.0f / 65535.0f); // 0..1 - float depthM = (1.0f - distanceNorm) * farM; // meters (undo shader) - int mm = (int)Mathf.Round(depthM * 1000.0f); // to mm - - if (mm < 0) mm = 0; - else if (mm > 65535) mm = 65535; - - dstU16[dstRow + x] = (ushort)mm; - } - } + var tex = _cameraSensor.texture0; + if (tex == null) return null; - // Pack to bytes - byte[] bytes = new byte[W * H * 2]; - Buffer.BlockCopy(dstU16, 0, bytes, 0, bytes.Length); + if (_rawImage != null) _rawImage.texture = tex; -<<<<<<< Updated upstream - data.height = (uint)H; - data.width = (uint)W; - data.encoding = "16UC1"; - data.is_bigendian = 0; - data.step = (uint)(W * 2); - data.data = bytes; - data.header.frame_id = "depth_camera"; + NativeArray raw = tex.GetRawTextureData(); + byte[] rgbaBytes = raw.ToArray(); - return base.GetMsg(ts); - // if (!sensorReady) return null; - // sensorReady = false; - - // var tex = _cameraSensor.texture0; // TextureFormat.RGBAFloat - // if (tex == null) return null; - - // int W = tex.width, H = tex.height, N = W * H; - - // // --- Read depth in METERS as float per pixel --- - // float[] src; // one float per pixel, row-major (Unity origin = bottom-left) - - // if (tex.format == TextureFormat.RFloat) - // { - // // Single-channel 32F texture - // src = tex.GetRawTextureData().ToArray(); - // } - // else - // { - // // RGBAFloat / RGBAHalf path: take R channel as meters - // var px = tex.GetPixelData(0); - // if (!px.IsCreated || px.Length != N) return null; - // src = new float[N]; - // for (int i = 0; i < N; i++) src[i] = px[i].r; - // } - - // // --- FLIP VERTICALLY: Unity (bottom-left) -> ROS/OpenCV (top-left) --- - // var flipped = new float[N]; - // for (int y = 0; y < H; y++) - // { - // int srcY = H - 1 - y; // take rows from bottom to top - // Array.Copy(src, srcY * W, flipped, y * W, W); - // } - - // // --- Pack to bytes and publish as 32FC1 --- - // var bytes = new byte[N * sizeof(float)]; - // Buffer.BlockCopy(flipped, 0, bytes, 0, bytes.Length); - - // data.height = (uint)H; - // data.width = (uint)W; - // data.encoding = "32FC1"; // meters - // data.is_bigendian = 0; - // data.step = (uint)(W * 4); - // data.data = bytes; - // data.header.frame_id = "depth_camera"; - - // return base.GetMsg(ts); -======= - data.data = bytes; + var jpg = _compressor.Compress( + rgbaBytes, 0, + tex.width, tex.height, + TJPixelFormats.TJPF_RGBA, + TJSubsamplingOptions.TJSAMP_GRAY, + CompressionQuality, + TJFlags.FASTDCT | TJFlags.BOTTOMUP + ); + data.format = "jpeg"; + data.data = jpg; return base.GetMsg(ts); - ->>>>>>> Stashed changes } } \ No newline at end of file diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs index b277b76..4219f38 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs @@ -13,7 +13,6 @@ using UnitySensors.Interface.Sensor; using UnitySensors.Utils.Noise; using Random = Unity.Mathematics.Random; -using UnityEngine.EventSystems; namespace UnitySensors.Sensor.Camera { @@ -34,7 +33,7 @@ public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInt private JobHandle _jobHandle; private IUpdateGaussianNoisesJob _updateGaussianNoisesJob; - private DepthMetersToPointsJob _depthMetersToPointsJob; + private ITextureToPointsJob _textureToPointsJob; private NativeArray _noises; private NativeArray _directions; @@ -42,8 +41,6 @@ public class DepthCameraSensor : CameraSensor, ITextureInterface, IPointCloudInt private PointCloud _pointCloud; private int _pointsNum; - private NativeArray _depthMeters; - public override UnityEngine.Camera m_camera { get => _camera; @@ -71,43 +68,23 @@ public int pointsNum public bool getPointCloud = false; - // fields to reuse buffers (avoid GC) - private byte[] _r16Bytes; // w*h*2 - private int _w, _h; + public override void Init() { _camera.fieldOfView = _fov; _camera.nearClipPlane = _minRange; _camera.farClipPlane = _maxRange; -<<<<<<< Updated upstream - //_rt = new RenderTexture(_resolution.x, _resolution.y, 0, RenderTextureFormat.ARGBFloat); - //_camera.targetTexture = _rt; + _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); -======= ->>>>>>> Stashed changes float f = m_camera.farClipPlane; mat.SetFloat("_F", f); - var desc = new RenderTextureDescriptor(_resolution.x, _resolution.y) - { - graphicsFormat = UnityEngine.Experimental.Rendering.GraphicsFormat.R16_UNorm, - depthBufferBits = 0, - msaaSamples = 1, - sRGB = false - }; - _rt = new RenderTexture(desc); - _camera.targetTexture = _rt; - _camera.depthTextureMode |= DepthTextureMode.Depth; - - _w = _resolution.x; _h = _resolution.y; - _r16Bytes = new byte[_w * _h * 2]; // staged buffer - SetupDirections(); SetupJob(); - Initialized = true; } private void SetupDirections() @@ -134,7 +111,7 @@ private void SetupJob() { points = new NativeArray(_pointsNum, Allocator.Persistent) }; - _depthMeters = new NativeArray(_pointsNum, Allocator.Persistent); + _noises = new NativeArray(pointsNum, Allocator.Persistent); if (_gaussianNoiseSigma == 0f) { @@ -153,14 +130,12 @@ private void SetupJob() }; } - _depthMetersToPointsJob = new DepthMetersToPointsJob() + _textureToPointsJob = new ITextureToPointsJob() { - near = _camera.nearClipPlane, - far = _camera.farClipPlane, + near = m_camera.nearClipPlane, + far = m_camera.farClipPlane, directions = _directions, - depthMeters = _depthMeters, noises = _noises, - hasNoises = (_gaussianNoiseSigma != 0f), points = _pointCloud.points }; } @@ -169,52 +144,21 @@ public override void UpdateSensor() { if (!LoadTexture()) return; -<<<<<<< Updated upstream - //if (getPointCloud) - //{ - // JobHandle jobHandle = default; - - // if (_gaussianNoiseSigma != 0f) - // { - // jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); - // } - - // _textureToPointsJob.depthPixels = _texture.GetPixelData(0); - // _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); - - // JobHandle.ScheduleBatchedJobs(); - // _jobHandle.Complete(); - //} -======= if (getPointCloud) { - // Convert R16 -> meters (NO vertical flip here) - int W = _w, H = _h; - float farM = _camera.farClipPlane; + JobHandle jobHandle = default; - int idx = 0; - for (int y = 0; y < H; y++) + if (_gaussianNoiseSigma != 0f) { - int rowOff = y * W * 2; - for (int x = 0; x < W; x++, idx++) - { - int off = rowOff + (x << 1); - int u16 = (_r16Bytes[off + 1] << 8) | _r16Bytes[off]; // little-endian - float norm = u16 * (1.0f / 65535.0f); - float dMeters = (1.0f - norm) * farM; // undo shader's normalization - _depthMeters[idx] = dMeters; - } + jobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1); } - JobHandle jh = default; - if (_gaussianNoiseSigma != 0f) - jh = _updateGaussianNoisesJob.Schedule(_pointsNum, 128); + _textureToPointsJob.depthPixels = _texture.GetPixelData(0); + _jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, jobHandle); - _jobHandle = _depthMetersToPointsJob.Schedule(_pointsNum, 128, jh); JobHandle.ScheduleBatchedJobs(); _jobHandle.Complete(); } ->>>>>>> Stashed changes if (onSensorUpdated != null) @@ -224,20 +168,16 @@ public override void UpdateSensor() private bool LoadTexture() { bool result = false; - AsyncGPUReadback.Request(_rt, 0, TextureFormat.R16, request => + AsyncGPUReadback.Request(_rt, 0, request => { - if (!request.hasError) -<<<<<<< Updated upstream + if (request.hasError) { - //var data = request.GetData(); - //_texture.LoadRawTextureData(data); - //_texture.Apply(false, false); -======= - { ->>>>>>> Stashed changes - var data = request.GetData(); // tightly packed R16 - // copy into our reusable managed buffer (NativeArray becomes invalid later) - data.CopyTo(_r16Bytes); + } + else + { + var data = request.GetData(); + _texture.LoadRawTextureData(data); + _texture.Apply(); result = true; } }); @@ -266,18 +206,6 @@ public void DisposeSensor() _noises.Dispose(); _directions.Dispose(); _rt.Release(); - - if (_depthMeters.IsCreated) _depthMeters.Dispose(); } - - // Expose the latest packed R16 bytes to the publisher - public ReadOnlySpan LatestR16Bytes() => _r16Bytes; - public int Width => _w; - public int Height => _h; -<<<<<<< Updated upstream -======= - - public bool Initialized { get; private set; } ->>>>>>> Stashed changes } } \ 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 97f101c..91cd22d 100644 --- a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs +++ b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs @@ -1,4 +1,4 @@ -// Copyright [2020-2024] Ryodo Tanaka (groadpg@gmail.com) and Akiro Harada +// Copyright [2020-2024] Ryodo Tanaka (groadpg@gmail.com) and Akiro Harada // SPDX-License-Identifier: Apache-2.0 using UnityEngine; @@ -17,25 +17,25 @@ public struct ITextureToPointsJob : IJobParallelFor public float far; [ReadOnly] public NativeArray directions; - - [ReadOnly] public NativeArray depthPixels; + [ReadOnly] public NativeArray depthPixels; // 8-bit/channel [ReadOnly] public NativeArray noises; public NativeArray points; public void Execute(int index) { - float distance = (1.0f - Mathf.Clamp01(depthPixels.AsReadOnly()[index].r)) * far; + byte r8 = depthPixels[index].r; + float r01 = r8 * (1.0f / 255.0f); // ← scale to 0..1 + + // shader outputs: distance01 = 1 - depthMeters/_F + float distance = (1.0f - math.saturate(r01)) * far; //meters + float distance_noised = distance + noises[index]; - distance = (near < distance && distance < far && near < distance_noised && distance_noised < far) - ? distance_noised - : 0; - - PointXYZ point = new PointXYZ() - { - position = directions[index] * distance - }; - points[index] = point; + if (!(near < distance && distance < far && near < distance_noised && distance_noised < far)) + distance_noised = 0f; + + points[index] = new PointXYZ { position = directions[index] * distance_noised }; } } + } \ No newline at end of file From b9621b9748a5e38a1ba8fac4c38527a35b6ff416 Mon Sep 17 00:00:00 2001 From: batouly <71108921+batouly@users.noreply.github.com> Date: Mon, 20 Oct 2025 13:03:02 +0300 Subject: [PATCH 8/8] Update DepthPointCloudTx to work with 16bit and 8bit --- Runtime/Tx/DepthPointCloudTx.cs | 91 +++++++++++++++++++--------- Runtime/Tx/DepthPointCloudTx.cs.meta | 2 +- 2 files changed, 64 insertions(+), 29 deletions(-) diff --git a/Runtime/Tx/DepthPointCloudTx.cs b/Runtime/Tx/DepthPointCloudTx.cs index d7f4392..751bc58 100644 --- a/Runtime/Tx/DepthPointCloudTx.cs +++ b/Runtime/Tx/DepthPointCloudTx.cs @@ -4,17 +4,27 @@ using UnityEngine; using UnitySensors.Sensor.Camera; using UnitySensors.Data.PointCloud; +using ProBridge.Utils; [AddComponentMenu("ProBridge/Tx/sensor_msgs/PointCloud2 (Depth)")] public class DepthPointCloudTx : ProBridgeTxStamped { - public DepthCameraTx depthTx; + public bool skipNaN = false; // true => drop NaN points (changes width/height semantics) + public bool isSource16bit; private bool sensorReady; + + private Depth16bitCameraTx depth16Tx; + private DepthCameraTx depthTx; + private Depth16bitCameraSensor sensor_16; private DepthCameraSensor sensor; protected override void AfterEnable() { + if (isSource16bit) + depth16Tx = GetComponent(); + else + depthTx = GetComponent(); } protected override void AfterDisable() @@ -27,36 +37,57 @@ protected override void AfterDisable() protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) { - if (sensor == null && depthTx) + if (isSource16bit) { - sensor = depthTx._cameraSensor; - sensor.onSensorUpdated += OnSensorUpdated; - return null; + if (sensor_16 == null && depth16Tx) + { + sensor_16 = depth16Tx._cameraSensor; + sensor_16.onSensorUpdated += OnSensorUpdated; + return null; + } } - if (!sensor.Initialized) + else { - Debug.Log("sensor not initialized!"); - return null; + if (sensor == null && depthTx) + { + sensor = depthTx._cameraSensor; + sensor.onSensorUpdated += OnSensorUpdated; + return null; + } } - if (!sensorReady) + if (isSource16bit ? sensor_16 == null : sensor == null) return null; - + + + if (!sensorReady) return null; sensorReady = false; - if (sensor == null || !sensor.getPointCloud) - return null; + PointCloud pc; + int W, H; - PointCloud pc = sensor.pointCloud; - if (pc.points.Length == 0) + if (isSource16bit) + { + if (!sensor_16.getPointCloud) return null; + pc = sensor_16.pointCloud; + W = sensor_16.Width; + H = sensor_16.Height; + } + else + { + if (!sensor.getPointCloud) return null; + pc = sensor.pointCloud; + W = sensor._resolution.x; + H = sensor._resolution.y; + } + + if (pc.points.Length == 0) { Debug.Log("empty pcd!"); - return null; } + return null; + } - int W = sensor.Width; - int H = sensor.Height; int N = pc.points.Length; - // Build PointCloud2 fields: x,y,z float32 var fields = new PointField[3]; fields[0] = new PointField { name = "x", offset = 0, datatype = PointField.FLOAT32, count = 1 }; @@ -70,40 +101,44 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) { bytes = new byte[N * pointStep]; var tmp = new float[3]; + for (int i = 0; i < N; i++) { - var p = pc.points[i].position; - tmp[0] = p.x; tmp[1] = p.y; tmp[2] = p.z; + var p= (Vector3)pc.points[i].position; + var pROS = p.ToRos(); + tmp[0] = (float) pROS.x; tmp[1] = (float)pROS.y; tmp[2] = (float)pROS.z; Buffer.BlockCopy(tmp, 0, bytes, i * pointStep, pointStep); } - data.height = (uint)H; + data.height = (uint)H; data.width = (uint)W; - data.is_dense = false; // NaNs allowed + data.is_dense = false; } else { - // compact: drop NaNs var list = new System.Collections.Generic.List(N * pointStep); var tmp = new float[3]; for (int i = 0; i < N; i++) { - var p = pc.points[i].position; - if (float.IsNaN(p.x) || float.IsNaN(p.y) || float.IsNaN(p.z)) continue; - tmp[0] = p.x; tmp[1] = p.y; tmp[2] = p.z; + var p = (Vector3)pc.points[i].position; + var pROS = p.ToRos(); + if (double.IsNaN(pROS.x) || double.IsNaN(pROS.y) || double.IsNaN(pROS.z)) continue; + + tmp[0] = (float)pROS.x; tmp[1] = (float)pROS.y; tmp[2] = (float)pROS.z; var row = new byte[pointStep]; Buffer.BlockCopy(tmp, 0, row, 0, pointStep); list.AddRange(row); } bytes = list.ToArray(); - data.height = 1; + data.height = 1; data.width = (uint)(bytes.Length / pointStep); - data.is_dense = true; + data.is_dense = true; } + data.fields = fields; data.is_bigendian = false; data.point_step = (uint)pointStep; diff --git a/Runtime/Tx/DepthPointCloudTx.cs.meta b/Runtime/Tx/DepthPointCloudTx.cs.meta index 1b37805..48f3a9b 100644 --- a/Runtime/Tx/DepthPointCloudTx.cs.meta +++ b/Runtime/Tx/DepthPointCloudTx.cs.meta @@ -1,2 +1,2 @@ fileFormatVersion: 2 -guid: 93fb9c1a14552f84d9010ce53d977018 \ No newline at end of file +guid: 5e35290327679ef488c88b096fb4fac8 \ No newline at end of file