Skip to content

Commit fe2e6f2

Browse files
authored
Merge pull request #41 from Field-Robotics-Japan/develop
Add Livox noise
2 parents 589ba12 + a422637 commit fe2e6f2

File tree

1 file changed

+83
-14
lines changed

1 file changed

+83
-14
lines changed

Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs

Lines changed: 83 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
using System;
12
using System.Collections;
23
using System.Collections.Generic;
34
using UnityEngine;
@@ -8,6 +9,8 @@
89
using UnityEngine.Jobs;
910
using Unity.Jobs;
1011

12+
using Random = Unity.Mathematics.Random;
13+
1114
namespace UnitySensors
1215
{
1316
public class LivoxSensor : Sensor
@@ -19,9 +22,13 @@ public class LivoxSensor : Sensor
1922
private int _scanSeparation = 40;
2023

2124
[SerializeField]
22-
private float _minRange = 0.1f;
25+
private float _minDistance = 0.1f;
26+
[SerializeField]
27+
private float _maxDistance = 100.0f;
28+
[SerializeField]
29+
private float _maxIntensity = 255.0f;
2330
[SerializeField]
24-
private float _maxRange = 100.0f;
31+
private float _gaussianNoiseSigma = 0.0f;
2532

2633
[SerializeField]
2734
private int _resolution = 100;
@@ -36,11 +43,16 @@ public class LivoxSensor : Sensor
3643
private Texture2D _texture;
3744

3845
private JobHandle _handle;
39-
private TextureToPointsJob _job;
46+
private TextureToPointsJob _textureToPointsJob;
47+
private UpdateGaussianNoisesJob _updateGaussianNoisesJob;
48+
private Random _random;
4049
public NativeArray<Vector3> points;
50+
public NativeArray<float> intensities;
4151
private NativeArray<Vector3> _directions;
4252
private NativeArray<int> _pixelIndices;
53+
private NativeArray<float> _noises;
4354

55+
private uint _randomSeed;
4456
private int _pointsNum;
4557
public uint pointsNum { get=>(uint)(_pointsNum);}
4658

@@ -80,8 +92,8 @@ private void SetupCamera()
8092

8193
_cam.targetTexture = _rt;
8294
_cam.fieldOfView = fov;
83-
_cam.nearClipPlane = _minRange;
84-
_cam.farClipPlane = _maxRange;
95+
_cam.nearClipPlane = _minDistance;
96+
_cam.farClipPlane = _maxDistance;
8597
_cam.gameObject.AddComponent<DepthCamera>();
8698
_cam.clearFlags = CameraClearFlags.SolidColor;
8799

@@ -109,23 +121,44 @@ private void SetupIndicesAndDirections()
109121
private void SetupJob()
110122
{
111123
points = new NativeArray<Vector3>(_pointsNum, Allocator.Persistent);
112-
_job = new TextureToPointsJob()
124+
intensities = new NativeArray<float>(_pointsNum, Allocator.Persistent);
125+
_randomSeed = (uint)Environment.TickCount;
126+
_random = new Random(_randomSeed);
127+
128+
_noises = new NativeArray<float>(_pointsNum, Allocator.Persistent);
129+
130+
_updateGaussianNoisesJob = new UpdateGaussianNoisesJob()
131+
{
132+
sigma = _gaussianNoiseSigma,
133+
random = _random,
134+
noises = _noises
135+
};
136+
137+
_textureToPointsJob = new TextureToPointsJob()
113138
{
114-
far = _maxRange,
115139
scanSeparation = _scanSeparation,
116140
separationCounter = 0,
141+
minDistance = _minDistance,
142+
minDistance_sqr = _minDistance * _minDistance,
143+
maxDistance = _maxDistance,
144+
maxIntensity = _maxIntensity,
117145
pixelIndices = _pixelIndices,
118146
directions = _directions,
119147
pixels = _texture.GetPixelData<Color>(0),
120-
points = points
148+
noises = _noises,
149+
points = points,
150+
intensities = intensities
121151
};
122152
}
123153

124154
protected override void UpdateSensor()
125155
{
126156
_handle.Complete();
127-
_job.separationCounter++;
128-
if (_job.separationCounter >= _scanSeparation) _job.separationCounter = 0;
157+
if (_randomSeed++ == 0) _randomSeed = 1;
158+
_updateGaussianNoisesJob.random.InitState(_randomSeed);
159+
160+
_textureToPointsJob.separationCounter++;
161+
if (_textureToPointsJob.separationCounter >= _scanSeparation) _textureToPointsJob.separationCounter = 0;
129162

130163
AsyncGPUReadback.Request(_rt, 0, request => {
131164
if (request.hasError)
@@ -140,7 +173,8 @@ protected override void UpdateSensor()
140173
}
141174
});
142175

143-
_handle = _job.Schedule(_pointsNum, 1);
176+
JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1);
177+
_handle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle);
144178

