Denso/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/OccupancyGridHelper.cs

using CartographerSharp.Mapping;
using CartographerSharp.Mapping.D2D;
using CartographerSharp.Transform;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;
//using Pose = RobotNet10.Shared.Geometry.Pose;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Helper class for occupancy grid generation and manipulation.
/// Contains pure functions extracted from OccupancyGridProvider.
/// </summary>
public static class OccupancyGridHelper
{
    #region Constants

    /// <summary>
    /// Initial size for empty occupancy grid (cells)
    /// </summary>
    public const int InitialEmptyGridSize = 100;

    /// <summary>
    /// Probability threshold for free space
    /// </summary>
    public const double FreeSpaceProbabilityThreshold = 0.01;

    /// <summary>
    /// Probability threshold for occupied space
    /// </summary>
    public const double OccupiedSpaceProbabilityThreshold = 0.99;

    /// <summary>
    /// Lower bound for unknown space probability range
    /// </summary>
    public const double UnknownSpaceLowerBound = 0.4;

    /// <summary>
    /// Upper bound for unknown space probability range
    /// </summary>
    public const double UnknownSpaceUpperBound = 0.6;

    #endregion

    #region Bounds Calculation

    /// <summary>
    /// Calculate global bounds of a submap by transforming its corners to global frame
    /// </summary>
    public static (double minX, double minY, double maxX, double maxY) CalculateSubmapGlobalBounds(
        Submap2D submap2D,
        Rigid3d globalPose)
    {
        var grid = submap2D.Grid;
        if (grid == null)
        {
            return (double.MaxValue, double.MaxValue, double.MinValue, double.MinValue);
        }

        var limits = grid.Limits;

        // Use cropped limits if available (only known cells) for more accurate bounds
        grid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);

        // If cropped limits are valid (has known cells), use them; otherwise use full limits
        var hasCroppedLimits = croppedLimits.NumXCells > 0 && croppedLimits.NumYCells > 0;

