1
+ using System ;
1
2
using System . Collections ;
2
3
using System . Collections . Generic ;
3
4
using UnityEngine ;
8
9
using UnityEngine . Jobs ;
9
10
using Unity . Jobs ;
10
11
12
+ using Random = Unity . Mathematics . Random ;
13
+
11
14
namespace UnitySensors
12
15
{
13
16
public class LivoxSensor : Sensor
@@ -19,9 +22,13 @@ public class LivoxSensor : Sensor
19
22
private int _scanSeparation = 40 ;
20
23
21
24
[ 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 ;
23
30
[ SerializeField ]
24
- private float _maxRange = 100 .0f;
31
+ private float _gaussianNoiseSigma = 0 .0f;
25
32
26
33
[ SerializeField ]
27
34
private int _resolution = 100 ;
@@ -36,11 +43,16 @@ public class LivoxSensor : Sensor
36
43
private Texture2D _texture ;
37
44
38
45
private JobHandle _handle ;
39
- private TextureToPointsJob _job ;
46
+ private TextureToPointsJob _textureToPointsJob ;
47
+ private UpdateGaussianNoisesJob _updateGaussianNoisesJob ;
48
+ private Random _random ;
40
49
public NativeArray < Vector3 > points ;
50
+ public NativeArray < float > intensities ;
41
51
private NativeArray < Vector3 > _directions ;
42
52
private NativeArray < int > _pixelIndices ;
53
+ private NativeArray < float > _noises ;
43
54
55
+ private uint _randomSeed ;
44
56
private int _pointsNum ;
45
57
public uint pointsNum { get => ( uint ) ( _pointsNum ) ; }
46
58
@@ -80,8 +92,8 @@ private void SetupCamera()
80
92
81
93
_cam . targetTexture = _rt ;
82
94
_cam . fieldOfView = fov ;
83
- _cam . nearClipPlane = _minRange ;
84
- _cam . farClipPlane = _maxRange ;
95
+ _cam . nearClipPlane = _minDistance ;
96
+ _cam . farClipPlane = _maxDistance ;
85
97
_cam . gameObject . AddComponent < DepthCamera > ( ) ;
86
98
_cam . clearFlags = CameraClearFlags . SolidColor ;
87
99
@@ -109,23 +121,44 @@ private void SetupIndicesAndDirections()
109
121
private void SetupJob ( )
110
122
{
111
123
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 ( )
113
138
{
114
- far = _maxRange ,
115
139
scanSeparation = _scanSeparation ,
116
140
separationCounter = 0 ,
141
+ minDistance = _minDistance ,
142
+ minDistance_sqr = _minDistance * _minDistance ,
143
+ maxDistance = _maxDistance ,
144
+ maxIntensity = _maxIntensity ,
117
145
pixelIndices = _pixelIndices ,
118
146
directions = _directions ,
119
147
pixels = _texture . GetPixelData < Color > ( 0 ) ,
120
- points = points
148
+ noises = _noises ,
149
+ points = points ,
150
+ intensities = intensities
121
151
} ;
122
152
}
123
153
124
154
protected override void UpdateSensor ( )
125
155
{
126
156
_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 ;
129
162
130
163
AsyncGPUReadback . Request ( _rt , 0 , request => {
131
164
if ( request . hasError )
@@ -140,7 +173,8 @@ protected override void UpdateSensor()
140
173
}
141
174
} ) ;
142
175
143
- _handle = _job . Schedule ( _pointsNum , 1 ) ;
176
+ JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob . Schedule ( _pointsNum , 1 ) ;
177
+ _handle = _textureToPointsJob . Schedule ( _pointsNum , 1 , updateGaussianNoisesJobHandle ) ;
144
178
145
179
JobHandle . ScheduleBatchedJobs ( ) ;
146
180
}
@@ -153,36 +187,71 @@ public void CompleteJob()
153
187
private void OnDestroy ( )
154
188
{
155
189
_handle . Complete ( ) ;
190
+ _noises . Dispose ( ) ;
156
191
_pixelIndices . Dispose ( ) ;
157
192
_directions . Dispose ( ) ;
158
193
points . Dispose ( ) ;
194
+ intensities . Dispose ( ) ;
159
195
160
196
_rt . Release ( ) ;
161
197
}
162
198
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
+
163
217
[ BurstCompile ]
164
218
private struct TextureToPointsJob : IJobParallelFor
165
219
{
166
- public float far ;
167
220
public int scanSeparation ;
168
221
public int separationCounter ;
169
222
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
+
170
232
[ ReadOnly ]
171
233
public NativeArray < int > pixelIndices ;
172
234
[ ReadOnly ]
173
235
public NativeArray < Vector3 > directions ;
174
236
175
237
[ ReadOnly ]
176
238
public NativeArray < Color > pixels ;
239
+ [ ReadOnly ]
240
+ public NativeArray < float > noises ;
177
241
178
242
public NativeArray < Vector3 > points ;
243
+ public NativeArray < float > intensities ;
179
244
180
245
public void Execute ( int index )
181
246
{
182
247
int offset = points . Length * separationCounter / scanSeparation ;
183
248
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 ;
186
255
}
187
256
}
188
257
}
0 commit comments