Skip to content

Commit

Permalink
Added ImageEncodeJob to speed up Msg encoding; updated Schedule's Bat…
Browse files Browse the repository at this point in the history 
…chCount in Job to improve performance; added logging of GPU read errors; removed unsafe code in PointCloud2MsgSerializer.
  • Loading branch information
@Bob-Eric
Bob-Eric committed Nov 19, 2024
1 parent 4e31334 commit bbcb79c
Show file tree
Hide file tree
Showing 12 changed files with 163 additions and 92 deletions.
4 changes: 2 additions & 2 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/DepthCamera/DepthCameraSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,8 +113,8 @@ protected override void UpdateSensor()
{
if (!LoadTexture()) return;

JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1);
_jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle);
JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1024);
_jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1024, updateGaussianNoisesJobHandle);
JobHandle.ScheduleBatchedJobs();
_jobHandle.Complete();

Expand Down
1 change: 1 addition & 0 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBCamera/RGBCameraSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ protected bool LoadTexture()
{
if (request.hasError)
{
Debug.LogError("GPU readback error detected.");
}
else
{
Expand Down
6 changes: 4 additions & 2 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/RGBDCamera/RGBDCameraSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,8 +130,8 @@ protected override void UpdateSensor()
{
if (!LoadDepthTexture() || !LoadColorTexture()) return;

JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1);
_jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle);
JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1024);
_jobHandle = _textureToPointsJob.Schedule(_pointsNum, 1024, updateGaussianNoisesJobHandle);
JobHandle.ScheduleBatchedJobs();
_jobHandle.Complete();