        // Get bounds of cropped (known) cells in submap local frame using GetCellCenter
        var cornerIndices = new[]
        {
            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
                                                      hasCroppedLimits ? croppedOffset.Y : 0),
            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
                                                      hasCroppedLimits ? croppedOffset.Y : 0),
            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
                                                      hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1),
            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
                                                      hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1)
        };

        double croppedMinX = double.MaxValue, croppedMinY = double.MaxValue;
        double croppedMaxX = double.MinValue, croppedMaxY = double.MinValue;

        foreach (var cornerIndex in cornerIndices)
        {
            var cellCenter = limits.GetCellCenter(cornerIndex);
            croppedMinX = Math.Min(croppedMinX, cellCenter.X);
            croppedMinY = Math.Min(croppedMinY, cellCenter.Y);
            croppedMaxX = Math.Max(croppedMaxX, cellCenter.X);
            croppedMaxY = Math.Max(croppedMaxY, cellCenter.Y);
        }

        // Transform corners to global frame
        var corners = new[]
        {
            new Vector2(croppedMinX, croppedMinY),
            new Vector2(croppedMaxX, croppedMinY),
            new Vector2(croppedMaxX, croppedMaxY),
            new Vector2(croppedMinX, croppedMaxY)
        };

        double submapGlobalMinX = double.MaxValue, submapGlobalMinY = double.MaxValue;
        double submapGlobalMaxX = double.MinValue, submapGlobalMaxY = double.MinValue;

        foreach (var corner in corners)
        {
            var corner3D = new Vector3(corner.X, corner.Y, 0.0);
            var globalCorner = globalPose.TransformPoint(corner3D);

            submapGlobalMinX = Math.Min((float)submapGlobalMinX, (float)globalCorner.X);
            submapGlobalMinY = Math.Min((float)submapGlobalMinY, (float)globalCorner.Y);
            submapGlobalMaxX = Math.Max((float)submapGlobalMaxX, (float)globalCorner.X);
            submapGlobalMaxY = Math.Max((float)submapGlobalMaxY, (float)globalCorner.Y);
        }

        return (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY);
    }

    /// <summary>
    /// Get current grid bounds from origin and dimensions
    /// </summary>
    public static (double minX, double minY, double maxX, double maxY) GetGridBounds(OccupancyGrid grid)
    {
        var gridMinX = grid.Origin.Position.X;
        var gridMinY = grid.Origin.Position.Y;
        var gridMaxX = gridMinX + (grid.Width * grid.Resolution);
        var gridMaxY = gridMinY + (grid.Height * grid.Resolution);
        return (gridMinX, gridMinY, gridMaxX, gridMaxY);
    }

    /// <summary>
    /// Calculate bounds from a list of submaps with padding
    /// </summary>
    public static (double minX, double minY, double maxX, double maxY) CalculateBounds(
        List<(Submap2D Submap, Rigid3d GlobalPose)> submapList,
        double mapPadding)
    {
        double minX = double.MaxValue, minY = double.MaxValue;
        double maxX = double.MinValue, maxY = double.MinValue;

        foreach (var (submap, globalPose) in submapList)
        {
            var (submapMinX, submapMinY, submapMaxX, submapMaxY) =
                CalculateSubmapGlobalBounds(submap, globalPose);

            minX = Math.Min(minX, submapMinX);
            minY = Math.Min(minY, submapMinY);
            maxX = Math.Max(maxX, submapMaxX);
            maxY = Math.Max(maxY, submapMaxY);
        }

        return (minX - mapPadding, minY - mapPadding, maxX + mapPadding, maxY + mapPadding);
    }

    #endregion

    #region Probability Conversion

    /// <summary>
    /// Convert probability value to occupancy grid value
    /// </summary>
    public static sbyte ConvertProbabilityToOccupancyValue(double probability)
    {
        if (probability < FreeSpaceProbabilityThreshold)
        {
            return 0;
        }
        if (probability > OccupiedSpaceProbabilityThreshold)
        {
            return 100;
        }
        if (probability < UnknownSpaceLowerBound || probability > UnknownSpaceUpperBound)
        {
            var occupancyValue = (sbyte)Math.Round(probability * 100.0);
            return (sbyte)Math.Clamp(occupancyValue, (sbyte)0, (sbyte)100);
        }
        return -1; // Unknown space
    }

    #endregion

    #region Grid Creation

    /// <summary>
    /// Create an empty occupancy grid with default size
    /// </summary>
    public static OccupancyGrid CreateEmptyOccupancyGrid(double resolution)
    {
        var origin = new Pose
        {
            Position = new Vector3(0, 0, 0),
            Orientation = new Quaternion(0, 0, 0, 1)
        };

        return new OccupancyGrid(resolution, InitialEmptyGridSize, InitialEmptyGridSize, origin);
    }

    // NOTE: GenerateOccupancyGridFromSubmapList and MergeSubmapIntoOccupancyGrid
    // have been moved to OccupancyGridGenerator for unified grid generation.
    // Use OccupancyGridGenerator.GenerateFromMapBuilder instead.

    #endregion

    #region Grid Expansion

    /// <summary>
    /// Expand grid if needed to fit multiple submaps (optimized version)
    /// </summary>
    public static OccupancyGrid? ExpandGridToFitSubmaps(
        OccupancyGrid currentGrid,
        List<(Submap2D Submap, Rigid3d GlobalPose, SubmapId SubmapId)> submaps,
        double resolution,
        double mapPadding)
    {
        if (currentGrid == null || submaps == null || submaps.Count == 0)
            return null;

        // Calculate combined bounds of all submaps
        double combinedMinX = double.MaxValue, combinedMinY = double.MaxValue;
        double combinedMaxX = double.MinValue, combinedMaxY = double.MinValue;
        bool hasValidBounds = false;

        foreach (var (submap2D, globalPose, _) in submaps)
        {
            var grid = submap2D.Grid;
            if (grid == null)
                continue;

            var (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY) =
                CalculateSubmapGlobalBounds(submap2D, globalPose);

            if (submapGlobalMinX < submapGlobalMaxX && submapGlobalMinY < submapGlobalMaxY &&
                submapGlobalMinX != double.MaxValue && submapGlobalMinY != double.MaxValue &&
                submapGlobalMaxX != double.MinValue && submapGlobalMaxY != double.MinValue)
            {
                combinedMinX = Math.Min(combinedMinX, submapGlobalMinX);
                combinedMinY = Math.Min(combinedMinY, submapGlobalMinY);
                combinedMaxX = Math.Max(combinedMaxX, submapGlobalMaxX);
                combinedMaxY = Math.Max(combinedMaxY, submapGlobalMaxY);
                hasValidBounds = true;
            }
        }

        if (!hasValidBounds)
        {
            return null;
        }

        var (gridMinX, gridMinY, gridMaxX, gridMaxY) = GetGridBounds(currentGrid);

        var needsExpansion = combinedMinX < gridMinX || combinedMinY < gridMinY ||
                             combinedMaxX > gridMaxX || combinedMaxY > gridMaxY;

        if (!needsExpansion)
        {
            return null;
        }

        var newMinX = Math.Min(gridMinX, combinedMinX) - mapPadding;
        var newMinY = Math.Min(gridMinY, combinedMinY) - mapPadding;
        var newMaxX = Math.Max(gridMaxX, combinedMaxX) + mapPadding;
        var newMaxY = Math.Max(gridMaxY, combinedMaxY) + mapPadding;

        var newWidth = (int)Math.Ceiling((newMaxX - newMinX) / resolution);
        var newHeight = (int)Math.Ceiling((newMaxY - newMinY) / resolution);

        var newOrigin = new Pose
        {
            Position = new Vector3(newMinX, newMinY, 0),
            Orientation = new Quaternion(0, 0, 0, 1)
        };

        var expandedGrid = new OccupancyGrid(resolution, newWidth, newHeight, newOrigin);

        // Copy existing grid data to expanded grid
        for (int y = 0; y < currentGrid.Height; y++)
        {
            for (int x = 0; x < currentGrid.Width; x++)
            {
                var (worldX, worldY) = currentGrid.GridToWorld(x, y);
                var (newGridX, newGridY) = expandedGrid.WorldToGrid(worldX, worldY);

                if (newGridX >= 0 && newGridX < expandedGrid.Width &&
                    newGridY >= 0 && newGridY < expandedGrid.Height)
                {
                    var cellValue = currentGrid.GetCell(x, y);
                    expandedGrid.SetCell(newGridX, newGridY, cellValue);
                }
            }
        }

        return expandedGrid;
    }

    #endregion
}