Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/ActiveSubmaps2D.cs

@@ -0,0 +1,346 @@
+/*
+ * 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 System.Threading.Channels;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// The first active submap will be created on the insertion of the first range
+/// data. Except during this initialization when no or only one single submap
+/// exists, there are always two submaps into which range data is inserted: an
+/// old submap that is used for matching, and a new one, which will be used for
+/// matching next, that is being initialized.
+///
+/// Once a certain number of range data have been inserted, the new submap is
+/// considered initialized: the old submap is no longer changed, the "new" submap
+/// is now the "old" submap and is used for scan-to-map matching. Moreover, a
+/// "new" submap gets created. The "old" submap is forgotten by this object.
+///
+/// The front (old) submap is always inserted synchronously since it is used for
+/// scan matching immediately. The back (new) submap is inserted asynchronously
+/// via a background worker to reduce per-frame blocking latency.
+/// </summary>
+public class ActiveSubmaps2D(SubmapsOptions2D options) : IDisposable
+{
+    private const int kInitialSubmapSize = 100;
+
+    private readonly SubmapsOptions2D _options = options;
+    private readonly List<Submap2D> _submaps = [];
+    private readonly ValueConversionTables _conversionTables = new();
+    private IRangeDataInserter? _rangeDataInserter;
+    private readonly Lock _insertLock = new(); // Lock to prevent concurrent inserts
+
+    // Async back submap insertion infrastructure.
+    // The back submap is queued to a Channel and processed by a single long-running
+    // worker task, preserving insertion order (SingleReader). The front submap is
+    // always inserted synchronously for immediate scan matching availability.
+    private readonly Channel<(RangeData rangeData, Submap2D submap, IRangeDataInserter inserter)>
+        _backSubmapChannel = Channel.CreateUnbounded<(RangeData, Submap2D, IRangeDataInserter)>(
+            new UnboundedChannelOptions { SingleReader = true });
+    private Task? _backSubmapWorker;
+    private int _backSubmapExpectedCount;  // Tracks intended NumRangeData of back submap (including queued)
+    private int _backSubmapPendingCount;   // Items queued but not yet processed (Interlocked)
+
+    /// <summary>
+    /// Inserts 'range_data' into the Submap collection.
+    /// Front submap: synchronous (used for scan matching immediately).
+    /// Back submap: queued to background worker (fire-and-forget).
+    /// </summary>
+    public List<Submap2D> InsertRangeData(RangeData rangeData)
+    {
+        lock (_insertLock)
+        {
+            var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
+            var submapPose = new Rigid3d(
+                new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
+                Quaternion.Identity);
+
+            // Use _backSubmapExpectedCount instead of back submap's NumRangeData
+            // because the back submap's actual count may lag (async insertions).
+            if (_submaps.Count == 0 ||
+                (_submaps.Count > 0 && _backSubmapExpectedCount == _options.NumRangeData))
+            {
+                // Drain all pending back submap insertions before submap rotation
+                // so the back submap is fully up-to-date when it becomes the front.
+                DrainBackSubmapQueue();
+                AddSubmap(submapPose);
+                _backSubmapExpectedCount = 0;
+            }
+
+            _rangeDataInserter ??= CreateRangeDataInserter();
+
+            if (_submaps.Count >= 2)
+            {
+                // Front submap: SYNCHRONOUS (used for scan matching immediately)
+                _submaps[0].InsertRangeData(rangeData, _rangeDataInserter);
+
+                // Back submap: ASYNC (queue to background worker)
+                // Increment pending BEFORE writing to channel to ensure drain correctness.
+                Interlocked.Increment(ref _backSubmapPendingCount);
+                _backSubmapChannel.Writer.TryWrite((rangeData, _submaps[1], _rangeDataInserter));
+                _backSubmapWorker ??= Task.Factory.StartNew(
+                    ProcessBackSubmapQueue, TaskCreationOptions.LongRunning);
+            }
+            else
+            {
+                // Only 1 submap - insert synchronously
+                for (int si = 0; si < _submaps.Count; si++)
+                {
+                    _submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
+                }
+            }
+            _backSubmapExpectedCount++;
+
+            // Finish front submap when it reaches 2x threshold.
+            // Front is always up-to-date (synchronous insert).
+            if (_submaps.Count > 0 && _submaps[0].NumRangeData == _options.NumRangeData * 2)
+            {
+                _submaps[0].Finish();
+            }
+
+            return [.. _submaps];
+        }
+    }
+
+    /// <summary>
+    /// Forces the current front submap to finish, creates a new submap at the range data origin,
+    /// and inserts range data into all active submaps. Used to break the deadlock when the robot
+    /// enters genuinely new territory and consecutive hard-limit Ceres failures accumulate.
+    /// </summary>
+    public List<Submap2D> ForceNewSubmapAndInsert(RangeData rangeData)
+    {
+        lock (_insertLock)
+        {
+            // Drain any pending back submap insertions before manipulating submaps.
+            DrainBackSubmapQueue();
+
+            var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
+            var submapPose = new Rigid3d(
+                new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
+                Quaternion.Identity);
+
+            if (_submaps.Count == 0)
+            {
+                // No submaps yet - just create the first one via normal flow
+                AddSubmap(submapPose);
+            }
+            else if (_submaps.Count == 1)
+            {
+                // FIX: When only 1 submap exists, finish it and REMOVE it before creating the new one.
+                // Previously, finishing + AddSubmap would leave [finished, new] and the subsequent
+                // InsertRangeData loop would crash on the finished front submap.
+                if (!_submaps[0].InsertionFinished)
+                {
+                    _submaps[0].Finish();
+                }
+                _submaps.RemoveAt(0);
+                AddSubmap(submapPose);
+            }
+            else
+            {
+                // 2 submaps: finish front if needed, then AddSubmap removes it and creates new
+                if (!_submaps[0].InsertionFinished)
+                {
+                    _submaps[0].Finish();
+                }
+                AddSubmap(submapPose);
+            }
+
+            // Reset expected count after submap manipulation.
+            _backSubmapExpectedCount = 0;
+
+            // Insert range data into all active submaps sequentially.
+            // ForceNewSubmap is a rare recovery path (consecutive Ceres failures),
+            // so sequential insert is simpler and avoids thread-safety risks.
+            _rangeDataInserter ??= CreateRangeDataInserter();
+
+            for (int si = 0; si < _submaps.Count; si++)
+            {
+                _submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
+            }
+            _backSubmapExpectedCount++;
+
+            return [.. _submaps];
+        }
+    }
+
+    /// <summary>
+    /// Gets the current active submaps.
+    /// </summary>
+    public List<Submap2D> Submaps()
+    {
+        return [.. _submaps];
+    }
+
+    /// <summary>
+    /// Background worker that sequentially processes queued back submap insertions.
+    /// Uses per-item error handling for resilience - a single failed insertion
+    /// should not crash the entire SLAM pipeline.
+    /// </summary>
+    private async Task ProcessBackSubmapQueue()
+    {
+        await foreach (var (rangeData, submap, inserter) in _backSubmapChannel.Reader.ReadAllAsync())
+        {
+            try
+            {
+                submap.InsertRangeData(rangeData, inserter);
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine($"[SUBMAP_ASYNC] Back submap insert error: {ex.Message}");
+            }
+            Interlocked.Decrement(ref _backSubmapPendingCount);
+        }
+    }
+
+    /// <summary>
+    /// Waits for all pending back submap insertions to complete.
+    /// Called before submap rotation (AddSubmap) to ensure the back submap is
+    /// fully up-to-date before it becomes the front submap used for scan matching.
+    /// Called infrequently (every NumRangeData frames, typically ~90).
+    /// </summary>
+    private void DrainBackSubmapQueue()
+    {
+        if (Volatile.Read(ref _backSubmapPendingCount) == 0) return;
+        var sw = new SpinWait();
+        while (Volatile.Read(ref _backSubmapPendingCount) > 0)
+        {
+            sw.SpinOnce();
+        }
+    }
+
+    public void Dispose()
+    {
+        _backSubmapChannel.Writer.Complete();
+        _backSubmapWorker?.GetAwaiter().GetResult();
+    }
+
+    private IRangeDataInserter CreateRangeDataInserter()
+    {
+        // Match C++ logic: switch case with LOG(FATAL) for unknown types
+        var options = _options.RangeDataInserterOptions;
+
+        switch (options.RangeDataInserterTypeValue)
+        {
+            case RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D:
+                if (options.ProbabilityGridRangeDataInserterOptions2D.HasValue)
+                {
+                    return new ProbabilityGridRangeDataInserter2D(options.ProbabilityGridRangeDataInserterOptions2D.Value);
+                }
+                throw new ArgumentException("ProbabilityGridRangeDataInserterOptions2D is required for ProbabilityGrid inserter");
+
+            case RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D:
+                if (options.TsdfRangeDataInserterOptions2D.HasValue)
+                {
+                    return new TSDFRangeDataInserter2D(options.TsdfRangeDataInserterOptions2D.Value);
+                }
+                throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF inserter");
+
+            default:
+                throw new ArgumentException($"Unknown RangeDataInserterType: {options.RangeDataInserterTypeValue}");
+        }
+    }
+
+    private Grid2D CreateGrid(Vector2 origin)
+    {
+        return CreateGridWithOptions(origin, _options.GridOptions2D);
+    }
+
+    private Grid2D? CreateHighResGrid(Vector2 origin)
+    {
+        if (_options.HighResGridOptions2D == null)
+            return null;
+        return CreateGridWithOptions(origin, _options.HighResGridOptions2D.Value);
+    }
+
+    private Grid2D CreateGridWithOptions(Vector2 origin, GridOptions2D gridOptions)
+    {
+        var resolution = gridOptions.Resolution;
+        // Calculate initial size to cover ±18m (matching MaxRange) at this resolution
+        var initialSize = Math.Max(100, (int)(kInitialSubmapSize * _options.GridOptions2D.Resolution / resolution));
+        var mapLimits = new MapLimits(
+            resolution,
+            new Vector2(
+                origin.X + 0.5 * initialSize * resolution,
+                origin.Y + 0.5 * initialSize * resolution
+            ),
+            new CellLimits(initialSize, initialSize)
+        );
+
+        // Match C++ logic: switch case with LOG(FATAL) for unknown types
+        switch (gridOptions.GridTypeValue)
+        {
+            case GridOptions2D.GridType.ProbabilityGrid:
+                return new ProbabilityGrid(mapLimits, _conversionTables);
+
+            case GridOptions2D.GridType.Tsdf:
+                // Match C++: Get truncation_distance and maximum_weight from range_data_inserter_options
+                // C++: options_.range_data_inserter_options().tsdf_range_data_inserter_options_2d()
+                if (_options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.HasValue)
+                {
+                    var tsdfOptions = _options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.Value;
+                    return new TSDF2D(
+                        mapLimits,
+                        tsdfOptions.TruncationDistance,
+                        tsdfOptions.MaximumWeight,
+                        _conversionTables
+                    );
+                }
+                throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF grid type");
+
+            case GridOptions2D.GridType.InvalidGrid:
+                throw new ArgumentException("Invalid grid type specified");
+
+            default:
+                throw new ArgumentException($"Unknown grid type: {gridOptions.GridTypeValue}");
+        }
+    }
+
+    private void AddSubmap(Rigid3d localSubmapPose)
+    {
+        // Match C++ logic: if (submaps_.size() >= 2) { CHECK(submaps_.front()->insertion_finished()); submaps_.erase(submaps_.begin()); }
+        if (_submaps.Count >= 2)
+        {
+            // This will crop the finished Submap before inserting a new Submap to
+            // reduce peak memory usage a bit.
+            if (!_submaps[0].InsertionFinished)
+            {
+                throw new InvalidOperationException("First submap must be finished before adding a new one");
+            }
+            _submaps.RemoveAt(0);
+        }
+
+        // Match C++: Extract origin from pose (C++ passes Vector2f directly, but we have Rigid3d)
+        var origin = new Vector2(localSubmapPose.Translation.X, localSubmapPose.Translation.Y);
+        var grid = CreateGrid(origin);
+        var highResGrid = CreateHighResGrid(origin);
+
+        // Match C++: Submap2D(origin, grid, conversion_tables)
+        // C++ constructor takes Vector2f origin, but C# Submap2D takes Rigid3d (which includes origin)
+        var submap = new Submap2D(
+            localSubmapPose, // Pass the full Rigid3d pose including rotation
+            grid,
+            _conversionTables,
+            highResGrid
+        );
+        _submaps.Add(submap);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/CellLimits.cs

@@ -0,0 +1,44 @@
+/*
+ * 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.
+ */
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Cell limits for 2D grids.
+/// </summary>
+public struct CellLimits(int numXCells, int numYCells)
+{
+    public int NumXCells { get; set; } = numXCells;
+    public int NumYCells { get; set; } = numYCells;
+
+    /// <summary>
+    /// Creates from proto representation.
+    /// Match C++: explicit CellLimits(const proto::CellLimits& cell_limits)
+    /// </summary>
+    public CellLimits(Models.Mapping.CellLimits proto)
+        : this(proto.NumXCells, proto.NumYCells)
+    {
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// Match C++: inline proto::CellLimits ToProto(const CellLimits& cell_limits)
+    /// </summary>
+    public readonly Models.Mapping.CellLimits ToProto()
+    {
+        return new Models.Mapping.CellLimits(NumXCells, NumYCells);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/Grid2D.cs

@@ -0,0 +1,550 @@
+/*
+ * Copyright 2018 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 System;
+using System.Runtime.InteropServices;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Grid type enumeration.
+/// </summary>
+public enum GridType
+{
+    ProbabilityGrid,
+    TSDF
+}
+
+/// <summary>
+/// Base class for 2D grids.
+/// </summary>
+public abstract class Grid2D : IGrid
+{
+    protected MapLimits _limits;
+    protected List<ushort> _correspondenceCostCells;
+    protected double _minCorrespondenceCost;
+    protected double _maxCorrespondenceCost;
+    protected List<int> _updateIndices;
+    protected (int minX, int minY, int maxX, int maxY) _knownCellsBox;
+    protected double[] _valueToCorrespondenceCostTable;
+
+    private const ushort kUnknownCorrespondenceValue = 0;
+
+    protected Grid2D(
+        MapLimits limits,
+        double minCorrespondenceCost,
+        double maxCorrespondenceCost,
+        ValueConversionTables conversionTables)
+    {
+        if (minCorrespondenceCost >= maxCorrespondenceCost)
+        {
+            throw new ArgumentException("min_correspondence_cost must be less than max_correspondence_cost");
+        }
+
+        _limits = limits;
+        _minCorrespondenceCost = minCorrespondenceCost;
+        _maxCorrespondenceCost = maxCorrespondenceCost;
+        _correspondenceCostCells = new List<ushort>(
+            limits.CellLimits.NumXCells * limits.CellLimits.NumYCells);
+        for (int i = 0; i < _correspondenceCostCells.Capacity; i++)
+        {
+            _correspondenceCostCells.Add(kUnknownCorrespondenceValue);
+        }
+        _updateIndices = [];
+        // Match C++: AlignedBox2i is empty by default (min > max)
+        // Use sentinel values to mark empty: minX > maxX indicates empty box
+        _knownCellsBox = (int.MaxValue, int.MaxValue, int.MinValue, int.MinValue);
+        _valueToCorrespondenceCostTable = conversionTables.GetConversionTable(
+            maxCorrespondenceCost, minCorrespondenceCost, maxCorrespondenceCost);
+    }
+
+    protected Grid2D(
+        Models.Mapping.Grid2D proto,
+        ValueConversionTables conversionTables)
+    {
+        _limits = new MapLimits(proto.Limits);
+        // Match C++: MinCorrespondenceCostFromProto/MaxCorrespondenceCostFromProto - older proto had 0,0
+        if (proto.MinCorrespondenceCost == 0 && proto.MaxCorrespondenceCost == 0)
+        {
+            _minCorrespondenceCost = 0.1;
+            _maxCorrespondenceCost = 0.9;
+        }
+        else
+        {
+            _minCorrespondenceCost = proto.MinCorrespondenceCost;
+            _maxCorrespondenceCost = proto.MaxCorrespondenceCost;
+        }
+        if (_minCorrespondenceCost >= _maxCorrespondenceCost)
+        {
+            throw new ArgumentException("min_correspondence_cost must be less than max_correspondence_cost");
+        }
+        // Match C++: Copy cells from proto
+        _correspondenceCostCells = new List<ushort>(proto.Cells?.Count ?? 0);
+        if (proto.Cells != null)
+        {
+            foreach (var cell in proto.Cells)
+            {
+                _correspondenceCostCells.Add((ushort)cell);
+            }
+        }
+        _updateIndices = [];
+        // Match C++: Copy known_cells_box from proto if present
+        var knownCellsBox = proto.KnownCellsBox;
+        _knownCellsBox = (knownCellsBox.MinX, knownCellsBox.MinY, knownCellsBox.MaxX, knownCellsBox.MaxY);
+        _valueToCorrespondenceCostTable = conversionTables.GetConversionTable(
+            _maxCorrespondenceCost, _minCorrespondenceCost, _maxCorrespondenceCost);
+    }
+
+    /// <summary>
+    /// Returns the limits of this Grid2D.
+    /// </summary>
+    public MapLimits Limits => _limits;
+
+    /// <summary>
+    /// Returns the correspondence cost cells.
+    /// </summary>
+    protected List<ushort> CorrespondenceCostCells => _correspondenceCostCells;
+
+    /// <summary>
+    /// Returns the update indices.
+    /// </summary>
+    protected List<int> UpdateIndices => _updateIndices;
+
+    /// <summary>
+    /// Returns the known cells box.
+    /// </summary>
+    protected (int minX, int minY, int maxX, int maxY) KnownCellsBox => _knownCellsBox;
+
+    /// <summary>
+    /// Returns mutable correspondence cost cells.
+    /// </summary>
+    protected List<ushort> MutableCorrespondenceCostCells => _correspondenceCostCells;
+
+    /// <summary>
+    /// Returns mutable update indices.
+    /// </summary>
+    protected List<int> MutableUpdateIndices => _updateIndices;
+
+    /// <summary>
+    /// Returns mutable known cells box.
+    /// </summary>
+    protected ref (int minX, int minY, int maxX, int maxY) MutableKnownCellsBox => ref _knownCellsBox;
+
+    /// <summary>
+    /// Returns the minimum possible correspondence cost.
+    /// </summary>
+    public double MinCorrespondenceCost => _minCorrespondenceCost;
+
+    /// <summary>
+    /// Returns the maximum possible correspondence cost.
+    /// </summary>
+    public double MaxCorrespondenceCost => _maxCorrespondenceCost;
+
+    /// <summary>
+    /// Gets the grid type.
+    /// </summary>
+    public abstract GridType GetGridType();
+
+    /// <summary>
+    /// Returns the correspondence cost of the cell with 'cell_index'.
+    /// </summary>
+    public double GetCorrespondenceCost(Array2i cellIndex)
+    {
+        if (!_limits.Contains(cellIndex))
+        {
+            return _maxCorrespondenceCost;
+        }
+        var flatIndex = ToFlatIndex(cellIndex);
+        if (flatIndex >= _correspondenceCostCells.Count)
+        {
+            return _maxCorrespondenceCost;
+        }
+        var value = _correspondenceCostCells[flatIndex];
+        if (value >= _valueToCorrespondenceCostTable.Length)
+        {
+            return _maxCorrespondenceCost;
+        }
+        return _valueToCorrespondenceCostTable[value];
+    }
+
+    /// <summary>
+    /// Copies all correspondence cost values into the destination array using bulk access.
+    /// Much faster than calling GetCorrespondenceCost per cell (avoids per-cell bounds checks,
+    /// struct allocation, and virtual dispatch). Data is in row-major order matching the internal layout.
+    /// </summary>
+    /// <param name="destination">Pre-allocated array of size >= NumXCells * NumYCells.</param>
+    public void CopyCorrespondenceCostData(double[] destination)
+    {
+        var numXCells = _limits.CellLimits.NumXCells;
+        var numYCells = _limits.CellLimits.NumYCells;
+        var totalCells = numXCells * numYCells;
+
+        if (destination.Length < totalCells)
+            throw new ArgumentException($"Destination array too small: {destination.Length} < {totalCells}", nameof(destination));
+
+        var cells = CollectionsMarshal.AsSpan(_correspondenceCostCells);
+        var table = _valueToCorrespondenceCostTable;
+        var maxCost = _maxCorrespondenceCost;
+        var tableLen = table.Length;
+        var count = Math.Min(totalCells, cells.Length);
+
+        for (int i = 0; i < count; i++)
+        {
+            var value = cells[i];
+            destination[i] = value < tableLen ? table[value] : maxCost;
+        }
+
+        // Fill remaining if cells is shorter than expected (shouldn't happen normally)
+        for (int i = count; i < totalCells; i++)
+        {
+            destination[i] = maxCost;
+        }
+    }
+
+    /// <summary>
+    /// Returns true if the probability at the specified index is known.
+    /// </summary>
+    public bool IsKnown(Array2i cellIndex)
+    {
+        if (!_limits.Contains(cellIndex))
+        {
+            return false;
+        }
+        
+        var flatIndex = ToFlatIndex(cellIndex);
+        var cellValue = _correspondenceCostCells[flatIndex];
+        const ushort kUpdateMarker = (ushort)(1u << 15);
+        var hasUpdateMarker = cellValue >= kUpdateMarker;
+        var valueWithoutMarker = hasUpdateMarker ? (ushort)(cellValue - kUpdateMarker) : cellValue;
+        var isKnown = valueWithoutMarker != kUnknownCorrespondenceValue;
+        
+        return isKnown;
+    }
+
+    /// <summary>
+    /// Finishes the update sequence.
+    /// Match C++: DCHECK_GE(correspondence_cost_cells_[update_indices_.back()], kUpdateMarker)
+    /// Optimized: uses Span for batch processing instead of per-element RemoveAt.
+    /// </summary>
+    public void FinishUpdate()
+    {
+        const ushort kUpdateMarker = (ushort)(1u << 15);
+        var indices = CollectionsMarshal.AsSpan(_updateIndices);
+        var cells = CollectionsMarshal.AsSpan(_correspondenceCostCells);
+        for (int i = indices.Length - 1; i >= 0; i--)
+        {
+            var idx = indices[i];
+#if DEBUG
+            if (cells[idx] < kUpdateMarker)
+            {
+                throw new InvalidOperationException($"Cell at index {idx} does not have update marker. Value: {cells[idx]}, Expected: >= {kUpdateMarker}");
+            }
+#endif
+            cells[idx] -= kUpdateMarker;
+        }
+        _updateIndices.Clear();
+    }
+
+    /// <summary>
+    /// Fills in 'offset' and 'limits' to define a subregion that contains all known cells.
+    /// Match C++: if (known_cells_box_.isEmpty()) - check by min > max
+    /// </summary>
+    public void ComputeCroppedLimits(out Array2i offset, out CellLimits limits)
+    {
+        // Match C++: AlignedBox2i::isEmpty() returns true if min > max
+        // Empty box is marked by minX > maxX (or minY > maxY)
+        if (_knownCellsBox.minX > _knownCellsBox.maxX || _knownCellsBox.minY > _knownCellsBox.maxY)
+        {
+            offset = Array2i.Zero;
+            limits = new CellLimits(1, 1);
+            return;
+        }
+
+        offset = new Array2i(_knownCellsBox.minX, _knownCellsBox.minY);
+        limits = new CellLimits(
+            _knownCellsBox.maxX - _knownCellsBox.minX + 1,
+            _knownCellsBox.maxY - _knownCellsBox.minY + 1);
+    }
+
+    /// <summary>
+    /// Grows the map as necessary to include 'point'. This changes the meaning of
+    /// these coordinates going forward. This method must be called immediately
+    /// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
+    /// </summary>
+    public virtual void GrowLimits(Vector2 point)
+    {
+        GrowLimits(point, [_correspondenceCostCells], [kUnknownCorrespondenceValue]);
+    }
+
+    /// <summary>
+    /// Grows limits for multiple grids.
+    /// </summary>
+    protected void GrowLimits(Vector2 point, List<ushort>[] grids, ushort[] gridsUnknownCellValues)
+    {
+        if (_updateIndices.Count > 0)
+        {
+            throw new InvalidOperationException("GrowLimits must be called after FinishUpdate");
+        }
+
+        // Log before resize if point is outside current limits
+        var needsGrow = !_limits.Contains(_limits.GetCellIndex(point));
+        
+        while (!_limits.Contains(_limits.GetCellIndex(point)))
+        {
+            var xOffset = _limits.CellLimits.NumXCells / 2;
+            var yOffset = _limits.CellLimits.NumYCells / 2;
+            
+            // CRITICAL FIX: Check for integer overflow before multiplying by 2
+            // Use long to prevent overflow during calculation
+            long newNumXCellsLong = 2L * _limits.CellLimits.NumXCells;
+            long newNumYCellsLong = 2L * _limits.CellLimits.NumYCells;
+            
+            // Check if result exceeds int.MaxValue
+            if (newNumXCellsLong > int.MaxValue || newNumYCellsLong > int.MaxValue)
+            {
+                throw new InvalidOperationException(
+                    $"Cannot grow grid: new size would exceed int.MaxValue. " +
+                    $"Current: NumXCells={_limits.CellLimits.NumXCells}, NumYCells={_limits.CellLimits.NumYCells}. " +
+                    $"New would be: NumXCells={newNumXCellsLong}, NumYCells={newNumYCellsLong}");
+            }
+            
+            // Match C++: limits_.max() + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset)
+            // C++ uses Eigen::Vector2d(y_offset, x_offset), which means:
+            // newMax.x() = oldMax.x() + resolution * y_offset
+            // newMax.y() = oldMax.y() + resolution * x_offset
+            var newMax = new Vector2(
+                (_limits.Max.X + _limits.Resolution * yOffset),
+                (_limits.Max.Y + _limits.Resolution * xOffset));
+            var newCellLimits = new CellLimits(
+                (int)newNumXCellsLong,
+                (int)newNumYCellsLong);
+            var newLimits = new MapLimits(_limits.Resolution, newMax, newCellLimits);
+
+            var stride = newLimits.CellLimits.NumXCells;
+            var offset = xOffset + stride * yOffset;
+            
+            // CRITICAL FIX: Use long for newSize calculation to prevent overflow
+            long newSizeLong = (long)newLimits.CellLimits.NumXCells * newLimits.CellLimits.NumYCells;
+            if (newSizeLong <= 0 || newSizeLong > int.MaxValue)
+            {
+                throw new InvalidOperationException(
+                    $"New size is invalid: {newSizeLong}. " +
+                    $"NumXCells={newLimits.CellLimits.NumXCells}, NumYCells={newLimits.CellLimits.NumYCells}");
+            }
+            var newSize = (int)newSizeLong;
+
+            for (int gridIndex = 0; gridIndex < grids.Length; gridIndex++)
+            {
+                var oldGrid = grids[gridIndex];
+                if (oldGrid == null || oldGrid.Count == 0)
+                {
+                    throw new InvalidOperationException($"Grid at index {gridIndex} is null or empty");
+                }
+
+                // Allocate and initialize new cells without per-element Add() loop.
+                // CollectionsMarshal.SetCount sets the internal _size field directly,
+                // avoiding repeated bounds checks; the backing array is zero-initialised
+                // by the runtime, so an explicit fill is only needed for non-zero values.
+                var newCells = new List<ushort>(newSize);
+                CollectionsMarshal.SetCount(newCells, newSize);
+                var newSpan = CollectionsMarshal.AsSpan(newCells);
+                var unknownValue = gridsUnknownCellValues[gridIndex];
+                if (unknownValue != 0)
+                    newSpan.Fill(unknownValue);
+
+                // Copy old rows into the correct offset in the new (larger) grid.
+                // Using Span.CopyTo() per row lets the JIT emit a single memcpy/memmove
+                // for each row instead of indexing element-by-element.
+                var oldSpan = CollectionsMarshal.AsSpan(oldGrid);
+                int numXCells = _limits.CellLimits.NumXCells;
+                int numYCells = _limits.CellLimits.NumYCells;
+                for (int i = 0; i < numYCells; i++)
+                {
+                    var srcRow = oldSpan.Slice(i * numXCells, numXCells);
+                    var dstRow = newSpan.Slice(offset + i * stride, numXCells);
+                    srcRow.CopyTo(dstRow);
+                }
+
+                // THREAD-SAFETY FIX: Instead of Clear()+AddRange() which creates a
+                // window where Count==0 (race with ConstraintBuilder2D ThreadPool readers),
+                // assign the completed newCells to the array slot. The field references
+                // are updated atomically via UpdateGridReferences below.
+                grids[gridIndex] = newCells;
+            }
+
+            // Atomic field swap: update _correspondenceCostCells (and _weightCells for TSDF2D)
+            // BEFORE updating _limits, so concurrent readers with old limits compute small
+            // indices into the (larger) new cells list — always safe.
+            UpdateGridReferences(grids);
+            _limits = newLimits;
+
+            // Match C++: if (!known_cells_box_.isEmpty()) { known_cells_box_.translate(...); }
+            // Update known cells box offset only if box is not empty
+            if (_knownCellsBox.minX <= _knownCellsBox.maxX && _knownCellsBox.minY <= _knownCellsBox.maxY)
+            {
+                _knownCellsBox = (
+                    _knownCellsBox.minX + xOffset,
+                    _knownCellsBox.minY + yOffset,
+                    _knownCellsBox.maxX + xOffset,
+                    _knownCellsBox.maxY + yOffset
+                );
+            }
+        }
+    }
+
+    /// <summary>
+    /// Updates field references after GrowLimits rebuilds the cells lists.
+    /// Called with the new lists before _limits is updated, ensuring concurrent
+    /// readers always see a valid (complete) cells list.
+    /// Override in derived classes that hold additional grid lists (e.g., TSDF2D._weightCells).
+    /// </summary>
+    protected virtual void UpdateGridReferences(List<ushort>[] grids)
+    {
+        _correspondenceCostCells = grids[0];
+    }
+
+    /// <summary>
+    /// Converts a 'cell_index' into an index into 'cells_'.
+    /// </summary>
+    protected int ToFlatIndex(Array2i cellIndex)
+    {
+        if (!_limits.Contains(cellIndex))
+        {
+            throw new ArgumentOutOfRangeException(nameof(cellIndex), "Cell index out of bounds");
+        }
+        return _limits.CellLimits.NumXCells * cellIndex.Y + cellIndex.X;
+    }
+
+    /// <summary>
+    /// Lightweight, read-only snapshot of grid cell data used for thread-safe
+    /// occupancy grid generation. Only the data needed for merging is captured;
+    /// the underlying Grid2D remains unaffected.
+    /// </summary>
+    public sealed class GridCellSnapshot
+    {
+        public readonly ushort[] Cells;
+        public readonly MapLimits Limits;
+        public readonly (int minX, int minY, int maxX, int maxY) KnownCellsBox;
+        public readonly double[] ValueToCorrespondenceCostTable;
+        public readonly double MinCorrespondenceCost;
+        public readonly double MaxCorrespondenceCost;
+
+        internal GridCellSnapshot(
+            ushort[] cells,
+            MapLimits limits,
+            (int minX, int minY, int maxX, int maxY) knownCellsBox,
+            double[] valueToCorrespondenceCostTable,
+            double minCorrespondenceCost,
+            double maxCorrespondenceCost)
+        {
+            Cells = cells;
+            Limits = limits;
+            KnownCellsBox = knownCellsBox;
+            ValueToCorrespondenceCostTable = valueToCorrespondenceCostTable;
+            MinCorrespondenceCost = minCorrespondenceCost;
+            MaxCorrespondenceCost = maxCorrespondenceCost;
+        }
+
+        public bool IsKnown(Array2i cellIndex)
+        {
+            if (!Limits.Contains(cellIndex)) return false;
+            var flatIndex = Limits.CellLimits.NumXCells * cellIndex.Y + cellIndex.X;
+            if (flatIndex < 0 || flatIndex >= Cells.Length) return false;
+            const ushort kUpdateMarker = (ushort)(1u << 15);
+            var value = Cells[flatIndex];
+            var raw = value >= kUpdateMarker ? (ushort)(value - kUpdateMarker) : value;
+            return raw != 0;
+        }
+
+        public void ComputeCroppedLimits(out Array2i offset, out CellLimits limits)
+        {
+            if (KnownCellsBox.minX > KnownCellsBox.maxX || KnownCellsBox.minY > KnownCellsBox.maxY)
+            {
+                offset = Array2i.Zero;
+                limits = new CellLimits(1, 1);
+                return;
+            }
+            offset = new Array2i(KnownCellsBox.minX, KnownCellsBox.minY);
+            limits = new CellLimits(
+                KnownCellsBox.maxX - KnownCellsBox.minX + 1,
+                KnownCellsBox.maxY - KnownCellsBox.minY + 1);
+        }
+    }
+
+    /// <summary>
+    /// Creates a snapshot of the current cell data for thread-safe reading.
+    /// The returned snapshot is decoupled from the live grid and will not be
+    /// affected by concurrent InsertRangeData / GrowLimits calls.
+    /// Intended for occupancy grid generation on active (non-finished) submaps.
+    /// </summary>
+    public GridCellSnapshot SnapshotCellData()
+    {
+        // Capture references / value copies before allocating the array.
+        var limits = _limits;
+        var knownCellsBox = _knownCellsBox;
+        var cells = CollectionsMarshal.AsSpan(_correspondenceCostCells);
+        var copy = new ushort[cells.Length];
+        cells.CopyTo(copy);
+        return new GridCellSnapshot(
+            copy, limits, knownCellsBox,
+            _valueToCorrespondenceCostTable,
+            _minCorrespondenceCost, _maxCorrespondenceCost);
+    }
+
+    /// <summary>
+    /// Computes a cropped grid containing only known cells.
+    /// </summary>
+    public abstract Grid2D ComputeCroppedGrid();
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// Match C++: CHECK(update_indices().empty()) before serializing
+    /// </summary>
+    public virtual Models.Mapping.Grid2D ToProto()
+    {
+        // Match C++: CHECK(update_indices().empty()) << "Serializing a grid during an update is not supported. Finish the update first.";
+        if (_updateIndices.Count > 0)
+        {
+            throw new InvalidOperationException("Serializing a grid during an update is not supported. Finish the update first.");
+        }
+
+        var cells = new List<int>(_correspondenceCostCells.Count);
+        foreach (var cell in _correspondenceCostCells)
+        {
+            cells.Add(cell);
+        }
+
+        var proto = new Models.Mapping.Grid2D
+        {
+            Limits = _limits.ToProto(),
+            Cells = cells,
+            MinCorrespondenceCost = _minCorrespondenceCost,
+            MaxCorrespondenceCost = _maxCorrespondenceCost
+        };
+
+        // Match C++: if (!known_cells_box().isEmpty()) { set known_cells_box }
+        if (_knownCellsBox.minX <= _knownCellsBox.maxX && _knownCellsBox.minY <= _knownCellsBox.maxY)
+        {
+            proto.KnownCellsBox = new Models.Mapping.Grid2D.CellBox(
+                _knownCellsBox.maxX, _knownCellsBox.maxY,
+                _knownCellsBox.minX, _knownCellsBox.minY);
+        }
+
+        return proto;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/MapLimits.cs

@@ -0,0 +1,132 @@
+/*
+ * 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.Models.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Defines the limits of a grid map.
+/// </summary>
+public class MapLimits
+{
+    private readonly double _resolution;
+    private readonly Vector2 _max;
+    private readonly CellLimits _cellLimits;
+
+    public MapLimits(double resolution, Vector2 max, CellLimits cellLimits)
+    {
+        if (resolution <= 0.0)
+        {
+            throw new ArgumentException("Resolution must be positive", nameof(resolution));
+        }
+        if (cellLimits.NumXCells <= 0 || cellLimits.NumYCells <= 0)
+        {
+            throw new ArgumentException("Cell limits must be positive", nameof(cellLimits));
+        }
+
+        _resolution = resolution;
+        _max = max;
+        _cellLimits = cellLimits;
+    }
+
+    /// <summary>
+    /// Creates from proto representation.
+    /// Match C++: explicit MapLimits(const proto::MapLimits& map_limits)
+    /// </summary>
+    public MapLimits(Models.Mapping.MapLimits proto)
+    {
+        _resolution = proto.Resolution;
+        _max = new Vector2(proto.Max.X, proto.Max.Y);
+        // Match C++: cell_limits_(map_limits.cell_limits())
+        _cellLimits = new CellLimits(proto.CellLimits);
+    }
+
+    /// <summary>
+    /// Returns the cell size in meters. All cells are square and the resolution is
+    /// the length of one side.
+    /// </summary>
+    public double Resolution => _resolution;
+
+    /// <summary>
+    /// Returns the corner of the limits, i.e., all pixels have positions with
+    /// smaller coordinates.
+    /// </summary>
+    public Vector2 Max => _max;
+
+    /// <summary>
+    /// Returns the limits of the grid in number of cells.
+    /// </summary>
+    public CellLimits CellLimits => _cellLimits;
+
+    /// <summary>
+    /// Returns the index of the cell containing the 'point' which may be outside
+    /// the map, i.e., negative or too large indices that will return false for
+    /// Contains().
+    /// </summary>
+    public Array2i GetCellIndex(Vector2 point)
+    {
+        // Index values are row major and the top left has (0, 0)
+        // and contains (centered_max_x, centered_max_y). We need to flip and rotate.
+        // Match C++: common::RoundToInt(x) = std::lround(x) - rounds to nearest long, then casts to int
+        // std::lround rounds to nearest integer using current rounding mode (default: round half away from zero)
+        // In C#, Math.Round with MidpointRounding.AwayFromZero matches std::lround behavior
+        // CRITICAL: Use std::lround equivalent - Math.Round with MidpointRounding.AwayFromZero
+        // But std::lround actually uses "round half to even" (banker's rounding) by default in C++11+
+        // However, C++ code uses common::RoundToInt which is std::lround, and std::lround uses current rounding mode
+        // In practice, std::lround with default rounding mode rounds half away from zero
+        // So Math.Round with MidpointRounding.AwayFromZero should match
+        var x = (int)Math.Round((_max.Y - point.Y) / _resolution - 0.5, MidpointRounding.AwayFromZero);
+        var y = (int)Math.Round((_max.X - point.X) / _resolution - 0.5, MidpointRounding.AwayFromZero);
+        return new Array2i(x, y);
+    }
+
+    /// <summary>
+    /// Returns the center of the cell at 'cell_index'.
+    /// </summary>
+    public Vector2 GetCellCenter(Array2i cellIndex)
+    {
+        return new Vector2(
+            (_max.X - Resolution * (cellIndex.Y + 0.5)),
+            (_max.Y - Resolution * (cellIndex.X + 0.5))
+        );
+    }
+
+    /// <summary>
+    /// Returns true if the grid contains 'cell_index'.
+    /// </summary>
+    public bool Contains(Array2i cellIndex)
+    {
+        return cellIndex.X >= 0 && cellIndex.Y >= 0 &&
+               cellIndex.X < _cellLimits.NumXCells &&
+               cellIndex.Y < _cellLimits.NumYCells;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public Models.Mapping.MapLimits ToProto()
+    {
+        var cellLimitsProto = _cellLimits.ToProto();
+        return new Models.Mapping.MapLimits(
+            _resolution,
+            new Vector2d(_max.X, _max.Y),
+            new Models.Mapping.CellLimits(cellLimitsProto.NumXCells, cellLimitsProto.NumYCells)
+        );
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/ProbabilityGrid.cs

@@ -0,0 +1,293 @@
+/*
+ * 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.Models.Mapping;
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Numbers;
+using System.IO.Compression;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Represents a 2D grid of probabilities.
+/// </summary>
+public class ProbabilityGrid : Grid2D
+{
+    private readonly ValueConversionTables _conversionTables;
+
+    public ProbabilityGrid(MapLimits limits, ValueConversionTables conversionTables)
+        : base(limits, ProbabilityValues.kMinCorrespondenceCost, ProbabilityValues.kMaxCorrespondenceCost, conversionTables)
+    {
+        _conversionTables = conversionTables;
+    }
+
+    public ProbabilityGrid(Models.Mapping.Grid2D proto, ValueConversionTables conversionTables)
+        : base(new MapLimits(proto.Limits.Resolution,
+                             new Vector2(proto.Limits.Max.X, proto.Limits.Max.Y),
+                             new CellLimits(proto.Limits.CellLimits.NumXCells, proto.Limits.CellLimits.NumYCells)),
+               proto.MinCorrespondenceCost > 0 ? proto.MinCorrespondenceCost : ProbabilityValues.kMinCorrespondenceCost,
+               proto.MaxCorrespondenceCost > 0 ? proto.MaxCorrespondenceCost : ProbabilityValues.kMaxCorrespondenceCost,
+               conversionTables)
+    {
+        _conversionTables = conversionTables;
+        
+        // Copy cells from proto
+        if (proto.Cells != null)
+        {
+            _correspondenceCostCells.Clear();
+            foreach (var cell in proto.Cells)
+            {
+                _correspondenceCostCells.Add((ushort)cell);
+            }
+        }
+        
+        // Copy known cells box
+        // Match C++: proto.has_known_cells_box() - use MinX <= MaxX to detect valid box,
+        // which correctly handles boxes at origin (0,0)-(0,0).
+        if (proto.KnownCellsBox.MinX <= proto.KnownCellsBox.MaxX &&
+            proto.KnownCellsBox.MinY <= proto.KnownCellsBox.MaxY)
+        {
+            _knownCellsBox = (proto.KnownCellsBox.MinX, proto.KnownCellsBox.MinY,
+                             proto.KnownCellsBox.MaxX, proto.KnownCellsBox.MaxY);
+        }
+    }
+
+    /// <summary>
+    /// Sets the probability of the cell at 'cell_index' to the given
+    /// 'probability'. Only allowed if the cell was unknown before.
+    /// </summary>
+    public void SetProbability(Array2i cellIndex, double probability)
+    {
+        var flatIndex = ToFlatIndex(cellIndex);
+        var cell = _correspondenceCostCells[flatIndex];
+        
+        const ushort kUnknownProbabilityValue = 0;
+        if (cell != kUnknownProbabilityValue)
+        {
+            throw new InvalidOperationException("Cell must be unknown before setting probability");
+        }
+
+        _correspondenceCostCells[flatIndex] = ProbabilityValues.CorrespondenceCostToValue(
+            ProbabilityValues.ProbabilityToCorrespondenceCost(probability));
+        
+        // Update known cells box
+        UpdateKnownCellsBox(cellIndex);
+    }
+
+    /// <summary>
+    /// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
+    /// to the probability of the cell at 'cell_index' if the cell has not already
+    /// been updated. Multiple updates of the same cell will be ignored until
+    /// FinishUpdate() is called. Returns true if the cell was updated.
+    /// </summary>
+    public bool ApplyLookupTable(Array2i cellIndex, List<ushort> table)
+    {
+        const ushort kUpdateMarker = (ushort)(1u << 15);
+        const int kValueCount = 32768;
+        
+        if (table.Count != kValueCount)
+        {
+            throw new ArgumentException($"Table size must be {kValueCount}", nameof(table));
+        }
+
+        var flatIndex = ToFlatIndex(cellIndex);
+        var cell = _correspondenceCostCells[flatIndex];
+        
+        if (cell >= kUpdateMarker)
+        {
+            return false; // Already updated
+        }
+
+        _updateIndices.Add(flatIndex);
+        _correspondenceCostCells[flatIndex] = table[cell];
+        // After applying lookup table, the cell value should have the update marker set
+        // (value >= kUpdateMarker), which will be removed in FinishUpdate()
+        UpdateKnownCellsBox(cellIndex);
+        
+        return true;
+    }
+
+    /// <summary>
+    /// Gets the grid type.
+    /// </summary>
+    public override GridType GetGridType()
+    {
+        return GridType.ProbabilityGrid;
+    }
+
+    /// <summary>
+    /// Returns the probability of the cell with 'cell_index'.
+    /// </summary>
+    public double GetProbability(Array2i cellIndex)
+    {
+        if (!_limits.Contains(cellIndex))
+        {
+            return ProbabilityValues.kMinProbability;
+        }
+        var flatIndex = ToFlatIndex(cellIndex);
+        var value = _correspondenceCostCells[flatIndex];
+        return ProbabilityValues.CorrespondenceCostToProbability(
+            ProbabilityValues.ValueToCorrespondenceCost(value));
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public override Models.Mapping.Grid2D ToProto()
+    {
+        var proto = base.ToProto();
+        proto.ProbabilityGrid2D = new Models.Mapping.ProbabilityGrid(); // Empty struct to indicate type
+        return proto;
+    }
+
+    /// <summary>
+    /// Computes a cropped grid containing only known cells.
+    /// Match C++ implementation: only copy known cells using SetProbability.
+    /// </summary>
+    public override Grid2D ComputeCroppedGrid()
+    {
+        ComputeCroppedLimits(out var offset, out var cellLimits);
+        var resolution = _limits.Resolution;
+        var max = new Vector2(
+            (_limits.Max.X - resolution * offset.Y),
+            (_limits.Max.Y - resolution * offset.X));
+        
+        var croppedGrid = new ProbabilityGrid(
+            new MapLimits(resolution, max, cellLimits),
+            _conversionTables);
+
+        // Match C++: for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
+        //              if (!IsKnown(xy_index + offset)) continue;
+        //              cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));
+        //            }
+        // Only copy known cells using SetProbability (which updates known_cells_box)
+        for (int y = 0; y < cellLimits.NumYCells; y++)
+        {
+            for (int x = 0; x < cellLimits.NumXCells; x++)
+            {
+                var xyIndex = new Array2i(x, y);
+                var oldIndex = new Array2i(offset.X + x, offset.Y + y);
+                if (!IsKnown(oldIndex))
+                {
+                    continue; // Skip unknown cells
+                }
+                croppedGrid.SetProbability(xyIndex, GetProbability(oldIndex));
+            }
+        }
+
+        return croppedGrid;
+    }
+
+    /// <summary>
+    /// Draws the probability grid to a submap texture for visualization.
+    /// Match C++: ProbabilityGrid::DrawToSubmapTexture
+    /// </summary>
+    /// <param name="localPose">The local pose of the submap.</param>
+    /// <returns>A texture containing the visualization data.</returns>
+    public SubmapQuery.Texture DrawToSubmapTexture(Rigid3d localPose)
+    {
+        ComputeCroppedLimits(out var offset, out var cellLimits);
+
+        // Build the cells data (value + alpha pairs)
+        var cellsData = new List<byte>(cellLimits.NumXCells * cellLimits.NumYCells * 2);
+
+        foreach (var xyIndex in new XYIndexRange(cellLimits))
+        {
+            var sourceIndex = new Array2i(xyIndex.X + offset.X, xyIndex.Y + offset.Y);
+
+            if (!IsKnown(sourceIndex))
+            {
+                cellsData.Add(0); // value (unknown log odds value)
+                cellsData.Add(0); // alpha
+                continue;
+            }
+
+            // We would like to add 'delta' but this is not possible using a value and
+            // alpha. We use premultiplied alpha, so when 'delta' is positive we can
+            // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
+            // zero, and use 'alpha' to subtract. This is only correct when the pixel
+            // is currently white, so walls will look too gray. This should be hard to
+            // detect visually for the user, though.
+            var probability = GetProbability(sourceIndex);
+            var delta = 128 - SubmapProbabilityUtils.ProbabilityToLogOddsInteger(probability);
+
+            byte alpha = (byte)(delta > 0 ? 0 : Math.Min(255, -delta));
+            byte value = (byte)(delta > 0 ? Math.Min(255, delta) : 0);
+
+            cellsData.Add(value);
+            cellsData.Add((value != 0 || alpha != 0) ? alpha : (byte)1);
+        }
+
+        // Compress using GZip
+        var compressedCells = CompressGzip(cellsData.ToArray());
+
+        // Calculate slice pose
+        var resolution = _limits.Resolution;
+        var maxX = _limits.Max.X - resolution * offset.Y;
+        var maxY = _limits.Max.Y - resolution * offset.X;
+
+        var slicePose = localPose.Inverse() *
+                        Rigid3d.FromTranslation(new Vector3(maxX, maxY, 0));
+
+        return new SubmapQuery.Texture(
+            compressedCells,
+            cellLimits.NumXCells,
+            cellLimits.NumYCells,
+            resolution,
+            slicePose);
+    }
+
+    /// <summary>
+    /// Compresses data using GZip.
+    /// Match C++: common::FastGzipString
+    /// </summary>
+    private static List<byte> CompressGzip(byte[] data)
+    {
+        using var memoryStream = new MemoryStream();
+        using (var gzipStream = new GZipStream(memoryStream, CompressionMode.Compress, leaveOpen: true))
+        {
+            gzipStream.Write(data, 0, data.Length);
+        }
+        return new List<byte>(memoryStream.ToArray());
+    }
+
+    /// <summary>
+    /// Updates the known cells box to include the given cell index.
+    /// Match C++: mutable_known_cells_box()->extend(cell_index.matrix())
+    /// </summary>
+    private void UpdateKnownCellsBox(Array2i cellIndex)
+    {
+        // Match C++: AlignedBox2i::extend() - if empty, sets min=max=point, otherwise extends bounds
+        // Empty box is marked by minX > maxX (or minY > maxY)
+        if (_knownCellsBox.minX > _knownCellsBox.maxX || _knownCellsBox.minY > _knownCellsBox.maxY)
+        {
+            // Box is empty, set min and max to cellIndex
+            _knownCellsBox = (cellIndex.X, cellIndex.Y, cellIndex.X, cellIndex.Y);
+        }
+        else
+        {
+            // Box is not empty, extend bounds
+            _knownCellsBox = (
+                Math.Min(_knownCellsBox.minX, cellIndex.X),
+                Math.Min(_knownCellsBox.minY, cellIndex.Y),
+                Math.Max(_knownCellsBox.maxX, cellIndex.X),
+                Math.Max(_knownCellsBox.maxY, cellIndex.Y)
+            );
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/ProbabilityGridRangeDataInserter2D.cs

@@ -0,0 +1,170 @@
+/*
+ * 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.Mapping.Internal.D2D;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using System.Runtime.InteropServices;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Range data inserter for probability grids in 2D.
+/// </summary>
+public class ProbabilityGridRangeDataInserter2D : IRangeDataInserter
+{
+    private const int kSubpixelScale = 1000;
+    private const double kPadding = 1e-6;
+
+    private readonly ProbabilityGridRangeDataInserterOptions2D _options;
+    private readonly List<ushort> _hitTable;
+    private readonly List<ushort> _missTable;
+
+    public ProbabilityGridRangeDataInserter2D(ProbabilityGridRangeDataInserterOptions2D 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.ComputeLookupTableToApplyCorrespondenceCostOdds(
+            ProbabilityValues.Odds(options.HitProbability));
+        _missTable = ProbabilityValues.ComputeLookupTableToApplyCorrespondenceCostOdds(
+            ProbabilityValues.Odds(options.MissProbability));
+    }
+
+    /// <summary>
+    /// Inserts 'range_data' into 'grid'.
+    /// </summary>
+    public void Insert(RangeData rangeData, IGrid grid)
+    {
+        if (grid is not ProbabilityGrid probabilityGrid)
+        {
+            throw new ArgumentException("Grid must be a ProbabilityGrid", nameof(grid));
+        }
+
+        // Match C++: By not finishing the update after hits are inserted, we give hits priority
+        // (i.e. no hits will be ignored because of a miss in the same cell).
+        CastRays(rangeData, _hitTable, _missTable, _options.InsertFreeSpace, probabilityGrid);
+        probabilityGrid.FinishUpdate();
+    }
+
+    private static void GrowAsNeeded(RangeData rangeData, ProbabilityGrid probabilityGrid)
+    {
+        var origin2D = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
+        var minX = origin2D.X - kPadding;
+        var minY = origin2D.Y - kPadding;
+        var maxX = origin2D.X + kPadding;
+        var maxY = origin2D.Y + kPadding;
+
+        foreach (var hit in rangeData.Returns.Points)
+        {
+            minX = Math.Min(minX, hit.Position.X - kPadding);
+            minY = Math.Min(minY, hit.Position.Y - kPadding);
+            maxX = Math.Max(maxX, hit.Position.X + kPadding);
+            maxY = Math.Max(maxY, hit.Position.Y + kPadding);
+        }
+
+        foreach (var miss in rangeData.Misses.Points)
+        {
+            minX = Math.Min(minX, miss.Position.X - kPadding);
+            minY = Math.Min(minY, miss.Position.Y - kPadding);
+            maxX = Math.Max(maxX, miss.Position.X + kPadding);
+            maxY = Math.Max(maxY, miss.Position.Y + kPadding);
+        }
+
+        probabilityGrid.GrowLimits(new Vector2(minX, minY));
+        probabilityGrid.GrowLimits(new Vector2(maxX, maxY));
+    }
+
+    private static void CastRays(
+        RangeData rangeData,
+        List<ushort> hitTable,
+        List<ushort> missTable,
+        bool insertFreeSpace,
+        ProbabilityGrid probabilityGrid)
+    {
+        GrowAsNeeded(rangeData, probabilityGrid);
+
+        var limits = probabilityGrid.Limits;
+        var superscaledResolution = limits.Resolution / kSubpixelScale;
+        var superscaledLimits = new MapLimits(
+            superscaledResolution,
+            limits.Max,
+            new CellLimits(
+                limits.CellLimits.NumXCells * kSubpixelScale,
+                limits.CellLimits.NumYCells * kSubpixelScale));
+
+        var origin2D = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
+
+        var begin = superscaledLimits.GetCellIndex(origin2D);
+
+        // Phase 1: Hit processing (serial - grid writes not thread-safe)
+        var returnCount = rangeData.Returns.Points.Count;
+        var ends = new Array2i[returnCount];
+        for (int i = 0; i < returnCount; i++)
+        {
+            var hit = rangeData.Returns.Points[i];
+            var hit2D = new Vector2(hit.Position.X, hit.Position.Y);
+            ends[i] = superscaledLimits.GetCellIndex(hit2D);
+            probabilityGrid.ApplyLookupTable(ends[i] / kSubpixelScale, hitTable);
+        }
+
+        if (!insertFreeSpace)
+        {
+            return;
+        }
+
+        // Reusable buffer for ray computation - avoids per-ray List<Array2i> allocation.
+        // Capacity 512 covers most rays without reallocation (typical ray length at
+        // 0.05m resolution with 10m max range ≈ 200 cells).
+        var rayBuffer = new List<Array2i>(512);
+
+        // Phase 2: Process miss rays from returns using reusable buffer
+        for (int i = 0; i < returnCount; i++)
+        {
+            rayBuffer.Clear();
+            RayToPixelMask.ComputeInto(begin, ends[i], kSubpixelScale, rayBuffer);
+            var raySpan = CollectionsMarshal.AsSpan(rayBuffer);
+            for (int j = 0; j < raySpan.Length; j++)
+            {
+                probabilityGrid.ApplyLookupTable(raySpan[j], missTable);
+            }
+        }
+
+        // Phase 3: Process miss rays from explicit misses using reusable buffer
+        var missCount = rangeData.Misses.Points.Count;
+        for (int i = 0; i < missCount; i++)
+        {
+            var miss = rangeData.Misses.Points[i];
+            var end = superscaledLimits.GetCellIndex(new Vector2(miss.Position.X, miss.Position.Y));
+            rayBuffer.Clear();
+            RayToPixelMask.ComputeInto(begin, end, kSubpixelScale, rayBuffer);
+            var raySpan = CollectionsMarshal.AsSpan(rayBuffer);
+            for (int j = 0; j < raySpan.Length; j++)
+            {
+                probabilityGrid.ApplyLookupTable(raySpan[j], missTable);
+            }
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/Submap2D.cs

@@ -0,0 +1,184 @@
+/*
+ * 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 System;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// 2D Submap implementation.
+/// </summary>
+public class Submap2D : Submap
+{
+    private Grid2D? _grid;
+    private Grid2D? _highResGrid;
+    private readonly ValueConversionTables _conversionTables;
+
+    public Submap2D(Rigid3d localSubmapPose, Grid2D grid, ValueConversionTables conversionTables, Grid2D? highResGrid = null)
+        : base(localSubmapPose) // Pass the full Rigid3d pose including rotation
+    {
+        _grid = grid;
+        _highResGrid = highResGrid;
+        _conversionTables = conversionTables;
+    }
+    
+    // Keep old constructor for backward compatibility (if needed)
+    public Submap2D(Vector2 origin, Grid2D grid, ValueConversionTables conversionTables)
+        : this(new Rigid3d(new Vector3(origin.X, origin.Y, 0.0), Quaternion.Identity), grid, conversionTables)
+    {
+    }
+
+    public Submap2D(Models.Mapping.Submap2D proto, ValueConversionTables conversionTables)
+        : base((Rigid3d)proto.LocalPose)
+    {
+        _conversionTables = conversionTables;
+        NumRangeData = proto.NumRangeData;
+        InsertionFinished = proto.Finished;
+
+        // Match C++: if (proto.has_grid()) - no resolution check
+        if (proto.Grid.HasValue)
+        {
+            var gridProto = proto.Grid.Value;
+            if (gridProto.ProbabilityGrid2D.HasValue)
+            {
+                _grid = new ProbabilityGrid(gridProto, conversionTables);
+            }
+            else if (gridProto.Tsdf2D.HasValue)
+            {
+                _grid = new TSDF2D(gridProto, conversionTables);
+            }
+            else
+            {
+                throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
+            }
+        }
+    }
+
+    /// <summary>
+    /// Gets the grid.
+    /// </summary>
+    public Grid2D? Grid => _grid;
+
+    /// <summary>
+    /// Gets the optional high resolution grid for fine-grained Ceres scan matching.
+    /// </summary>
+    public Grid2D? HighResGrid => _highResGrid;
+
+    /// <summary>
+    /// Insert 'range_data' into this submap using 'range_data_inserter'. The
+    /// submap must not be finished yet.
+    /// </summary>
+    public void InsertRangeData(RangeData rangeData, IRangeDataInserter rangeDataInserter)
+    {
+        if (InsertionFinished)
+        {
+            throw new InvalidOperationException("Cannot insert range data into finished submap");
+        }
+        if (_grid == null)
+        {
+            throw new InvalidOperationException("Grid is not initialized");
+        }
+        // Insert range data directly (already transformed to submap frame in ActiveSubmaps2D)
+        if (_highResGrid != null)
+        {
+            // Main grid and high-res grid are independent - insert in parallel.
+            var highResGrid = _highResGrid;
+            var highResTask = Task.Run(() => rangeDataInserter.Insert(rangeData, (IGrid)highResGrid));
+            rangeDataInserter.Insert(rangeData, (IGrid)_grid);
+            highResTask.GetAwaiter().GetResult();
+        }
+        else
+        {
+            rangeDataInserter.Insert(rangeData, (IGrid)_grid);
+        }
+        NumRangeData++;
+    }
+
+    /// <summary>
+    /// Finishes the submap.
+    /// Match C++: grid_ = grid_->ComputeCroppedGrid(); to reduce memory usage
+    /// </summary>
+    public void Finish()
+    {
+        if (_grid == null)
+        {
+            throw new InvalidOperationException("Grid is not initialized");
+        }
+        if (InsertionFinished)
+        {
+            throw new InvalidOperationException("Submap is already finished");
+        }
+        // Match C++: Crop grid to reduce memory usage when submap is finished
+        _grid = _grid.ComputeCroppedGrid();
+        _highResGrid = _highResGrid?.ComputeCroppedGrid();
+        InsertionFinished = true;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public override Models.Mapping.Submap ToProto(bool includeGridData)
+    {
+        var submap2D = new Models.Mapping.Submap2D(
+            (Rigid3dProto)LocalPose,
+            NumRangeData,
+            InsertionFinished,
+            includeGridData && _grid != null ? _grid.ToProto() : null
+        );
+
+        // Note: SubmapId will be set by caller
+        // Note: SubmapId will be set by caller when creating full Submap
+        return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), submap2D, null);
+    }
+
+    /// <summary>
+    /// Updates from proto representation.
+    /// Match C++: CHECK(proto.has_submap_2d())
+    /// </summary>
+    public override void UpdateFromProto(Models.Mapping.Submap proto)
+    {
+        if (!proto.Submap2D.HasValue)
+        {
+            throw new ArgumentException("Proto must contain Submap2D", nameof(proto));
+        }
+
+        var submap2D = proto.Submap2D.Value;
+        NumRangeData = submap2D.NumRangeData;
+        InsertionFinished = submap2D.Finished;
+        
+        // Match C++: if (proto.submap_2d().has_grid()) - no resolution check
+        if (submap2D.Grid.HasValue)
+        {
+            var gridProto = submap2D.Grid.Value;
+            if (gridProto.ProbabilityGrid2D.HasValue)
+            {
+                _grid = new ProbabilityGrid(gridProto, _conversionTables);
+            }
+            else if (gridProto.Tsdf2D.HasValue)
+            {
+                _grid = new TSDF2D(gridProto, _conversionTables);
+            }
+            else
+            {
+                throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
+            }
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/TSDF2D.cs

@@ -0,0 +1,382 @@
+/*
+ * Copyright 2018 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.Mapping.Internal.D2D;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Numbers;
+using System.IO.Compression;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Represents a 2D grid of truncated signed distances and weights.
+/// </summary>
+public class TSDF2D : Grid2D
+{
+    private readonly ValueConversionTables _conversionTables;
+    private readonly TSDValueConverter _valueConverter;
+    private List<ushort> _weightCells;
+
+    public TSDF2D(MapLimits limits, double truncationDistance, double maxWeight,
+                   ValueConversionTables conversionTables)
+        : base(limits, -truncationDistance, truncationDistance, conversionTables)
+    {
+        _conversionTables = conversionTables;
+        _valueConverter = new TSDValueConverter(truncationDistance, maxWeight, conversionTables);
+        
+        var size = limits.CellLimits.NumXCells * limits.CellLimits.NumYCells;
+        _weightCells = new List<ushort>(size);
+        for (int i = 0; i < size; i++)
+        {
+            _weightCells.Add(TSDValueConverter.GetUnknownWeightValue());
+        }
+    }
+
+    public TSDF2D(Models.Mapping.Grid2D proto, ValueConversionTables conversionTables)
+        : base(new MapLimits(proto.Limits.Resolution,
+                             new Vector2(proto.Limits.Max.X, proto.Limits.Max.Y),
+                             new CellLimits(proto.Limits.CellLimits.NumXCells, proto.Limits.CellLimits.NumYCells)),
+               proto.MinCorrespondenceCost > 0 ? proto.MinCorrespondenceCost : -0.3,
+               proto.MaxCorrespondenceCost > 0 ? proto.MaxCorrespondenceCost : 0.3,
+               conversionTables)
+    {
+        if (proto.Tsdf2D == null)
+        {
+            throw new ArgumentException("Proto must have TSDF2D data", nameof(proto));
+        }
+
+        _conversionTables = conversionTables;
+        var tsdfProto = proto.Tsdf2D.Value;
+        _valueConverter = new TSDValueConverter(
+            tsdfProto.TruncationDistance, tsdfProto.MaxWeight, conversionTables);
+
+        // Match C++: Grid2D(proto, ...) copies cells from proto.cells() into correspondence_cost_cells_
+        // Since we call base constructor with new MapLimits (not proto), we must copy manually.
+        // Without this, all TSD values remain 0 (unknown) after deserialization.
+        if (proto.Cells != null)
+        {
+            _correspondenceCostCells.Clear();
+            foreach (var cell in proto.Cells)
+            {
+                _correspondenceCostCells.Add((ushort)cell);
+            }
+        }
+
+        _weightCells = new List<ushort>(tsdfProto.WeightCells.Count);
+        foreach (var cell in tsdfProto.WeightCells)
+        {
+            if (cell > ushort.MaxValue)
+            {
+                throw new ArgumentException("Weight cell value exceeds ushort.MaxValue");
+            }
+            _weightCells.Add((ushort)cell);
+        }
+
+        // Match C++: Grid2D constructor copies known_cells_box from proto if present
+        // Since we call base constructor with new MapLimits, we need to copy known_cells_box manually
+        // C++ Grid2D constructor: if (proto.has_known_cells_box()) { known_cells_box_ = ... }
+        // Check if known_cells_box is not empty (minX > maxX indicates empty in C++ AlignedBox2i)
+        if (proto.KnownCellsBox.MinX <= proto.KnownCellsBox.MaxX &&
+            proto.KnownCellsBox.MinY <= proto.KnownCellsBox.MaxY)
+        {
+            _knownCellsBox = (proto.KnownCellsBox.MinX, proto.KnownCellsBox.MinY,
+                             proto.KnownCellsBox.MaxX, proto.KnownCellsBox.MaxY);
+        }
+    }
+
+    /// <summary>
+    /// Sets the TSD and weight of the cell at 'cell_index'.
+    /// Only allowed if the cell was not updated in the current update cycle.
+    /// </summary>
+    public void SetCell(Array2i cellIndex, double tsd, double weight)
+    {
+        var flatIndex = ToFlatIndex(cellIndex);
+        var tsdfCell = _correspondenceCostCells[flatIndex];
+        
+        ushort kUpdateMarker = TSDValueConverter.GetUpdateMarker();
+        if (tsdfCell >= kUpdateMarker)
+        {
+            return; // Already updated
+        }
+
+        _updateIndices.Add(flatIndex);
+        UpdateKnownCellsBox(cellIndex);
+
+        // Set TSD with update marker
+        _correspondenceCostCells[flatIndex] = (ushort)(_valueConverter.TSDToValue(tsd) + kUpdateMarker);
+        
+        // Set weight
+        _weightCells[flatIndex] = _valueConverter.WeightToValue(weight);
+    }
+
+    /// <summary>
+    /// Returns the TSD of the cell with 'cell_index'.
+    /// </summary>
+    public double GetTSD(Array2i cellIndex)
+    {
+        if (_limits.Contains(cellIndex))
+        {
+            return _valueConverter.ValueToTSD(_correspondenceCostCells[ToFlatIndex(cellIndex)]);
+        }
+        return _valueConverter.GetMinTSD();
+    }
+
+    /// <summary>
+    /// Returns the weight of the cell with 'cell_index'.
+    /// </summary>
+    public double GetWeight(Array2i cellIndex)
+    {
+        if (_limits.Contains(cellIndex))
+        {
+            var flatIndex = ToFlatIndex(cellIndex);
+            return _valueConverter.ValueToWeight(_weightCells[flatIndex]);
+        }
+        return _valueConverter.GetMinWeight();
+    }
+
+    /// <summary>
+    /// Returns the TSD and weight of the cell with 'cell_index'.
+    /// </summary>
+    public (double tsd, double weight) GetTSDAndWeight(Array2i cellIndex)
+    {
+        if (_limits.Contains(cellIndex))
+        {
+            var flatIndex = ToFlatIndex(cellIndex);
+            return (_valueConverter.ValueToTSD(_correspondenceCostCells[flatIndex]),
+                    _valueConverter.ValueToWeight(_weightCells[flatIndex]));
+        }
+        return (_valueConverter.GetMinTSD(), _valueConverter.GetMinWeight());
+    }
+
+    /// <summary>
+    /// Returns true if the cell at 'cell_index' was updated in the current update cycle.
+    /// </summary>
+    public bool CellIsUpdated(Array2i cellIndex)
+    {
+        var flatIndex = ToFlatIndex(cellIndex);
+        var tsdfCell = _correspondenceCostCells[flatIndex];
+        return tsdfCell >= TSDValueConverter.GetUpdateMarker();
+    }
+
+    /// <summary>
+    /// Gets the grid type.
+    /// </summary>
+    public override GridType GetGridType()
+    {
+        return GridType.TSDF;
+    }
+
+    /// <summary>
+    /// Grows the map as necessary to include 'point'.
+    /// </summary>
+    public override void GrowLimits(Vector2 point)
+    {
+        GrowLimits(point, [_correspondenceCostCells, _weightCells],
+                   [TSDValueConverter.GetUnknownTSDValue(), TSDValueConverter.GetUnknownWeightValue()]);
+    }
+
+    protected override void UpdateGridReferences(List<ushort>[] grids)
+    {
+        base.UpdateGridReferences(grids);
+        _weightCells = grids[1];
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public override Models.Mapping.Grid2D ToProto()
+    {
+        var proto = base.ToProto();
+        
+        var weightCells = new List<int>(_weightCells.Count);
+        foreach (var cell in _weightCells)
+        {
+            weightCells.Add(cell);
+        }
+
+        proto.Tsdf2D = new Models.Mapping.TSDF2D(
+            _valueConverter.GetMaxTSD(),
+            _valueConverter.GetMaxWeight(),
+            weightCells);
+
+        return proto;
+    }
+
+    /// <summary>
+    /// Computes a cropped grid containing only known cells.
+    /// </summary>
+    public override Grid2D ComputeCroppedGrid()
+    {
+        ComputeCroppedLimits(out var offset, out var cellLimits);
+        var resolution = _limits.Resolution;
+        var max = new Vector2(
+            (_limits.Max.X - resolution * offset.Y),
+            (_limits.Max.Y - resolution * offset.X));
+
+        var croppedGrid = new TSDF2D(
+            new MapLimits(resolution, max, cellLimits),
+            _valueConverter.GetMaxTSD(),
+            _valueConverter.GetMaxWeight(),
+            _conversionTables);
+
+        // Copy known cells
+        for (int y = 0; y < cellLimits.NumYCells; y++)
+        {
+            for (int x = 0; x < cellLimits.NumXCells; x++)
+            {
+                var oldIndex = new Array2i(offset.X + x, offset.Y + y);
+                if (_limits.Contains(oldIndex) && IsKnown(oldIndex))
+                {
+                    var newIndex = new Array2i(x, y);
+                    var (tsd, weight) = GetTSDAndWeight(oldIndex);
+                    croppedGrid.SetCell(newIndex, tsd, weight);
+                }
+            }
+        }
+
+        croppedGrid.FinishUpdate();
+        return croppedGrid;
+    }
+
+    /// <summary>
+    /// Draws the TSDF grid to a submap texture for visualization.
+    /// Match C++: TSDF2D::DrawToSubmapTexture
+    ///
+    /// TSDF convention:
+    /// - tsd > 0: Free space (away from obstacles)
+    /// - tsd < 0: Occupied space (inside/near obstacles)
+    /// - tsd = 0: On obstacle surface
+    ///
+    /// Output texture convention (matching ProbabilityGrid):
+    /// - delta > 0 → value high, alpha 0 → bright pixel → FREE
+    /// - delta < 0 → value 0, alpha high → dark pixel → OCCUPIED
+    /// </summary>
+    /// <param name="localPose">The local pose of the submap.</param>
+    /// <returns>A texture containing the visualization data.</returns>
+    public SubmapQuery.Texture DrawToSubmapTexture(Rigid3d localPose)
+    {
+        ComputeCroppedLimits(out var offset, out var cellLimits);
+
+        // Build the cells data (value + alpha pairs)
+        var cellsData = new List<byte>(cellLimits.NumXCells * cellLimits.NumYCells * 2);
+
+        var maxTsd = _valueConverter.GetMaxTSD();
+        var maxWeight = _valueConverter.GetMaxWeight();
+
+        foreach (var xyIndex in new XYIndexRange(cellLimits))
+        {
+            var sourceIndex = new Array2i(xyIndex.X + offset.X, xyIndex.Y + offset.Y);
+
+            if (!IsKnown(sourceIndex))
+            {
+                cellsData.Add(0); // value
+                cellsData.Add(0); // alpha
+                continue;
+            }
+
+            // We would like to add 'delta' but this is not possible using a value and
+            // alpha. We use premultiplied alpha, so when 'delta' is positive we can
+            // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
+            // zero, and use 'alpha' to subtract. This is only correct when the pixel
+            // is currently white, so walls will look too gray. This should be hard to
+            // detect visually for the user, though.
+            var tsd = GetTSD(sourceIndex);
+            var weight = GetWeight(sourceIndex);
+            var normalizedWeight = weight / maxWeight;
+
+            // FIXED: Keep the sign of TSD to distinguish free vs occupied space
+            // tsd > 0 (free) → normalizedTsd > 0 → delta > 0 → value high (bright)
+            // tsd < 0 (occupied) → normalizedTsd < 0 → delta < 0 → alpha high (dark)
+            //
+            // Normalize TSD to [-1, 1] range while preserving sign
+            var normalizedTsd = Math.Clamp(tsd / maxTsd, -1.0, 1.0);
+
+            // Apply sqrt scaling to magnitude only, preserve sign for better visualization
+            // This makes the gradient more visible near the surface
+            var magnitude = Math.Pow(Math.Abs(normalizedTsd), 0.5);
+            var signedMagnitude = normalizedTsd >= 0 ? magnitude : -magnitude;
+
+            // Scale by weight and convert to delta
+            // delta range: [-127, 127] scaled by weight
+            var delta = (int)Math.Round(normalizedWeight * signedMagnitude * 127.0,
+                                        MidpointRounding.AwayFromZero);
+
+            byte alpha = (byte)(delta < 0 ? Math.Min(255, -delta) : 0);
+            byte value = (byte)(delta > 0 ? Math.Min(255, delta) : 0);
+
+            cellsData.Add(value);
+            cellsData.Add((value != 0 || alpha != 0) ? alpha : (byte)1);
+        }
+
+        // Compress using GZip
+        var compressedCells = CompressGzip(cellsData.ToArray());
+
+        // Calculate slice pose
+        var resolution = _limits.Resolution;
+        var maxX = _limits.Max.X - resolution * offset.Y;
+        var maxY = _limits.Max.Y - resolution * offset.X;
+
+        var slicePose = localPose.Inverse() *
+                        Rigid3d.FromTranslation(new Vector3(maxX, maxY, 0));
+
+        return new SubmapQuery.Texture(
+            compressedCells,
+            cellLimits.NumXCells,
+            cellLimits.NumYCells,
+            resolution,
+            slicePose);
+    }
+
+    /// <summary>
+    /// Compresses data using GZip.
+    /// Match C++: common::FastGzipString
+    /// </summary>
+    private static List<byte> CompressGzip(byte[] data)
+    {
+        using var memoryStream = new MemoryStream();
+        using (var gzipStream = new GZipStream(memoryStream, CompressionMode.Compress, leaveOpen: true))
+        {
+            gzipStream.Write(data, 0, data.Length);
+        }
+        return new List<byte>(memoryStream.ToArray());
+    }
+
+    /// <summary>
+    /// Updates the known cells box to include the given cell index.
+    /// Match C++: AlignedBox2i::isEmpty() logic: min > max indicates empty
+    /// </summary>
+    private void UpdateKnownCellsBox(Array2i cellIndex)
+    {
+        // Match C++ AlignedBox2i::isEmpty() logic: min > max indicates empty
+        if (_knownCellsBox.minX > _knownCellsBox.maxX || _knownCellsBox.minY > _knownCellsBox.maxY)
+        {
+            _knownCellsBox = (cellIndex.X, cellIndex.Y, cellIndex.X, cellIndex.Y);
+        }
+        else
+        {
+            _knownCellsBox = (
+                Math.Min(_knownCellsBox.minX, cellIndex.X),
+                Math.Min(_knownCellsBox.minY, cellIndex.Y),
+                Math.Max(_knownCellsBox.maxX, cellIndex.X),
+                Math.Max(_knownCellsBox.maxY, cellIndex.Y)
+            );
+        }
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/TSDFRangeDataInserter2D.cs

@@ -0,0 +1,273 @@
+/*
+ * Copyright 2018 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.Mapping.Internal.D2D;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using System.Runtime.InteropServices;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Range data inserter for TSDF grids in 2D.
+/// </summary>
+public class TSDFRangeDataInserter2D : IRangeDataInserter
+{
+    private const int kSubpixelScale = 1000;
+    private const double kMinRangeMeters = 1e-6;
+    private static readonly double kSqrtTwoPi = Math.Sqrt(2.0 * Math.PI);
+
+    private readonly TSDFRangeDataInserterOptions2D _options;
+
+    public TSDFRangeDataInserter2D(TSDFRangeDataInserterOptions2D options)
+    {
+        _options = options;
+    }
+
+    /// <summary>
+    /// Inserts 'range_data' into 'grid'.
+    /// </summary>
+    public void Insert(RangeData rangeData, IGrid grid)
+    {
+        if (grid is not TSDF2D tsdf)
+        {
+            throw new ArgumentException("Grid must be a TSDF2D", nameof(grid));
+        }
+
+        // Match C++: No FinishUpdate() before GrowAsNeeded()
+        var truncationDistance = _options.TruncationDistance;
+        GrowAsNeeded(rangeData, truncationDistance, tsdf);
+
+        // Compute normals if needed
+        bool scaleUpdateWeightAngleScanNormalToRay =
+            _options.UpdateWeightAngleScanNormalToRayKernelBandwidth != 0.0;
+        RangeData sortedRangeData = rangeData;
+        List<double> normals = [];
+
+        if (_options.ProjectSdfDistanceToScanNormal || scaleUpdateWeightAngleScanNormalToRay)
+        {
+            // Sort range data by angle from origin
+            var returns = new List<RangefinderPoint>(rangeData.Returns.Points);
+            returns.Sort(new RangeDataSorter(rangeData.Origin));
+            
+            sortedRangeData = new RangeData(
+                rangeData.Origin,
+                new PointCloud(returns),
+                rangeData.Misses);
+
+            normals = NormalEstimation2D.EstimateNormals(
+                sortedRangeData,
+                _options.NormalEstimationOptions);
+        }
+
+        var origin = new Vector2(sortedRangeData.Origin.X, sortedRangeData.Origin.Y);
+        // Reusable buffer for ray computation - avoids per-hit List<Array2i> allocation
+        var rayBuffer = new List<Array2i>(512);
+        for (int hitIndex = 0; hitIndex < sortedRangeData.Returns.Count; hitIndex++)
+        {
+            var hitPoint = sortedRangeData.Returns.Points[hitIndex];
+            var hit = new Vector2(hitPoint.Position.X, hitPoint.Position.Y);
+            var normal = normals.Count > 0 ? normals[hitIndex] : double.NaN;
+            InsertHit(hit, origin, normal, tsdf, rayBuffer);
+        }
+
+        tsdf.FinishUpdate();
+    }
+
+    private void InsertHit(Vector2 hit, Vector2 origin, double normal, TSDF2D tsdf, List<Array2i> rayBuffer)
+    {
+        var ray = hit - origin;
+        var range = ray.Length();
+        var truncationDistance = _options.TruncationDistance;
+
+        if (range < truncationDistance) return;
+
+        var truncationRatio = truncationDistance / range;
+        var rayBegin = _options.UpdateFreeSpace
+            ? origin
+            : origin + (1.0 - truncationRatio) * ray;
+        var rayEnd = origin + (1.0 + truncationRatio) * ray;
+
+        var superscaledRay = SuperscaleRay(rayBegin, rayEnd, tsdf);
+        rayBuffer.Clear();
+        RayToPixelMask.ComputeInto(
+            superscaledRay.Item1, superscaledRay.Item2, kSubpixelScale, rayBuffer);
+
+        // Precompute weight factors
+        double weightFactorAngleRayNormal = 1.0;
+        if (_options.UpdateWeightAngleScanNormalToRayKernelBandwidth != 0.0)
+        {
+            var negativeRay = -ray;
+            double angleRayNormal = MathUtils.NormalizeAngleDifference(
+                normal - Math.Atan2(negativeRay.Y, negativeRay.X));
+            weightFactorAngleRayNormal = GaussianKernel(
+                angleRayNormal,
+                _options.UpdateWeightAngleScanNormalToRayKernelBandwidth);
+        }
+
+        double weightFactorRange = 1.0;
+        if (_options.UpdateWeightRangeExponent != 0)
+        {
+            weightFactorRange = ComputeRangeWeightFactor(
+                range, _options.UpdateWeightRangeExponent);
+        }
+
+        // Update cells using Span for bounds-check-free iteration
+        var raySpan = CollectionsMarshal.AsSpan(rayBuffer);
+        for (int i = 0; i < raySpan.Length; i++)
+        {
+            var cellIndex = raySpan[i];
+            if (tsdf.CellIsUpdated(cellIndex)) continue;
+
+            var cellCenter = tsdf.Limits.GetCellCenter(cellIndex);
+            double distanceCellToOrigin = (cellCenter - origin).Length();
+            double updateTSD = range - distanceCellToOrigin;
+
+            // Match C++: if (options_.project_sdf_distance_to_scan_normal()) {
+            // No NaN check in C++
+            if (_options.ProjectSdfDistanceToScanNormal)
+            {
+                double normalOrientation = normal;
+                var normalVector = new Vector2(
+                    Math.Cos(normalOrientation),
+                    Math.Sin(normalOrientation));
+                updateTSD = Vector2.Dot(cellCenter - hit, normalVector);
+            }
+
+            updateTSD = Math.Clamp(updateTSD, -truncationDistance, truncationDistance);
+
+            double updateWeight = weightFactorRange * weightFactorAngleRayNormal;
+            if (_options.UpdateWeightDistanceCellToHitKernelBandwidth != 0.0)
+            {
+                updateWeight *= GaussianKernel(
+                    updateTSD,
+                    _options.UpdateWeightDistanceCellToHitKernelBandwidth);
+            }
+
+            UpdateCell(cellIndex, updateTSD, updateWeight, tsdf);
+        }
+    }
+
+    private void UpdateCell(Array2i cell, double updateSdf, double updateWeight, TSDF2D tsdf)
+    {
+        if (updateWeight == 0.0) return;
+
+        var (currentTSD, currentWeight) = tsdf.GetTSDAndWeight(cell);
+        double updatedWeight = currentWeight + updateWeight;
+        double updatedSDF = (currentTSD * currentWeight + updateSdf * updateWeight) / updatedWeight;
+        updatedWeight = Math.Min(updatedWeight, _options.MaximumWeight);
+        
+        tsdf.SetCell(cell, updatedSDF, updatedWeight);
+    }
+
+    private static void GrowAsNeeded(RangeData rangeData, double truncationDistance, TSDF2D tsdf)
+    {
+        // Match C++: Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
+        // Then only extend end_position, not hit.position
+        var origin2D = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
+        var boundingBoxMin = origin2D;
+        var boundingBoxMax = origin2D;
+
+        foreach (var hit in rangeData.Returns.Points)
+        {
+            var hit2D = new Vector2(hit.Position.X, hit.Position.Y);
+            var direction = Vector2.Normalize(hit2D - origin2D);
+            var endPosition = hit2D + truncationDistance * direction;
+            
+            // Match C++: bounding_box.extend(end_position.head<2>());
+            // Only extend end_position, not hit.position
+            boundingBoxMin.X = Math.Min(boundingBoxMin.X, endPosition.X);
+            boundingBoxMin.Y = Math.Min(boundingBoxMin.Y, endPosition.Y);
+            boundingBoxMax.X = Math.Max(boundingBoxMax.X, endPosition.X);
+            boundingBoxMax.Y = Math.Max(boundingBoxMax.Y, endPosition.Y);
+        }
+
+        const double kPadding = 1e-6;
+        tsdf.GrowLimits(boundingBoxMin - new Vector2(kPadding, kPadding));
+        tsdf.GrowLimits(boundingBoxMax + new Vector2(kPadding, kPadding));
+    }
+
+    private static (Array2i, Array2i) SuperscaleRay(
+        Vector2 begin, Vector2 end, TSDF2D tsdf)
+    {
+        // Match C++: const MapLimits superscaled_limits(
+        //     superscaled_resolution, limits.max(), ...);
+        // Use limits.max() directly, no calculation
+        var limits = tsdf.Limits;
+        var superscaledResolution = limits.Resolution / kSubpixelScale;
+        var superscaledCellLimits = new CellLimits(
+            limits.CellLimits.NumXCells * kSubpixelScale,
+            limits.CellLimits.NumYCells * kSubpixelScale);
+        var superscaledLimits = new MapLimits(
+            superscaledResolution, limits.Max, superscaledCellLimits);
+
+        var superscaledBegin = superscaledLimits.GetCellIndex(begin);
+        var superscaledEnd = superscaledLimits.GetCellIndex(end);
+        
+        // Match C++: return std::make_pair(superscaled_begin, superscaled_end);
+        // No multiplication by kSubpixelScale - GetCellIndex already returns superscaled indices
+        return (superscaledBegin, superscaledEnd);
+    }
+
+    // Match C++: No sigma == 0 check
+    private static double GaussianKernel(double x, double sigma)
+    {
+        return 1.0 / (kSqrtTwoPi * sigma) * Math.Exp(-0.5 * x * x / (sigma * sigma));
+    }
+
+    private static double ComputeRangeWeightFactor(double range, int exponent)
+    {
+        if (Math.Abs(range) <= kMinRangeMeters) return 0.0;
+        return 1.0 / Math.Pow(range, exponent);
+    }
+
+    /// <summary>
+    /// Sorts range data points by angle from origin.
+    /// </summary>
+    private class RangeDataSorter : IComparer<RangefinderPoint>
+    {
+        private readonly Vector2 _origin;
+
+        public RangeDataSorter(Vector3 origin)
+        {
+            _origin = new Vector2(origin.X, origin.Y);
+        }
+
+        public int Compare(RangefinderPoint lhs, RangefinderPoint rhs)
+        {
+            var deltaLhs = Vector2.Normalize(
+                new Vector2(lhs.Position.X, lhs.Position.Y) - _origin);
+            var deltaRhs = Vector2.Normalize(
+                new Vector2(rhs.Position.X, rhs.Position.Y) - _origin);
+
+            if ((deltaLhs.Y < 0.0) != (deltaRhs.Y < 0.0))
+            {
+                return deltaLhs.Y < 0.0 ? -1 : 1;
+            }
+            else if (deltaLhs.Y < 0.0)
+            {
+                return deltaLhs.X < deltaRhs.X ? -1 : (deltaLhs.X > deltaRhs.X ? 1 : 0);
+            }
+            else
+            {
+                return deltaLhs.X > deltaRhs.X ? -1 : (deltaLhs.X < deltaRhs.X ? 1 : 0);
+            }
+        }
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/XYIndex.cs

@@ -0,0 +1,110 @@
+/*
+ * 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 System.Collections;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// Iterates in row-major order through a range of xy-indices.
+/// </summary>
+/// <remarks>
+/// Constructs a new iterator for the specified range.
+/// </remarks>
+public class XYIndexRangeIterator(Array2i minXYIndex, Array2i maxXYIndex) : IEnumerator<Array2i>
+{
+    private readonly Array2i _minXYIndex = minXYIndex;
+    private readonly Array2i _maxXYIndex = maxXYIndex;
+    // Match C++: IEnumerator starts before first element
+    // Initialize to position before first element (minXYIndex with X decremented by 1)
+    private Array2i _xyIndex = new Array2i(minXYIndex.X - 1, minXYIndex.Y);
+
+    /// <summary>
+    /// Constructs a new iterator for everything contained in 'cell_limits'.
+    /// </summary>
+    public XYIndexRangeIterator(CellLimits cellLimits)
+        : this(Array2i.Zero, new Array2i(cellLimits.NumXCells - 1, cellLimits.NumYCells - 1))
+    {
+    }
+
+    public Array2i Current => _xyIndex;
+
+    object IEnumerator.Current => Current;
+
+    // Match C++ operator++() logic
+    public bool MoveNext()
+    {
+        // Match C++: if (xy_index_.x() < max_xy_index_.x()) { ++xy_index_.x(); }
+        // else { xy_index_.x() = min_xy_index_.x(); ++xy_index_.y(); }
+        if (_xyIndex.X < _maxXYIndex.X)
+        {
+            _xyIndex = new Array2i(_xyIndex.X + 1, _xyIndex.Y);
+        }
+        else
+        {
+            _xyIndex = new Array2i(_minXYIndex.X, _xyIndex.Y + 1);
+        }
+        
+        // Check if we've reached the end (match C++ end() condition: min_x, max_y + 1)
+        // End condition: Y > maxY || (Y == maxY && X > maxX)
+        if (_xyIndex.Y > _maxXYIndex.Y || 
+            (_xyIndex.Y == _maxXYIndex.Y && _xyIndex.X > _maxXYIndex.X))
+        {
+            return false;
+        }
+        return true;
+    }
+
+    public void Reset()
+    {
+        // Match C++: Reset to position before first element
+        _xyIndex = new Array2i(_minXYIndex.X - 1, _minXYIndex.Y);
+    }
+
+    public void Dispose()
+    {
+        // Nothing to dispose
+        GC.SuppressFinalize(this);
+    }
+
+    public bool IsEnd()
+    {
+        return _xyIndex.Y > _maxXYIndex.Y || 
+               (_xyIndex.Y == _maxXYIndex.Y && _xyIndex.X > _maxXYIndex.X);
+    }
+}
+
+/// <summary>
+/// Range of XY indices for iteration.
+/// </summary>
+public class XYIndexRange(Array2i minXYIndex, Array2i maxXYIndex) : IEnumerable<Array2i>
+{
+    public XYIndexRange(CellLimits cellLimits)
+        : this(Array2i.Zero, new Array2i(cellLimits.NumXCells - 1, cellLimits.NumYCells - 1))
+    {
+    }
+
+    public IEnumerator<Array2i> GetEnumerator()
+    {
+        return new XYIndexRangeIterator(minXYIndex, maxXYIndex);
+    }
+
+    IEnumerator IEnumerable.GetEnumerator()
+    {
+        return GetEnumerator();
+    }
+}