Expand All @@ -146,6 +146,7 @@ private bool LoadDepthTexture()
{
if (request.hasError)
{
Debug.LogError("GPU readback error detected.");
}
else
{
Expand All @@ -166,6 +167,7 @@ private bool LoadColorTexture()
{
if (request.hasError)
{
Debug.LogError("GPU readback error detected.");
}
else
{
Expand Down
5 changes: 3 additions & 2 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/DepthBufferLiDAR/DepthBufferLiDARSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,8 +147,8 @@ protected override void UpdateSensor()
{
if (!LoadTexture()) return;

JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(pointsNum, 1);
_jobHandle = _textureToPointsJob.Schedule(pointsNum, 1, updateGaussianNoisesJobHandle);
JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(pointsNum, 1024);
_jobHandle = _textureToPointsJob.Schedule(pointsNum, 1024, updateGaussianNoisesJobHandle);

JobHandle.ScheduleBatchedJobs();
_jobHandle.Complete();
Expand All @@ -166,6 +166,7 @@ private bool LoadTexture()
{
if (request.hasError)
{
Debug.LogError("GPU readback error detected.");
}
else
{
Expand Down
6 changes: 3 additions & 3 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/RaycastLiDAR/RaycastLiDARSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,10 +91,10 @@ protected override void UpdateSensor()
_updateRaycastCommandsJob.origin = _transform.position;
_updateRaycastCommandsJob.localToWorldMatrix = _transform.localToWorldMatrix;

JobHandle updateRaycastCommandsJobHandle = _updateRaycastCommandsJob.Schedule(pointsNum, 1);
JobHandle updateRaycastCommandsJobHandle = _updateRaycastCommandsJob.Schedule(pointsNum, 1024);
JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(pointsNum, 1, updateRaycastCommandsJobHandle);
JobHandle raycastJobHandle = RaycastCommand.ScheduleBatch(_raycastCommands, _raycastHits, 256, updateGaussianNoisesJobHandle);
_jobHandle = _raycastHitsToPointsJob.Schedule(pointsNum, 1, raycastJobHandle);
JobHandle raycastJobHandle = RaycastCommand.ScheduleBatch(_raycastCommands, _raycastHits, 1024, updateGaussianNoisesJobHandle);
_jobHandle = _raycastHitsToPointsJob.Schedule(pointsNum, 1024, raycastJobHandle);

JobHandle.ScheduleBatchedJobs();
_jobHandle.Complete();
Expand Down
7 changes: 4 additions & 3 deletions Assets/UnitySensorsROS/Runtime/Prefabs/Camera/DepthCamera_ros.prefab
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ MonoBehaviour:
_source: {fileID: 4853532333615571030}
_header:
_source: {fileID: 4853532333615571030}
_frame_id: /camera_frame
_frame_id: camera_frame
--- !u!114 &9201004833233192004
MonoBehaviour:
m_ObjectHideFlags: 0
Expand All @@ -143,7 +143,7 @@ MonoBehaviour:
_serializer:
_header:
_source: {fileID: 4853532333615571030}
_frame_id: /camera_frame
_frame_id: camera_frame
_source: {fileID: 4853532333615571030}
--- !u!114 &7171801529083048031
MonoBehaviour:
Expand All @@ -165,4 +165,5 @@ MonoBehaviour:
_encoding: 2
_header:
_source: {fileID: 4853532333615571030}
_frame_id: /camera_frame
_frame_id: camera_frame
_encodedTexture: {fileID: 0}
128 changes: 55 additions & 73 deletions Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/ImageMsgSerializer.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@
using UnitySensors.Attribute;
using UnitySensors.Interface.Sensor;
using UnitySensors.ROS.Serializer.Std;
using UnitySensors.ROS.Utils.Image;
using Unity.Jobs;
using Unity.Collections;

namespace UnitySensors.ROS.Serializer.Sensor
{
Expand All @@ -16,45 +19,22 @@ private enum SourceTexture
Texture0,
Texture1
}
private enum Encoding
{
_RGB8,
_32FC1,
_16UC1
}
private struct ColorRGB24
{
public byte r;
public byte g;
public byte b;
}
private struct Color32FC1
{
public float r;
}
private struct Color16UC1
{
public ushort r;
}

[SerializeField, Interface(typeof(ITextureInterface))]
private Object _source;
[SerializeField]
private SourceTexture _sourceTexture;
[SerializeField]
private Encoding _encoding;
[SerializeField, Attribute.ReadOnly]
private Texture2D _encodedTexture;

[SerializeField]
private HeaderSerializer _header;

private ITextureInterface _sourceInterface;
private Texture2D _flippedTexture;
private float distanceFactor;
private ImageEncodeJob _imageEncodeJob;

private Color[] _sourcePixels;
private ColorRGB24[] _targetPixelsRGB24;
private Color32FC1[] _targetPixels32FC1;
private Color16UC1[] _targetPixels16UC1;

public override void Init()
{
Expand All @@ -81,34 +61,57 @@ public override void Init()
_msg.encoding = "32FC1";
bytesPerPixel = 1 * 4;
textureFormat = TextureFormat.RFloat;
distanceFactor = _sourceInterface.texture0FarClipPlane;
_targetPixels32FC1 = new Color32FC1[width * height];
float distanceFactor = _sourceInterface.texture0FarClipPlane;
_imageEncodeJob = new()
{
width = width,
height = height,
distanceFactor = distanceFactor,
encoding = _encoding,
targetTexture16UC1 = new NativeArray<Color16UC1>(0, Allocator.Persistent),
targetTextureRGB8 = new NativeArray<ColorRGB8>(0, Allocator.Persistent)
};
break;
case Encoding._16UC1:
_msg.encoding = "16UC1";
bytesPerPixel = 1 * 2;
textureFormat = TextureFormat.R16;
distanceFactor = _sourceInterface.texture0FarClipPlane * 1000;
_targetPixels16UC1 = new Color16UC1[width * height];
_imageEncodeJob = new()
{
width = width,
height = height,
distanceFactor = distanceFactor,
encoding = _encoding,
targetTexture32FC1 = new NativeArray<Color32FC1>(0, Allocator.Persistent),
targetTextureRGB8 = new NativeArray<ColorRGB8>(0, Allocator.Persistent)
};
break;
case Encoding._RGB8:
default:
_msg.encoding = "rgb8";
bytesPerPixel = 3 * 1;
textureFormat = TextureFormat.RGB24;
distanceFactor = 1;
_targetPixelsRGB24 = new ColorRGB24[width * height];
_imageEncodeJob = new()
{
width = width,
height = height,
distanceFactor = distanceFactor,
encoding = _encoding,
targetTexture32FC1 = new NativeArray<Color32FC1>(0, Allocator.Persistent),
targetTexture16UC1 = new NativeArray<Color16UC1>(0, Allocator.Persistent),
};
break;
}
_sourcePixels = new Color[width * height];

_msg.is_bigendian = 0;
_msg.width = (uint)width;
_msg.height = (uint)height;
_msg.step = (uint)(bytesPerPixel * width);
_msg.data = new byte[_msg.step * height];

_flippedTexture = new Texture2D(width, height, textureFormat, false);
_encodedTexture = new Texture2D(width, height, textureFormat, false);

}

Expand All @@ -117,58 +120,37 @@ public override ImageMsg Serialize()
_msg.header = _header.Serialize();
var texture = _sourceTexture == SourceTexture.Texture0 ? _sourceInterface.texture0 : _sourceInterface.texture1;

FlipTextureVertically(texture, _flippedTexture);

// Manually copy the data to the message to avoid GC allocation
_flippedTexture.GetRawTextureData<byte>().CopyTo(_msg.data);
return _msg;
}
private void FlipTextureVertically(Texture2D sourceTexture, Texture2D targetTexture)
{
// TODO: Use shader or jobs to flip the texture
int width = sourceTexture.width;
int height = sourceTexture.height;

// Manually copy the data to the message to avoid GC allocation
sourceTexture.GetPixelData<Color>(0).CopyTo(_sourcePixels);
_imageEncodeJob.sourceTextureRawData = texture.GetRawTextureData<Color>();

switch (_encoding)
{
case Encoding._32FC1:
for (int j = 0; j < height; j++)
{
for (int i = 0; i < width; i++)
{
_targetPixels32FC1[j * width + i].r = _sourcePixels[(height - j - 1) * width + i].r * distanceFactor;
}
}
targetTexture.SetPixelData(_targetPixels32FC1, 0);
_imageEncodeJob.targetTexture32FC1 = _encodedTexture.GetRawTextureData<Color32FC1>();
break;
case Encoding._16UC1:
for (int j = 0; j < height; j++)
{
for (int i = 0; i < width; i++)
{
_targetPixels16UC1[j * width + i].r = (ushort)(_sourcePixels[(height - j - 1) * width + i].r * distanceFactor);
}
}
targetTexture.SetPixelData(_targetPixels16UC1, 0);
_imageEncodeJob.targetTexture16UC1 = _encodedTexture.GetRawTextureData<Color16UC1>();
break;
case Encoding._RGB8:
default:
for (int j = 0; j < height; j++)
{
for (int i = 0; i < width; i++)
{
_targetPixelsRGB24[j * width + i].r = (byte)(_sourcePixels[(height - j - 1) * width + i].r * 255);
_targetPixelsRGB24[j * width + i].g = (byte)(_sourcePixels[(height - j - 1) * width + i].g * 255);
_targetPixelsRGB24[j * width + i].b = (byte)(_sourcePixels[(height - j - 1) * width + i].b * 255);
}
}
targetTexture.SetPixelData(_targetPixelsRGB24, 0);
_imageEncodeJob.targetTextureRGB8 = _encodedTexture.GetRawTextureData<ColorRGB8>();
break;
}
targetTexture.Apply();

_imageEncodeJob.Schedule(texture.width * texture.height, 1024).Complete();

_encodedTexture.Apply();

// Manually copy the data to the message to avoid GC allocation
_encodedTexture.GetRawTextureData<byte>().CopyTo(_msg.data);
return _msg;
}

public override void OnDestroy()
{
base.OnDestroy();
_imageEncodeJob.targetTexture32FC1.Dispose();
_imageEncodeJob.targetTexture16UC1.Dispose();
_imageEncodeJob.targetTextureRGB8.Dispose();
}
}
}
8 changes: 2 additions & 6 deletions Assets/UnitySensorsROS/Runtime/Scripts/Serializers/SensorMsgs/PointCloud2MsgSerializer.cs
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
using UnityEngine;
using Unity.Collections;
using Unity.Collections.LowLevel.Unsafe;
using Unity.Jobs;
using RosMessageTypes.Sensor;

Expand Down Expand Up @@ -61,11 +60,8 @@ public override PointCloud2Msg Serialize()
{
_msg.header = _header.Serialize();

unsafe
{
UnsafeUtility.MemCpy(NativeArrayUnsafeUtility.GetUnsafePtr(_data), NativeArrayUnsafeUtility.GetUnsafePtr(_sourceInterface.pointCloud.points), _data.Length);
}
_jobHandle = _invertXJob.Schedule(_pointsNum, 1);
_sourceInterface.pointCloud.points.Reinterpret<byte>(PointUtilitiesSO.pointDataSizes[typeof(T)]).CopyTo(_data);
_jobHandle = _invertXJob.Schedule(_pointsNum, 1024);
_jobHandle.Complete();

_data.CopyTo(_msg.data);
Expand Down
8 changes: 8 additions & 0 deletions Assets/UnitySensorsROS/Runtime/Scripts/Utils/Image.meta
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Loading

0 comments on commit bbcb79c

Please sign in to comment.