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

/*
 * 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]);
            }
        }
    }
}