Merge pull request #91 from Autumn60/test/restruct

Fix pointcloud noise calc
Field-Robotics-Japan · Jan 6, 2024 · 3ca0ad9 · 3ca0ad9
2 parents 47e2c04 + f318b28
commit 3ca0ad9
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 5 deletions.
diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/Camera/DepthCamera/ITextureToPointsJob.cs
@@ -27,8 +27,10 @@ public struct ITextureToPointsJob : IJobParallelFor
 
         public void Execute(int index)
         {
-            float distance = Mathf.Clamp01(1.0f - pixels.AsReadOnly()[index].r) * far;
-            distance = (near < distance && distance < far) ? distance + noises[index] : 0;
+            float distance = (1.0f - Mathf.Clamp01(pixels.AsReadOnly()[index].r)) * far;
+            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

diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/DepthBufferLiDAR/ITextureToPointsJob.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/DepthBufferLiDAR/ITextureToPointsJob.cs
@@ -32,8 +32,9 @@ public struct ITextureToPointsJob : IJobParallelFor
         public void Execute(int index)
         {
             int pixelIndex = pixelIndices[index + indexOffset];
-            float distance = Mathf.Clamp01(1.0f - pixels.AsReadOnly()[pixelIndex].r) * far;
-            distance = (near < distance && distance < far) ? distance + noises[index] : 0;
+            float distance = (1.0f - Mathf.Clamp01(pixels.AsReadOnly()[pixelIndex].r)) * far;
+            float distance_noised = distance + noises[index];
+            distance = (near < distance && distance < far && near < distance_noised && distance_noised < far) ? distance_noised : 0;
             PointXYZI point = new PointXYZI()
             {
                 position = directions[index + indexOffset] * distance

diff --git a/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/RaycastLiDAR/IRaycastHitsToPointsJob.cs b/Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/RaycastLiDAR/IRaycastHitsToPointsJob.cs
@@ -34,7 +34,8 @@ public struct IRaycastHitsToPointsJob : IJobParallelFor
         public void Execute(int index)
         {
             float distance = raycastHits[index].distance;
-            distance = (minRange < distance && distance < maxRange) ? distance + noises[index] : 0;
+            float distance_noised = distance + noises[index];
+            distance = (minRange < distance && distance < maxRange && minRange < distance_noised && distance_noised < maxRange) ? distance_noised : 0;
             PointXYZI point = new PointXYZI()
             {
                 position = directions[index + indexOffset] * distance,