145179
JobHandle.ScheduleBatchedJobs();
146180
}
@@ -153,36 +187,71 @@ public void CompleteJob()
153187
private void OnDestroy()
154188
{
155189
_handle.Complete();
190+
_noises.Dispose();
156191
_pixelIndices.Dispose();
157192
_directions.Dispose();
158193
points.Dispose();
194+
intensities.Dispose();
159195

160196
_rt.Release();
161197
}
162198

199+
[BurstCompile]
200+
private struct UpdateGaussianNoisesJob : IJobParallelFor
201+
{
202+
public float sigma;
203+
public Random random;
204+
public NativeArray<float> noises;
205+
206+
public void Execute(int index)
207+
{
208+
var rand2 = random.NextFloat();
209+
var rand3 = random.NextFloat();
210+
float normrand =
211+
(float)Math.Sqrt(-2.0f * Math.Log(rand2)) *
212+
(float)Math.Cos(2.0f * Math.PI * rand3);
213+
noises[index] = sigma * normrand;
214+
}
215+
}
216+
163217
[BurstCompile]
164218
private struct TextureToPointsJob : IJobParallelFor
165219
{
166-
public float far;
167220
public int scanSeparation;
168221
public int separationCounter;
169222

223+
[ReadOnly]
224+
public float minDistance;
225+
[ReadOnly]
226+
public float minDistance_sqr;
227+
[ReadOnly]
228+
public float maxDistance;
229+
[ReadOnly]
230+
public float maxIntensity;
231+
170232
[ReadOnly]
171233
public NativeArray<int> pixelIndices;
172234
[ReadOnly]
173235
public NativeArray<Vector3> directions;
174236

175237
[ReadOnly]
176238
public NativeArray<Color> pixels;
239+
[ReadOnly]
240+
public NativeArray<float> noises;
177241

178242
public NativeArray<Vector3> points;
243+
public NativeArray<float> intensities;
179244

180245
public void Execute(int index)
181246
{
182247
int offset = points.Length * separationCounter / scanSeparation;
183248
int pixelIndex = pixelIndices.AsReadOnly()[index + offset];
184-
float distance = pixels.AsReadOnly()[pixelIndex].r;
185-
points[index] = directions.AsReadOnly()[index + offset] * far * Mathf.Clamp01(1.0f - distance);
249+
float distance = maxDistance * Mathf.Clamp01(1.0f - pixels.AsReadOnly()[pixelIndex].r) + noises[index];
250+
bool isValid = (minDistance <= distance && distance <= maxDistance);
251+
if (!isValid) distance = 0;
252+
points[index] = directions.AsReadOnly()[index + offset] * distance;
253+
float distance_sqr = distance * distance;
254+
intensities[index] = isValid ? maxIntensity * minDistance_sqr / distance_sqr : 0;
186255
}
187256
}
188257
}

0 commit comments

Comments
 (0)