BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/3D/Submap3D.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.Models.Transform;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using  RobotNet10.Shared.Numbers;
using RotationalScanMatcher = CartographerSharp.Mapping.Internal.D3D.ScanMatching.RotationalScanMatcher;

namespace CartographerSharp.Mapping.D3D;

/// <summary>
/// 3D Submap implementation.
/// </summary>
public class Submap3D : Submap
{
    private HybridGrid _highResolutionHybridGrid;
    private HybridGrid _lowResolutionHybridGrid;
    private IntensityHybridGrid? _highResolutionIntensityHybridGrid;
    private List<double> _rotationalScanMatcherHistogram;

    public Submap3D(
        double highResolution,
        double lowResolution,
        Rigid3d localSubmapPose,
        List<double> rotationalScanMatcherHistogram)
        : base(localSubmapPose)
    {
        _highResolutionHybridGrid = new HybridGrid(highResolution);
        _lowResolutionHybridGrid = new HybridGrid(lowResolution);
        _highResolutionIntensityHybridGrid = new IntensityHybridGrid(highResolution);
        _rotationalScanMatcherHistogram = [.. rotationalScanMatcherHistogram];
    }

    public Submap3D(Models.Mapping.Submap3D proto)
        : base((Rigid3d)proto.LocalPose)
    {
        // Initialize with default values first
        _highResolutionHybridGrid = new HybridGrid(0.05f); // Default resolution
        _lowResolutionHybridGrid = new HybridGrid(0.05f);
        _rotationalScanMatcherHistogram = [];
        UpdateFromProto(proto);
    }

    /// <summary>
    /// Gets the high resolution hybrid grid.
    /// </summary>
    public HybridGrid HighResolutionHybridGrid => _highResolutionHybridGrid;

    /// <summary>
    /// Gets the low resolution hybrid grid.
    /// </summary>
    public HybridGrid LowResolutionHybridGrid => _lowResolutionHybridGrid;

    /// <summary>
    /// Gets the high resolution intensity hybrid grid.
    /// </summary>
    public IntensityHybridGrid? HighResolutionIntensityHybridGrid => _highResolutionIntensityHybridGrid;

    /// <summary>
    /// Forgets the intensity hybrid grid to reduce memory usage.
    /// </summary>
    public void ForgetIntensityHybridGrid()
    {
        _highResolutionIntensityHybridGrid = null;
    }

    /// <summary>
    /// Gets the rotational scan matcher histogram.
    /// </summary>
    public IReadOnlyList<double> RotationalScanMatcherHistogram => _rotationalScanMatcherHistogram;

    /// <summary>
    /// Insert 'range_data' into this submap using 'range_data_inserter'. The
    /// submap must not be finished yet.
    /// </summary>
    public void InsertData(
        RangeData rangeDataInLocal,
        RangeDataInserter3D rangeDataInserter,
        double highResolutionMaxRange,
        Quaternion localFromGravityAligned,
        List<double> scanHistogramInGravity)
    {
        if (InsertionFinished)
        {
            throw new InvalidOperationException("Cannot insert data into finished submap");
        }

        // Transform range data into submap frame
        var submapInverse = LocalPose.Inverse();
        var submapInverseFloat = new Rigid3f(
            (Vector3)submapInverse.Translation,
            submapInverse.Rotation);
        var transformedRangeData = RangeDataOperations.Transform(rangeDataInLocal, submapInverseFloat);

        // Filter range data by max range for high resolution grid
        var filteredRangeData = FilterRangeDataByMaxRange(transformedRangeData, highResolutionMaxRange);

        // Insert into high resolution grid with intensity
        rangeDataInserter.Insert(
            filteredRangeData,
            _highResolutionHybridGrid,
            _highResolutionIntensityHybridGrid);

        // Insert into low resolution grid without intensity
        rangeDataInserter.Insert(
            transformedRangeData,
            _lowResolutionHybridGrid,
            null);

        NumRangeData++;

        // Update rotational scan matcher histogram
        // C++: yaw_in_submap_from_gravity = GetYaw(local_pose().inverse().rotation() * local_from_gravity_aligned)
        //      rotational_scan_matcher_histogram_ += RotationalScanMatcher::RotateHistogram(scan_histogram_in_gravity, yaw_in_submap_from_gravity)
        var yawInSubmapFromGravity = TransformOperations.GetYaw(submapInverse.Rotation * localFromGravityAligned);
        
        if (_rotationalScanMatcherHistogram.Count == scanHistogramInGravity.Count)
        {
            var rotatedHistogram = RotationalScanMatcher.RotateHistogram(
                scanHistogramInGravity.ToArray(),
                yawInSubmapFromGravity);

            for (int i = 0; i < rotatedHistogram.Length; i++)
            {
                _rotationalScanMatcherHistogram[i] += rotatedHistogram[i];
            }
        }
        else if (scanHistogramInGravity.Count > 0)
        {
            // Log warning for histogram size mismatch - this can cause rotational matching failures
            System.Diagnostics.Debug.WriteLine(
                $"Warning: Histogram size mismatch in Submap3D.InsertData: " +
                $"expected {_rotationalScanMatcherHistogram.Count}, got {scanHistogramInGravity.Count}. " +
                "Rotational scan matching may not work correctly.");
        }
    }

