Add noise

Author: Autumn60

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

@@ -1,3 +1,4 @@
+using System;
 using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
@@ -8,6 +9,8 @@
 using UnityEngine.Jobs;
 using Unity.Jobs;
 
+using Random = Unity.Mathematics.Random;
+
 namespace UnitySensors
 {
     public class LivoxSensor : Sensor
@@ -22,6 +25,8 @@ public class LivoxSensor : Sensor
         private float _minRange = 0.1f;
         [SerializeField]
         private float _maxRange = 100.0f;
+        [SerializeField]
+        private float _gaussianNoiseSigma = 0.0f;
 
         [SerializeField]
         private int _resolution = 100;
@@ -36,11 +41,15 @@ public class LivoxSensor : Sensor
         private Texture2D _texture;
 
         private JobHandle _handle;
-        private TextureToPointsJob _job;
+        private TextureToPointsJob _textureToPointsJob;
+        private UpdateGaussianNoisesJob _updateGaussianNoisesJob;
+        private Random _random;
         public NativeArray<Vector3> points;
         private NativeArray<Vector3> _directions;
         private NativeArray<int> _pixelIndices;
+        private NativeArray<float> _noises;
 
+        private uint _randomSeed;
         private int _pointsNum;
         public uint pointsNum { get=>(uint)(_pointsNum);}
 
@@ -109,23 +118,39 @@ private void SetupIndicesAndDirections()
         private void SetupJob()
         {
             points = new NativeArray<Vector3>(_pointsNum, Allocator.Persistent);
-            _job = new TextureToPointsJob()
+            _randomSeed = (uint)Environment.TickCount;
+            _random = new Random(_randomSeed);
+
+            _noises = new NativeArray<float>(_pointsNum, Allocator.Persistent);
+
+            _updateGaussianNoisesJob = new UpdateGaussianNoisesJob()
+            {
+                sigma = _gaussianNoiseSigma,
+                random = _random,
+                noises = _noises
+            };
+
+            _textureToPointsJob = new TextureToPointsJob()
             {
                 far = _maxRange,
                 scanSeparation = _scanSeparation,
                 separationCounter = 0,
                 pixelIndices = _pixelIndices,
                 directions = _directions,
                 pixels = _texture.GetPixelData<Color>(0),
+                noises = _noises,
                 points = points
             };
         }
 
         protected override void UpdateSensor()
         {
             _handle.Complete();
-            _job.separationCounter++;
-            if (_job.separationCounter >= _scanSeparation) _job.separationCounter = 0;
+            if (_randomSeed++ == 0) _randomSeed = 1;
+            _updateGaussianNoisesJob.random.InitState(_randomSeed);
+
+            _textureToPointsJob.separationCounter++;
+            if (_textureToPointsJob.separationCounter >= _scanSeparation) _textureToPointsJob.separationCounter = 0;
 
             AsyncGPUReadback.Request(_rt, 0, request => {
                 if (request.hasError)
@@ -140,7 +165,8 @@ protected override void UpdateSensor()
                 }
             });
 
-            _handle = _job.Schedule(_pointsNum, 1);
+            JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1);
+            _handle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle);
 
             JobHandle.ScheduleBatchedJobs();
         }
@@ -153,13 +179,32 @@ public void CompleteJob()
         private void OnDestroy()
         {
             _handle.Complete();
+            _noises.Dispose();
             _pixelIndices.Dispose();
             _directions.Dispose();
             points.Dispose();
 
             _rt.Release();
         }
 
+        [BurstCompile]
+        private struct UpdateGaussianNoisesJob : IJobParallelFor
+        {
+            public float sigma;
+            public Random random;
+            public NativeArray<float> noises;
+
+            public void Execute(int index)
+            {
+                var rand2 = random.NextFloat();
+                var rand3 = random.NextFloat();
+                float normrand =
+                    (float)Math.Sqrt(-2.0f * Math.Log(rand2)) *
+                    (float)Math.Cos(2.0f * Math.PI * rand3);
+                noises[index] = sigma * normrand;
+            }
+        }
+
         [BurstCompile]
         private struct TextureToPointsJob : IJobParallelFor
         {
@@ -174,6 +219,8 @@ private struct TextureToPointsJob : IJobParallelFor
 
             [ReadOnly]
             public NativeArray<Color> pixels;
+            [ReadOnly]
+            public NativeArray<float> noises;
 
             public NativeArray<Vector3> points;
 
@@ -182,7 +229,7 @@ public void Execute(int index)
                 int offset = points.Length * separationCounter / scanSeparation;
                 int pixelIndex = pixelIndices.AsReadOnly()[index + offset];
                 float distance = pixels.AsReadOnly()[pixelIndex].r;
-                points[index] = directions.AsReadOnly()[index + offset] * far * Mathf.Clamp01(1.0f - distance);
+                points[index] = directions.AsReadOnly()[index + offset] * (far * Mathf.Clamp01(1.0f - distance) + noises[index]);
             }
         }
     }