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/Tx/DepthCameraTx.cs b/Runtime/Tx/DepthCameraTx.cs index 60437cb..8e0b45d 100644 --- a/Runtime/Tx/DepthCameraTx.cs +++ b/Runtime/Tx/DepthCameraTx.cs @@ -28,7 +28,7 @@ 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. @@ -75,14 +75,12 @@ 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."); + sensorReady = false; - if (_rawImage != null) - _rawImage.texture = tex; + var tex = _cameraSensor.texture0; + if (tex == null) return null; - sensorReady = false; + if (_rawImage != null) _rawImage.texture = tex; NativeArray raw = tex.GetRawTextureData(); byte[] rgbaBytes = raw.ToArray(); @@ -98,7 +96,7 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) data.format = "jpeg"; data.data = jpg; - return base.GetMsg(ts); } + } \ No newline at end of file diff --git a/Runtime/Tx/DepthPointCloudTx.cs b/Runtime/Tx/DepthPointCloudTx.cs new file mode 100644 index 0000000..751bc58 --- /dev/null +++ b/Runtime/Tx/DepthPointCloudTx.cs @@ -0,0 +1,150 @@ +using System; +using ProBridge.Tx; +using sensor_msgs.msg; +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 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() + { + if (sensor != null) + sensor.onSensorUpdated -= OnSensorUpdated; + } + + private void OnSensorUpdated() => sensorReady = true; + + protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts) + { + if (isSource16bit) + { + if (sensor_16 == null && depth16Tx) + { + sensor_16 = depth16Tx._cameraSensor; + sensor_16.onSensorUpdated += OnSensorUpdated; + return null; + } + } + else + { + if (sensor == null && depthTx) + { + sensor = depthTx._cameraSensor; + sensor.onSensorUpdated += OnSensorUpdated; + return null; + } + } + + if (isSource16bit ? sensor_16 == null : sensor == null) + return null; + + + if (!sensorReady) return null; + sensorReady = false; + + PointCloud pc; + int W, H; + + 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; + } + + 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= (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.width = (uint)W; + data.is_dense = false; + } + else + { + var list = new System.Collections.Generic.List(N * pointStep); + var tmp = new float[3]; + + for (int i = 0; i < N; i++) + { + 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.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..48f3a9b --- /dev/null +++ b/Runtime/Tx/DepthPointCloudTx.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 5e35290327679ef488c88b096fb4fac8 \ 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 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: diff --git a/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs b/Runtime/Utils/UnitySensors/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs index 5e562d1..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