    /// <summary>
    /// Finishes the submap.
    /// </summary>
    public void Finish()
    {
        if (InsertionFinished)
        {
            throw new InvalidOperationException("Submap is already finished");
        }
        InsertionFinished = true;
    }

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public override Models.Mapping.Submap ToProto(bool includeGridData)
    {
        Models.Mapping.HybridGrid highResGrid;
        Models.Mapping.HybridGrid lowResGrid;

        if (includeGridData)
        {
            highResGrid = _highResolutionHybridGrid.ToProto();
            lowResGrid = _lowResolutionHybridGrid.ToProto();
        }
        else
        {
            // Create empty grids with just resolution
            highResGrid = new Models.Mapping.HybridGrid
            {
                Resolution = _highResolutionHybridGrid.Resolution,
                XIndices = [],
                YIndices = [],
                ZIndices = [],
                Values = []
            };
            lowResGrid = new Models.Mapping.HybridGrid
            {
                Resolution = _lowResolutionHybridGrid.Resolution,
                XIndices = [],
                YIndices = [],
                ZIndices = [],
                Values = []
            };
        }

        var submap3D = new Models.Mapping.Submap3D(
            (Rigid3dProto)LocalPose,
            NumRangeData,
            InsertionFinished,
            highResGrid,
            lowResGrid,
            [.. _rotationalScanMatcherHistogram]);

        // Note: SubmapId will be set by caller
        return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), null, submap3D);
    }

    /// <summary>
    /// Updates from proto representation.
    /// </summary>
    public override void UpdateFromProto(Models.Mapping.Submap proto)
    {
        if (!proto.Submap3D.HasValue)
        {
            throw new ArgumentException("Proto must contain Submap3D", nameof(proto));
        }

        UpdateFromProto(proto.Submap3D.Value);
    }

    private void UpdateFromProto(Models.Mapping.Submap3D submap3D)
    {
        NumRangeData = submap3D.NumRangeData;
        InsertionFinished = submap3D.Finished;

        if (submap3D.HighResolutionHybridGrid.Values != null && submap3D.HighResolutionHybridGrid.Values.Count > 0)
        {
            _highResolutionHybridGrid = new HybridGrid(submap3D.HighResolutionHybridGrid);
        }

        if (submap3D.LowResolutionHybridGrid.Values != null && submap3D.LowResolutionHybridGrid.Values.Count > 0)
        {
            _lowResolutionHybridGrid = new HybridGrid(submap3D.LowResolutionHybridGrid);
        }

        _rotationalScanMatcherHistogram = [.. submap3D.RotationalScanMatcherHistogram ?? []];
    }

    /// <summary>
    /// Filters 'range_data', retaining only the returns that have no more than
    /// 'max_range' distance from the origin. Removes misses.
    /// </summary>
    public static RangeData FilterRangeDataByMaxRange(RangeData rangeData, double maxRange)
    {
        var filteredReturns = new List<RangefinderPoint>();
        
        foreach (var point in rangeData.Returns.Points)
        {
            var distance = Vector3.Distance(point.Position, rangeData.Origin);
            if (distance <= maxRange)
            {
                filteredReturns.Add(point);
            }
        }

        return new RangeData(
            rangeData.Origin,
            new PointCloud(filteredReturns),
            new PointCloud()); // Misses are removed
    }
}