Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/3D/RangeDataInserter3D.cs

@@ -0,0 +1,164 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Common.Math;
+using CartographerSharp.Sensor;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D3D;
+
+/// <summary>
+/// Options for 3D range data inserter.
+/// </summary>
+public struct RangeDataInserterOptions3D(
+    double hitProbability,
+    double missProbability,
+    int numFreeSpaceVoxels,
+    double intensityThreshold)
+{
+    public double HitProbability { get; set; } = hitProbability;
+    public double MissProbability { get; set; } = missProbability;
+    public int NumFreeSpaceVoxels { get; set; } = numFreeSpaceVoxels;
+    public double IntensityThreshold { get; set; } = intensityThreshold;
+}
+
+/// <summary>
+/// Range data inserter for 3D hybrid grids.
+/// </summary>
+public class RangeDataInserter3D
+{
+    private readonly RangeDataInserterOptions3D _options;
+    private readonly List<ushort> _hitTable;
+    private readonly List<ushort> _missTable;
+
+    public RangeDataInserter3D(RangeDataInserterOptions3D options)
+    {
+        if (options.HitProbability <= 0.5)
+        {
+            throw new ArgumentException("hit_probability must be greater than 0.5", nameof(options));
+        }
+        if (options.MissProbability >= 0.5)
+        {
+            throw new ArgumentException("miss_probability must be less than 0.5", nameof(options));
+        }
+
+        _options = options;
+        _hitTable = ProbabilityValues.ComputeLookupTableToApplyOdds(
+            ProbabilityValues.Odds(options.HitProbability));
+        _missTable = ProbabilityValues.ComputeLookupTableToApplyOdds(
+            ProbabilityValues.Odds(options.MissProbability));
+    }
+
+    /// <summary>
+    /// Inserts 'range_data' into 'hybrid_grid' and optionally into 'intensity_hybrid_grid'.
+    /// </summary>
+    public void Insert(
+        RangeData rangeData,
+        HybridGrid hybridGrid,
+        IntensityHybridGrid? intensityHybridGrid)
+    {
+        ArgumentNullException.ThrowIfNull(hybridGrid);
+
+        // Insert hits
+        foreach (var hit in rangeData.Returns.Points)
+        {
+            var hitCell = hybridGrid.GetCellIndex(hit.Position);
+            hybridGrid.ApplyLookupTable(hitCell, _hitTable);
+        }
+
+        // By not starting a new update after hits are inserted, we give hits priority
+        // (i.e. no hits will be ignored because of a miss in the same cell).
+        InsertMissesIntoGrid(_missTable, rangeData.Origin, rangeData.Returns, hybridGrid, _options.NumFreeSpaceVoxels);
+
+        if (intensityHybridGrid != null)
+        {
+            InsertIntensitiesIntoGrid(rangeData.Returns, intensityHybridGrid, _options.IntensityThreshold);
+        }
+
+        hybridGrid.FinishUpdate();
+    }
+
+    /// <summary>
+    /// Inserts misses into the grid along rays from origin to returns.
+    /// </summary>
+    private static void InsertMissesIntoGrid(
+        List<ushort> missTable,
+        Vector3 origin,
+        PointCloud returns,
+        HybridGrid hybridGrid,
+        int numFreeSpaceVoxels)
+    {
+        var originCell = hybridGrid.GetCellIndex(origin);
+        
+        foreach (var hit in returns.Points)
+        {
+            var hitCell = hybridGrid.GetCellIndex(hit.Position);
+            var delta = hitCell - originCell;
+            
+            // Calculate the maximum absolute component of delta
+            var numSamples = Math.Max(Math.Max(Math.Abs(delta.X), Math.Abs(delta.Y)), Math.Abs(delta.Z));
+            
+            if (numSamples >= (1 << 15))
+            {
+                throw new InvalidOperationException($"Number of samples {numSamples} exceeds maximum");
+            }
+
+            // 'numSamples' is the number of samples we equi-distantly place on the
+            // line between 'origin' and 'hit'. (including a fractional part for sub-
+            // voxels) It is chosen so that between two samples we change from one voxel
+            // to the next on the fastest changing dimension.
+            //
+            // Only the last 'numFreeSpaceVoxels' are updated for performance.
+            var startPosition = Math.Max(0, numSamples - numFreeSpaceVoxels);
+            for (int position = startPosition; position < numSamples; position++)
+            {
+                var missCell = originCell + new Array3i(
+                    delta.X * position / numSamples,
+                    delta.Y * position / numSamples,
+                    delta.Z * position / numSamples);
+                hybridGrid.ApplyLookupTable(missCell, missTable);
+            }
+        }
+    }
+
+    /// <summary>
+    /// Inserts intensities into the intensity hybrid grid.
+    /// </summary>
+    private static void InsertIntensitiesIntoGrid(
+        PointCloud returns,
+        IntensityHybridGrid intensityHybridGrid,
+        double intensityThreshold)
+    {
+        if (returns.Intensities.Count > 0)
+        {
+            for (int i = 0; i < returns.Count; i++)
+            {
+                if (i >= returns.Intensities.Count)
+                {
+                    break;
+                }
+                
+                if (returns.Intensities[i] > intensityThreshold)
+                {
+                    continue;
+                }
+                
+                var hitCell = intensityHybridGrid.GetCellIndex(returns.Points[i].Position);
+                intensityHybridGrid.AddIntensity(hitCell, returns.Intensities[i]);
+            }
+        }
+    }
+}