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();
+    }
+}

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

@@ -0,0 +1,156 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D3D;
+
+/// <summary>
+/// Options for 3D submaps.
+/// </summary>
+public struct SubmapsOptions3D(
+    double highResolution,
+    double highResolutionMaxRange,
+    double lowResolution,
+    int numRangeData,
+    RangeDataInserterOptions3D rangeDataInserterOptions)
+{
+    public double HighResolution { get; set; } = highResolution;
+    public double HighResolutionMaxRange { get; set; } = highResolutionMaxRange;
+    public double LowResolution { get; set; } = lowResolution;
+    public int NumRangeData { get; set; } = numRangeData;
+    public RangeDataInserterOptions3D RangeDataInserterOptions { get; set; } = rangeDataInserterOptions;
+}
+
+/// <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.
+/// </summary>
+public class ActiveSubmaps3D
+{
+    private readonly SubmapsOptions3D _options;
+    private readonly List<Submap3D> _submaps = [];
+    private readonly RangeDataInserter3D _rangeDataInserter;
+
+    public ActiveSubmaps3D(SubmapsOptions3D options)
+    {
+        if (options.NumRangeData <= 0)
+        {
+            throw new ArgumentException("num_range_data must be greater than 0", nameof(options));
+        }
+
+        _options = options;
+        _rangeDataInserter = new RangeDataInserter3D(options.RangeDataInserterOptions);
+    }
+
+    /// <summary>
+    /// Inserts 'range_data_in_local' into the Submap collection.
+    /// 'local_from_gravity_aligned' is used for the orientation of new submaps so
+    /// that the z axis approximately aligns with gravity.
+    /// 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
+    /// submaps of the Submap collection.
+    /// </summary>
+    public List<Submap3D> InsertData(
+        RangeData rangeDataInLocal,
+        Quaternion localFromGravityAligned,
+        List<double> rotationalScanMatcherHistogramInGravity)
+    {
+        // Create new submap if needed
+        if (_submaps.Count == 0 ||
+            _submaps[^1].NumRangeData == _options.NumRangeData)
+        {
+            var localSubmapPose = new Rigid3d(
+                (Vector3)rangeDataInLocal.Origin,
+                localFromGravityAligned);
+            AddSubmap(localSubmapPose, rotationalScanMatcherHistogramInGravity.Count);
+        }
+
+        // Insert into all active submaps
+        foreach (var submap in _submaps)
+        {
+            submap.InsertData(
+                rangeDataInLocal,
+                _rangeDataInserter,
+                _options.HighResolutionMaxRange,
+                localFromGravityAligned,
+                rotationalScanMatcherHistogramInGravity);
+        }
+
+        // Finish the first submap if it has reached 2 * num_range_data
+        if (_submaps.Count > 0 && _submaps[0].NumRangeData == 2 * _options.NumRangeData)
+        {
+            _submaps[0].Finish();
+        }
+
+        return [.. _submaps];
+    }
+
+    /// <summary>
+    /// Gets the current active submaps.
+    /// </summary>
+    public List<Submap3D> Submaps()
+    {
+        return [.. _submaps];
+    }
+
+    /// <summary>
+    /// Adds a new submap to the collection.
+    /// </summary>
+    private void AddSubmap(Rigid3d localSubmapPose, int rotationalScanMatcherHistogramSize)
+    {
+        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");
+            }
+
+            // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use
+            // active submaps and their associated intensity hybrid grids for scan
+            // matching, we call `ForgetIntensityHybridGrid` once we remove the submap
+            // from active submaps and no longer need the intensity hybrid grid.
+            _submaps[0].ForgetIntensityHybridGrid();
+            _submaps.RemoveAt(0);
+        }
+
+        var initialRotationalScanMatcherHistogram = new List<double>(rotationalScanMatcherHistogramSize);
+        for (int i = 0; i < rotationalScanMatcherHistogramSize; i++)
+        {
+            initialRotationalScanMatcherHistogram.Add(0.0);
+        }
+
+        var submap = new Submap3D(
+            _options.HighResolution,
+            _options.LowResolution,
+            localSubmapPose,
+            initialRotationalScanMatcherHistogram);
+
+        _submaps.Add(submap);
+    }
+}

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

@@ -0,0 +1,848 @@
+/*
+ * 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;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D3D;
+
+/// <summary>
+/// Utility functions for HybridGrid indexing.
+/// </summary>
+internal static class HybridGridUtils
+{
+    /// <summary>
+    /// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat z-major index.
+    /// </summary>
+    public static int ToFlatIndex(Array3i index, int bits)
+    {
+        if (index.X < 0 || index.Y < 0 || index.Z < 0 ||
+            index.X >= (1 << bits) || index.Y >= (1 << bits) || index.Z >= (1 << bits))
+        {
+            throw new ArgumentOutOfRangeException(nameof(index), 
+                $"Index {index} is out of range for bits={bits}");
+        }
+        return (((index.Z << bits) + index.Y) << bits) + index.X;
+    }
+
+    /// <summary>
+    /// Converts a flat z-major 'index' to a 3-dimensional index with each dimension
+    /// from 0 to 2^'bits' - 1.
+    /// </summary>
+    public static Array3i To3DIndex(int index, int bits)
+    {
+        if (index < 0 || index >= (1 << (3 * bits)))
+        {
+            throw new ArgumentOutOfRangeException(nameof(index),
+                $"Index {index} is out of range for bits={bits}");
+        }
+        int mask = (1 << bits) - 1;
+        return new Array3i(
+            index & mask,
+            (index >> bits) & mask,
+            (index >> bits) >> bits);
+    }
+
+    /// <summary>
+    /// Checks if a value is the default value.
+    /// </summary>
+    public static bool IsDefaultValue<T>(T value) where T : struct
+    {
+        return EqualityComparer<T>.Default.Equals(value, default);
+    }
+
+    /// <summary>
+    /// Checks if a list is empty (default value for collections).
+    /// </summary>
+    public static bool IsDefaultValue<T>(List<T>? value)
+    {
+        return value == null || value.Count == 0;
+    }
+}
+
+/// <summary>
+/// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of
+/// type 'TValueType' in contiguous memory. Indices in each dimension are 0-based.
+/// </summary>
+internal class FlatGrid<TValueType> where TValueType : struct
+{
+    private const int kBits = 3; // Fixed at 3 bits = 8x8x8 = 512 cells
+    private const int kGridSize = 1 << kBits; // 8
+    private const int kTotalCells = 1 << (3 * kBits); // 512
+
+    private readonly TValueType[] _cells;
+
+    public FlatGrid()
+    {
+        _cells = new TValueType[kTotalCells];
+        // Values are already default-initialized
+    }
+
+    /// <summary>
+    /// Returns the number of voxels per dimension.
+    /// </summary>
+    public static int GridSize => kGridSize;
+
+    /// <summary>
+    /// Returns the value stored at 'index', each dimension of 'index' being
+    /// between 0 and grid_size() - 1.
+    /// </summary>
+    public TValueType GetValue(Array3i index)
+    {
+        return _cells[HybridGridUtils.ToFlatIndex(index, kBits)];
+    }
+
+    /// <summary>
+    /// Returns a reference to the value at 'index' to allow changing it.
+    /// </summary>
+    public ref TValueType GetMutableValue(Array3i index)
+    {
+        return ref _cells[HybridGridUtils.ToFlatIndex(index, kBits)];
+    }
+
+    /// <summary>
+    /// Iterator for iterating over all values not comparing equal to the default constructed value.
+    /// </summary>
+    public class Iterator : IEnumerator<(Array3i Index, TValueType Value)>
+    {
+        private readonly FlatGrid<TValueType> _grid;
+        private int _currentIndex;
+        private (Array3i Index, TValueType Value)? _current;
+
+        public Iterator(FlatGrid<TValueType> grid)
+        {
+            _grid = grid;
+            _currentIndex = -1;
+            MoveNext();
+        }
+
+        public bool MoveNext()
+        {
+            _currentIndex++;
+            while (_currentIndex < _grid._cells.Length)
+            {
+                var value = _grid._cells[_currentIndex];
+                if (!HybridGridUtils.IsDefaultValue(value))
+                {
+                    var index = HybridGridUtils.To3DIndex(_currentIndex, kBits);
+                    _current = (index, value);
+                    return true;
+                }
+                _currentIndex++;
+            }
+            _current = null;
+            return false;
+        }
+
+        public void Reset()
+        {
+            _currentIndex = -1;
+            _current = null;
+        }
+
+        public (Array3i Index, TValueType Value) Current => _current!.Value;
+
+        object IEnumerator.Current => Current;
+
+        public void Dispose()
+        {
+            GC.SuppressFinalize(this);
+        }
+    }
+
+    /// <summary>
+    /// Gets an enumerator for all non-default values.
+    /// </summary>
+    public Iterator GetEnumerator()
+    {
+        return new Iterator(this);
+    }
+}
+
+/// <summary>
+/// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type 'FlatGrid'.
+/// Wrapped grids are constructed on first access via 'GetMutableValue()'.
+/// This is a concrete implementation for the specific case: NestedGrid<FlatGrid<TValueType>, 3>
+/// </summary>
+internal class NestedGrid<TValueType> where TValueType : struct
+{
+    private const int kBits = 3; // Fixed at 3 bits = 8x8x8 = 512 meta cells
+    private const int kWrappedGridSize = 8; // FlatGrid<TValueType>.GridSize = 8
+    private readonly FlatGrid<TValueType>?[] _metaCells;
+
+    public NestedGrid()
+    {
+        _metaCells = new FlatGrid<TValueType>?[1 << (3 * kBits)]; // 512
+    }
+
+    public static int GridSize => kWrappedGridSize << kBits; // 8 * 8 = 64
+
+    public TValueType GetValue(Array3i index)
+    {
+        var metaIndex = NestedGrid<TValueType>.GetMetaIndex(index);
+        var metaCell = _metaCells[HybridGridUtils.ToFlatIndex(metaIndex, kBits)];
+        if (metaCell == null)
+        {
+            return default;
+        }
+        var innerIndex = index - metaIndex * kWrappedGridSize;
+        return metaCell.GetValue(innerIndex);
+    }
+
+    public ref TValueType GetMutableValue(Array3i index)
+    {
+        var metaIndex = NestedGrid<TValueType>.GetMetaIndex(index);
+        var flatIndex = HybridGridUtils.ToFlatIndex(metaIndex, kBits);
+        if (_metaCells[flatIndex] == null)
+        {
+            _metaCells[flatIndex] = new FlatGrid<TValueType>();
+        }
+        var innerIndex = index - metaIndex * kWrappedGridSize;
+        return ref _metaCells[flatIndex]!.GetMutableValue(innerIndex);
+    }
+
+    public IEnumerator<(Array3i Index, TValueType Value)> GetEnumerator()
+    {
+        return new Iterator(this);
+    }
+
+    private static Array3i GetMetaIndex(Array3i index)
+    {
+        if (index.X < 0 || index.Y < 0 || index.Z < 0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(index), $"Index {index} has negative components");
+        }
+        var metaIndex = index / kWrappedGridSize;
+        if (metaIndex.X >= (1 << kBits) || metaIndex.Y >= (1 << kBits) || metaIndex.Z >= (1 << kBits))
+        {
+            throw new ArgumentOutOfRangeException(nameof(index), $"Meta index {metaIndex} is out of range");
+        }
+        return metaIndex;
+    }
+
+    /// <summary>
+    /// Iterator for iterating over all non-default values.
+    /// </summary>
+    public class Iterator : IEnumerator<(Array3i Index, TValueType Value)>
+    {
+        private readonly NestedGrid<TValueType> _grid;
+        private int _currentMetaIndex;
+        private FlatGrid<TValueType>.Iterator? _nestedIterator;
+        private (Array3i Index, TValueType Value)? _current;
+
+        public Iterator(NestedGrid<TValueType> grid)
+        {
+            _grid = grid;
+            _currentMetaIndex = -1;
+            AdvanceToValidNestedIterator();
+        }
+
+        private void AdvanceToValidNestedIterator()
+        {
+            while (_currentMetaIndex < _grid._metaCells.Length - 1)
+            {
+                _currentMetaIndex++;
+                if (_currentMetaIndex >= _grid._metaCells.Length)
+                {
+                    _nestedIterator = null;
+                    _current = null;
+                    return;
+                }
+
+                var metaCell = _grid._metaCells[_currentMetaIndex];
+                if (metaCell != null)
+                {
+                    _nestedIterator = metaCell.GetEnumerator();
+                    if (_nestedIterator.MoveNext())
+                    {
+                        var (innerIndex, value) = _nestedIterator.Current;
+                        var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, kBits);
+                        var fullIndex = metaIndex * kWrappedGridSize + innerIndex;
+                        _current = (fullIndex, value);
+                        return;
+                    }
+                }
+            }
+            _nestedIterator = null;
+            _current = null;
+        }
+
+        public bool MoveNext()
+        {
+            if (_nestedIterator != null && _nestedIterator.MoveNext())
+            {
+                var (innerIndex, value) = _nestedIterator.Current;
+                var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, kBits);
+                var fullIndex = metaIndex * kWrappedGridSize + innerIndex;
+                _current = (fullIndex, value);
+                return true;
+            }
+
+            AdvanceToValidNestedIterator();
+            return _current.HasValue;
+        }
+
+        public void Reset()
+        {
+            _currentMetaIndex = -1;
+            _nestedIterator = null;
+            _current = null;
+            AdvanceToValidNestedIterator();
+        }
+
+        public (Array3i Index, TValueType Value) Current => _current!.Value;
+
+        object IEnumerator.Current => Current;
+
+        public void Dispose()
+        {
+            _nestedIterator?.Dispose();
+            _current = null;
+            GC.SuppressFinalize(this);
+        }
+    }
+}
+
+/// <summary>
+/// A grid consisting of 2x2x2 grids of type 'NestedGrid' initially. Wrapped grids
+/// are constructed on first access via 'GetMutableValue()'. If necessary, the grid
+/// grows to twice the size in each dimension. The range of indices is (almost)
+/// symmetric around the origin, i.e. negative indices are allowed.
+/// </summary>
+internal class DynamicGrid<TValueType> where TValueType : struct
+{
+    private const int kWrappedGridSize = 64; // NestedGrid<TValueType>.GridSize = 64
+    private int _bits; // Starts at 1 (2x2x2 = 8 meta cells)
+    private NestedGrid<TValueType>?[] _metaCells;
+
+    public DynamicGrid()
+    {
+        _bits = 1;
+        _metaCells = new NestedGrid<TValueType>?[8]; // 2^3 = 8
+    }
+
+    /// <summary>
+    /// Returns the current number of voxels per dimension.
+    /// </summary>
+    public int GridSize => kWrappedGridSize << _bits;
+
+    /// <summary>
+    /// Returns the value stored at 'index'.
+    /// </summary>
+    public TValueType GetValue(Array3i index)
+    {
+        var shiftedIndex = index + new Array3i(GridSize >> 1, GridSize >> 1, GridSize >> 1);
+        
+        // Check bounds using unsigned comparison for performance
+        if (shiftedIndex.X < 0 || shiftedIndex.Y < 0 || shiftedIndex.Z < 0 ||
+            shiftedIndex.X >= GridSize || shiftedIndex.Y >= GridSize || shiftedIndex.Z >= GridSize)
+        {
+            return default;
+        }
+
+        var metaIndex = GetMetaIndex(shiftedIndex);
+        var metaCell = _metaCells[HybridGridUtils.ToFlatIndex(metaIndex, _bits)];
+        if (metaCell == null)
+        {
+            return default;
+        }
+        var innerIndex = shiftedIndex - metaIndex * kWrappedGridSize;
+        return metaCell.GetValue(innerIndex);
+    }
+
+    /// <summary>
+    /// Returns a reference to the value at 'index' to allow changing it, dynamically
+    /// growing the DynamicGrid and constructing new NestedGrids as needed.
+    /// </summary>
+    public ref TValueType GetMutableValue(Array3i index)
+    {
+        var shiftedIndex = index + new Array3i(GridSize >> 1, GridSize >> 1, GridSize >> 1);
+        
+        // Check bounds using unsigned comparison for performance
+        if (shiftedIndex.X < 0 || shiftedIndex.Y < 0 || shiftedIndex.Z < 0 ||
+            shiftedIndex.X >= GridSize || shiftedIndex.Y >= GridSize || shiftedIndex.Z >= GridSize)
+        {
+            // SAFEGUARD: Store old bits to detect if Grow() actually increased size
+            var oldBits = _bits;
+            
+            // Grow the grid
+            Grow();
+            
+            // SAFEGUARD: Check if Grow() actually increased size (prevent infinite recursion)
+            if (_bits == oldBits)
+            {
+                throw new InvalidOperationException(
+                    $"Cannot grow grid further. Index {index} is out of bounds even after grow attempt. " +
+                    $"Current bits={_bits}, GridSize={GridSize}, shiftedIndex={shiftedIndex}");
+            }
+            
+            // SAFEGUARD: Recalculate shiftedIndex after grow and check bounds again
+            shiftedIndex = index + new Array3i(GridSize >> 1, GridSize >> 1, GridSize >> 1);
+            
+            // SAFEGUARD: If still out of bounds after grow, throw exception instead of infinite recursion
+            if (shiftedIndex.X < 0 || shiftedIndex.Y < 0 || shiftedIndex.Z < 0 ||
+                shiftedIndex.X >= GridSize || shiftedIndex.Y >= GridSize || shiftedIndex.Z >= GridSize)
+            {
+                throw new ArgumentOutOfRangeException(nameof(index),
+                    $"Index {index} is out of bounds even after growing grid to maximum size. " +
+                    $"GridSize={GridSize}, shiftedIndex={shiftedIndex}, bits={_bits}");
+            }
+            
+            return ref GetMutableValue(index); // Recursive call after grow (now safe)
+        }
+
+        var metaIndex = GetMetaIndex(shiftedIndex);
+        var flatIndex = HybridGridUtils.ToFlatIndex(metaIndex, _bits);
+        if (_metaCells[flatIndex] == null)
+        {
+            _metaCells[flatIndex] = new NestedGrid<TValueType>();
+        }
+        var innerIndex = shiftedIndex - metaIndex * kWrappedGridSize;
+        return ref _metaCells[flatIndex]!.GetMutableValue(innerIndex);
+    }
+
+    /// <summary>
+    /// Iterator for iterating over all values not comparing equal to the default constructed value.
+    /// </summary>
+    public class Iterator : IEnumerator<(Array3i Index, TValueType Value)>
+    {
+        private readonly DynamicGrid<TValueType> _grid;
+        private readonly int _bits;
+        private int _currentMetaIndex;
+        private IEnumerator<(Array3i Index, TValueType Value)>? _nestedIterator;
+        private (Array3i Index, TValueType Value)? _current;
+
+        public Iterator(DynamicGrid<TValueType> grid)
+        {
+            _grid = grid;
+            _bits = grid._bits;
+            _currentMetaIndex = -1;
+            AdvanceToValidNestedIterator();
+        }
+
+        private void AdvanceToValidNestedIterator()
+        {
+            while (_currentMetaIndex < _grid._metaCells.Length - 1)
+            {
+                _currentMetaIndex++;
+                if (_currentMetaIndex >= _grid._metaCells.Length)
+                {
+                    _nestedIterator = null;
+                    _current = null;
+                    return;
+                }
+
+                var metaCell = _grid._metaCells[_currentMetaIndex];
+                if (metaCell != null)
+                {
+                    _nestedIterator = metaCell.GetEnumerator();
+                    if (_nestedIterator.MoveNext())
+                    {
+                        var (innerIndex, value) = _nestedIterator.Current;
+                        var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, _bits);
+                        var shiftedIndex = metaIndex * kWrappedGridSize + innerIndex;
+                        var originalIndex = shiftedIndex - new Array3i(
+                            (1 << (_bits - 1)) * kWrappedGridSize,
+                            (1 << (_bits - 1)) * kWrappedGridSize,
+                            (1 << (_bits - 1)) * kWrappedGridSize);
+                        _current = (originalIndex, value);
+                        return;
+                    }
+                }
+            }
+            _nestedIterator = null;
+            _current = null;
+        }
+
+        public bool MoveNext()
+        {
+            if (_nestedIterator != null && _nestedIterator.MoveNext())
+            {
+                var (innerIndex, value) = _nestedIterator.Current;
+                var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, _bits);
+                var shiftedIndex = metaIndex * kWrappedGridSize + innerIndex;
+                var originalIndex = shiftedIndex - new Array3i(
+                    (1 << (_bits - 1)) * kWrappedGridSize,
+                    (1 << (_bits - 1)) * kWrappedGridSize,
+                    (1 << (_bits - 1)) * kWrappedGridSize);
+                _current = (originalIndex, value);
+                return true;
+            }
+
+            AdvanceToValidNestedIterator();
+            return _current.HasValue;
+        }
+
+        public void Reset()
+        {
+            _currentMetaIndex = -1;
+            _nestedIterator = null;
+            _current = null;
+            AdvanceToValidNestedIterator();
+        }
+
+        public (Array3i Index, TValueType Value) Current => _current!.Value;
+
+        object IEnumerator.Current => Current;
+
+        public void Dispose()
+        {
+            _nestedIterator?.Dispose();
+            _current = null;
+            GC.SuppressFinalize(this);
+        }
+
+        /// <summary>
+        /// Advances iterator to end (for end() implementation).
+        /// </summary>
+        public void AdvanceToEnd()
+        {
+            _currentMetaIndex = _grid._metaCells.Length;
+            _nestedIterator = null;
+            _current = null;
+        }
+    }
+
+    /// <summary>
+    /// Gets an enumerator for all non-default values.
+    /// </summary>
+    public Iterator GetEnumerator()
+    {
+        return new Iterator(this);
+    }
+
+    private Array3i GetMetaIndex(Array3i index)
+    {
+        if (index.X < 0 || index.Y < 0 || index.Z < 0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(index), $"Index {index} has negative components");
+        }
+        var metaIndex = index / kWrappedGridSize;
+        if (metaIndex.X >= (1 << _bits) || metaIndex.Y >= (1 << _bits) || metaIndex.Z >= (1 << _bits))
+        {
+            throw new ArgumentOutOfRangeException(nameof(index), $"Meta index {metaIndex} is out of range");
+        }
+        return metaIndex;
+    }
+
+    /// <summary>
+    /// Grows this grid by a factor of 2 in each of the 3 dimensions.
+    /// </summary>
+    private void Grow()
+    {
+        var newBits = _bits + 1;
+        if (newBits > 8)
+        {
+            throw new InvalidOperationException($"Cannot grow grid beyond bits=8 (current bits={_bits})");
+        }
+
+        var newMetaCells = new NestedGrid<TValueType>?[8 * _metaCells.Length];
+        
+        for (int z = 0; z < (1 << _bits); z++)
+        {
+            for (int y = 0; y < (1 << _bits); y++)
+            {
+                for (int x = 0; x < (1 << _bits); x++)
+                {
+                    var originalMetaIndex = new Array3i(x, y, z);
+                    var newMetaIndex = originalMetaIndex + new Array3i(1 << (_bits - 1), 1 << (_bits - 1), 1 << (_bits - 1));
+                    var originalFlatIndex = HybridGridUtils.ToFlatIndex(originalMetaIndex, _bits);
+                    var newFlatIndex = HybridGridUtils.ToFlatIndex(newMetaIndex, newBits);
+                    newMetaCells[newFlatIndex] = _metaCells[originalFlatIndex];
+                }
+            }
+        }
+
+        _metaCells = newMetaCells;
+        _bits = newBits;
+    }
+}
+
+/// <summary>
+/// Represents a 3D grid as a wide, shallow tree.
+/// This is the base class for HybridGrid and IntensityHybridGrid.
+/// </summary>
+/// <remarks>
+/// Creates a new tree-based grid with voxels having edge length 'resolution'
+/// around the origin which becomes the center of the cell at index (0, 0, 0).
+/// </remarks>
+public class HybridGridBase<TValueType>(double resolution) where TValueType : struct
+{
+    private readonly DynamicGrid<TValueType> _grid = new();
+
+    /// <summary>
+    /// Returns the resolution (edge length of each voxel).
+    /// </summary>
+    public double Resolution => resolution;
+
+    /// <summary>
+    /// Returns the value stored at 'index'.
+    /// </summary>
+    protected TValueType GetValue(Array3i index)
+    {
+        return _grid.GetValue(index);
+    }
+
+    /// <summary>
+    /// Returns a reference to the value at 'index' to allow changing it.
+    /// </summary>
+    protected ref TValueType GetMutableValue(Array3i index)
+    {
+        return ref _grid.GetMutableValue(index);
+    }
+
+    /// <summary>
+    /// Returns the index of the cell containing the 'point'. Indices are integer
+    /// vectors identifying cells, for this the coordinates are rounded to the next
+    /// multiple of the resolution.
+    /// </summary>
+    public Array3i GetCellIndex(Vector3 point)
+    {
+        var index = new Vector3(point.X / resolution, point.Y / resolution, point.Z / resolution);
+        return new Array3i(
+            (int)System.Math.Round(index.X),
+            (int)System.Math.Round(index.Y),
+            (int)System.Math.Round(index.Z));
+    }
+
+    /// <summary>
+    /// Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1).
+    /// </summary>
+    public static Array3i GetOctant(int i)
+    {
+        if (i < 0 || i >= 8)
+        {
+            throw new ArgumentOutOfRangeException(nameof(i), $"Octant index {i} must be in range [0, 7]");
+        }
+        return new Array3i(
+            (i & 1) != 0 ? 1 : 0,
+            (i & 2) != 0 ? 1 : 0,
+            (i & 4) != 0 ? 1 : 0);
+    }
+
+    /// <summary>
+    /// Returns the center of the cell at 'index'.
+    /// </summary>
+    public Vector3 GetCenterOfCell(Array3i index)
+    {
+        return new Vector3(
+            index.X * resolution,
+            index.Y * resolution,
+            index.Z * resolution);
+    }
+
+    /// <summary>
+    /// Gets an enumerator for all non-default values.
+    /// </summary>
+    public IEnumerator<(Array3i Index, TValueType Value)> GetEnumerator()
+    {
+        return _grid.GetEnumerator();
+    }
+}
+
+/// <summary>
+/// A grid containing probability values stored using 15 bits, and an update
+/// marker per voxel.
+/// Points are expected to be close to the origin. Points far from the origin
+/// require the grid to grow dynamically. For centimeter resolution, points
+/// can only be tens of meters from the origin.
+/// The hard limit of cell indexes is +/- 8192 around the origin.
+/// </summary>
+public class HybridGrid : HybridGridBase<ushort>
+{
+    private const ushort kUpdateMarker = (ushort)(1u << 15);
+    private readonly List<Array3i> _updateIndices;
+
+    /// <summary>
+    /// Creates a new HybridGrid with the specified resolution.
+    /// </summary>
+    public HybridGrid(double resolution) : base(resolution)
+    {
+        _updateIndices = [];
+    }
+
+    /// <summary>
+    /// Creates a HybridGrid from a proto.
+    /// </summary>
+    public HybridGrid(Models.Mapping.HybridGrid proto) : base(proto.Resolution)
+    {
+        _updateIndices = [];
+        
+        if (proto.XIndices == null || proto.YIndices == null || proto.ZIndices == null || proto.Values == null)
+        {
+            throw new ArgumentException("Proto must have valid indices and values", nameof(proto));
+        }
+
+        if (proto.XIndices.Count != proto.Values.Count ||
+            proto.YIndices.Count != proto.Values.Count ||
+            proto.ZIndices.Count != proto.Values.Count)
+        {
+            throw new ArgumentException(
+                $"Proto indices and values count mismatch: X={proto.XIndices.Count}, Y={proto.YIndices.Count}, Z={proto.ZIndices.Count}, Values={proto.Values.Count}",
+                nameof(proto));
+        }
+
+        for (int i = 0; i < proto.Values.Count; i++)
+        {
+            var index = new Array3i(proto.XIndices[i], proto.YIndices[i], proto.ZIndices[i]);
+            var probability = ProbabilityValues.ValueToProbability((ushort)proto.Values[i]);
+            SetProbability(index, probability);
+        }
+    }
+
+    /// <summary>
+    /// Sets the probability of the cell at 'index' to the given 'probability'.
+    /// </summary>
+    public void SetProbability(Array3i index, double probability)
+    {
+        var clampedProbability = ProbabilityValues.ClampProbability(probability);
+        var value = ProbabilityValues.ProbabilityToValue(clampedProbability);
+        GetMutableValue(index) = value;
+    }
+
+    /// <summary>
+    /// Finishes the update sequence by removing update markers from all updated cells.
+    /// </summary>
+    public void FinishUpdate()
+    {
+        foreach (var index in _updateIndices)
+        {
+            ref var cell = ref GetMutableValue(index);
+            if (cell >= kUpdateMarker)
+            {
+                cell = (ushort)(cell - kUpdateMarker);
+            }
+        }
+        _updateIndices.Clear();
+    }
+
+    /// <summary>
+    /// Applies the 'table' (lookup table from ComputeLookupTableToApplyOdds) to the
+    /// probability of the cell at '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.
+    /// 
+    /// If this is the first call to ApplyLookupTable() for the specified cell, its value
+    /// will be set to probability corresponding to the table entry.
+    /// </summary>
+    public bool ApplyLookupTable(Array3i index, List<ushort> table)
+    {
+        if (table == null || table.Count != kUpdateMarker)
+        {
+            throw new ArgumentException($"Table must have size {kUpdateMarker}", nameof(table));
+        }
+
+        ref var cell = ref GetMutableValue(index);
+        if (cell >= kUpdateMarker)
+        {
+            return false; // Already updated
+        }
+
+        _updateIndices.Add(index);
+        cell = table[cell];
+        return true;
+    }
+
+    /// <summary>
+    /// Returns the probability of the cell with 'index'.
+    /// </summary>
+    public double GetProbability(Array3i index)
+    {
+        return ProbabilityValues.ValueToProbability(GetValue(index));
+    }
+
+    /// <summary>
+    /// Returns true if the probability at the specified 'index' is known.
+    /// </summary>
+    public bool IsKnown(Array3i index)
+    {
+        return GetValue(index) != 0;
+    }
+
+    /// <summary>
+    /// Converts this HybridGrid to a proto.
+    /// </summary>
+    public Models.Mapping.HybridGrid ToProto()
+    {
+        if (_updateIndices.Count > 0)
+        {
+            throw new InvalidOperationException(
+                "Serializing a grid during an update is not supported. Finish the update first.");
+        }
+
+        var result = new Models.Mapping.HybridGrid
+        {
+            Resolution = Resolution,
+            XIndices = [],
+            YIndices = [],
+            ZIndices = [],
+            Values = []
+        };
+
+        foreach (var (index, value) in this)
+        {
+            result.XIndices.Add(index.X);
+            result.YIndices.Add(index.Y);
+            result.ZIndices.Add(index.Z);
+            result.Values.Add(value);
+        }
+
+        return result;
+    }
+}
+
+/// <summary>
+/// Average intensity data structure for IntensityHybridGrid.
+/// </summary>
+public struct AverageIntensityData
+{
+    public double Sum { get; set; }
+    public int Count { get; set; }
+}
+
+/// <summary>
+/// Hybrid grid for storing intensity data (average intensity per voxel).
+/// </summary>
+/// <remarks>
+/// Creates a new IntensityHybridGrid with the specified resolution.
+/// </remarks>
+public class IntensityHybridGrid(double resolution) : HybridGridBase<AverageIntensityData>(resolution)
+{
+
+    /// <summary>
+    /// Adds intensity value to the cell at 'index'.
+    /// </summary>
+    public void AddIntensity(Array3i index, double intensity)
+    {
+        ref var cell = ref GetMutableValue(index);
+        cell.Count += 1;
+        cell.Sum += intensity;
+    }
+
+    /// <summary>
+    /// Returns the average intensity of the cell at 'index'.
+    /// </summary>
+    public double GetIntensity(Array3i index)
+    {
+        var cell = GetValue(index);
+        if (cell.Count == 0)
+        {
+            return 0.0;
+        }
+        return cell.Sum / cell.Count;
+    }
+}

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

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

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

@@ -0,0 +1,263 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+using RotationalScanMatcher = CartographerSharp.Mapping.Internal.D3D.ScanMatching.RotationalScanMatcher;
+
+namespace CartographerSharp.Mapping.D3D;
+
+/// <summary>
+/// 3D Submap implementation.
+/// </summary>
+public class Submap3D : Submap
+{
+    private HybridGrid _highResolutionHybridGrid;
+    private HybridGrid _lowResolutionHybridGrid;
+    private IntensityHybridGrid? _highResolutionIntensityHybridGrid;
+    private List<double> _rotationalScanMatcherHistogram;
+
+    public Submap3D(
+        double highResolution,
+        double lowResolution,
+        Rigid3d localSubmapPose,
+        List<double> rotationalScanMatcherHistogram)
+        : base(localSubmapPose)
+    {
+        _highResolutionHybridGrid = new HybridGrid(highResolution);
+        _lowResolutionHybridGrid = new HybridGrid(lowResolution);
+        _highResolutionIntensityHybridGrid = new IntensityHybridGrid(highResolution);
+        _rotationalScanMatcherHistogram = [.. rotationalScanMatcherHistogram];
+    }
+
+    public Submap3D(Models.Mapping.Submap3D proto)
+        : base((Rigid3d)proto.LocalPose)
+    {
+        // Initialize with default values first
+        _highResolutionHybridGrid = new HybridGrid(0.05f); // Default resolution
+        _lowResolutionHybridGrid = new HybridGrid(0.05f);
+        _rotationalScanMatcherHistogram = [];
+        UpdateFromProto(proto);
+    }
+
+    /// <summary>
+    /// Gets the high resolution hybrid grid.
+    /// </summary>
+    public HybridGrid HighResolutionHybridGrid => _highResolutionHybridGrid;
+
+    /// <summary>
+    /// Gets the low resolution hybrid grid.
+    /// </summary>
+    public HybridGrid LowResolutionHybridGrid => _lowResolutionHybridGrid;
+
+    /// <summary>
+    /// Gets the high resolution intensity hybrid grid.
+    /// </summary>
+    public IntensityHybridGrid? HighResolutionIntensityHybridGrid => _highResolutionIntensityHybridGrid;
+
+    /// <summary>
+    /// Forgets the intensity hybrid grid to reduce memory usage.
+    /// </summary>
+    public void ForgetIntensityHybridGrid()
+    {
+        _highResolutionIntensityHybridGrid = null;
+    }
+
+    /// <summary>
+    /// Gets the rotational scan matcher histogram.
+    /// </summary>
+    public IReadOnlyList<double> RotationalScanMatcherHistogram => _rotationalScanMatcherHistogram;
+
+    /// <summary>
+    /// Insert 'range_data' into this submap using 'range_data_inserter'. The
+    /// submap must not be finished yet.
+    /// </summary>
+    public void InsertData(
+        RangeData rangeDataInLocal,
+        RangeDataInserter3D rangeDataInserter,
+        double highResolutionMaxRange,
+        Quaternion localFromGravityAligned,
+        List<double> scanHistogramInGravity)
+    {
+        if (InsertionFinished)
+        {
+            throw new InvalidOperationException("Cannot insert data into finished submap");
+        }
+
+        // Transform range data into submap frame
+        var submapInverse = LocalPose.Inverse();
+        var submapInverseFloat = new Rigid3f(
+            (Vector3)submapInverse.Translation,
+            submapInverse.Rotation);
+        var transformedRangeData = RangeDataOperations.Transform(rangeDataInLocal, submapInverseFloat);
+
+        // Filter range data by max range for high resolution grid
+        var filteredRangeData = FilterRangeDataByMaxRange(transformedRangeData, highResolutionMaxRange);
+
+        // Insert into high resolution grid with intensity
+        rangeDataInserter.Insert(
+            filteredRangeData,
+            _highResolutionHybridGrid,
+            _highResolutionIntensityHybridGrid);
+
+        // Insert into low resolution grid without intensity
+        rangeDataInserter.Insert(
+            transformedRangeData,
+            _lowResolutionHybridGrid,
+            null);
+
+        NumRangeData++;
+
+        // Update rotational scan matcher histogram
+        // C++: yaw_in_submap_from_gravity = GetYaw(local_pose().inverse().rotation() * local_from_gravity_aligned)
+        //      rotational_scan_matcher_histogram_ += RotationalScanMatcher::RotateHistogram(scan_histogram_in_gravity, yaw_in_submap_from_gravity)
+        var yawInSubmapFromGravity = TransformOperations.GetYaw(submapInverse.Rotation * localFromGravityAligned);
+        
+        if (_rotationalScanMatcherHistogram.Count == scanHistogramInGravity.Count)
+        {
+            var rotatedHistogram = RotationalScanMatcher.RotateHistogram(
+                scanHistogramInGravity.ToArray(),
+                yawInSubmapFromGravity);
+
+            for (int i = 0; i < rotatedHistogram.Length; i++)
+            {
+                _rotationalScanMatcherHistogram[i] += rotatedHistogram[i];
+            }
+        }
+        else if (scanHistogramInGravity.Count > 0)
+        {
+            // Log warning for histogram size mismatch - this can cause rotational matching failures
+            System.Diagnostics.Debug.WriteLine(
+                $"Warning: Histogram size mismatch in Submap3D.InsertData: " +
+                $"expected {_rotationalScanMatcherHistogram.Count}, got {scanHistogramInGravity.Count}. " +
+                "Rotational scan matching may not work correctly.");
+        }
+    }
+
+    /// <summary>
+    /// Finishes the submap.
+    /// </summary>
+    public void Finish()
+    {
+        if (InsertionFinished)
+        {
+            throw new InvalidOperationException("Submap is already finished");
+        }
+        InsertionFinished = true;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public override Models.Mapping.Submap ToProto(bool includeGridData)
+    {
+        Models.Mapping.HybridGrid highResGrid;
+        Models.Mapping.HybridGrid lowResGrid;
+
+        if (includeGridData)
+        {
+            highResGrid = _highResolutionHybridGrid.ToProto();
+            lowResGrid = _lowResolutionHybridGrid.ToProto();
+        }
+        else
+        {
+            // Create empty grids with just resolution
+            highResGrid = new Models.Mapping.HybridGrid
+            {
+                Resolution = _highResolutionHybridGrid.Resolution,
+                XIndices = [],
+                YIndices = [],
+                ZIndices = [],
+                Values = []
+            };
+            lowResGrid = new Models.Mapping.HybridGrid
+            {
+                Resolution = _lowResolutionHybridGrid.Resolution,
+                XIndices = [],
+                YIndices = [],
+                ZIndices = [],
+                Values = []
+            };
+        }
+
+        var submap3D = new Models.Mapping.Submap3D(
+            (Rigid3dProto)LocalPose,
+            NumRangeData,
+            InsertionFinished,
+            highResGrid,
+            lowResGrid,
+            [.. _rotationalScanMatcherHistogram]);
+
+        // Note: SubmapId will be set by caller
+        return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), null, submap3D);
+    }
+
+    /// <summary>
+    /// Updates from proto representation.
+    /// </summary>
+    public override void UpdateFromProto(Models.Mapping.Submap proto)
+    {
+        if (!proto.Submap3D.HasValue)
+        {
+            throw new ArgumentException("Proto must contain Submap3D", nameof(proto));
+        }
+
+        UpdateFromProto(proto.Submap3D.Value);
+    }
+
+    private void UpdateFromProto(Models.Mapping.Submap3D submap3D)
+    {
+        NumRangeData = submap3D.NumRangeData;
+        InsertionFinished = submap3D.Finished;
+
+        if (submap3D.HighResolutionHybridGrid.Values != null && submap3D.HighResolutionHybridGrid.Values.Count > 0)
+        {
+            _highResolutionHybridGrid = new HybridGrid(submap3D.HighResolutionHybridGrid);
+        }
+
+        if (submap3D.LowResolutionHybridGrid.Values != null && submap3D.LowResolutionHybridGrid.Values.Count > 0)
+        {
+            _lowResolutionHybridGrid = new HybridGrid(submap3D.LowResolutionHybridGrid);
+        }
+
+        _rotationalScanMatcherHistogram = [.. submap3D.RotationalScanMatcherHistogram ?? []];
+    }
+
+    /// <summary>
+    /// Filters 'range_data', retaining only the returns that have no more than
+    /// 'max_range' distance from the origin. Removes misses.
+    /// </summary>
+    public static RangeData FilterRangeDataByMaxRange(RangeData rangeData, double maxRange)
+    {
+        var filteredReturns = new List<RangefinderPoint>();
+        
+        foreach (var point in rangeData.Returns.Points)
+        {
+            var distance = Vector3.Distance(point.Position, rangeData.Origin);
+            if (distance <= maxRange)
+            {
+                filteredReturns.Add(point);
+            }
+        }
+
+        return new RangeData(
+            rangeData.Origin,
+            new PointCloud(filteredReturns),
+            new PointCloud()); // Misses are removed
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/IMapBuilder.cs

@@ -0,0 +1,100 @@
+/*
+ * Copyright 2017 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.Mapping;
+using CartographerSharp.Transform;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// This interface is used for both library and RPC implementations.
+/// Implementations wire up the complete SLAM stack.
+/// </summary>
+public interface IMapBuilder : IDisposable
+{
+    /// <summary>
+    /// Creates a new trajectory builder and returns its index.
+    /// </summary>
+    int AddTrajectoryBuilder(
+        HashSet<ITrajectoryBuilder.SensorId> expectedSensorIds,
+        TrajectoryBuilderOptions trajectoryOptions);
+
+    /// <summary>
+    /// Creates a new trajectory and returns its index. Querying the trajectory
+    /// builder for it will return 'null'.
+    /// </summary>
+    int AddTrajectoryForDeserialization(
+        TrajectoryBuilderOptionsWithSensorIds optionsWithSensorIdsProto);
+
+    /// <summary>
+    /// Returns the 'ITrajectoryBuilder' corresponding to the specified
+    /// 'trajectory_id' or 'null' if the trajectory has no corresponding builder.
+    /// </summary>
+    ITrajectoryBuilder? GetTrajectoryBuilder(int trajectoryId);
+
+    /// <summary>
+    /// Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,
+    /// i.e. no further sensor data is expected.
+    /// </summary>
+    void FinishTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an
+    /// error string on failure, or an empty string on success.
+    /// </summary>
+    string SubmapToProto(SubmapId submapId, out Models.Mapping.SubmapQuery.Response response);
+
+    /// <summary>
+    /// Serializes the current state to a proto stream. If
+    /// 'include_unfinished_submaps' is set to true, unfinished submaps, i.e.
+    /// submaps that have not yet received all rangefinder data insertions, will
+    /// be included in the serialized state.
+    /// </summary>
+    void SerializeState(bool includeUnfinishedSubmaps, IO.IProtoStreamWriter writer);
+
+    /// <summary>
+    /// Serializes the current state to a proto stream file on the host system.
+    /// Returns true if the file was successfully written.
+    /// </summary>
+    bool SerializeStateToFile(bool includeUnfinishedSubmaps, string filename);
+
+    /// <summary>
+    /// Loads the SLAM state from a proto stream. Returns the remapping of new trajectory_ids.
+    /// </summary>
+    Dictionary<int, int> LoadState(IO.IProtoStreamReader reader, bool loadFrozenState);
+
+    /// <summary>
+    /// Loads the SLAM state from a pbstream file. Returns the remapping of new trajectory_ids.
+    /// </summary>
+    Dictionary<int, int> LoadStateFromFile(string filename, bool loadFrozenState);
+
+    /// <summary>
+    /// Starts relocalization (match C++ MapBuilder::StartRelocalization).
+    /// Sets pose graph localization callback and initial poses so constraint builder
+    /// uses them when finding constraints (MaybeAddLocalizationConstraint).
+    /// Call after AddTrajectoryBuilder when starting localization (e.g. xloc calls
+    /// StartRelocalization(initial_poses, callback) after AddTrajectory).
+    /// </summary>
+    /// <param name="initialPoses">Initial poses in global frame (e.g. from MCL).</param>
+    /// <param name="callback">Invoked when relocalization succeeds or when max nodes searched without success (trajectoryId=-1).</param>
+    void StartRelocalization(IReadOnlyList<Rigid3d> initialPoses, Action<int, long, Rigid3d>? callback);
+
+    int NumTrajectoryBuilders { get; }
+
+    IPoseGraph PoseGraph { get; }
+
+    List<TrajectoryBuilderOptionsWithSensorIds> GetAllTrajectoryBuilderOptions();
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/IPoseExtrapolator.cs

@@ -0,0 +1,100 @@
+/*
+ * Copyright 2017 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Interface for pose extrapolation.
+/// </summary>
+public interface IPoseExtrapolator
+{
+    /// <summary>
+    /// Returns the time of the last added pose or long.MinValue if no pose was added yet.
+    /// </summary>
+    long GetLastPoseTime();
+
+    /// <summary>
+    /// Returns the time of the last extrapolated pose.
+    /// </summary>
+    long GetLastExtrapolatedTime();
+
+    /// <summary>
+    /// Adds a pose at the given time.
+    /// </summary>
+    void AddPose(long time, Rigid3d pose);
+
+    /// <summary>
+    /// Adds IMU data.
+    /// </summary>
+    void AddImuData(ImuData imuData);
+
+    /// <summary>
+    /// Adds odometry data.
+    /// </summary>
+    void AddOdometryData(OdometryData odometryData);
+
+    /// <summary>
+    /// Extrapolates pose to the given time.
+    /// </summary>
+    Rigid3d ExtrapolatePose(long time);
+
+    /// <summary>
+    /// Extrapolates pose with low-pass filter to reduce jitter during direction changes.
+    /// Match C++: ExtrapolatePose_filter
+    /// </summary>
+    Rigid3d ExtrapolatePoseFilter(long time);
+
+    /// <summary>
+    /// Extrapolates poses with gravity for multiple times.
+    /// </summary>
+    ExtrapolationResult ExtrapolatePosesWithGravity(List<long> times);
+
+    /// <summary>
+    /// Returns the current gravity alignment estimate as a rotation from
+    /// the tracking frame into a gravity aligned frame.
+    /// </summary>
+    Quaternion EstimateGravityOrientation(long time);
+}
+
+/// <summary>
+/// Result of pose extrapolation with gravity.
+/// </summary>
+public struct ExtrapolationResult
+{
+    /// <summary>
+    /// The poses for the requested times at index 0 to N-1 (previous poses).
+    /// </summary>
+    public List<Rigid3f> PreviousPoses { get; set; }
+    
+    /// <summary>
+    /// The pose for the requested time at index N (current pose).
+    /// </summary>
+    public Rigid3d CurrentPose { get; set; }
+    
+    /// <summary>
+    /// Current velocity estimate.
+    /// </summary>
+    public Vector3 CurrentVelocity { get; set; }
+    
+    /// <summary>
+    /// Gravity alignment estimate as a rotation from the tracking frame into a gravity aligned frame.
+    /// </summary>
+    public Quaternion GravityFromTracking { get; set; }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/IPoseGraph.cs

@@ -0,0 +1,460 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Interface for pose graph operations.
+/// </summary>
+public interface IPoseGraph
+{
+    /// <summary>
+    /// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
+    /// pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
+    /// 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
+    /// </summary>
+    public struct Constraint(SubmapId submapId, NodeId nodeId, Constraint.Pose pose, Constraint.Tag tag, double score = 0.0, Constraint.State state = Constraint.State.Enabled)
+    {
+        /// <summary>
+        /// Constraint pose information.
+        /// </summary>
+        public struct Pose(Rigid3d zbarIj, double translationWeight, double rotationWeight)
+        {
+            public Rigid3d ZbarIj { get; set; } = zbarIj;
+            public double TranslationWeight { get; set; } = translationWeight;
+            public double RotationWeight { get; set; } = rotationWeight;
+        }
+
+        public SubmapId SubmapId { get; set; } = submapId;
+        public NodeId NodeId { get; set; } = nodeId;
+
+        /// <summary>
+        /// Pose of the node 'j' relative to submap 'i'.
+        /// </summary>
+        public Pose ConstraintPose { get; set; } = pose;
+
+        /// <summary>
+        /// Differentiates between intra-submap (where node 'j' was inserted into
+        /// submap 'i') and inter-submap constraints (where node 'j' was not inserted
+        /// into submap 'i').
+        /// </summary>
+        public enum Tag
+        {
+            IntraSubmap,
+            InterSubmap
+        }
+
+        public Tag ConstraintTag { get; set; } = tag;
+
+        /// <summary>
+        /// Match C++: score field for constraint quality.
+        /// </summary>
+        public double Score { get; set; } = score;
+
+        /// <summary>
+        /// Match C++: state enum for enabled/disabled constraints.
+        /// </summary>
+        public enum State
+        {
+            Enabled,
+            Disabled
+        }
+
+        public State ConstraintState { get; set; } = state;
+    }
+
+    /// <summary>
+    /// Landmark node information.
+    /// </summary>
+    public struct LandmarkNode(
+        List<LandmarkNode.LandmarkObservation>? landmarkObservations = null,
+        Rigid3d? globalLandmarkPose = null,
+        bool frozen = false)
+    {
+        /// <summary>
+        /// Landmark observation.
+        /// </summary>
+        public struct LandmarkObservation(
+            int trajectoryId,
+            long time,
+            Rigid3d landmarkToTrackingTransform,
+            double translationWeight,
+            double rotationWeight)
+        {
+            public int TrajectoryId { get; set; } = trajectoryId;
+            public long Time { get; set; } = time;
+            public Rigid3d LandmarkToTrackingTransform { get; set; } = landmarkToTrackingTransform;
+            public double TranslationWeight { get; set; } = translationWeight;
+            public double RotationWeight { get; set; } = rotationWeight;
+        }
+
+        public List<LandmarkObservation> LandmarkObservations { get; set; } = landmarkObservations ?? [];
+        public Rigid3d? GlobalLandmarkPose { get; set; } = globalLandmarkPose;
+        public bool Frozen { get; set; } = frozen;
+    }
+
+    /// <summary>
+    /// Submap pose information.
+    /// </summary>
+    public struct SubmapPose(int version, Rigid3d pose)
+    {
+        public int Version { get; set; } = version;
+        public Rigid3d Pose { get; set; } = pose;
+    }
+
+    /// <summary>
+    /// Submap data with pose.
+    /// </summary>
+    public struct SubmapData(Submap? submap, Rigid3d pose)
+    {
+        public Submap? Submap { get; set; } = submap;
+        public Rigid3d Pose { get; set; } = pose;
+    }
+
+    /// <summary>
+    /// Trajectory data.
+    /// </summary>
+    public struct TrajectoryData(
+        double gravityConstant = 9.8,
+        Quaternion? imuCalibration = null,
+        Rigid3d? fixedFrameOriginInMap = null)
+    {
+        public double GravityConstant { get; set; } = gravityConstant;
+        public Quaternion ImuCalibration { get; set; } = imuCalibration ?? Quaternion.Identity;
+        public Rigid3d? FixedFrameOriginInMap { get; set; } = fixedFrameOriginInMap;
+    }
+
+    /// <summary>
+    /// Trajectory state enumeration.
+    /// </summary>
+    public enum TrajectoryState
+    {
+        Active,
+        Finished,
+        Frozen,
+        Deleted
+    }
+
+    /// <summary>
+    /// Gets the total number of work items added to the work queue.
+    /// </summary>
+    public int WorkItemsAdded { get; }
+
+    /// <summary>
+    /// Gets the total number of work items completed by the work queue.
+    /// </summary>
+    public int WorkItemsCompleted { get; }
+
+    /// <summary>
+    /// Gets the number of work items currently pending in the work queue.
+    /// </summary>
+    public int WorkItemsPending { get; }
+
+    /// <summary>
+    /// Gets the current number of items in the work queue.
+    /// </summary>
+    public int WorkQueueCount { get; }
+
+    /// <summary>
+    /// Gets the number of nodes started in the constraint builder.
+    /// </summary>
+    public int ConstraintBuilderNodesStarted { get; }
+
+    /// <summary>
+    /// Gets the number of nodes finished in the constraint builder.
+    /// </summary>
+    public int ConstraintBuilderNodesFinished { get; }
+
+    /// <summary>
+    /// Gets the total number of trajectory nodes in the pose graph.
+    /// Used for progress tracking during optimization.
+    /// </summary>
+    public int TrajectoryNodesCount { get; }
+
+    /// <summary>
+    /// Gets the total number of constraint tasks dispatched for scan matching.
+    /// </summary>
+    public int ConstraintTasksTotal { get; }
+
+    /// <summary>
+    /// Gets the number of constraint tasks that have finished scan matching.
+    /// </summary>
+    public int ConstraintTasksFinished { get; }
+
+    /// <summary>
+    /// Inserts an IMU measurement.
+    /// </summary>
+    void AddImuData(int trajectoryId, ImuData imuData);
+
+    /// <summary>
+    /// Inserts an odometry measurement.
+    /// </summary>
+    void AddOdometryData(int trajectoryId, OdometryData odometryData);
+
+    /// <summary>
+    /// Inserts a fixed frame pose measurement.
+    /// </summary>
+    void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData);
+
+    /// <summary>
+    /// Inserts landmarks observations.
+    /// </summary>
+    void AddLandmarkData(int trajectoryId, LandmarkData landmarkData);
+
+    /// <summary>
+    /// Drains the work queue to ensure all pending operations are completed.
+    /// This should be called before finishing a trajectory to avoid race conditions.
+    /// </summary>
+    void DrainWorkQueue();
+
+    /// <summary>
+    /// Finishes the given trajectory.
+    /// </summary>
+    void FinishTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Freezes a trajectory. Poses in this trajectory will not be optimized.
+    /// </summary>
+    void FreezeTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Adds a 'submap' from a proto with the given 'global_pose' to the
+    /// appropriate trajectory.
+    /// </summary>
+    void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap);
+
+    /// <summary>
+    /// Adds a 'node' from a proto with the given 'global_pose' to the
+    /// appropriate trajectory.
+    /// </summary>
+    void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node);
+
+    /// <summary>
+    /// Sets the trajectory data from a proto.
+    /// </summary>
+    void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data);
+
+    /// <summary>
+    /// Adds information that 'node_id' was inserted into 'submap_id'. The submap
+    /// has to be deserialized first.
+    /// </summary>
+    void AddNodeToSubmap(NodeId nodeId, SubmapId submapId);
+
+    /// <summary>
+    /// Adds serialized constraints. The corresponding trajectory nodes and submaps
+    /// have to be deserialized before calling this function.
+    /// </summary>
+    void AddSerializedConstraints(List<Constraint> constraints);
+
+    /// <summary>
+    /// Adds a 'trimmer'. It will be used after all data added before it has been
+    /// included in the pose graph.
+    /// </summary>
+    void AddTrimmer(PoseGraphTrimmer trimmer);
+
+    /// <summary>
+    /// Returns the current trajectory clusters.
+    /// </summary>
+    List<List<int>> GetConnectedTrajectories();
+
+    /// <summary>
+    /// Returns the IMU data.
+    /// </summary>
+    Dictionary<int, List<ImuData>> GetImuData();
+
+    /// <summary>
+    /// Returns the odometry data.
+    /// </summary>
+    Dictionary<int, List<OdometryData>> GetOdometryData();
+
+    /// <summary>
+    /// Returns the fixed frame pose data.
+    /// </summary>
+    Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData();
+
+    /// <summary>
+    /// Returns the landmark data.
+    /// </summary>
+    Dictionary<string, LandmarkNode> GetLandmarkNodes();
+
+    /// <summary>
+    /// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
+    /// respect to 'to_trajectory_id' at time 'time'.
+    /// </summary>
+    void SetInitialTrajectoryPose(
+        int fromTrajectoryId,
+        int toTrajectoryId,
+        Rigid3d pose,
+        long time);
+
+    /// <summary>
+    /// Waits for all computations to finish and computes optimized poses.
+    /// </summary>
+    void RunFinalOptimization();
+
+    /// <summary>
+    /// Returns data for all submaps.
+    /// </summary>
+    MapById<SubmapId, SubmapData> GetAllSubmapData();
+
+    /// <summary>
+    /// Returns the current optimized transform and submap itself for the given
+    /// 'submap_id'. Returns 'null' for the 'submap' member if the submap does
+    /// not exist (anymore).
+    /// </summary>
+    SubmapData GetSubmapData(SubmapId submapId);
+
+    /// <summary>
+    /// Returns the global poses for all submaps.
+    /// </summary>
+    MapById<SubmapId, SubmapPose> GetAllSubmapPoses();
+
+    /// <summary>
+    /// Returns the transform converting data in the local map frame (i.e. the
+    /// continuous, non-loop-closed frame) into the global map frame (i.e. the
+    /// discontinuous, loop-closed frame).
+    /// </summary>
+    Rigid3d GetLocalToGlobalTransform(int trajectoryId);
+
+    /// <summary>
+    /// Returns the current optimized trajectories.
+    /// </summary>
+    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
+
+    /// <summary>
+    /// Returns the current optimized trajectory poses.
+    /// </summary>
+    MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses();
+
+    /// <summary>
+    /// Returns the states of trajectories.
+    /// </summary>
+    Dictionary<int, TrajectoryState> GetTrajectoryStates();
+
+    /// <summary>
+    /// Returns the current optimized landmark poses.
+    /// </summary>
+    Dictionary<string, Rigid3d> GetLandmarkPoses();
+
+    /// <summary>
+    /// Sets global pose of landmark 'landmark_id' to given 'global_pose'.
+    /// </summary>
+    void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false);
+
+    /// <summary>
+    /// Deletes a trajectory asynchronously.
+    /// </summary>
+    void DeleteTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Checks if the given trajectory is finished.
+    /// </summary>
+    bool IsTrajectoryFinished(int trajectoryId);
+
+    /// <summary>
+    /// Checks if the given trajectory is frozen.
+    /// </summary>
+    bool IsTrajectoryFrozen(int trajectoryId);
+
+    /// <summary>
+    /// Returns the trajectory data.
+    /// </summary>
+    Dictionary<int, TrajectoryData> GetTrajectoryData();
+
+    /// <summary>
+    /// Returns the collection of constraints.
+    /// </summary>
+    List<Constraint> Constraints();
+
+    /// <summary>
+    /// Serializes the constraints and trajectories. If
+    /// 'include_unfinished_submaps' is set to 'true', unfinished submaps, i.e.
+    /// submaps that have not yet received all rangefinder data insertions, will
+    /// be included, otherwise not.
+    /// </summary>
+    Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps);
+
+    /// <summary>
+    /// Sets the callback function that is invoked whenever the global optimization
+    /// problem is solved.
+    /// </summary>
+    void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback);
+
+    /// <summary>
+    /// Sets the transform from local map frame to global map frame (map origin).
+    /// Used when loading state from pbstream; matches C++ SetTransformToMap.
+    /// </summary>
+    void SetTransformToMap(Rigid3d transform);
+
+    /// <summary>
+    /// Returns the transform from local map frame to global map frame.
+    /// Matches C++ GetTransformToMap.
+    /// </summary>
+    Rigid3d GetTransformToMap();
+
+    /// <summary>
+    /// Manually compute a constraint between a node and submap using global scan matching.
+    /// Match C++: manualComputeConstraint (pose_graph_interface.h:181-192)
+    /// </summary>
+    (double Score, Constraint? Constraint) ManualComputeConstraint(NodeId nodeId, SubmapId submapId);
+
+    /// <summary>
+    /// Manually compute constraint score from an initial pose estimate.
+    /// Match C++: manualComputeConstraintScore (pose_graph_interface.h:194-197)
+    /// </summary>
+    double ManualComputeConstraintScore(NodeId nodeId, SubmapId submapId, Rigid3d initialPose);
+
+    /// <summary>
+    /// Manually compute scan matcher score with refined pose output.
+    /// Match C++: manualComputeScanMatcher (pose_graph_interface.h:199-202)
+    /// </summary>
+    double ManualComputeScanMatcher(NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate);
+
+    /// <summary>
+    /// Manually relocalize a trajectory against finished submaps.
+    /// Match C++: ManualRelocalization (pose_graph_interface.h:212-216)
+    /// </summary>
+    bool ManualRelocalization(int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback);
+}
+
+/// <summary>
+/// Callback for global SLAM optimization.
+/// </summary>
+public delegate void GlobalSlamOptimizationCallback(
+    Dictionary<int, SubmapId> submapIds,
+    Dictionary<int, NodeId> nodeIds);
+
+/// <summary>
+/// Result of manual relocalization operation.
+/// Match C++: LocalizationResultCallback (pose_graph_interface.h)
+/// </summary>
+public struct LocalizationResult
+{
+    public NodeId NodeId { get; init; }
+    public SubmapId SubmapId { get; init; }
+    public Rigid3d GlobalPose { get; init; }
+    public double Score { get; init; }
+}
+
+/// <summary>
+/// Callback for localization/relocalization results.
+/// Match C++: LocalizationResultCallback (pose_graph_interface.h)
+/// </summary>
+public delegate void LocalizationResultCallback(LocalizationResult result);

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/IRangeDataInserter.cs

@@ -0,0 +1,38 @@
+/*
+ * 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.Sensor;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Interface for range data inserters.
+/// </summary>
+public interface IRangeDataInserter
+{
+    /// <summary>
+    /// Inserts 'range_data' into 'grid'.
+    /// </summary>
+    void Insert(RangeData rangeData, IGrid grid);
+}
+
+/// <summary>
+/// Grid interface for range data insertion.
+/// </summary>
+public interface IGrid
+{
+    // Base interface - specific grid types will implement this
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/ITrajectoryBuilder.cs

@@ -0,0 +1,325 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// This interface is used for both 2D and 3D SLAM. Implementations wire up a
+/// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
+/// to detect loop closure, and a sparse pose graph optimization to compute
+/// optimized pose estimates.
+/// </summary>
+public interface ITrajectoryBuilder
+{
+    /// <summary>
+    /// Result of inserting data into submaps.
+    /// </summary>
+    public struct InsertionResult(NodeId nodeId, TrajectoryNode.Data? constantData, List<Submap> insertionSubmaps)
+    {
+        public NodeId NodeId { get; set; } = nodeId;
+        public TrajectoryNode.Data? ConstantData { get; set; } = constantData;
+        public List<Submap> InsertionSubmaps { get; set; } = insertionSubmaps ?? [];
+    }
+
+    /// <summary>
+    /// Result of matching sensor data (scan matching, localization, etc.)
+    /// Returned when range data accumulation is completed.
+    /// Match C++: MatchingResult{time, pose_estimate, range_data_in_local, insertion_result, pose_confidence, ceres_score}
+    /// </summary>
+    public struct MatchingResult(
+        int trajectoryId,
+        long time,
+        Rigid3d localPose,
+        RangeData rangeDataInLocal,
+        InsertionResult? insertionResult,
+        double poseConfidence = -1.0,
+        double ceresScore = -1.0,
+        PointCloud? samplePointCloudGlobal = null)
+    {
+        public int TrajectoryId { get; set; } = trajectoryId;
+        public long Time { get; set; } = time;
+        public Rigid3d LocalPose { get; set; } = localPose;
+        public RangeData RangeDataInLocal { get; set; } = rangeDataInLocal;
+        public InsertionResult? InsertionResult { get; set; } = insertionResult;
+        /// <summary>
+        /// Pose confidence score from real-time correlative scan matcher.
+        /// Match C++: pose_confidence field in MatchingResult.
+        /// Value is -1.0 if confidence score is not provided.
+        /// </summary>
+        public double PoseConfidence { get; set; } = poseConfidence;
+        /// <summary>
+        /// Ceres solver final cost from scan matching.
+        /// Match C++: ceres_score tracked in ScanMatch function.
+        /// Value is -1.0 if ceres score is not available.
+        /// </summary>
+        public double CeresScore { get; set; } = ceresScore;
+        /// <summary>
+        /// Sample point cloud for visualization/debugging.
+        /// IMPORTANT: When returned from LocalTrajectoryBuilder2D, this is in LOCAL trajectory frame.
+        /// GlobalTrajectoryBuilder2D transforms it to GLOBAL map frame before returning to the caller.
+        /// After GlobalTrajectoryBuilder2D processing, this IS in global frame as the name suggests.
+        /// </summary>
+        public PointCloud? SamplePointCloudGlobal { get; set; } = samplePointCloudGlobal;
+    }
+
+    /// <summary>
+    /// Sensor identifier.
+    /// </summary>
+    public struct SensorId(SensorId.SensorType type, string id) : IEquatable<SensorId>, IComparable<SensorId>
+    {
+        public enum SensorType
+        {
+            Range = 0,
+            Imu,
+            Odometry,
+            FixedFramePose,
+            Landmark,
+            LocalSlamResult
+        }
+
+        public SensorType Type { get; set; } = type;
+        public string Id { get; set; } = id;
+
+        public readonly bool Equals(SensorId other)
+        {
+            return Type == other.Type && Id == other.Id;
+        }
+
+        public readonly override bool Equals(object? obj)
+        {
+            return obj is SensorId other && Equals(other);
+        }
+
+        public readonly override int GetHashCode()
+        {
+            return HashCode.Combine(Type, Id);
+        }
+
+        public readonly int CompareTo(SensorId other)
+        {
+            var typeComparison = Type.CompareTo(other.Type);
+            if (typeComparison != 0) return typeComparison;
+            return string.Compare(Id, other.Id, StringComparison.Ordinal);
+        }
+
+        public static bool operator ==(SensorId left, SensorId right)
+        {
+            return left.Equals(right);
+        }
+
+        public static bool operator !=(SensorId left, SensorId right)
+        {
+            return !left.Equals(right);
+        }
+
+        public static bool operator <(SensorId left, SensorId right)
+        {
+            return left.CompareTo(right) < 0;
+        }
+
+        public static bool operator <=(SensorId left, SensorId right)
+        {
+            return left.CompareTo(right) <= 0;
+        }
+
+        public static bool operator >(SensorId left, SensorId right)
+        {
+            return left.CompareTo(right) > 0;
+        }
+
+        public static bool operator >=(SensorId left, SensorId right)
+        {
+            return left.CompareTo(right) >= 0;
+        }
+    }
+
+    /// <summary>
+    /// Adds timed point cloud data from a sensor.
+    /// Returns MatchingResult when range data accumulation is completed, otherwise null.
+    /// </summary>
+    MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData);
+
+    /// <summary>
+    /// Adds IMU data from a sensor.
+    /// </summary>
+    void AddSensorData(string sensorId, ImuData imuData);
+
+    /// <summary>
+    /// Adds odometry data from a sensor.
+    /// </summary>
+    void AddSensorData(string sensorId, OdometryData odometryData);
+
+    /// <summary>
+    /// Adds fixed frame pose data from a sensor.
+    /// </summary>
+    void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData);
+
+    /// <summary>
+    /// Adds landmark data from a sensor.
+    /// </summary>
+    void AddSensorData(string sensorId, LandmarkData landmarkData);
+
+    /// <summary>
+    /// Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
+    /// is invalid to add local SLAM results for a trajectory that has a
+    /// 'LocalTrajectoryBuilder2D/3D'.
+    /// </summary>
+    void AddLocalSlamResultData(LocalSlamResultData localSlamResultData);
+
+    /// <summary>
+    /// Tries to get the current pose from the internal extrapolator at the given time.
+    /// Returns null when there is no local trajectory builder / extrapolator, or when
+    /// the requested time is before the last pose time. Allows callers (e.g. CartographerService)
+    /// to always read a live extrapolated pose instead of only the last scan-matched pose.
+    /// </summary>
+    /// <param name="time">Time in ticks (e.g. DateTime.UtcNow.Ticks) to extrapolate to.</param>
+    /// <returns>Extrapolated pose in trajectory local frame, or null if not available.</returns>
+    Rigid3d? TryGetExtrapolatedPose(long time);
+
+    /// <summary>
+    /// Tries to get the current pose with low-pass filter to reduce jitter during direction changes.
+    /// Match C++: ExtrapolatePose_filter - should be used for publishing pose to external systems.
+    /// </summary>
+    /// <param name="time">Time in ticks (e.g. DateTime.UtcNow.Ticks) to extrapolate to.</param>
+    /// <returns>Filtered extrapolated pose in trajectory local frame, or null if not available.</returns>
+    Rigid3d? TryGetExtrapolatedPoseFilter(long time);
+}
+
+/// <summary>
+/// Local SLAM result data.
+/// </summary>
+public class LocalSlamResultData(
+    int trajectoryId,
+    long time,
+    Rigid3d localPose,
+    RangeData rangeData,
+    ITrajectoryBuilder.InsertionResult? insertionResult = null)
+{
+    public int TrajectoryId { get; set; } = trajectoryId;
+    public long Time { get; set; } = time;
+    public Rigid3d LocalPose { get; set; } = localPose;
+    public RangeData RangeData { get; set; } = rangeData;
+    public ITrajectoryBuilder.InsertionResult? InsertionResult { get; set; } = insertionResult;
+
+    /// <summary>
+    /// Adds this local SLAM result to the pose graph.
+    /// This is used when running in pure localization mode or when replaying data.
+    /// </summary>
+    /// <param name="trajectoryId">The trajectory ID to add the result to</param>
+    /// <param name="poseGraph">The pose graph to add the result to</param>
+    public void AddToPoseGraph(int trajectoryId, IPoseGraph poseGraph)
+    {
+        if (InsertionResult == null)
+        {
+            // No insertion result, nothing to add to pose graph
+            return;
+        }
+
+        var insertionResult = InsertionResult.Value;
+        
+        // Add node to pose graph
+        // Note: The actual AddNode implementation depends on whether we're using 2D or 3D
+        // For now, we'll need to cast to the specific pose graph type
+        if (poseGraph is Mapping.Internal.D2D.PoseGraph2D poseGraph2D)
+        {
+            // For 2D, we need to convert Submap to Submap2D
+            var submaps2D = insertionResult.InsertionSubmaps
+                .OfType<Mapping.D2D.Submap2D>()
+                .ToList();
+            
+            if (submaps2D.Count != insertionResult.InsertionSubmaps.Count)
+            {
+                throw new InvalidOperationException("Cannot add 3D submaps to 2D pose graph");
+            }
+
+            if(insertionResult.ConstantData is null)
+                throw new NullReferenceException(nameof(insertionResult.ConstantData));
+
+            // Add node to pose graph
+            poseGraph2D.AddNode(insertionResult.ConstantData, trajectoryId, submaps2D);
+        }
+        else if (poseGraph is Mapping.Internal.D3D.PoseGraph3D poseGraph3D)
+        {
+            // For 3D, we need to convert Submap to Submap3D
+            var submaps3D = insertionResult.InsertionSubmaps
+                .OfType<Mapping.D3D.Submap3D>()
+                .ToList();
+            
+            if (submaps3D.Count != insertionResult.InsertionSubmaps.Count)
+            {
+                throw new InvalidOperationException(
+                    "Cannot add 2D submaps to 3D pose graph");
+            }
+
+            if (insertionResult.ConstantData is null)
+                throw new NullReferenceException(nameof(insertionResult.ConstantData));
+
+            poseGraph3D.AddNode(insertionResult.ConstantData, trajectoryId, submaps3D);
+        }
+        else
+        {
+            throw new InvalidOperationException(
+                $"Unknown pose graph type: {poseGraph.GetType().Name}");
+        }
+    }
+}
+
+/// <summary>
+/// Conversion utilities for SensorId.
+/// </summary>
+public static class SensorIdOperations
+{
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public static Models.Mapping.SensorId ToProto(ITrajectoryBuilder.SensorId sensorId)
+    {
+        var type = sensorId.Type switch
+        {
+            ITrajectoryBuilder.SensorId.SensorType.Range => Models.Mapping.SensorId.SensorType.Range,
+            ITrajectoryBuilder.SensorId.SensorType.Imu => Models.Mapping.SensorId.SensorType.Imu,
+            ITrajectoryBuilder.SensorId.SensorType.Odometry => Models.Mapping.SensorId.SensorType.Odometry,
+            ITrajectoryBuilder.SensorId.SensorType.FixedFramePose => Models.Mapping.SensorId.SensorType.FixedFramePose,
+            ITrajectoryBuilder.SensorId.SensorType.Landmark => Models.Mapping.SensorId.SensorType.Landmark,
+            ITrajectoryBuilder.SensorId.SensorType.LocalSlamResult => Models.Mapping.SensorId.SensorType.LocalSlamResult,
+            _ => throw new ArgumentException($"Unknown sensor type: {sensorId.Type}")
+        };
+
+        return new Models.Mapping.SensorId(type, sensorId.Id);
+    }
+
+    /// <summary>
+    /// Creates from proto representation.
+    /// </summary>
+    public static ITrajectoryBuilder.SensorId FromProto(Models.Mapping.SensorId sensorIdProto)
+    {
+        var type = sensorIdProto.Type switch
+        {
+            Models.Mapping.SensorId.SensorType.Range => ITrajectoryBuilder.SensorId.SensorType.Range,
+            Models.Mapping.SensorId.SensorType.Imu => ITrajectoryBuilder.SensorId.SensorType.Imu,
+            Models.Mapping.SensorId.SensorType.Odometry => ITrajectoryBuilder.SensorId.SensorType.Odometry,
+            Models.Mapping.SensorId.SensorType.FixedFramePose => ITrajectoryBuilder.SensorId.SensorType.FixedFramePose,
+            Models.Mapping.SensorId.SensorType.Landmark => ITrajectoryBuilder.SensorId.SensorType.Landmark,
+            Models.Mapping.SensorId.SensorType.LocalSlamResult => ITrajectoryBuilder.SensorId.SensorType.LocalSlamResult,
+            _ => throw new ArgumentException($"Unknown sensor type: {sensorIdProto.Type}")
+        };
+
+        return new ITrajectoryBuilder.SensorId(type, sensorIdProto.Id);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Id.cs

@@ -0,0 +1,179 @@
+/*
+ * 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;
+
+/// <summary>
+/// Uniquely identifies a trajectory node using a combination of a unique
+/// trajectory ID and a zero-based index of the node inside that trajectory.
+/// </summary>
+public struct NodeId(int trajectoryId, int nodeIndex) : IEquatable<NodeId>, IComparable<NodeId>, IIdType
+{
+    public int TrajectoryId { get; set; } = trajectoryId;
+    public int NodeIndex { get; set; } = nodeIndex;
+
+    public readonly int GetTrajectoryId() => TrajectoryId;
+    public readonly int GetIndex() => NodeIndex;
+
+    public readonly bool Equals(NodeId other)
+    {
+        return TrajectoryId == other.TrajectoryId && NodeIndex == other.NodeIndex;
+    }
+
+    public readonly override bool Equals(object? obj)
+    {
+        return obj is NodeId other && Equals(other);
+    }
+
+    public readonly override int GetHashCode()
+    {
+        return HashCode.Combine(TrajectoryId, NodeIndex);
+    }
+
+    public static bool operator ==(NodeId left, NodeId right)
+    {
+        return left.Equals(right);
+    }
+
+    public static bool operator !=(NodeId left, NodeId right)
+    {
+        return !left.Equals(right);
+    }
+
+    public readonly int CompareTo(NodeId other)
+    {
+        var trajectoryComparison = TrajectoryId.CompareTo(other.TrajectoryId);
+        if (trajectoryComparison != 0)
+        {
+            return trajectoryComparison;
+        }
+        return NodeIndex.CompareTo(other.NodeIndex);
+    }
+
+    public static bool operator <(NodeId left, NodeId right)
+    {
+        return left.CompareTo(right) < 0;
+    }
+
+    public static bool operator <=(NodeId left, NodeId right)
+    {
+        return left.CompareTo(right) <= 0;
+    }
+
+    public static bool operator >(NodeId left, NodeId right)
+    {
+        return left.CompareTo(right) > 0;
+    }
+
+    public static bool operator >=(NodeId left, NodeId right)
+    {
+        return left.CompareTo(right) >= 0;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public readonly NodeId ToProto()
+    {
+        return new NodeId(TrajectoryId, NodeIndex);
+    }
+
+    public readonly override string ToString()
+    {
+        return $"({TrajectoryId}, {NodeIndex})";
+    }
+}
+
+/// <summary>
+/// Uniquely identifies a submap using a combination of a unique trajectory ID
+/// and a zero-based index of the submap inside that trajectory.
+/// </summary>
+public struct SubmapId(int trajectoryId, int submapIndex) : IEquatable<SubmapId>, IComparable<SubmapId>, IIdType
+{
+    public int TrajectoryId { get; set; } = trajectoryId;
+    public int SubmapIndex { get; set; } = submapIndex;
+
+    public readonly int GetTrajectoryId() => TrajectoryId;
+    public readonly int GetIndex() => SubmapIndex;
+
+    public readonly bool Equals(SubmapId other)
+    {
+        return TrajectoryId == other.TrajectoryId && SubmapIndex == other.SubmapIndex;
+    }
+
+    public readonly override bool Equals(object? obj)
+    {
+        return obj is SubmapId other && Equals(other);
+    }
+
+    public readonly override int GetHashCode()
+    {
+        return HashCode.Combine(TrajectoryId, SubmapIndex);
+    }
+
+    public static bool operator ==(SubmapId left, SubmapId right)
+    {
+        return left.Equals(right);
+    }
+
+    public static bool operator !=(SubmapId left, SubmapId right)
+    {
+        return !left.Equals(right);
+    }
+
+    public readonly int CompareTo(SubmapId other)
+    {
+        var trajectoryComparison = TrajectoryId.CompareTo(other.TrajectoryId);
+        if (trajectoryComparison != 0)
+        {
+            return trajectoryComparison;
+        }
+        return SubmapIndex.CompareTo(other.SubmapIndex);
+    }
+
+    public static bool operator <(SubmapId left, SubmapId right)
+    {
+        return left.CompareTo(right) < 0;
+    }
+
+    public static bool operator <=(SubmapId left, SubmapId right)
+    {
+        return left.CompareTo(right) <= 0;
+    }
+
+    public static bool operator >(SubmapId left, SubmapId right)
+    {
+        return left.CompareTo(right) > 0;
+    }
+
+    public static bool operator >=(SubmapId left, SubmapId right)
+    {
+        return left.CompareTo(right) >= 0;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public readonly SubmapId ToProto()
+    {
+        return new SubmapId(TrajectoryId, SubmapIndex);
+    }
+
+    public readonly override string ToString()
+    {
+        return $"({TrajectoryId}, {SubmapIndex})";
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/ImuTracker.cs

@@ -0,0 +1,234 @@
+/*
+ * 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.Transform;
+using System;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Keeps track of the orientation using angular velocities and linear
+/// accelerations from an IMU. Because averaged linear acceleration (assuming
+/// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
+/// though yaw does.
+/// </summary>
+public class ImuTracker
+{
+    private readonly double _imuGravityTimeConstant;
+    private long _time;
+    private long _lastLinearAccelerationTime;
+    private Quaternion _orientation;
+    private Vector3 _gravityVector;
+    private Vector3 _imuAngularVelocity;
+
+    // Recovery tracking: count consecutive invalid gravity states
+    private int _invalidGravityCount;
+    private const int MaxInvalidGravityBeforeReset = 10; // Reset after 10 consecutive invalid states
+
+    public ImuTracker(double imuGravityTimeConstant, long time)
+    {
+        _imuGravityTimeConstant = imuGravityTimeConstant;
+        _time = time;
+        _lastLinearAccelerationTime = long.MinValue;
+        _orientation = Quaternion.Identity;
+        _gravityVector = Vector3.UnitZ;
+        _imuAngularVelocity = Vector3.Zero;
+        _invalidGravityCount = 0;
+    }
+
+    /// <summary>
+    /// Copy constructor.
+    /// </summary>
+    public ImuTracker(ImuTracker other)
+    {
+        _imuGravityTimeConstant = other._imuGravityTimeConstant;
+        _time = other._time;
+        _lastLinearAccelerationTime = other._lastLinearAccelerationTime;
+        _orientation = other._orientation;
+        _gravityVector = other._gravityVector;
+        _imuAngularVelocity = other._imuAngularVelocity;
+        _invalidGravityCount = other._invalidGravityCount;
+    }
+
+    /// <summary>
+    /// Advances to the given 'time' and updates the orientation to reflect this.
+    /// </summary>
+    public void Advance(long time)
+    {
+        if (time < _time)
+        {
+            // DEBUG: Log detailed timestamp information
+            var timeDiffMs = (_time - time) / TimeSpan.TicksPerMillisecond;
+            
+            // If the difference is small (< 100ms), it's likely due to synchronization issues
+            // between multiple sensors or old scans being reprocessed. In this case, we advance
+            // to current time instead of throwing. This is a workaround for RangeDataCollator
+            // synchronization issues and old scan reprocessing.
+            if (timeDiffMs >= 100)
+            {
+                // For larger differences, throw exception as it indicates a real problem
+                throw new ArgumentException($"time ({time}) must be >= current time ({_time}), diff={timeDiffMs}ms", nameof(time));
+            }
+        }
+
+        var deltaT = (time - _time) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)
+        var rotation = TransformOperations.AngleAxisVectorToRotationQuaternion(
+            _imuAngularVelocity * deltaT);
+        _orientation = Quaternion.Normalize(_orientation * rotation);
+        
+        // Rotate gravity vector by inverse rotation (conjugate)
+        // In C++: gravity_vector_ = rotation.conjugate() * gravity_vector_
+        // In C#: equivalent to Transform with conjugate quaternion
+        var rotationConjugate = Quaternion.Conjugate(rotation);
+        _gravityVector = Vector3.Transform(_gravityVector, rotationConjugate);
+        
+        _time = time;
+    }
+
+    /// <summary>
+    /// Updates from an IMU reading (in the IMU frame).
+    /// </summary>
+    public void AddImuLinearAccelerationObservation(Vector3 imuLinearAcceleration)
+    {
+        // Validate input: reject zero or near-zero acceleration (invalid sensor data)
+        var inputMagnitude = imuLinearAcceleration.Length();
+        if (inputMagnitude < 0.1) // Less than 0.1 m/s² is invalid (should be ~9.81 when stationary)
+        {
+            // Skip invalid reading, don't update state
+            return;
+        }
+
+        // Update the 'gravity_vector_' with an exponential moving average using the
+        // 'imu_gravity_time_constant'.
+        var deltaT = _lastLinearAccelerationTime > long.MinValue
+            ? (_time - _lastLinearAccelerationTime) / 10_000_000.0 // Convert ticks to seconds (10 million ticks per second)
+            : double.PositiveInfinity;
+        _lastLinearAccelerationTime = _time;
+
+        var alpha = 1.0 - Math.Exp(-deltaT / _imuGravityTimeConstant);
+        _gravityVector = (1.0 - alpha) * _gravityVector + alpha * imuLinearAcceleration;
+
+        // Change the 'orientation_' so that it agrees with the current 'gravity_vector_'.
+        // Match C++: FromTwoVectors(gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ())
+        // This computes rotation from gravity_vector_ (in IMU frame) to UnitZ transformed to IMU frame
+        var unitZInImuFrame = Vector3.Transform(Vector3.UnitZ, Quaternion.Inverse(_orientation));
+        var rotation = FromTwoVectors(_gravityVector, unitZInImuFrame);
+        _orientation = Quaternion.Normalize(_orientation * rotation);
+
+        // Validate: gravity vector transformed by orientation should point up (positive Z in world frame)
+        // When IMU is level, gravity in world frame should be (0, 0, +g) after transform
+        var transformedGravity = Vector3.Transform(_gravityVector, _orientation);
+        var normalizedTransformedGravity = Vector3.Normalize(transformedGravity);
+
+        if (transformedGravity.Z <= 0 || normalizedTransformedGravity.Z < 0.9)
+        {
+            _invalidGravityCount++;
+
+            // Recovery: if too many consecutive invalid states, reset to known good state
+            if (_invalidGravityCount >= MaxInvalidGravityBeforeReset)
+            {
+                Console.WriteLine($"[ImuTracker] RECOVERY: Resetting after {_invalidGravityCount} consecutive invalid gravity states. " +
+                    $"TransformedGravity=({transformedGravity.X:F3},{transformedGravity.Y:F3},{transformedGravity.Z:F3})");
+
+                // Reset gravity vector to point in the direction of current acceleration
+                // (assuming robot is mostly stationary, acceleration ≈ gravity)
+                _gravityVector = Vector3.Normalize(imuLinearAcceleration) * 9.81;
+
+                // Reset orientation to align gravity with world Z-axis
+                var gravityDirection = Vector3.Normalize(_gravityVector);
+                _orientation = FromTwoVectors(gravityDirection, Vector3.UnitZ);
+
+                _invalidGravityCount = 0;
+            }
+        }
+        else
+        {
+            // Valid state - reset counter
+            _invalidGravityCount = 0;
+        }
+    }
+
+    /// <summary>
+    /// Updates from an IMU reading (in the IMU frame).
+    /// </summary>
+    public void AddImuAngularVelocityObservation(Vector3 imuAngularVelocity)
+    {
+        _imuAngularVelocity = imuAngularVelocity;
+    }
+
+    /// <summary>
+    /// Query the current time.
+    /// </summary>
+    public long Time => _time;
+
+    /// <summary>
+    /// Query the current orientation estimate.
+    /// </summary>
+    public Quaternion Orientation => _orientation;
+
+    /// <summary>
+    /// Computes a quaternion that rotates vector 'a' to vector 'b'.
+    /// Equivalent to Eigen::Quaterniond::FromTwoVectors().
+    /// </summary>
+    private static Quaternion FromTwoVectors(Vector3 a, Vector3 b)
+    {
+        // Normalize input vectors
+        a = Vector3.Normalize(a);
+        b = Vector3.Normalize(b);
+        
+        // If vectors are parallel, return identity
+        var dot = Vector3.Dot(a, b);
+        if (Math.Abs(dot - 1.0) < 1e-6)
+        {
+            return Quaternion.Identity;
+        }
+        
+        // If vectors are opposite, need special handling
+        if (Math.Abs(dot + 1.0) < 1e-6)
+        {
+            // Find an orthogonal vector to 'a'
+            Vector3 orthogonal;
+            if (Math.Abs(a.X) < Math.Abs(a.Y))
+            {
+                orthogonal = Vector3.UnitX;
+            }
+            else
+            {
+                orthogonal = Vector3.UnitY;
+            }
+            orthogonal = Vector3.Normalize(Vector3.Cross(a, orthogonal));
+            
+            // Create 180-degree rotation around orthogonal axis
+            return Quaternion.CreateFromAxisAngle(orthogonal, Math.PI);
+        }
+        
+        // General case: compute rotation axis and angle
+        var axis = Vector3.Cross(a, b);
+        var axisLength = axis.Length();
+        
+        if (axisLength < 1e-6)
+        {
+            return Quaternion.Identity;
+        }
+        
+        axis = Vector3.Normalize(axis);
+        var angle = Math.Acos(Math.Clamp(dot, -1.0, 1.0));
+        
+        return Quaternion.CreateFromAxisAngle(axis, angle);
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/GlobalTrajectoryBuilder2D.cs

@@ -0,0 +1,204 @@
+/*
+ * 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.
+ */
+
+// MAPPING FROM C++:
+// In the original Cartographer, there is a single file internal/global_trajectory_builder.cc
+// containing a template class GlobalTrajectoryBuilder<LocalTrajectoryBuilder, PoseGraph>
+// and two factory functions: CreateGlobalTrajectoryBuilder2D and CreateGlobalTrajectoryBuilder3D.
+// C# has no templates, so we use separate classes: GlobalTrajectoryBuilder2D (this file)
+// and GlobalTrajectoryBuilder3D in Internal/3D/. Logic matches the 2D instantiation of the template.
+
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Wires up local SLAM (LocalTrajectoryBuilder2D) with the PoseGraph for 2D mapping.
+/// Corresponds to GlobalTrajectoryBuilder&lt;LocalTrajectoryBuilder2D, PoseGraph2D&gt; in C++.
+/// Handles sensor data, triggers local SLAM, and adds results to the pose graph.
+/// Original Cartographer has no RelocalizationScanMatch; initial pose comes from trajectory options (initial_trajectory_pose) and PoseGraph (SetLocalizationInitialPoses for constraints). MCL refines pose before adding trajectory.
+/// </summary>
+public class GlobalTrajectoryBuilder2D(
+    LocalTrajectoryBuilder2D? localTrajectoryBuilder,
+    int trajectoryId,
+    PoseGraph2D poseGraph,
+    MotionFilter? poseGraphOdometryMotionFilter = null) : ITrajectoryBuilder
+{
+    /// <summary>
+    /// AddSensorData(TimedPointCloudData). Matches C++ GlobalTrajectoryBuilder::AddSensorData flow:
+    /// 1) CHECK local_trajectory_builder, 2) matching_result = AddRangeData, 3) if null return,
+    /// 4) if insertion_result != null: AddNode, build insertion_result (C++ also sets pose_graph_->confidence_score),
+    /// 5) invoke local_slam_result_callback.
+    /// </summary>
+    public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
+    {
+        // C++: CHECK(local_trajectory_builder_) "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder."
+        if (localTrajectoryBuilder == null)
+        {
+            throw new InvalidOperationException("Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.");
+        }
+
+        // C++: matching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data)
+        var matchingResult = localTrajectoryBuilder.AddRangeData(sensorId, timedPointCloudData);
+        // C++: if (matching_result == nullptr) return;
+        if (matchingResult == null)
+        {
+            return null;
+        }
+
+        // C++: kLocalSlamMatchingResults->Increment(); then if (matching_result->insertion_result != nullptr) { ... AddNode; pose_graph_->confidence_score = matching_result->confidence_score; insertion_result = ... }
+        var result = matchingResult.Value;
+        ITrajectoryBuilder.InsertionResult? insertionResult = null;
+
+        if (result.InsertionResult.HasValue)
+        {
+            var insertionResultValue = result.InsertionResult.Value;
+            if (insertionResultValue.ConstantData is null)
+            {
+                throw new NullReferenceException(nameof(insertionResultValue.ConstantData));
+            }
+
+            // Cast submaps to Submap2D for PoseGraph2D.AddNode
+            var submaps2D = insertionResultValue.InsertionSubmaps.Cast<Mapping.D2D.Submap2D>().ToList();
+            
+            var nodeId = poseGraph.AddNode(
+                insertionResultValue.ConstantData,
+                trajectoryId,
+                submaps2D);
+
+            // C++: pose_graph_->confidence_score = matching_result->confidence_score (when MatchingResult has confidence_score)
+            // TODO: set poseGraph.ConfidenceScore when MatchingResult and PoseGraph2D expose it.
+
+            if (nodeId.TrajectoryId != trajectoryId)
+            {
+                throw new InvalidOperationException($"Node trajectory ID {nodeId.TrajectoryId} does not match expected {trajectoryId}");
+            }
+            
+            // Update insertionResult with NodeId
+            insertionResult = new ITrajectoryBuilder.InsertionResult(
+                nodeId,
+                insertionResultValue.ConstantData,
+                insertionResultValue.InsertionSubmaps);
+            
+            // Update result with new insertionResult (including NodeId)
+            result = new ITrajectoryBuilder.MatchingResult(
+                trajectoryId,
+                result.Time,
+                result.LocalPose,
+                result.RangeDataInLocal,
+                insertionResult,
+                result.PoseConfidence,
+                result.CeresScore,
+                result.SamplePointCloudGlobal
+            );
+        }
+        
+        // Transform sample point cloud from trajectory frame (already global orientation) to pose-graph global frame.
+        // LocalTrajectoryBuilder2D returns trajectory-with-global-orientation; client/grid expect full global.
+        PointCloud? sampleInGlobal = result.SamplePointCloudGlobal;
+        if (sampleInGlobal != null && sampleInGlobal.Count > 0)
+        {
+            var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
+            var localToGlobalF = new Rigid3f((Vector3)localToGlobal.Translation, localToGlobal.Rotation);
+            sampleInGlobal = PointCloudOperations.Transform(sampleInGlobal, localToGlobalF);
+            result = new ITrajectoryBuilder.MatchingResult(
+                trajectoryId,
+                result.Time,
+                result.LocalPose,
+                result.RangeDataInLocal,
+                result.InsertionResult,
+                result.PoseConfidence,
+                result.CeresScore,
+                sampleInGlobal
+            );
+        }
+        
+        return result;
+    }
+
+    public void AddSensorData(string sensorId, ImuData imuData)
+    {
+        // Add to local trajectory builder if available
+        localTrajectoryBuilder?.AddImuData(imuData);
+        
+        // Always add to pose graph for global optimization
+        poseGraph.AddImuData(trajectoryId, imuData);
+    }
+
+    public void AddSensorData(string sensorId, OdometryData odometryData)
+    {
+        if (!odometryData.Pose.IsValid())
+        {
+            throw new ArgumentException($"Invalid odometry pose: {odometryData.Pose}", nameof(odometryData));
+        }
+
+        // Add to local trajectory builder if available
+        localTrajectoryBuilder?.AddOdometryData(odometryData);
+
+        // Apply motion filter if configured
+        if (poseGraphOdometryMotionFilter != null &&
+            poseGraphOdometryMotionFilter.IsSimilar(odometryData.Time, odometryData.Pose))
+        {
+            return; // Filtered out due to similar motion
+        }
+
+        // Add to pose graph
+        poseGraph.AddOdometryData(trajectoryId, odometryData);
+    }
+
+    public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
+    {
+        if (fixedFramePoseData.Pose.HasValue && !fixedFramePoseData.Pose.Value.IsValid())
+        {
+            throw new ArgumentException(
+                $"Invalid fixed frame pose: {fixedFramePoseData.Pose.Value}",
+                nameof(fixedFramePoseData));
+        }
+
+        poseGraph.AddFixedFramePoseData(trajectoryId, fixedFramePoseData);
+    }
+
+    public void AddSensorData(string sensorId, LandmarkData landmarkData)
+    {
+        poseGraph.AddLandmarkData(trajectoryId, landmarkData);
+    }
+
+    public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
+    {
+        if (localTrajectoryBuilder != null)
+        {
+            throw new InvalidOperationException(
+                "Can't add LocalSlamResultData with local_trajectory_builder_ present.");
+        }
+
+        // Add the local SLAM result directly to the pose graph
+        localSlamResultData.AddToPoseGraph(trajectoryId, poseGraph);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        return localTrajectoryBuilder?.TryGetExtrapolatedPose(time);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        return localTrajectoryBuilder?.TryGetExtrapolatedPoseFilter(time);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/LocalTrajectoryBuilder2D.cs

@@ -0,0 +1,910 @@
+using System.Diagnostics;
+using CartographerSharp.Mapping.D2D;
+using CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+using CartographerSharp.Metrics;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Numbers;
+using QuaternionGeometry = RobotNet10.Shared.Geometry.Quaternion;
+using QuaternionNumbers = RobotNet10.Shared.Numbers.Quaternion;
+using Submap2D = CartographerSharp.Mapping.D2D.Submap2D;
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
+/// without loop closure.
+/// </summary>
+public class LocalTrajectoryBuilder2D(
+    LocalTrajectoryBuilderOptions2D options,
+    List<string> expectedRangeSensorIds) : IDisposable
+{
+    private bool _disposed;
+    public struct InsertionResult(TrajectoryNode.Data? constantData, List<Submap2D> insertionSubmaps)
+    {
+        public TrajectoryNode.Data? ConstantData { get; set; } = constantData;
+        public List<Submap2D> InsertionSubmaps { get; set; } = insertionSubmaps ?? [];
+    }
+
+    private readonly ActiveSubmaps2D _activeSubmaps = new(options.SubmapsOptions);
+    private readonly MotionFilter _motionFilter = new(options.MotionFilterOptions);
+    private readonly RealTimeCorrelativeScanMatcher2D _realTimeCorrelativeScanMatcher = new(options.RealTimeCorrelativeScanMatcherOptions);
+    private readonly CeresScanMatcher2D _ceresScanMatcher = new(options.CeresScanMatcherOptions);
+    private PoseExtrapolator? _extrapolator;
+    private int _numAccumulated = 0;
+    private RangeData _accumulatedRangeData;
+    private List<Rigid3f>? _lastRangeDataPoses; // Store poses from last accumulation for setting origin
+    private readonly RangeDataCollator _rangeDataCollator = new(expectedRangeSensorIds);
+
+    // Store first odometry data for initial pose if UseOdometryDirectly is enabled
+    private OdometryData? _firstOdometryData;
+
+    // Tracks consecutive hard-limit Ceres failures. When this reaches
+    // MaxConsecutiveHighCostBeforeNewSubmap, a new submap is forced.
+    // NOT reset on isSimilar (motion-filtered) frames.
+    private int _consecutiveHardLimitCount = 0;
+
+    // Tracks number of times AddAccumulatedRangeData has been called.
+    // For the first 5 calls, isSimilar is ignored to ensure matching is performed.
+    private int _accumulatedRangeDataCallCount = 0;
+
+
+    // Match C++: last_sensor_time_ for sensor_duration calculation
+    private long? _lastSensorTime;
+
+    // CSV log for frame timing diagnostics
+    private static readonly StreamWriter _matchingLog = InitMatchingLog();
+    private static StreamWriter InitMatchingLog()
+    {
+        var sw = new StreamWriter("matching.log", append: true) { AutoFlush = true };
+        sw.WriteLine("DateTime,TotalMs,ScanMatchMs,InsertMs,OdomWinAfterMs,CeresScore,ResidualDist,ResidualAngleDeg");
+        return sw;
+    }
+
+    // Match C++ lines 300-320: Metrics tracking for performance monitoring
+    // Using Stopwatch.GetTimestamp() for high-resolution wall time (matches C++ std::chrono::steady_clock::now())
+    private long? _lastWallTimestamp;
+    private double? _lastThreadCpuTimeSeconds;
+
+    // Match C++ metrics: Histogram/Gauge metrics for scan matching
+    // Using Null() for now - can be replaced with actual metrics if FamilyFactory is configured
+    private static readonly Histogram _kRealTimeCorrelativeScanMatcherScoreMetric = Histogram.Null();
+    private static readonly Histogram _kCeresScanMatcherCostMetric = Histogram.Null();
+    private static readonly Histogram _kScanMatcherResidualDistanceMetric = Histogram.Null();
+    private static readonly Histogram _kScanMatcherResidualAngleMetric = Histogram.Null();
+    private static readonly Gauge _kLocalSlamLatencyMetric = Gauge.Null();
+    private static readonly Gauge _kLocalSlamRealTimeRatio = Gauge.Null();
+    private static readonly Gauge _kLocalSlamCpuRealTimeRatio = Gauge.Null();
+
+    private readonly Stopwatch _samplePointCloudStopwatch = Stopwatch.StartNew();
+
+    /// <summary>
+    /// Returns 'MatchingResult' when range data accumulation completed,
+    /// otherwise 'null'. Range data must be approximately horizontal
+    /// for 2D SLAM.
+    /// </summary>
+    public ITrajectoryBuilder.MatchingResult? AddRangeData(string sensorId, TimedPointCloudData rangeData)
+    {
+        try
+        {
+            var synchronizedData = _rangeDataCollator.AddRangeData(sensorId, rangeData);
+            // Match C++ line 127-130: if (synchronized_data.ranges.empty()) return nullptr
+            if (synchronizedData.Ranges.Count == 0)
+            {
+                return null;
+            }
+
+            // Match C++ line 147: CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f)
+            // The last point's time must be <= 0 (relative to synchronized time)
+            var lastPointTime = synchronizedData.Ranges[^1].PointTime.Time;
+            if (lastPointTime > 0.0)
+            {
+                throw new InvalidOperationException($"Last point time ({lastPointTime}) must be <= 0 (relative to synchronized time)");
+            }
+
+            var time = synchronizedData.Time;
+            
+            if (!options.UseImuData)
+            {
+                InitializeExtrapolator(time);
+            }
+
+            if (_extrapolator == null)
+            {
+                return null;
+            }
+
+            // Match C++ lines 148-154:
+            // const common::Time time_first_point = time + common::FromSeconds(synchronized_data.ranges.front().point_time.time);
+            // if (time_first_point < extrapolator_->GetLastPoseTime()) { return nullptr; }
+            var timeFirstPoint = time + (long)Math.Round(synchronizedData.Ranges[0].PointTime.Time * TimeSpan.TicksPerSecond);
+            if (timeFirstPoint < _extrapolator.GetLastPoseTime())
+            {
+                return null;
+            }
+            
+            if (_numAccumulated == 0)
+            {
+                // 'accumulated_range_data_.origin' is uninitialized until the last accumulation.
+                // Match C++: accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
+                _accumulatedRangeData = new RangeData(RobotNet10.Shared.Numbers.Vector3.Zero, new PointCloud(), new PointCloud());
+            }
+            
+            // Motion compensation is ALWAYS enabled (matches C++ behavior).
+            // Match C++: std::vector<transform::Rigid3f> range_data_poses;
+            // Match C++ lines 156-172: Build range_data_poses with per-point extrapolation
+            var rangeDataPoses = new List<Rigid3f>(synchronizedData.Ranges.Count);
+            bool warned = false;
+
+            for (int idx = 0; idx < synchronizedData.Ranges.Count; idx++)
+            {
+                var range = synchronizedData.Ranges[idx];
+                // Match C++: common::Time time_point = time + common::FromSeconds(range.point_time.time);
+                var pointTime = time + (long)Math.Round(range.PointTime.Time * TimeSpan.TicksPerSecond);
+
+                // Match C++: if (time_point < extrapolator_->GetLastExtrapolatedTime())
+                // Call live each iteration (C++ calls extrapolator_->GetLastExtrapolatedTime() per iteration)
+                var lastExtrapolatedTime = _extrapolator.GetLastExtrapolatedTime();
+                if (pointTime < lastExtrapolatedTime)
+                {
+                    if (!warned)
+                    {
+                        warned = true;
+                    }
+                    pointTime = lastExtrapolatedTime;
+                }
+                
+                // Match C++: range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast<double>());
+                var poseAtTime = _extrapolator.ExtrapolatePose(pointTime);
+                var poseAtTimeF = new Rigid3f(poseAtTime.Translation, poseAtTime.Rotation);
+                rangeDataPoses.Add(poseAtTimeF);
+            }
+
+            int returnsCount = 0;
+            int missesCount = 0;
+            int skippedCount = 0;
+            
+            for (int i = 0; i < synchronizedData.Ranges.Count; i++)
+            {
+                var range = synchronizedData.Ranges[i];
+                var hit = range.PointTime;
+
+                // MEDIUM FIX: Validate Origins collection is not empty before accessing
+                // If empty, use zero vector as fallback origin (matches C++ behavior when origin is unset)
+                RobotNet10.Shared.Numbers.Vector3 originBeforeTransform;
+                if (synchronizedData.Origins.Count == 0)
+                {
+                    originBeforeTransform = RobotNet10.Shared.Numbers.Vector3.Zero;
+                }
+                else
+                {
+                    var originIndex = range.OriginIndex < synchronizedData.Origins.Count
+                        ? range.OriginIndex
+                        : 0;
+                    originBeforeTransform = synchronizedData.Origins[originIndex];
+                }
+                var originInLocal = rangeDataPoses[i].TransformPoint(originBeforeTransform);
+                
+                var hitInLocal = rangeDataPoses[i].TransformPoint(hit.Position);
+                
+                // Match C++: const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
+                var delta = hitInLocal - originInLocal;
+                
+                // Match C++: const double range = delta.norm();
+                var rangeLength = delta.Length();
+                
+                // Match C++: if (range >= options_.min_range())
+                if (rangeLength >= options.MinRange)
+                {
+                    // Match C++: if (range <= options_.max_range())
+                    if (rangeLength <= options.MaxRange)
+                    {
+                        // Match C++: accumulated_range_data_.returns.push_back(hit_in_local);
+                        _accumulatedRangeData.Returns.Add(new RangefinderPoint { Position = hitInLocal });
+                        returnsCount++;
+                    }
+                    else
+                    {
+                        // Match C++: hit_in_local.position = origin_in_local + options_.missing_data_ray_length() / range * delta;
+                        var missPoint = new RangefinderPoint
+                        {
+                            Position = originInLocal + delta / rangeLength * options.MissingDataRayLength
+                        };
+                        // Match C++: accumulated_range_data_.misses.push_back(hit_in_local);
+                        _accumulatedRangeData.Misses.Add(missPoint);
+                        missesCount++;
+                    }
+                }
+                else
+                {
+                    skippedCount++;
+                }
+            }
+            
+            // Match C++: ++num_accumulated_;
+            _numAccumulated++;
+            // Store poses for setting origin when accumulation completes (match C++: range_data_poses.back())
+            // Note: In C++, range_data_poses is a local variable, but we need to store it for later use.
+            _lastRangeDataPoses = rangeDataPoses;
+            
+            // Match C++: if (num_accumulated_ >= options_.num_accumulated_range_data())
+            if (_numAccumulated >= options.NumAccumulatedRangeData)
+            {
+                // Match C++ lines 206-211: sensor_duration calculation
+                var currentSensorTime = synchronizedData.Time;
+                long? sensorDuration = null;
+                if (_lastSensorTime.HasValue)
+                {
+                    sensorDuration = currentSensorTime - _lastSensorTime.Value;
+                }
+                _lastSensorTime = currentSensorTime;
+
+                // Match C++: num_accumulated_ = 0;
+                _numAccumulated = 0;
+                
+                // Match C++: const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(extrapolator_->EstimateGravityOrientation(time));
+                var gravityAlignment = _extrapolator.EstimateGravityOrientation(time);
+                var gravityAlignmentRigid = new Rigid3d(RobotNet10.Shared.Numbers.Vector3.Zero, gravityAlignment);
+                var gravityAlignmentRigidF = new Rigid3f(gravityAlignmentRigid.Translation, gravityAlignmentRigid.Rotation);
+
+                // Match C++: accumulated_range_data_.origin = range_data_poses.back().translation();
+                // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time 'time'.
+                Rigid3f lastPoseF;
+                if (_lastRangeDataPoses != null && _lastRangeDataPoses.Count > 0)
+                {
+                    // Match C++: use last pose from rangeDataPoses
+                    lastPoseF = _lastRangeDataPoses[^1];
+                }
+                else
+                {
+                    // Fallback: use extrapolated pose at sensor time (should not happen in normal operation)
+                    var lastPose = _extrapolator.ExtrapolatePose(time);
+                    lastPoseF = new Rigid3f(lastPose.Translation, lastPose.Rotation);
+                }
+                
+                // Match C++: Set origin from last pose
+                _accumulatedRangeData = new RangeData(
+                    lastPoseF.Translation,
+                    _accumulatedRangeData.Returns,
+                    _accumulatedRangeData.Misses
+                );
+
+                // Match C++: TransformToGravityAlignedFrameAndFilter(
+                //     gravity_alignment.cast<double>() * range_data_poses.back().inverse(),
+                //     accumulated_range_data_)
+                // CRITICAL: Transform formula must match C++ exactly:
+                // transform_to_gravity_aligned = gravity_alignment.cast<double>() * range_data_poses.back().inverse()
+                var transformToGravityAligned = gravityAlignmentRigidF * lastPoseF.Inverse();
+                
+                // Clear stored poses after use (they're no longer needed)
+                _lastRangeDataPoses = null;
+                var gravityAlignedRangeData = TransformToGravityAlignedFrameAndFilter(
+                    transformToGravityAligned,
+                    _accumulatedRangeData
+                );
+                
+                if (gravityAlignedRangeData.Returns.Count == 0)
+                {
+                    return null;
+                }
+                
+                // Match C++: AddAccumulatedRangeData(time, ...) - use original time, not clamped
+                return AddAccumulatedRangeData(time, gravityAlignedRangeData, gravityAlignmentRigid, sensorDuration);
+            }
+        }
+        catch(Exception ex)
+        {
+            Console.WriteLine(ex);
+        }
+
+        return null;
+    }
+
+    public void AddImuData(ImuData imuData)
+    {
+        if (!options.UseImuData)
+        {
+            throw new InvalidOperationException("An unexpected IMU packet was added.");
+        }
+
+        InitializeExtrapolator(imuData.Time);
+        _extrapolator?.AddImuData(imuData);
+    }
+
+    public void AddOdometryData(OdometryData odometryData)
+    {
+        // Store first odometry data if extrapolator is not initialized yet
+        // This allows us to use it for initial pose when UseOdometryDirectly is enabled
+        if (_extrapolator == null)
+        {
+            _firstOdometryData ??= odometryData;
+            // Until we've initialized the extrapolator we cannot add odometry data.
+            return;
+        }
+
+        _extrapolator.AddOdometryData(odometryData);
+    }
+
+    private void InitializeExtrapolator(long time)
+    {
+        if (_extrapolator != null)
+        {
+            return;
+        }
+
+        var poseQueueDuration = options.PoseExtrapolatorOptions.ConstantVelocity.PoseQueueDuration * TimeSpan.TicksPerSecond; // Convert seconds to ticks (10 million ticks per second)
+        var imuGravityTimeConstant = options.PoseExtrapolatorOptions.ConstantVelocity.ImuGravityTimeConstant;
+
+        // Match C++: PoseExtrapolator constructor takes only 2 parameters
+        _extrapolator = new PoseExtrapolator((long)poseQueueDuration, imuGravityTimeConstant);
+
+        // Use custom initial pose if set, otherwise use Identity (Match C++ default behavior)
+        Rigid3d initialPose = Rigid3d.Identity;
+
+        _extrapolator.AddPose(time, initialPose);
+    }
+
+    /// <summary>
+    /// Validates that the given time is not older than the extrapolator's last pose time or last extrapolated time.
+    /// This is a safety check to prevent out-of-order data processing.
+    /// Note: C++ relies on CHECK macros and direct comparisons; this provides equivalent validation in C#.
+    /// </summary>
+    /// <returns>True if time is valid, false otherwise.</returns>
+    private bool ValidateTime(long time, out long validatedTime)
+    {
+        if (_extrapolator == null)
+        {
+            validatedTime = time;
+            return true;
+        }
+
+        var lastPoseTime = _extrapolator.GetLastPoseTime();
+        var lastExtrapolatedTime = _extrapolator.GetLastExtrapolatedTime();
+        var minValidTime = Math.Max(lastPoseTime, lastExtrapolatedTime);
+        
+        if (time < minValidTime)
+        {
+            validatedTime = time;
+            return false; // Time is invalid and we don't want to adjust it
+        }
+        
+        validatedTime = time;
+        return true;
+    }
+
+    /// <summary>
+    /// Tries to get the current pose from the extrapolator at the given time.
+    /// Returns null when extrapolator is not initialized or time is before the last pose time.
+    /// Used by ITrajectoryBuilder.TryGetExtrapolatedPose so callers (e.g. CartographerService) can read a live pose.
+    /// </summary>
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        if (_extrapolator == null)
+            return null;
+        if (time < _extrapolator.GetLastPoseTime())
+            return null;
+        try
+        {
+            return _extrapolator.ExtrapolatePose(time);
+        }
+        catch (ArgumentException)
+        {
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Tries to get the current pose with low-pass filter to reduce jitter during direction changes.
+    /// Match C++: ExtrapolatePose_filter - should be used for publishing pose to external systems.
+    /// </summary>
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        if (_extrapolator == null)
+            return null;
+        if (time < _extrapolator.GetLastPoseTime())
+            return null;
+        try
+        {
+            return _extrapolator.ExtrapolatePoseFilter(time);
+        }
+        catch (ArgumentException)
+        {
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Creates a sample point cloud with maximum 360 points (1 degree per point).
+    /// For each 1-degree bin, keeps only the closest point to origin.
+    /// Fast algorithm: O(n) where n is number of points in Returns.
+    /// </summary>
+    private static PointCloud CreateSample360PointCloud(PointCloud gravityAlignedPoints)
+    {
+        if (gravityAlignedPoints.Count == 0)
+        {
+            return new PointCloud();
+        }
+
+        // Create 360 bins (0-359 degrees), storing closest point for each bin
+        var bins = new (RangefinderPoint point, double distanceSq)?[360];
+        
+        foreach (var point in gravityAlignedPoints.Points)
+        {
+            var pos = point.Position;
+            
+            // Calculate angle in radians, then convert to degrees
+            var angleRad = Math.Atan2(pos.Y, pos.X);
+            var angleDeg = angleRad * (180.0 / Math.PI);
+            
+            // Normalize to 0-359 range
+            var degreeIndex = ((int)Math.Round(angleDeg) + 360) % 360;
+            
+            // Calculate distance squared (faster than distance)
+            var distanceSq = pos.X * pos.X + pos.Y * pos.Y;
+            
+            // Keep only the closest point for each degree bin
+            var existing = bins[degreeIndex];
+            if (!existing.HasValue || distanceSq < existing.Value.distanceSq)
+            {
+                bins[degreeIndex] = (point, distanceSq);
+            }
+        }
+        
+        // Collect all sampled points
+        var sampledPoints = new List<RangefinderPoint>();
+        for (int i = 0; i < 360; i++)
+        {
+            var bin = bins[i];
+            if (bin.HasValue)
+            {
+                sampledPoints.Add(bin.Value.point);
+            }
+        }
+        
+        return new PointCloud(sampledPoints);
+    }
+
+    private ITrajectoryBuilder.MatchingResult? AddAccumulatedRangeData(
+        long time,
+        RangeData gravityAlignedRangeData,
+        Rigid3d gravityAlignment,
+        long? sensorDuration)
+    {
+        if (gravityAlignedRangeData.Returns.Count == 0)
+        {
+            return null;
+        }
+
+        // Increment call count to track first 5 calls where isSimilar is ignored
+        _accumulatedRangeDataCallCount++;
+
+        var swAccum = Stopwatch.StartNew();
+
+        if (_extrapolator == null)
+        {
+            throw new InvalidOperationException("Extrapolator is null when trying to extrapolate pose");
+        }
+
+        // Match C++ line 240-241: extrapolator_->ExtrapolatePose(time)
+        // Non-monotonic timestamps from RangeDataCollator are handled in
+        // PoseExtrapolator.ExtrapolatePose by resetting _extrapolationImuTracker when needed.
+        var nonGravityAlignedPosePrediction = _extrapolator.ExtrapolatePose(time);
+        
+        // Match C++: pose_prediction = Project2D(non_gravity_aligned_pose_prediction * gravity_alignment.inverse())
+        var poseBeforeProject = nonGravityAlignedPosePrediction * gravityAlignment.Inverse();
+        var posePrediction2D = TransformOperations.Project2D(poseBeforeProject);
+        
+        // [DIAG] Log frame context to correlate odom-window size with grid resize events
+        /*{
+            var odomWinMs = _extrapolator.GetOdometryWindowMs();
+            var dtMs = sensorDuration.HasValue ? sensorDuration.Value / 10_000.0 : 0;
+            Console.WriteLine($"[FRAME_PRE] t={time / 10_000_000.0:F3}s, dt={dtMs:F1}ms, " +
+                $"pred=({posePrediction2D.Translation.X:F3},{posePrediction2D.Translation.Y:F3},{posePrediction2D.Rotation * 180 / Math.PI:F1}deg), " +
+                $"odomWin={odomWinMs:F0}ms");
+        }*/
+
+        // Use configured adaptive voxel filter options
+        var filteredGravityAlignedPointCloud = AdaptiveVoxelFilter.Filter(
+            gravityAlignedRangeData.Returns,
+            options.AdaptiveVoxelFilterOptions
+        );
+        
+        if (filteredGravityAlignedPointCloud.Count == 0)
+        {
+            return null;
+        }
+
+        // Match C++ lines 259-272: motion filter gate - if similar, skip ScanMatch and use pose_prediction; skip InsertIntoSubmap.
+        bool isSimilar = _motionFilter.IsSimilar(time, nonGravityAlignedPosePrediction);
+
+        // For the first 5 calls, ignore isSimilar and always perform matching
+        bool shouldPerformMatching = !isSimilar || _accumulatedRangeDataCallCount <= 5;
+
+        // Match C++: ceresScore and poseConfidence tracking (line 250 in C++)
+        double poseConfidence = -1.0;
+
+        Rigid2d poseEstimate2DValue;
+        double scanMatchCeresScore = -1.0;
+        // Three-tier Ceres cost threshold control variables
+        bool shouldAddPose = true;
+        bool shouldInsert = false;
+        bool shouldForceNewSubmap = false;
+        long swScanMatchElapsedMilliseconds = 0;
+        double residualDist = double.MaxValue;
+        double residualAngleDeg = double.MaxValue;
+        if (shouldPerformMatching)
+        {
+            // Match C++ lines 263-270: ScanMatch returns pose and ceres_score
+            var swScanMatch = Stopwatch.StartNew();
+            var scanMatchResult = ScanMatch(posePrediction2D, filteredGravityAlignedPointCloud);
+            swScanMatch.Stop();
+            swScanMatchElapsedMilliseconds = swScanMatch.ElapsedMilliseconds;
+            poseEstimate2DValue = scanMatchResult.poseEstimate;
+            scanMatchCeresScore = scanMatchResult.ceresScore;
+
+            // [DIAG] Log scan match residual: large values mean prediction was far from truth
+            residualDist = (poseEstimate2DValue.Translation - posePrediction2D.Translation).Length();
+            residualAngleDeg = Math.Abs(poseEstimate2DValue.Rotation - posePrediction2D.Rotation) * 180.0 / Math.PI;
+
+            // === Three-tier Ceres cost threshold logic ===
+            // Tier 1 (Normal): ceresScore <= SoftLimit → AddPose + InsertIntoSubmap, reset counter
+            // Tier 2 (Soft):   SoftLimit < ceresScore <= HardLimit → AddPose + InsertIntoSubmap, counter += 1
+            // Tier 3 (Hard):   ceresScore > HardLimit → no AddPose, no InsertIntoSubmap, counter += 2
+            // Force new submap when _consecutiveHardLimitCount >= MaxConsecutiveHighCostBeforeNewSubmap
+            bool softEnabled = options.CeresScoreSoftLimit > 0;
+            bool hardEnabled = options.CeresScoreHardLimit > 0;
+
+            if ((softEnabled || hardEnabled) && scanMatchCeresScore >= 0)
+            {
+                var submaps = _activeSubmaps.Submaps();
+                var firstSubmapNumRangeData = submaps.Count > 0 ? submaps[0].NumRangeData : 0;
+                const int minRangeDataForConvergenceCheck = 3;
+                bool passesSubmapCheck = firstSubmapNumRangeData >= minRangeDataForConvergenceCheck;
+
+                if (hardEnabled && passesSubmapCheck && scanMatchCeresScore > options.CeresScoreHardLimit)
+                {
+                    // TIER 3 - HARD: pose is unreliable, use odometry prediction
+                    poseEstimate2DValue = posePrediction2D;
+                    shouldAddPose = false;
+                    _consecutiveHardLimitCount += 2;
+                    //Console.WriteLine($"[SCAN_DIAG] HARD limit: ceres_score={scanMatchCeresScore:F4} > {options.CeresScoreHardLimit:F4}, consecutive={_consecutiveHardLimitCount}");
+                }
+                else if (softEnabled && passesSubmapCheck && scanMatchCeresScore > options.CeresScoreSoftLimit)
+                {
+                    // TIER 2 - SOFT: trust pose, still insert, but accumulate toward new submap
+                    shouldAddPose = true;
+                    shouldInsert = true;
+                    _consecutiveHardLimitCount += 1;
+                    //Console.WriteLine($"[SCAN_DIAG] SOFT limit: ceres_score={scanMatchCeresScore:F4} > {options.CeresScoreSoftLimit:F4}, consecutive={_consecutiveHardLimitCount}");
+                }
+                else
+                {
+                    // TIER 1 - NORMAL: good match
+                    shouldAddPose = true;
+                    shouldInsert = true;
+                    _consecutiveHardLimitCount = 0;
+                }
+
+                // Check force new submap threshold (applies to both soft and hard accumulation)
+                if (options.MaxConsecutiveHighCostBeforeNewSubmap > 0 &&
+                    _consecutiveHardLimitCount >= options.MaxConsecutiveHighCostBeforeNewSubmap)
+                {
+                    Console.WriteLine($"[SCAN_DIAG] FORCE NEW SUBMAP: consecutive={_consecutiveHardLimitCount} >= {options.MaxConsecutiveHighCostBeforeNewSubmap}");
+                    shouldAddPose = true;
+                    shouldForceNewSubmap = true;
+                    _consecutiveHardLimitCount = 0;
+                }
+            }
+            else
+            {
+                // Thresholds disabled or ceresScore is -1.0 (no grid) - normal insert
+                shouldAddPose = true;
+                shouldInsert = true;
+                _consecutiveHardLimitCount = 0;
+            }
+
+            // Match C++ lines 100-104: compute pose_confidence if provide_confidence_score is enabled
+            if (options.ProvideConfidenceScore)
+            {
+                var submaps = _activeSubmaps.Submaps();
+                var grid = submaps.Count > 0 ? submaps[0].Grid : null;
+                if (grid != null)
+                {
+                    poseConfidence = _realTimeCorrelativeScanMatcher.LocalPose_Confidence(
+                        poseEstimate2DValue,
+                        filteredGravityAlignedPointCloud,
+                        grid);
+                }
+            }
+        }
+        else
+        {
+            //Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [FRAME_SIMILAR]");
+            // Match C++ line 271: pose_estimate_2d = pose_prediction when is_similar
+            poseEstimate2DValue = posePrediction2D;
+            // Note: _consecutiveHardLimitCount is NOT reset on isSimilar frames
+        }
+
+        Rigid3d poseEstimate3D = TransformOperations.Embed3D(poseEstimate2DValue);
+        var poseEstimate = poseEstimate3D * gravityAlignment;
+
+        // Match C++ line 281: extrapolator_->AddPose(time, pose_estimate)
+        // Skip AddPose for hard-limit failures (pose is unreliable, use odometry instead)
+        if (shouldAddPose)
+        {
+            _extrapolator.AddPose(time, poseEstimate);
+        }
+        Rigid2d transformPose2D = poseEstimate2DValue;
+
+        var transformPose3D = TransformOperations.Embed3D(transformPose2D);
+        var transformPose3F = new Rigid3f((RobotNet10.Shared.Numbers.Vector3)transformPose3D.Translation, transformPose3D.Rotation);
+
+        // C++: TransformRangeData(gravity_aligned_range_data, transform::Embed3D(pose_estimate_2d->cast<double>()))
+        var rangeDataInLocal = RangeDataOperations.Transform(
+            gravityAlignedRangeData,
+            transformPose3F
+        );
+
+        // Insert decision: normal insert, force new submap, or skip
+        InsertionResult? localInsertionResult = null;
+        if (shouldForceNewSubmap)
+        {
+            // Force new submap: finish current front, create new, insert range data
+            var swInsert = Stopwatch.StartNew();
+            var forceSubmaps = _activeSubmaps.ForceNewSubmapAndInsert(rangeDataInLocal);
+            swInsert.Stop();
+            swAccum.Stop();
+
+            var constantData = new TrajectoryNode.Data
+            {
+                Time = time,
+                GravityAlignment = gravityAlignment.Rotation,
+                FilteredGravityAlignedPointCloud = filteredGravityAlignedPointCloud,
+                LocalPose = poseEstimate
+            };
+            localInsertionResult = new InsertionResult(constantData, forceSubmaps);
+            Console.WriteLine($"[SCAN_DIAG] FORCED NEW SUBMAP: elapsed={swAccum.ElapsedMilliseconds}ms, insert={swInsert.ElapsedMilliseconds}ms, ceresScore={scanMatchCeresScore:F4}");
+        }
+        else if (shouldPerformMatching && shouldInsert)
+        {
+            // Normal insert into existing submaps
+            var swInsert = Stopwatch.StartNew();
+            localInsertionResult = InsertIntoSubmap(
+                time,
+                rangeDataInLocal,
+                filteredGravityAlignedPointCloud,
+                poseEstimate,
+                gravityAlignment.Rotation
+            );
+            swInsert.Stop();
+            swAccum.Stop();
+            // [DIAG] odomWinAfter: if insert triggered GrowLimits, odomWinAfter >> normal scan period
+            var odomWinAfterMs = _extrapolator.GetOdometryWindowMs();
+            /*Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [FRAME_POST] total={swAccum.ElapsedMilliseconds}ms, " +
+                $"scanMatch={swScanMatchElapsedMilliseconds}ms, insert={swInsert.ElapsedMilliseconds}ms, " +
+                $"odomWinAfter={odomWinAfterMs:F0}ms, ceresScore={scanMatchCeresScore:F4}");*/
+            _matchingLog.WriteLine($"{DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff},{swAccum.ElapsedMilliseconds},{swScanMatchElapsedMilliseconds},{swInsert.ElapsedMilliseconds},{odomWinAfterMs:F0},{scanMatchCeresScore:F4},{residualDist:F4},{residualAngleDeg:F2}");
+        }
+
+        ITrajectoryBuilder.InsertionResult? insertionResult = null;
+        if (localInsertionResult.HasValue)
+        {
+            var localInsertion = localInsertionResult.Value;
+            insertionResult = new ITrajectoryBuilder.InsertionResult(
+                nodeId: default, // NodeId will be assigned by PoseGraph
+                constantData: localInsertion.ConstantData,
+                insertionSubmaps: [.. localInsertion.InsertionSubmaps.Cast<Submap>()]
+            );
+        }
+
+        // Note: This point cloud is in LOCAL trajectory frame. It will be transformed to
+        // GLOBAL frame by GlobalTrajectoryBuilder2D before being returned to the caller.
+        // Throttled to 1Hz to reduce CPU cost of visualization-only data.
+        PointCloud? samplePointCloudLocal = null;
+        if (_samplePointCloudStopwatch.ElapsedMilliseconds >= 1000)
+        {
+            _samplePointCloudStopwatch.Restart();
+            samplePointCloudLocal = CreateSample360PointCloud(rangeDataInLocal.Returns);
+        }
+
+        // Match C++ lines 300-320: Record wall time and CPU time metrics
+        // Convert sensorDuration from ticks to TimeSpan
+        TimeSpan? sensorDurationTimeSpan = sensorDuration.HasValue
+            ? TimeSpan.FromTicks(sensorDuration.Value)
+            : null;
+        RecordMetrics(sensorDurationTimeSpan);
+
+        // Match C++ line 321-323: return MatchingResult{time, pose_estimate, range_data_in_local, insertion_result, pose_confidence, 0}
+        // C++ returns 0 for ceres_score (the commented line "ceres_score = summary.final_cost/..." was never enabled)
+        return new ITrajectoryBuilder.MatchingResult(
+            trajectoryId: 0,
+            time: time,
+            localPose: poseEstimate,
+            rangeDataInLocal: rangeDataInLocal,
+            insertionResult: insertionResult,
+            poseConfidence: poseConfidence,
+            ceresScore: 0,  // Match C++: always returns 0
+            samplePointCloudGlobal: samplePointCloudLocal);
+    }
+
+    /// <summary>
+    /// Match C++ lines 300-320: Record wall time and CPU time metrics.
+    /// </summary>
+    private void RecordMetrics(TimeSpan? sensorDuration)
+    {
+        // Match C++ line 300: const auto wall_time = std::chrono::steady_clock::now()
+        // Using Stopwatch.GetTimestamp() for high-resolution timing
+        var currentWallTimestamp = Stopwatch.GetTimestamp();
+
+        // Match C++ lines 301-307: Calculate wall time duration since last call
+        if (_lastWallTimestamp.HasValue)
+        {
+            // Convert timestamp difference to seconds
+            var ticksElapsed = currentWallTimestamp - _lastWallTimestamp.Value;
+            var wallTimeDurationSeconds = (double)ticksElapsed / Stopwatch.Frequency;
+
+            // Match C++ line 303: kLocalSlamLatencyMetric->Set(wall_time_duration_seconds)
+            _kLocalSlamLatencyMetric.Set(wallTimeDurationSeconds);
+
+            // Match C++ lines 304-307: Real-time ratio (sensor_duration / wall_time_duration)
+            if (sensorDuration.HasValue && wallTimeDurationSeconds > 0)
+            {
+                _kLocalSlamRealTimeRatio.Set(sensorDuration.Value.TotalSeconds / wallTimeDurationSeconds);
+            }
+        }
+
+        // Match C++ lines 309-318: Thread CPU time metrics
+        // Note: .NET doesn't have direct thread CPU time API, using process time as approximation
+        var threadCpuTimeSeconds = Process.GetCurrentProcess().TotalProcessorTime.TotalSeconds;
+        if (_lastThreadCpuTimeSeconds.HasValue)
+        {
+            var threadCpuDuration = threadCpuTimeSeconds - _lastThreadCpuTimeSeconds.Value;
+            if (sensorDuration.HasValue && threadCpuDuration > 0)
+            {
+                // Match C++ lines 314-316: kLocalSlamCpuRealTimeRatio
+                _kLocalSlamCpuRealTimeRatio.Set(sensorDuration.Value.TotalSeconds / threadCpuDuration);
+            }
+        }
+
+        // Match C++ lines 319-320: Update last values
+        _lastWallTimestamp = currentWallTimestamp;
+        _lastThreadCpuTimeSeconds = threadCpuTimeSeconds;
+    }
+
+    private RangeData TransformToGravityAlignedFrameAndFilter(
+        Rigid3f transformToGravityAlignedFrame,
+        RangeData rangeData)
+    {
+        var transformedRangeData = RangeDataOperations.Transform(rangeData, transformToGravityAlignedFrame);
+        var cropped = RangeDataOperations.Crop(transformedRangeData, options.MinZ, options.MaxZ);
+        var filteredReturns = VoxelFilter.Filter(cropped.Returns, options.VoxelFilterSize);
+        var filteredMisses = VoxelFilter.Filter(cropped.Misses, options.VoxelFilterSize);
+        
+        return new RangeData(cropped.Origin, filteredReturns, filteredMisses);
+    }
+
+    /// <summary>
+    /// Match C++: ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud, ceres_score, pose_confidence)
+    /// Returns pose estimate and ceres_score (final cost from Ceres solver).
+    /// C++ behavior: The solver always produces a result (possibly suboptimal if not converged).
+    /// </summary>
+    private (Rigid2d poseEstimate, double ceresScore) ScanMatch(
+        Rigid2d posePrediction,
+        PointCloud filteredGravityAlignedPointCloud)
+    {
+        var submaps = _activeSubmaps.Submaps();
+
+        if (submaps.Count == 0) return (posePrediction, -1.0);
+
+        var matchingSubmap = submaps[0];
+
+        // Use point cloud directly (no transformation needed)
+        var pointCloudInSubmapFrame = filteredGravityAlignedPointCloud;
+        var posePredictionInSubmapFrame = posePrediction;
+
+        var grid = matchingSubmap.Grid;
+        if (grid == null) return (posePrediction, -1.0);
+        var highResGrid = matchingSubmap.HighResGrid;
+
+        Rigid2d initialCeresPose = posePrediction;
+
+        // Match C++ lines 86-91: Use online correlative scan matcher if enabled
+        if (options.UseOnlineCorrelativeScanMatching && _realTimeCorrelativeScanMatcher != null)
+        {
+            var score = _realTimeCorrelativeScanMatcher.Match(
+                posePredictionInSubmapFrame,
+                pointCloudInSubmapFrame,
+                grid,
+                out var refinedPose);
+            initialCeresPose = refinedPose;
+
+            // Match C++ line 90: kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score)
+            _kRealTimeCorrelativeScanMatcherScoreMetric.Observe(score);
+        }
+
+        // CRITICAL: Initialize summary to null before try block to ensure it's always in scope
+        CeresSharp.SolverSummary? summary = null;
+        double ceresScore = -1.0;
+        try
+        {
+            // Use Ceres scan matcher for fine alignment
+            _ceresScanMatcher.Match(
+                posePredictionInSubmapFrame.Translation,
+                initialCeresPose,
+                pointCloudInSubmapFrame,
+                grid,
+                out Rigid2d poseEstimate,
+                out summary,
+                highResGrid);
+
+            // Match C++ lines 107-117: Record Ceres metrics
+            if (summary != null)
+            {
+                ceresScore = summary.FinalCost;
+
+                // Match C++ line 108: kCeresScanMatcherCostMetric->Observe(summary.final_cost)
+                _kCeresScanMatcherCostMetric.Observe(ceresScore);
+
+                // Match C++ lines 109-112: Residual distance metric
+                var residualDistance = (poseEstimate.Translation - posePrediction.Translation).Length();
+                _kScanMatcherResidualDistanceMetric.Observe(residualDistance);
+
+                // Match C++ lines 113-116: Residual angle metric
+                var residualAngle = Math.Abs(poseEstimate.Rotation - posePrediction.Rotation);
+                _kScanMatcherResidualAngleMetric.Observe(residualAngle);
+            }
+
+            return (poseEstimate, ceresScore);
+        }
+        finally
+        {
+            // CRITICAL: Always dispose summary in finally block to prevent memory leaks
+            summary?.Dispose();
+        }
+    }
+
+    /// <summary>
+    /// Inserts range data into active submaps. Called only when !is_similar (motion filter gate is in AddAccumulatedRangeData).
+    /// Match C++: motion_filter check is commented out in InsertIntoSubmap; gate is in AddAccumulatedRangeData.
+    /// </summary>
+    private InsertionResult? InsertIntoSubmap(
+        long time,
+        RangeData rangeDataInLocal,
+        PointCloud filteredGravityAlignedPointCloud,
+        Rigid3d poseEstimate,
+        QuaternionNumbers gravityAlignment)
+    {
+        var insertionSubmaps = _activeSubmaps.InsertRangeData(rangeDataInLocal);
+
+        var constantData = new TrajectoryNode.Data
+        {
+            Time = time,
+            GravityAlignment = gravityAlignment,
+            FilteredGravityAlignedPointCloud = filteredGravityAlignedPointCloud,
+            LocalPose = poseEstimate
+        };
+
+        return insertionSubmaps == null ? null : new InsertionResult(constantData, insertionSubmaps);
+    }
+
+    /// <summary>
+    /// Gets the trajectory builder options.
+    /// Used by GlobalTrajectoryBuilder2D for accessing scan matcher options during relocalization.
+    /// </summary>
+    public LocalTrajectoryBuilderOptions2D GetOptions()
+    {
+        return options;
+    }
+
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            _activeSubmaps.Dispose();
+            _ceresScanMatcher?.Dispose();
+            _disposed = true;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/NormalEstimation2D.cs

@@ -0,0 +1,133 @@
+/*
+ * 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.Models.Mapping;
+using CartographerSharp.Sensor;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Estimates surface normals from range data for TSDF computation.
+/// </summary>
+public static class NormalEstimation2D
+{
+    private const double kMinNormalLength = 1e-6;
+
+    /// <summary>
+    /// Estimates the normal for each 'return' in 'range_data'.
+    /// Assumes the angles in the range data returns are sorted with respect to
+    /// the orientation of the vector from 'origin' to 'return'.
+    /// </summary>
+    public static List<double> EstimateNormals(
+        RangeData rangeData,
+        NormalEstimationOptions2D normalEstimationOptions)
+    {
+        var normals = new List<double>(rangeData.Returns.Count);
+        var maxNumSamples = normalEstimationOptions.NumNormalSamples;
+        var sampleRadius = normalEstimationOptions.SampleRadius;
+
+        for (int currentPoint = 0; currentPoint < rangeData.Returns.Count; currentPoint++)
+        {
+            var hit = rangeData.Returns.Points[currentPoint].Position;
+            
+            // Find sample window begin
+            int sampleWindowBegin = currentPoint;
+            for (; sampleWindowBegin > 0 &&
+                   currentPoint - sampleWindowBegin < maxNumSamples / 2 &&
+                   Vector3.Distance(hit, rangeData.Returns.Points[sampleWindowBegin - 1].Position) < sampleRadius;
+                   sampleWindowBegin--)
+            {
+            }
+
+            // Find sample window end
+            int sampleWindowEnd = currentPoint;
+            for (;
+                 sampleWindowEnd < rangeData.Returns.Count &&
+                 sampleWindowEnd - currentPoint < (int)Math.Ceiling(maxNumSamples / 2.0) + 1 &&
+                 Vector3.Distance(hit, rangeData.Returns.Points[sampleWindowEnd].Position) < sampleRadius;
+                 sampleWindowEnd++)
+            {
+            }
+
+            var normalEstimate = EstimateNormal(
+                rangeData.Returns,
+                currentPoint,
+                sampleWindowBegin,
+                sampleWindowEnd,
+                rangeData.Origin);
+            normals.Add(normalEstimate);
+        }
+
+        return normals;
+    }
+
+    /// <summary>
+    /// Estimate the normal of an estimation_point as the arithmetic mean of the normals
+    /// of the vectors from estimation_point to each point in the sample_window.
+    /// </summary>
+    private static double EstimateNormal(
+        PointCloud returns,
+        int estimationPointIndex,
+        int sampleWindowBegin,
+        int sampleWindowEnd,
+        Vector3 sensorOrigin)
+    {
+        var estimationPoint = returns.Points[estimationPointIndex].Position;
+        
+        if (sampleWindowEnd - sampleWindowBegin < 2)
+        {
+            return NormalTo2DAngle(sensorOrigin - estimationPoint);
+        }
+
+        Vector3 meanNormal = Vector3.Zero;
+        var estimationPointToObservation = sensorOrigin - estimationPoint;
+
+        for (int samplePointIndex = sampleWindowBegin; samplePointIndex < sampleWindowEnd; samplePointIndex++)
+        {
+            if (samplePointIndex == estimationPointIndex) continue;
+
+            var samplePoint = returns.Points[samplePointIndex].Position;
+            var tangent = estimationPoint - samplePoint;
+            var sampleNormal = new Vector3(-tangent.Y, tangent.X, 0.0);
+
+            if (sampleNormal.Length() < kMinNormalLength)
+            {
+                continue;
+            }
+
+            // Ensure sample_normal points towards 'sensor_origin'.
+            if (Vector3.Dot(sampleNormal, estimationPointToObservation) < 0)
+            {
+                sampleNormal = -sampleNormal;
+            }
+
+            sampleNormal = Vector3.Normalize(sampleNormal);
+            meanNormal += sampleNormal;
+        }
+
+        return NormalTo2DAngle(meanNormal);
+    }
+
+    /// <summary>
+    /// Converts a 3D normal vector to a 2D angle (in radians).
+    /// </summary>
+    private static double NormalTo2DAngle(Vector3 v)
+    {
+        return Math.Atan2(v.Y, v.X);
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/OverlappingSubmapsTrimmer2D.cs

@@ -0,0 +1,364 @@
+/*
+ * 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.D2D;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Trims submaps from the pose graph based on overlap area.
+/// Removes older submaps that overlap significantly with newer ones,
+/// keeping only the freshest submaps while ensuring minimum coverage.
+/// </summary>
+public class OverlappingSubmapsTrimmer2D : PoseGraphTrimmer
+{
+    private readonly int _freshSubmapsCount;
+    private readonly double _minCoveredArea;
+    private readonly int _minAddedSubmapsCount;
+
+    // Current finished submap count (matches C++ current_submap_count_)
+    private int _currentSubmapCount = 0;
+
+    public OverlappingSubmapsTrimmer2D(
+        int freshSubmapsCount,
+        double minCoveredArea,
+        int minAddedSubmapsCount)
+    {
+        if (freshSubmapsCount < 0)
+            throw new ArgumentException("freshSubmapsCount must be non-negative", nameof(freshSubmapsCount));
+        if (minCoveredArea < 0)
+            throw new ArgumentException("minCoveredArea must be non-negative", nameof(minCoveredArea));
+        if (minAddedSubmapsCount < 0)
+            throw new ArgumentException("minAddedSubmapsCount must be non-negative", nameof(minAddedSubmapsCount));
+
+        _freshSubmapsCount = freshSubmapsCount;
+        _minCoveredArea = minCoveredArea;
+        _minAddedSubmapsCount = minAddedSubmapsCount;
+    }
+
+    public override void Trim(ITrimmable trimmable)
+    {
+        var submapData = trimmable.GetOptimizedSubmapData();
+        
+        // Match C++: if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_)
+        if (submapData.Count - _currentSubmapCount <= _minAddedSubmapsCount)
+        {
+            return;
+        }
+
+        // Get first submap's map limits to initialize coverage grid
+        if (submapData.Count == 0)
+        {
+            return;
+        }
+
+        var firstSubmapData = submapData.First();
+        if (firstSubmapData.Data.Submap is not Mapping.D2D.Submap2D firstSubmap || firstSubmap.Grid == null)
+        {
+            return;
+        }
+
+        var firstSubmapMapLimits = firstSubmap.Grid.Limits;
+        var coverageGrid = new SubmapCoverageGrid2D(firstSubmapMapLimits);
+
+        // Compute submap freshness from intra-submap constraints
+        var submapFreshness = ComputeSubmapFreshness(
+            submapData,
+            trimmable.GetTrajectoryNodes(),
+            trimmable.GetConstraints());
+
+        // Add all submaps to coverage grid
+        var allSubmapIds = AddSubmapsToSubmapCoverageGrid2D(
+            submapFreshness,
+            submapData,
+            coverageGrid);
+
+        // Find submaps to trim
+        // Match C++: min_covered_area_ / common::Pow2(coverage_grid.resolution())
+        var minCoveredCellsCount = (int)Math.Round(_minCoveredArea / MathUtils.Pow2(coverageGrid.Resolution));
+        var submapIdsToRemove = FindSubmapIdsToTrim(
+            coverageGrid,
+            allSubmapIds,
+            _freshSubmapsCount,
+            minCoveredCellsCount);
+
+        // Update current submap count (matches C++: current_submap_count_ = submap_data.size() - submap_ids_to_remove.size())
+        _currentSubmapCount = submapData.Count - submapIdsToRemove.Count;
+
+        // Trim the submaps
+        foreach (var id in submapIdsToRemove)
+        {
+            trimmable.TrimSubmap(id);
+        }
+    }
+
+    /// <summary>
+    /// Tracks which submaps cover which cells in a global coordinate system.
+    /// </summary>
+    private class SubmapCoverageGrid2D(Mapping.D2D.MapLimits mapLimits)
+    {
+        // Aliases for documentation only (no type-safety).
+        public record CellId(long X, long Y);
+
+        public record StoredType(SubmapId SubmapId, long Time);
+
+        private readonly Vector2 _offset = mapLimits.Max;
+        private readonly double _resolution = mapLimits.Resolution;
+        private readonly Dictionary<CellId, List<StoredType>> _cells = [];
+
+        public void AddPoint(Vector2 point, SubmapId submapId, long time)
+        {
+            var cellId = new CellId(
+                (long)Math.Round((_offset.X - point.X) / _resolution, MidpointRounding.AwayFromZero),
+                (long)Math.Round((_offset.Y - point.Y) / _resolution, MidpointRounding.AwayFromZero));
+            
+            if (!_cells.TryGetValue(cellId, out var storedTypes))
+            {
+                storedTypes = [];
+                _cells[cellId] = storedTypes;
+            }
+            storedTypes.Add(new StoredType(submapId, time));
+        }
+
+        public Dictionary<CellId, List<StoredType>> Cells => _cells;
+        public double Resolution => _resolution;
+    }
+
+    /// <summary>
+    /// Uses intra-submap constraints and trajectory node timestamps to identify time
+    /// of the last range data insertion to the submap.
+    /// </summary>
+    private static Dictionary<SubmapId, long> ComputeSubmapFreshness(
+        MapById<SubmapId, IPoseGraph.SubmapData> submapData,
+        MapById<NodeId, TrajectoryNode> trajectoryNodes,
+        List<IPoseGraph.Constraint> constraints)
+    {
+        var submapFreshness = new Dictionary<SubmapId, long>();
+
+        // Find the node with the largest NodeId per SubmapId.
+        var submapToLatestNode = new Dictionary<SubmapId, NodeId>();
+        foreach (var constraint in constraints)
+        {
+            if (constraint.ConstraintTag != IPoseGraph.Constraint.Tag.IntraSubmap)
+            {
+                continue;
+            }
+
+            if (!submapToLatestNode.TryGetValue(constraint.SubmapId, out var existingNodeId))
+            {
+                submapToLatestNode[constraint.SubmapId] = constraint.NodeId;
+                continue;
+            }
+
+            // Keep the maximum NodeId (matches C++: std::max)
+            if (CompareNodeIds(constraint.NodeId, existingNodeId) > 0)
+            {
+                submapToLatestNode[constraint.SubmapId] = constraint.NodeId;
+            }
+        }
+
+        // Find timestamp of every latest node.
+        foreach (var (submapId, nodeId) in submapToLatestNode)
+        {
+            if (!submapData.Contains(submapId))
+            {
+                // Log warning equivalent (C++: LOG(WARNING))
+                continue;
+            }
+
+            if (!trajectoryNodes.Contains(nodeId))
+            {
+                continue;
+            }
+            var trajectoryNode = trajectoryNodes[nodeId];
+
+            if (trajectoryNode.ConstantData == null)
+            {
+                continue;
+            }
+
+            submapFreshness[submapId] = trajectoryNode.ConstantData.Time;
+        }
+
+        return submapFreshness;
+    }
+
+    /// <summary>
+    /// Compares two NodeIds. Returns positive if lhs > rhs, negative if lhs < rhs, 0 if equal.
+    /// </summary>
+    private static int CompareNodeIds(NodeId lhs, NodeId rhs)
+    {
+        var trajectoryCompare = lhs.TrajectoryId.CompareTo(rhs.TrajectoryId);
+        if (trajectoryCompare != 0)
+        {
+            return trajectoryCompare;
+        }
+        return lhs.NodeIndex.CompareTo(rhs.NodeIndex);
+    }
+
+    /// <summary>
+    /// Iterates over every cell in a submap, transforms the center of the cell to
+    /// the global frame and then adds the submap id and the timestamp of the most
+    /// recent range data insertion into the global grid.
+    /// </summary>
+    private static HashSet<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
+        Dictionary<SubmapId, long> submapFreshness,
+        MapById<SubmapId, IPoseGraph.SubmapData> submapData,
+        SubmapCoverageGrid2D coverageGrid)
+    {
+        var allSubmapIds = new HashSet<SubmapId>();
+
+        foreach (var submap in submapData)
+        {
+            if (!submapFreshness.TryGetValue(submap.Id, out var freshness))
+            {
+                continue;
+            }
+
+            if (submap.Data.Submap is not Mapping.D2D.Submap2D submap2D || !submap2D.InsertionFinished)
+            {
+                continue;
+            }
+
+            if (submap2D.Grid == null)
+            {
+                continue;
+            }
+
+            allSubmapIds.Add(submap.Id);
+
+            var grid = submap2D.Grid;
+            
+            // Iterate over every cell in a submap.
+            grid.ComputeCroppedLimits(out var offset, out var cellLimits);
+            if (cellLimits.NumXCells == 0 || cellLimits.NumYCells == 0)
+            {
+                // Log warning equivalent (C++: LOG(WARNING))
+                continue;
+            }
+
+            var globalFrameFromSubmapFrame = submap.Data.Pose;
+            var submapFrameFromLocalFrame = submap2D.LocalPose.Inverse();
+
+            foreach (var xyIndex in new XYIndexRange(cellLimits))
+            {
+                var index = xyIndex + offset;
+                if (!grid.IsKnown(index))
+                {
+                    continue;
+                }
+
+                // Match C++: center_of_cell_in_local_frame calculation
+                // C++: grid.limits().max().x() - grid.limits().resolution() * (index.y() + 0.5)
+                // C++: grid.limits().max().y() - grid.limits().resolution() * (index.x() + 0.5)
+                var centerOfCellInLocalFrame = new Rigid3d(
+                    new Vector3(
+                        (grid.Limits.Max.X - grid.Limits.Resolution * (index.Y + 0.5)),
+                        (grid.Limits.Max.Y - grid.Limits.Resolution * (index.X + 0.5)),
+                        0.0),
+                    Quaternion.Identity);
+
+                // Match C++: transform::Project2D(global_frame_from_submap_frame * submap_frame_from_local_frame * center_of_cell_in_local_frame)
+                var centerOfCellInGlobalFrame = TransformOperations.Project2D(
+                    globalFrameFromSubmapFrame *
+                    submapFrameFromLocalFrame *
+                    centerOfCellInLocalFrame);
+
+                coverageGrid.AddPoint(
+                    centerOfCellInGlobalFrame.Translation,
+                    submap.Id,
+                    freshness);
+            }
+        }
+
+        return allSubmapIds;
+    }
+
+    /// <summary>
+    /// Returns IDs of submaps that have less than 'min_covered_cells_count' cells
+    /// not overlapped by at least 'fresh_submaps_count' submaps.
+    /// </summary>
+    private static List<SubmapId> FindSubmapIdsToTrim(
+        SubmapCoverageGrid2D coverageGrid,
+        HashSet<SubmapId> allSubmapIds,
+        int freshSubmapsCount,
+        int minCoveredCellsCount)
+    {
+        var submapToCoveredCellsCount = new Dictionary<SubmapId, int>();
+
+        foreach (var (cellId, storedTypes) in coverageGrid.Cells)
+        {
+            var submapsPerCell = new List<(SubmapId SubmapId, long Time)>();
+            foreach (var storedType in storedTypes)
+            {
+                submapsPerCell.Add((storedType.SubmapId, storedType.Time));
+            }
+
+            // In case there are several submaps covering the cell, only the freshest
+            // submaps are kept.
+            if (submapsPerCell.Count > freshSubmapsCount)
+            {
+                // Sort by time in descending order (matches C++: std::sort with > comparison)
+                submapsPerCell.Sort((left, right) => right.Time.CompareTo(left.Time));
+                submapsPerCell = [.. submapsPerCell.Take(freshSubmapsCount)];
+            }
+
+            foreach (var (submapId, _) in submapsPerCell)
+            {
+                if (!submapToCoveredCellsCount.TryGetValue(submapId, out var count))
+                {
+                    count = 0;
+                }
+                submapToCoveredCellsCount[submapId] = count + 1;
+            }
+        }
+
+        var submapIdsToKeep = new List<SubmapId>();
+        foreach (var (submapId, cellsCount) in submapToCoveredCellsCount)
+        {
+            if (cellsCount < minCoveredCellsCount)
+            {
+                continue;
+            }
+            submapIdsToKeep.Add(submapId);
+        }
+
+        // Match C++: std::set_difference(all_submap_ids, submap_ids_to_keep)
+        submapIdsToKeep.Sort((a, b) =>
+        {
+            var trajectoryCompare = a.TrajectoryId.CompareTo(b.TrajectoryId);
+            if (trajectoryCompare != 0)
+            {
+                return trajectoryCompare;
+            }
+            return a.SubmapIndex.CompareTo(b.SubmapIndex);
+        });
+
+        var result = new List<SubmapId>();
+        foreach (var submapId in allSubmapIds)
+        {
+            if (!submapIdsToKeep.Contains(submapId))
+            {
+                result.Add(submapId);
+            }
+        }
+
+        return result;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/RayToPixelMask.cs

@@ -0,0 +1,239 @@
+/*
+ * 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;
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Ray to pixel mask utilities.
+/// </summary>
+public static class RayToPixelMask
+{
+    /// <summary>
+    /// Compute all pixels that contain some part of the line segment connecting
+    /// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
+    /// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
+    /// greater than zero. Return values are in pixels and not scaled.
+    /// </summary>
+    public static List<Array2i> Compute(
+        Array2i scaledBegin,
+        Array2i scaledEnd,
+        int subpixelScale)
+    {
+        var result = new List<Array2i>();
+        ComputeInto(scaledBegin, scaledEnd, subpixelScale, result);
+        return result;
+    }
+
+    /// <summary>
+    /// Same as Compute but appends results into an existing list (caller must Clear beforehand).
+    /// This avoids per-ray List allocation when processing many rays in a loop.
+    /// </summary>
+    public static void ComputeInto(
+        Array2i scaledBegin,
+        Array2i scaledEnd,
+        int subpixelScale,
+        List<Array2i> pixelMask)
+    {
+        // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
+        // coordinate.
+        if (scaledBegin.X > scaledEnd.X)
+        {
+            ComputeInto(scaledEnd, scaledBegin, subpixelScale, pixelMask);
+            return;
+        }
+
+        // Match C++ CHECK_GE assertions for all coordinates
+        if (scaledBegin.X < 0 || scaledBegin.Y < 0 || scaledEnd.X < 0 || scaledEnd.Y < 0)
+        {
+            throw new ArgumentException("Scaled coordinates must be non-negative");
+        }
+
+        // Track last added element to avoid consecutive duplicates (avoids pixelMask[^1] indexer overhead)
+        int lastX = int.MinValue, lastY = int.MinValue;
+
+        // Special case: We have to draw a vertical line in full pixels, as
+        // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
+        if (scaledBegin.X / subpixelScale == scaledEnd.X / subpixelScale)
+        {
+            var cx = scaledBegin.X / subpixelScale;
+            var cy = Math.Min(scaledBegin.Y, scaledEnd.Y) / subpixelScale;
+            pixelMask.Add(new Array2i(cx, cy));
+            lastX = cx; lastY = cy;
+            var endY = Math.Max(scaledBegin.Y, scaledEnd.Y) / subpixelScale;
+            for (; cy <= endY; cy++)
+            {
+                if (cx != lastX || cy != lastY)
+                {
+                    pixelMask.Add(new Array2i(cx, cy));
+                    lastX = cx; lastY = cy;
+                }
+            }
+            return;
+        }
+
+        // Match C++ int64 types to prevent integer overflow
+        long dx = (long)scaledEnd.X - scaledBegin.X;
+        long dy = (long)scaledEnd.Y - scaledBegin.Y;
+        long denominator = 2L * subpixelScale * dx;
+
+        // The current full pixel coordinates. We start at 'scaled_begin'.
+        int curX = scaledBegin.X / subpixelScale;
+        int curY = scaledBegin.Y / subpixelScale;
+        pixelMask.Add(new Array2i(curX, curY));
+        lastX = curX; lastY = curY;
+
+        // The center of the subpixel part of 'scaled_begin.y()' assuming the
+        // 'denominator', i.e., sub_y / denominator is in (0, 1).
+        long subY = (2L * (scaledBegin.Y % subpixelScale) + 1) * dx;
+
+        // The distance from the from 'scaled_begin' to the right pixel border, to be
+        // divided by 2 * 'subpixel_scale'.
+        long firstPixel = 2L * subpixelScale - 2L * (scaledBegin.X % subpixelScale) - 1;
+        // The same from the left pixel border to 'scaled_end'.
+        long lastPixel = 2L * (scaledEnd.X % subpixelScale) + 1;
+
+        // The full pixel x coordinate of 'scaled_end'.
+        var endX = Math.Max(scaledBegin.X, scaledEnd.X) / subpixelScale;
+
+        // Move from 'scaled_begin' to the next pixel border to the right.
+        subY += dy * firstPixel;
+
+        if (dy > 0)
+        {
+            while (true)
+            {
+                if (curX != lastX || curY != lastY)
+                {
+                    pixelMask.Add(new Array2i(curX, curY));
+                    lastX = curX; lastY = curY;
+                }
+                while (subY > denominator)
+                {
+                    subY -= denominator;
+                    curY++;
+                    if (curX != lastX || curY != lastY)
+                    {
+                        pixelMask.Add(new Array2i(curX, curY));
+                        lastX = curX; lastY = curY;
+                    }
+                }
+                curX++;
+                if (subY == denominator)
+                {
+                    subY -= denominator;
+                    curY++;
+                }
+                if (curX == endX)
+                {
+                    break;
+                }
+                // Move from one pixel border to the next.
+                subY += dy * 2L * subpixelScale;
+            }
+            // Move from the pixel border on the right to 'scaled_end'.
+            subY += dy * lastPixel;
+
+            if (curX != lastX || curY != lastY)
+            {
+                pixelMask.Add(new Array2i(curX, curY));
+                lastX = curX; lastY = curY;
+            }
+            while (subY > denominator)
+            {
+                subY -= denominator;
+                curY++;
+                if (curX != lastX || curY != lastY)
+                {
+                    pixelMask.Add(new Array2i(curX, curY));
+                    lastX = curX; lastY = curY;
+                }
+            }
+
+            // Match C++ CHECK_NE(sub_y, denominator) - subY should not equal denominator
+            if (subY == denominator)
+            {
+                throw new InvalidOperationException("subY should not equal denominator");
+            }
+
+            // Match C++ CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale)
+            var expectedY = scaledEnd.Y / subpixelScale;
+            if (curY != expectedY)
+            {
+                throw new InvalidOperationException($"Current y should equal scaledEnd.Y / subpixelScale: current.Y={curY}, scaledEnd.Y/subpixelScale={expectedY}, scaledBegin=({scaledBegin.X}, {scaledBegin.Y}), scaledEnd=({scaledEnd.X}, {scaledEnd.Y}), subpixelScale={subpixelScale}");
+            }
+            return;
+        }
+
+        // Same for lines non-ascending in y coordinates.
+        while (true)
+        {
+            if (curX != lastX || curY != lastY)
+            {
+                pixelMask.Add(new Array2i(curX, curY));
+                lastX = curX; lastY = curY;
+            }
+            while (subY < 0)
+            {
+                subY += denominator;
+                curY--;
+                if (curX != lastX || curY != lastY)
+                {
+                    pixelMask.Add(new Array2i(curX, curY));
+                    lastX = curX; lastY = curY;
+                }
+            }
+            curX++;
+            if (subY == 0)
+            {
+                subY += denominator;
+                curY--;
+            }
+            if (curX == endX)
+            {
+                break;
+            }
+            subY += dy * 2L * subpixelScale;
+        }
+        subY += dy * lastPixel;
+        if (curX != lastX || curY != lastY)
+        {
+            pixelMask.Add(new Array2i(curX, curY));
+            lastX = curX; lastY = curY;
+        }
+        while (subY < 0)
+        {
+            subY += denominator;
+            curY--;
+            if (curX != lastX || curY != lastY)
+            {
+                pixelMask.Add(new Array2i(curX, curY));
+                lastX = curX; lastY = curY;
+            }
+        }
+        // Match C++: CHECK_NE(sub_y, 0)
+        if (subY == 0)
+        {
+            throw new InvalidOperationException("subY should not equal 0");
+        }
+        // Match C++: CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale)
+        if (curY != scaledEnd.Y / subpixelScale)
+        {
+            throw new InvalidOperationException($"Current y should equal scaledEnd.y / subpixelScale: current.Y={curY}, scaledEnd.Y/subpixelScale={scaledEnd.Y / subpixelScale}, scaledBegin=({scaledBegin.X}, {scaledBegin.Y}), scaledEnd=({scaledEnd.X}, {scaledEnd.Y}), subpixelScale={subpixelScale}");
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/CeresScanMatcher2D.cs

@@ -0,0 +1,476 @@
+/*
+ * 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.Mapping.D2D;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using CeresSharp;
+using CeresSharp.Enums;
+using System.Collections.Generic;
+using System.Diagnostics;
+using  RobotNet10.Shared.Numbers;
+using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
+using TSDF2DGrid = CartographerSharp.Mapping.D2D.TSDF2D;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Align scans with an existing map using Ceres.
+/// </summary>
+public class CeresScanMatcher2D : IDisposable
+{
+    private readonly CeresScanMatcherOptions2D _options;
+    private readonly SolverOptions _solverOptions;
+    private bool _disposed;
+
+    // Static counters for tracking scan matching statistics
+    private static int _totalMatchAttempts = 0;
+    private static int _successfulMatches = 0;
+
+    // Thread-safe counter for tracking active scan matching operations
+    private static int _activeScanMatchingCount = 0;
+
+    // Static cache for BiCubicInterpolator resources to avoid expensive re-computation
+    // PrecomputeGridData() takes ~1000ms per call, caching reduces this to near-zero
+    private static readonly GridInterpolatorCache _interpolatorCache = new(maxCacheSize: 10);
+
+    public CeresScanMatcher2D(CeresScanMatcherOptions2D options)
+    {
+        _options = options;
+
+        // Initialize CeresSharp solver options
+        // Match C++ ceres_scan_matcher_2d.cc line 66-68: CreateCeresSolverOptions(options.ceres_solver_options())
+        // C++ Ceres defaults: function_tolerance=1e-6, gradient_tolerance=1e-10, parameter_tolerance=1e-8
+        // These are NOT explicitly set in C++, so they use Ceres library defaults.
+        _solverOptions = new SolverOptions
+        {
+            // Set linear solver type to DENSE_QR for 2D scan matching (match C++ line 68)
+            LinearSolverType = LinearSolverType.DenseQr,
+
+            // Configure from CeresSolverOptions if available, otherwise use C++ Ceres defaults
+            MaxNumIterations = options.CeresSolverOptions?.MaxNumIterations ?? 50, // C++ Ceres default is 50
+            NumThreads = options.CeresSolverOptions?.NumThreads ?? 1, // Single thread for scan matching
+            UseNonmonotonicSteps = options.CeresSolverOptions?.UseNonmonotonicSteps ?? false,
+
+            // Tolerance settings - use C++ Ceres defaults unless explicitly configured
+            // C++ Ceres defaults: function_tolerance=1e-6, gradient_tolerance=1e-10, parameter_tolerance=1e-8
+            // Note: If convergence issues occur with small costs (~0.1), consider relaxing these:
+            //   FunctionTolerance: 1e-3 (allows 0.1% cost reduction)
+            //   GradientTolerance: 1e-6
+            //   ParameterTolerance: 1e-6
+            FunctionTolerance = options.CeresSolverOptions?.FunctionTolerance ?? 1e-6,  // C++ Ceres default
+            GradientTolerance = options.CeresSolverOptions?.GradientTolerance ?? 1e-10, // C++ Ceres default
+            ParameterTolerance = options.CeresSolverOptions?.ParameterTolerance ?? 1e-8 // C++ Ceres default
+        };
+    }
+
+    /// <summary>
+    /// Aligns 'point_cloud' within the 'grid' given an
+    /// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
+    /// 'summary'.
+    /// </summary>
+    /// <param name="targetTranslation">Target translation to match</param>
+    /// <param name="initialPoseEstimate">Initial pose estimate</param>
+    /// <param name="pointCloud">Point cloud to match</param>
+    /// <param name="grid">Grid to match against</param>
+    /// <param name="poseEstimate">Output pose estimate</param>
+    /// <param name="summary">Output solver summary</param>
+    /// <param name="highResGrid">Optional high resolution grid for matching</param>
+    public void Match(
+        Vector2 targetTranslation,
+        Rigid2d initialPoseEstimate,
+        PointCloud pointCloud,
+        Grid2D grid,
+        out Rigid2d poseEstimate,
+        out SolverSummary summary,
+        Grid2D? highResGrid = null)
+    {
+        ArgumentNullException.ThrowIfNull(pointCloud);
+        ArgumentNullException.ThrowIfNull(grid);
+        if (pointCloud.Count == 0)
+        {
+            poseEstimate = initialPoseEstimate;
+            // Create empty summary for empty point cloud case
+            // NOTE: summary is an 'out' parameter, so caller is responsible for disposing it
+            // CRITICAL: Create minimal problem to avoid memory leak
+            using var emptyProblem = new Problem();
+            // CRITICAL: Create new SolverOptions for each solve to avoid any state leakage
+            using var emptyOptions = new SolverOptions
+            {
+                LinearSolverType = LinearSolverType.DenseQr,
+                MaxNumIterations = 1 // Minimal iterations for empty case
+            };
+            summary = emptyProblem.Solve(emptyOptions);
+            return;
+        }
+        
+        // Validate weights
+        if (_options.OccupiedSpaceWeight <= 0.0)
+            throw new ArgumentException("OccupiedSpaceWeight must be positive", nameof(_options));
+        if (_options.TranslationWeight <= 0.0)
+            throw new ArgumentException("TranslationWeight must be positive", nameof(_options));
+        if (_options.RotationWeight <= 0.0)
+            throw new ArgumentException("RotationWeight must be positive", nameof(_options));
+
+        // Initialize pose parameters [x, y, theta]
+        var poseParams = new double[3]
+        {
+            initialPoseEstimate.Translation.X,
+            initialPoseEstimate.Translation.Y,
+            initialPoseEstimate.Rotation
+        };
+
+        // Increment active scan matching counter (thread-safe)
+        Interlocked.Increment(ref _activeScanMatchingCount);
+
+        // List to store cost function instances that need explicit disposal
+        // These instances must be disposed after Problem.Solve completes:
+        // - OccupiedSpaceCostFunction2D: holds unmanaged resources (BiCubicInterpolator) that MUST be disposed
+        // - TSDFMatchCostFunction2D: implements IDisposable pattern (good practice to dispose)
+        // Note: DynamicAutoDiffCostFunction objects added to Problem are owned by Problem and will
+        // be disposed when Problem is disposed. However, the underlying cost function instances
+        // (OccupiedSpaceCostFunction2D, TSDFMatchCostFunction2D) are NOT owned by Problem and must
+        // be explicitly disposed.
+        var costFunctionInstances = new List<IDisposable>();
+        
+        try
+        {
+            var swTotal = Stopwatch.StartNew();
+            var swStep = new Stopwatch();
+
+            // Create Ceres problem
+            // Problem will own all CostFunction objects added via AddResidualBlock
+            // and dispose them when Problem is disposed (via 'using' statement)
+            using var problem = new Problem();
+
+            // Add parameter block
+            problem.AddParameterBlock(poseParams, 3);
+
+            // Reuse parameter blocks array to avoid creating new arrays for each AddResidualBlock call
+            // This reduces allocation overhead when Match is called frequently
+            var parameterBlocks = new double[][] { poseParams };
+
+            // Match C++: Always use standard weights, regardless of high res grid
+            // C++: options_.occupied_space_weight(), options_.translation_weight(), options_.rotation_weight()
+            var occupiedSpaceWeight = _options.OccupiedSpaceWeight;
+            var translationWeight = _options.TranslationWeight;
+            var rotationWeight = _options.RotationWeight;
+
+            // Add occupied space cost function for main grid
+            swStep.Restart();
+            switch (grid.GetGridType())
+            {
+                case GridType.ProbabilityGrid:
+                    {
+                        // Use cached interpolator to avoid expensive PrecomputeGridData() (~1000ms savings)
+                        var cachedInterpolator = _interpolatorCache.GetOrCreate(grid);
+
+                        // Create the underlying cost function instance with cached interpolator
+                        var occupiedSpaceCostFunctionInstance = new OccupiedSpaceCostFunction2D(
+                            occupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
+                            pointCloud,
+                            grid,
+                            cachedInterpolator);
+                        costFunctionInstances.Add(occupiedSpaceCostFunctionInstance);
+
+                        var occupiedSpaceCost = new DynamicAutoDiffCostFunction(
+                            occupiedSpaceCostFunctionInstance.Evaluate,
+                            numResiduals: pointCloud.Count,
+                            parameterBlockSizes: [3] // [x, y, theta]
+                        );
+                        problem.AddResidualBlock(occupiedSpaceCost, null, parameterBlocks);
+                    }
+                    break;
+                case GridType.TSDF:
+                    if (grid is TSDF2DGrid tsdfGrid)
+                    {
+                        // Create the underlying cost function instance explicitly to track it
+                        var tsdfMatchCostFunctionInstance = new TSDFMatchCostFunction2D(
+                            occupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
+                            pointCloud,
+                            tsdfGrid);
+                        costFunctionInstances.Add(tsdfMatchCostFunctionInstance);
+                        
+                        var tsdfMatchCost = new DynamicAutoDiffCostFunction(
+                            tsdfMatchCostFunctionInstance.Evaluate,
+                            numResiduals: pointCloud.Count,
+                            parameterBlockSizes: [3] // [x, y, theta]
+                        );
+                        // residualBlockId is just an identifier, doesn't need to be stored or freed
+                        _ = problem.AddResidualBlock(tsdfMatchCost, null, parameterBlocks);
+                    }
+                    break;
+
+                default:
+                    throw new ArgumentException($"Unsupported grid type: {grid.GetGridType()}", nameof(grid));
+            }
+            swStep.Stop();
+            var mainGridCostMs = swStep.Elapsed.TotalMilliseconds;
+
+            // Add high resolution grid cost function if provided
+            swStep.Restart();
+            double highResCacheMs = 0, highResCostFuncMs = 0, highResAddBlockMs = 0;
+            if (highResGrid != null)
+            {
+                switch (highResGrid.GetGridType())
+                {
+                    case GridType.ProbabilityGrid:
+                        {
+                            var highResOccupiedSpaceWeight = _options.HighResOccupiedSpaceWeight ?? _options.OccupiedSpaceWeight;
+
+                            // Use cached interpolator to avoid expensive PrecomputeGridData() (~1000ms savings)
+                            var swHrSub = Stopwatch.StartNew();
+                            var cachedHighResInterpolator = _interpolatorCache.GetOrCreate(highResGrid);
+                            swHrSub.Stop();
+                            highResCacheMs = swHrSub.Elapsed.TotalMilliseconds;
+
+                            // Create the underlying cost function instance with cached interpolator
+                            swHrSub.Restart();
+                            var highResOccupiedSpaceCostFunctionInstance = new OccupiedSpaceCostFunction2D(
+                                highResOccupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
+                                pointCloud,
+                                highResGrid,
+                                cachedHighResInterpolator);
+                            costFunctionInstances.Add(highResOccupiedSpaceCostFunctionInstance);
+
+                            var highResOccupiedSpaceCost = new DynamicAutoDiffCostFunction(
+                                highResOccupiedSpaceCostFunctionInstance.Evaluate,
+                                numResiduals: pointCloud.Count,
+                                parameterBlockSizes: [3] // [x, y, theta]
+                            );
+                            swHrSub.Stop();
+                            highResCostFuncMs = swHrSub.Elapsed.TotalMilliseconds;
+
+                            // residualBlockId is just an identifier, doesn't need to be stored or freed
+                            swHrSub.Restart();
+                            _ = problem.AddResidualBlock(highResOccupiedSpaceCost, null, parameterBlocks);
+                            swHrSub.Stop();
+                            highResAddBlockMs = swHrSub.Elapsed.TotalMilliseconds;
+                        }
+                        break;
+                    case GridType.TSDF:
+                        if (highResGrid is TSDF2DGrid highResTsdfGrid)
+                        {
+                            var highResOccupiedSpaceWeight = _options.HighResOccupiedSpaceWeight ?? _options.OccupiedSpaceWeight;
+                            // Create the underlying cost function instance explicitly to track it
+                            var swHrSub = Stopwatch.StartNew();
+                            var highResTsdfMatchCostFunctionInstance = new TSDFMatchCostFunction2D(
+                                highResOccupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
+                                pointCloud,
+                                highResTsdfGrid);
+                            costFunctionInstances.Add(highResTsdfMatchCostFunctionInstance);
+
+                            var highResTsdfMatchCost = new DynamicAutoDiffCostFunction(
+                                highResTsdfMatchCostFunctionInstance.Evaluate,
+                                numResiduals: pointCloud.Count,
+                                parameterBlockSizes: [3] // [x, y, theta]
+                            );
+                            swHrSub.Stop();
+                            highResCostFuncMs = swHrSub.Elapsed.TotalMilliseconds;
+
+                            // residualBlockId is just an identifier, doesn't need to be stored or freed
+                            swHrSub.Restart();
+                            _ = problem.AddResidualBlock(highResTsdfMatchCost, null, parameterBlocks);
+                            swHrSub.Stop();
+                            highResAddBlockMs = swHrSub.Elapsed.TotalMilliseconds;
+                        }
+                        break;
+
+                    default:
+                        throw new ArgumentException($"Unsupported high resolution grid type: {highResGrid.GetGridType()}", nameof(highResGrid));
+                }
+            }
+            swStep.Stop();
+            var highResGridCostMs = swStep.Elapsed.TotalMilliseconds;
+
+            // Add translation delta cost function
+            var translationCost = TranslationDeltaCostFunctor2D.CreateAutoDiffCostFunction(
+                translationWeight,
+                targetTranslation
+            );
+            // residualBlockId is just an identifier, doesn't need to be stored or freed
+            _ = problem.AddResidualBlock(translationCost, null, parameterBlocks);
+            var rotationCost = RotationDeltaCostFunctor2D.CreateAutoDiffCostFunction(
+                rotationWeight,
+                poseParams[2]
+            );
+            // residualBlockId is just an identifier, doesn't need to be stored or freed
+            _ = problem.AddResidualBlock(rotationCost, null, parameterBlocks);
+
+            // Solve the optimization problem
+            // CRITICAL: SolverSummary holds unmanaged resources (SolverSummaryHandle) that MUST be disposed
+            // by the caller. Since summary is an 'out' parameter and may be used by caller after Match returns,
+            // we cannot dispose it here. The caller MUST dispose summary after use to prevent memory leaks.
+            swStep.Restart();
+            summary = problem.Solve(_solverOptions);
+            swStep.Stop();
+            var solveMs = swStep.Elapsed.TotalMilliseconds;
+
+            try
+            {
+                swTotal.Stop();
+                var totalMs = swTotal.Elapsed.TotalMilliseconds;
+
+                // Log timing if any step takes significant time (> 10ms)
+                if (totalMs > 100.0)
+                {
+                    Console.WriteLine($"[CeresScanMatcher2D] Match: total={totalMs:F1}ms, mainGridCost={mainGridCostMs:F1}ms, highResCost={highResGridCostMs:F1}ms (cache={highResCacheMs:F1}ms, costFunc={highResCostFuncMs:F1}ms, addBlock={highResAddBlockMs:F1}ms), solve={solveMs:F1}ms, points={pointCloud.Count}, iterations={summary?.Iterations ?? 0}, cache={GetInterpolatorCacheStats()}");
+                }
+
+                // Update statistics (thread-safe)
+                Interlocked.Increment(ref _totalMatchAttempts);
+                bool isSuccess = summary != null &&
+                                summary.InitialCost != -1.0 &&
+                                summary.TerminationType == TerminationType.Convergence;
+
+                if (isSuccess)
+                {
+                    Interlocked.Increment(ref _successfulMatches);
+                }
+
+                // Extract result
+                poseEstimate = new Rigid2d(
+                    new Vector2(poseParams[0], poseParams[1]),
+                    poseParams[2]
+                );
+            }
+            catch
+            {
+                // If exception occurs after Solve() but before return, dispose summary
+                // to prevent native handle leak (caller won't receive the out parameter)
+                summary?.Dispose();
+                summary = null!;
+                throw;
+            }
+        }
+        finally
+        {
+            // Dispose cost function instances after Problem.Solve completes (or on exception)
+            // CRITICAL: Problem.Solve() has completed, so native code is no longer using callbacks.
+            // Problem will be disposed by 'using' statement, which will dispose DynamicAutoDiffCostFunction
+            // objects. However, the underlying cost function instances (OccupiedSpaceCostFunction2D,
+            // TSDFMatchCostFunction2D) are NOT owned by Problem and must be explicitly disposed:
+            // - OccupiedSpaceCostFunction2D: holds unmanaged resources (BiCubicInterpolator) that MUST be disposed
+            // - TSDFMatchCostFunction2D: implements IDisposable pattern (should be disposed for consistency)
+            // It is safe to dispose these instances here because:
+            // 1. Problem.Solve() has completed, so callbacks are no longer called
+            // 2. Problem will be disposed immediately after this finally block (via 'using' statement)
+            // Dispose all cost function instances to free unmanaged resources
+            foreach (var instance in costFunctionInstances)
+            {
+                try
+                {
+                    instance?.Dispose();
+                }
+                catch
+                {
+                    // Ignore disposal errors - instance may already be disposed or may have been disposed
+                    // by finalizer in case of exception
+                }
+            }
+            // Note: costFunctionInstances will go out of scope after method returns, allowing GC collection.
+            // No need to explicitly call Clear().
+            
+            // Note: We do NOT call GC.Collect here because:
+            // 1. Unmanaged resources are explicitly disposed above
+            // 2. Forced GC can cause performance issues and is generally not recommended
+            // 3. The GC will run automatically when needed
+            
+            // Decrement active scan matching counter (thread-safe)
+            Interlocked.Decrement(ref _activeScanMatchingCount);
+        }
+    }
+
+    /// <summary>
+    /// Gets the number of active scan matching operations currently running.
+    /// </summary>
+    public static int GetActiveScanMatchingCount()
+    {
+        return _activeScanMatchingCount;
+    }
+
+    /// <summary>
+    /// Waits for all active scan matching operations to complete.
+    /// </summary>
+    /// <param name="maxWaitTime">Maximum time to wait</param>
+    /// <param name="checkInterval">Interval between checks</param>
+    /// <returns>True if all scan matching completed, false if timeout</returns>
+    public static bool WaitForAllScanMatchingToComplete(TimeSpan maxWaitTime, TimeSpan checkInterval)
+    {
+        var startTime = DateTime.UtcNow;
+
+        while (DateTime.UtcNow - startTime < maxWaitTime)
+        {
+            var activeCount = _activeScanMatchingCount;
+            if (activeCount == 0)
+            {
+                return true; // All scan matching completed
+            }
+
+            Thread.Sleep(checkInterval);
+        }
+
+        // Timeout - check one more time
+        return _activeScanMatchingCount == 0;
+    }
+
+    /// <summary>
+    /// Gets the number of cached interpolators.
+    /// </summary>
+    public static int GetInterpolatorCacheSize()
+    {
+        return _interpolatorCache.Count;
+    }
+
+    /// <summary>
+    /// Gets cache statistics as a formatted string.
+    /// </summary>
+    public static string GetInterpolatorCacheStats()
+    {
+        return $"size={_interpolatorCache.Count}, hits={_interpolatorCache.CacheHits}, misses={_interpolatorCache.CacheMisses}, hitRate={_interpolatorCache.HitRate:P1}";
+    }
+
+    /// <summary>
+    /// Invalidates cached interpolator for a specific grid.
+    /// Call this when a grid is modified to force re-computation on next scan match.
+    /// </summary>
+    /// <param name="grid">The grid whose cache entry should be invalidated.</param>
+    public static void InvalidateGridCache(Grid2D grid)
+    {
+        _interpolatorCache.Invalidate(grid);
+    }
+
+    /// <summary>
+    /// Clears all cached interpolators.
+    /// </summary>
+    public static void ClearInterpolatorCache()
+    {
+        _interpolatorCache.Clear();
+    }
+
+    /// <summary>
+    /// Disposes the solver options and other managed resources.
+    /// </summary>
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            _solverOptions?.Dispose();
+            _disposed = true;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/CorrelativeScanMatcher2D.cs

@@ -0,0 +1,278 @@
+/*
+ * 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.D2D;
+using CartographerSharp.Sensor;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Discrete scan representation as a list of integer cell indices.
+/// </summary>
+public class DiscreteScan2D : List<Array2i>
+{
+}
+
+/// <summary>
+/// Describes the search space for scan matching.
+/// </summary>
+public class SearchParameters
+{
+    /// <summary>
+    /// Linear search window in pixel offsets; bounds are inclusive.
+    /// </summary>
+    public struct LinearBounds(int minX, int maxX, int minY, int maxY)
+    {
+        public int MinX { get; set; } = minX;
+        public int MaxX { get; set; } = maxX;
+        public int MinY { get; set; } = minY;
+        public int MaxY { get; set; } = maxY;
+    }
+
+    public int NumAngularPerturbations { get; set; }
+    public double AngularPerturbationStepSize { get; set; }
+    public double Resolution { get; set; }
+    public int NumScans { get; set; }
+    public List<LinearBounds> LinearBoundsList { get; set; } // Per rotated scans
+
+    // === MEMORY OPTIMIZATION: Cap maximum NumScans to prevent excessive allocations ===
+    // Each scan creates a rotated point cloud copy + discretized version
+    // With 500 points per scan, 500 scans = ~18MB. 3000 scans = ~108MB per MatchFullSubmap call.
+    // Multiple concurrent calls can cause GB-level memory spikes.
+    private const int MaxNumScans = 500;
+
+    public SearchParameters(
+        double linearSearchWindow,
+        double angularSearchWindow,
+        PointCloud pointCloud,
+        double resolution)
+    {
+        Resolution = resolution;
+
+        // Compute max scan range
+        double maxScanRange = 3.0 * resolution;
+        foreach (var point in pointCloud)
+        {
+            var range = new Vector2(point.Position.X, point.Position.Y).Length();
+            maxScanRange = Math.Max(range, maxScanRange);
+        }
+
+        // Compute angular perturbation step size
+        const double kSafetyMargin = 1.0 - 1e-3;
+        var resolutionSquared = resolution * resolution;
+        var maxScanRangeSquared = maxScanRange * maxScanRange;
+        // FIX: Clamp argument to valid Acos range [-1, 1] to prevent NaN
+        // This can occur with extreme resolution/maxScanRange ratios
+        var acosArg = Math.Clamp(1.0 - resolutionSquared / (2.0 * maxScanRangeSquared), -1.0, 1.0);
+        AngularPerturbationStepSize = kSafetyMargin * Math.Acos(acosArg);
+
+        NumAngularPerturbations = (int)Math.Ceiling(angularSearchWindow / AngularPerturbationStepSize);
+        NumScans = 2 * NumAngularPerturbations + 1;
+
+        // === MEMORY OPTIMIZATION: Cap NumScans to prevent memory exhaustion ===
+        // If NumScans exceeds limit, increase angular step size to reduce scan count
+        if (NumScans > MaxNumScans)
+        {
+            var originalNumScans = NumScans;
+            var originalStepSize = AngularPerturbationStepSize;
+
+            // Recalculate with capped scans
+            NumAngularPerturbations = (MaxNumScans - 1) / 2;
+            NumScans = 2 * NumAngularPerturbations + 1;
+            AngularPerturbationStepSize = angularSearchWindow / NumAngularPerturbations;
+
+            /*Console.WriteLine($"[SearchParameters] CAPPED NumScans: {originalNumScans} -> {NumScans}, " +
+                $"AngularStep: {originalStepSize * 180 / Math.PI:F4}° -> {AngularPerturbationStepSize * 180 / Math.PI:F4}°, " +
+                $"AngularSearchWindow={angularSearchWindow * 180 / Math.PI:F1}°");*/
+        }
+
+        // Compute linear bounds for each rotated scan
+        var numLinearPerturbations = (int)Math.Ceiling(linearSearchWindow / resolution);
+        LinearBoundsList = [];
+        for (int i = 0; i < NumScans; i++)
+        {
+            LinearBoundsList.Add(new LinearBounds(
+                -numLinearPerturbations,
+                numLinearPerturbations,
+                -numLinearPerturbations,
+                numLinearPerturbations
+            ));
+        }
+    }
+
+    public SearchParameters(
+        int numLinearPerturbations,
+        int numAngularPerturbations,
+        double angularPerturbationStepSize,
+        double resolution)
+    {
+        NumAngularPerturbations = numAngularPerturbations;
+        AngularPerturbationStepSize = angularPerturbationStepSize;
+        Resolution = resolution;
+        NumScans = 2 * numAngularPerturbations + 1;
+        
+        //var linearSearchWindow = numLinearPerturbations * resolution;
+        LinearBoundsList = [];
+        for (int i = 0; i < NumScans; i++)
+        {
+            LinearBoundsList.Add(new LinearBounds(
+                -numLinearPerturbations,
+                numLinearPerturbations,
+                -numLinearPerturbations,
+                numLinearPerturbations
+            ));
+        }
+    }
+
+    /// <summary>
+    /// Tightens the search window as much as possible.
+    /// </summary>
+    public void ShrinkToFit(List<DiscreteScan2D> scans, CellLimits cellLimits)
+    {
+        if (scans.Count != NumScans)
+            throw new ArgumentException($"scans.Count ({scans.Count}) must equal NumScans ({NumScans})", nameof(scans));
+        if (LinearBoundsList.Count != NumScans)
+            throw new ArgumentException($"LinearBoundsList.Count ({LinearBoundsList.Count}) must equal NumScans ({NumScans})", nameof(LinearBoundsList));
+
+        for (int i = 0; i < NumScans; i++)
+        {
+            var scan = scans[i];
+            
+            // Compute min_bound and max_bound like C++: min_bound.min(-xy_index) and max_bound.max(cell_limits - xy_index)
+            var minBound = Array2i.Zero;
+            var maxBound = Array2i.Zero;
+            
+            foreach (var xyIndex in scan)
+            {
+                // min_bound = min_bound.min(-xy_index)
+                minBound = new Array2i(
+                    Math.Min(minBound.X, -xyIndex.X),
+                    Math.Min(minBound.Y, -xyIndex.Y)
+                );
+                
+                // max_bound = max_bound.max(cell_limits - xy_index)
+                var cellLimitMinusXY = new Array2i(
+                    cellLimits.NumXCells - 1 - xyIndex.X,
+                    cellLimits.NumYCells - 1 - xyIndex.Y
+                );
+                maxBound = new Array2i(
+                    Math.Max(maxBound.X, cellLimitMinusXY.X),
+                    Math.Max(maxBound.Y, cellLimitMinusXY.Y)
+                );
+            }
+
+            var bounds = LinearBoundsList[i];
+            bounds.MinX = Math.Max(bounds.MinX, minBound.X);
+            bounds.MaxX = Math.Min(bounds.MaxX, maxBound.X);
+            bounds.MinY = Math.Max(bounds.MinY, minBound.Y);
+            bounds.MaxY = Math.Min(bounds.MaxY, maxBound.Y);
+            
+            LinearBoundsList[i] = bounds;
+        }
+    }
+}
+
+/// <summary>
+/// A possible solution for scan matching.
+/// </summary>
+public struct Candidate2D(int scanIndex, int xIndexOffset, int yIndexOffset, SearchParameters searchParameters) : IComparable<Candidate2D>
+{
+    public int ScanIndex { get; set; } = scanIndex;
+    public int XIndexOffset { get; set; } = xIndexOffset;
+    public int YIndexOffset { get; set; } = yIndexOffset;
+    public double X { get; set; } = -yIndexOffset * searchParameters.Resolution;
+    public double Y { get; set; } = -xIndexOffset * searchParameters.Resolution;
+    public double Orientation { get; set; } = (scanIndex - searchParameters.NumAngularPerturbations) * searchParameters.AngularPerturbationStepSize;
+    public double Score { get; set; } = 0.0;
+
+    public readonly int CompareTo(Candidate2D other)
+    {
+        return Score.CompareTo(other.Score);
+    }
+
+    public static bool operator <(Candidate2D left, Candidate2D right)
+    {
+        return left.Score < right.Score;
+    }
+
+    public static bool operator >(Candidate2D left, Candidate2D right)
+    {
+        return left.Score > right.Score;
+    }
+}
+
+/// <summary>
+/// Generates a collection of rotated scans.
+/// </summary>
+public static class ScanMatchingUtilities
+{
+    public static List<PointCloud> GenerateRotatedScans(
+        PointCloud pointCloud,
+        SearchParameters searchParameters)
+    {
+        var rotatedScans = new List<PointCloud>();
+        
+        for (int i = 0; i < searchParameters.NumScans; i++)
+        {
+            var angle = (i - searchParameters.NumAngularPerturbations) * searchParameters.AngularPerturbationStepSize;
+            var rotation = Matrix3x2.CreateRotation(angle);
+            
+            var rotatedScan = new PointCloud();
+            foreach (var point in pointCloud)
+            {
+                var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotation);
+                rotatedScan.Add(new RangefinderPoint
+                {
+                    Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
+                });
+            }
+            
+            rotatedScans.Add(rotatedScan);
+        }
+        
+        return rotatedScans;
+    }
+
+    /// <summary>
+    /// Translates and discretizes the rotated scans into a vector of integer indices.
+    /// </summary>
+    public static List<DiscreteScan2D> DiscretizeScans(
+        MapLimits mapLimits,
+        List<PointCloud> scans,
+        Vector2 initialTranslation)
+    {
+        var discreteScans = new List<DiscreteScan2D>();
+        
+        foreach (var scan in scans)
+        {
+            var discreteScan = new DiscreteScan2D();
+            foreach (var point in scan)
+            {
+                var translatedPoint = new Vector2(
+                    point.Position.X + initialTranslation.X,
+                    point.Position.Y + initialTranslation.Y
+                );
+                var cellIndex = mapLimits.GetCellIndex(translatedPoint);
+                discreteScan.Add(cellIndex);
+            }
+            discreteScans.Add(discreteScan);
+        }
+        
+        return discreteScans;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/FastCorrelativeScanMatcher2D.cs

@@ -0,0 +1,435 @@
+/*
+ * 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.Buffers;
+using CartographerSharp.Common.Math;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
+using MapLimits2D = CartographerSharp.Mapping.D2D.MapLimits;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// An implementation of "Real-Time Correlative Scan Matching" by Olson.
+/// It is similar to the RealTimeCorrelativeScanMatcher but has a different
+/// trade-off: Scan matching is faster because more effort is put into the
+/// precomputation done for a given map. However, this map is immutable after
+/// construction.
+/// </summary>
+public class FastCorrelativeScanMatcher2D(Grid2D grid, FastCorrelativeScanMatcherOptions2D _options)
+{
+    private readonly MapLimits2D _limits = grid.Limits;
+    private readonly PrecomputationGridStack2D _precomputationGridStack = new(grid, _options);
+
+    /// <summary>
+    /// Aligns 'pointCloud' within the 'grid' given an
+    /// 'initialPoseEstimate'. If a score above 'minScore' (excluding equality)
+    /// is possible, true is returned, and 'score' and 'poseEstimate' are updated
+    /// with the result.
+    /// </summary>
+    public bool Match(
+        Rigid2d initialPoseEstimate,
+        PointCloud pointCloud,
+        double minScore,
+        out double score,
+        out Rigid2d poseEstimate)
+    {
+        var searchParameters = new SearchParameters(
+            _options.LinearSearchWindow,
+            _options.AngularSearchWindow,
+            pointCloud,
+            _limits.Resolution);
+        
+        return MatchWithSearchParameters(
+            searchParameters,
+            initialPoseEstimate,
+            pointCloud,
+            minScore,
+            out score,
+            out poseEstimate);
+    }
+
+    /// <summary>
+    /// Aligns 'pointCloud' within the 'grid' using localization search windows.
+    /// Match C++: LocalizationMatch() with localization_linear_search_window and localization_angular_search_window.
+    /// </summary>
+    public bool LocalizationMatch(
+        Rigid2d initialPoseEstimate,
+        PointCloud pointCloud,
+        double minScore,
+        out double score,
+        out Rigid2d poseEstimate)
+    {
+        if (!_options.LocalizationLinearSearchWindow.HasValue || !_options.LocalizationAngularSearchWindow.HasValue)
+        {
+            score = 0.0;
+            poseEstimate = initialPoseEstimate;
+            return false;
+        }
+
+        var searchParameters = new SearchParameters(
+            _options.LocalizationLinearSearchWindow.Value,
+            _options.LocalizationAngularSearchWindow.Value,
+            pointCloud,
+            _limits.Resolution);
+
+        return MatchWithSearchParameters(
+            searchParameters,
+            initialPoseEstimate,
+            pointCloud,
+            minScore,
+            out score,
+            out poseEstimate);
+    }
+
+    /// <summary>
+    /// Aligns 'pointCloud' with custom search windows and optional resolution.
+    /// Match C++: MatchWithCustomizeParameters().
+    /// </summary>
+    /// <param name="resolution">Resolution for search; use -1.0 to use grid resolution.</param>
+    public bool MatchWithCustomizeParameters(
+        double linearSearchWindow,
+        double angularSearchWindow,
+        double resolution,
+        Rigid2d initialPoseEstimate,
+        PointCloud pointCloud,
+        double minScore,
+        out double score,
+        out Rigid2d poseEstimate)
+    {
+        var res = resolution >= 0.0 ? resolution : _limits.Resolution;
+        var searchParameters = new SearchParameters(
+            linearSearchWindow,
+            angularSearchWindow,
+            pointCloud,
+            res);
+
+        return MatchWithSearchParameters(
+            searchParameters,
+            initialPoseEstimate,
+            pointCloud,
+            minScore,
+            out score,
+            out poseEstimate);
+    }
+
+    /// <summary>
+    /// Aligns 'pointCloud' within the full 'grid', i.e., not
+    /// restricted to the configured search window. If a score above 'minScore'
+    /// (excluding equality) is possible, true is returned, and 'score' and
+    /// 'poseEstimate' are updated with the result.
+    /// Match C++: Always uses full submap search with 1e3 * resolution and PI.
+    /// </summary>
+    public bool MatchFullSubmap(
+        PointCloud pointCloud,
+        double minScore,
+        out double score,
+        out Rigid2d poseEstimate)
+    {
+        // Match C++ exactly: Always use full submap search (1e3 cells/direction, 180 degrees)
+        // C++: SearchParameters(1e3 * limits_.resolution(), M_PI, point_cloud, limits_.resolution())
+        var linearSearchWindow = 1e3 * _limits.Resolution;
+        var angularSearchWindow = Math.PI;
+
+        var searchParameters = new SearchParameters(
+            linearSearchWindow,  // Linear search window, 1e3 cells/direction
+            angularSearchWindow, // Angular search window, 180 degrees in both directions
+            pointCloud,
+            _limits.Resolution);
+
+        // Match C++: center = Rigid2d::Translation(limits_.max() - 0.5 * resolution * Vector2d(num_y_cells, num_x_cells))
+        var centerTranslation = _limits.Max -
+            (0.5 * _limits.Resolution) *
+            new Vector2(_limits.CellLimits.NumYCells, _limits.CellLimits.NumXCells);
+        var center = new Rigid2d(centerTranslation, 0.0);
+
+        return MatchWithSearchParameters(
+            searchParameters,
+            center,
+            pointCloud,
+            minScore,
+            out score,
+            out poseEstimate);
+    }
+
+    /// <summary>
+    /// The actual implementation of the scan matcher, called by Match() and
+    /// MatchFullSubmap() with appropriate 'initialPoseEstimate' and 'searchParameters'.
+    /// </summary>
+    private bool MatchWithSearchParameters(
+        SearchParameters searchParameters,
+        Rigid2d initialPoseEstimate,
+        PointCloud pointCloud,
+        double minScore,
+        out double score,
+        out Rigid2d poseEstimate)
+    {
+        score = 0.0;
+        poseEstimate = initialPoseEstimate;
+
+        var initialAngle = initialPoseEstimate.Rotation;
+
+        // Rotate point cloud to align with initial rotation
+        var rotationMatrix = Matrix3x2.CreateRotation(initialAngle);
+        var rotatedPointCloud = new PointCloud();
+        foreach (var point in pointCloud)
+        {
+            var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotationMatrix);
+            rotatedPointCloud.Add(new RangefinderPoint
+            {
+                Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
+            });
+        }
+
+        // Generate rotated scans
+        var rotatedScans = ScanMatchingUtilities.GenerateRotatedScans(rotatedPointCloud, searchParameters);
+        
+        // Discretize scans
+        var initialTranslation = new Vector2(initialPoseEstimate.Translation.X, initialPoseEstimate.Translation.Y);
+        var discreteScans = ScanMatchingUtilities.DiscretizeScans(_limits, rotatedScans, initialTranslation);
+        
+        // Shrink search parameters to fit
+        searchParameters.ShrinkToFit(discreteScans, _limits.CellLimits);
+
+        // Compute lowest resolution candidates
+        var lowestResolutionCandidates = ComputeLowestResolutionCandidates(discreteScans, searchParameters);
+        
+        // Branch and bound search
+        var bestCandidate = BranchAndBound(
+            discreteScans,
+            searchParameters,
+            lowestResolutionCandidates,
+            _precomputationGridStack.MaxDepth,
+            minScore);
+        
+        // === MEMORY CLEANUP: Clear large lists to help GC ===
+        // These lists are on LOH (>85KB) and won't be collected until Gen2 GC
+        // Clearing them allows GC to reclaim memory sooner
+        // Note: PointCloud doesn't have Clear(), so we just let GC handle it
+        rotatedScans.Clear();
+
+        foreach (var scan in discreteScans)
+        {
+            scan.Clear();
+        }
+        discreteScans.Clear();
+
+        lowestResolutionCandidates.Clear();
+
+        // Force Gen2 GC every N calls to prevent LOH fragmentation
+        // Gen2 GC is expensive but necessary to reclaim LOH memory
+        if (Interlocked.Increment(ref _matchCallCount) % 10 == 0)
+        {
+            GC.Collect(2, GCCollectionMode.Optimized, false);
+        }
+
+        if (bestCandidate.Score > minScore)
+        {
+            score = bestCandidate.Score;
+            poseEstimate = new Rigid2d(
+                new Vector2(
+                    (initialPoseEstimate.Translation.X + bestCandidate.X),
+                    (initialPoseEstimate.Translation.Y + bestCandidate.Y)),
+                initialAngle + bestCandidate.Orientation);
+            return true;
+        }
+
+        return false;
+    }
+
+    // Counter for periodic GC
+    private static int _matchCallCount = 0;
+
+    /// <summary>
+    /// Computes lowest resolution candidates for branch-and-bound search.
+    /// </summary>
+    private List<Candidate2D> ComputeLowestResolutionCandidates(
+        List<DiscreteScan2D> discreteScans,
+        SearchParameters searchParameters)
+    {
+        var lowestResolutionCandidates = GenerateLowestResolutionCandidates(searchParameters);
+        ScoreCandidates(
+            _precomputationGridStack.Get(_precomputationGridStack.MaxDepth),
+            discreteScans,
+            lowestResolutionCandidates);
+        return lowestResolutionCandidates;
+    }
+
+    /// <summary>
+    /// Generates candidates at the lowest resolution for branch-and-bound search.
+    /// </summary>
+    private List<Candidate2D> GenerateLowestResolutionCandidates(SearchParameters searchParameters)
+    {
+        var linearStepSize = 1 << _precomputationGridStack.MaxDepth;
+        var candidates = new List<Candidate2D>();
+
+        // === DEBUG: Estimate candidate count before generation ===
+        long estimatedCandidates = 0;
+        for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
+        {
+            var bounds = searchParameters.LinearBoundsList[scanIndex];
+            var xSteps = (bounds.MaxX - bounds.MinX) / linearStepSize + 1;
+            var ySteps = (bounds.MaxY - bounds.MinY) / linearStepSize + 1;
+            estimatedCandidates += (long)xSteps * ySteps;
+        }
+        for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
+        {
+            var bounds = searchParameters.LinearBoundsList[scanIndex];
+            for (int xIndexOffset = bounds.MinX; xIndexOffset <= bounds.MaxX; xIndexOffset += linearStepSize)
+            {
+                for (int yIndexOffset = bounds.MinY; yIndexOffset <= bounds.MaxY; yIndexOffset += linearStepSize)
+                {
+                    candidates.Add(new Candidate2D(scanIndex, xIndexOffset, yIndexOffset, searchParameters));
+                }
+            }
+        }
+
+        return candidates;
+    }
+
+    /// <summary>
+    /// Scores candidates using the precomputation grid.
+    /// </summary>
+    private static void ScoreCandidates(PrecomputationGrid2D precomputationGrid, List<DiscreteScan2D> discreteScans, List<Candidate2D> candidates)
+    {
+        for (int i = 0; i < candidates.Count; i++)
+        {
+            var candidate = candidates[i];
+            if (candidate.ScanIndex >= discreteScans.Count)
+            {
+                candidate.Score = 0.0;
+                candidates[i] = candidate;
+                continue;
+            }
+
+            var discreteScan = discreteScans[candidate.ScanIndex];
+            if (discreteScan.Count == 0)
+            {
+                candidate.Score = 0.0;
+                candidates[i] = candidate;
+                continue;
+            }
+
+            int sum = 0;
+            foreach (var xyIndex in discreteScan)
+            {
+                var proposedXYIndex = new Array2i(
+                    xyIndex.X + candidate.XIndexOffset,
+                    xyIndex.Y + candidate.YIndexOffset);
+                sum += precomputationGrid.GetValue(proposedXYIndex);
+            }
+
+            // CRITICAL FIX: Use floating-point division to match C++ behavior
+            // C++ uses: static_cast<float>(sum) / static_cast<float>(discrete_scan.size())
+            candidate.Score = precomputationGrid.ToScore((double)sum / discreteScan.Count);
+            candidates[i] = candidate;
+        }
+
+        // Sort candidates by score (descending)
+        candidates.Sort((a, b) => b.Score.CompareTo(a.Score));
+    }
+
+    /// <summary>
+    /// Branch-and-bound search for best candidate.
+    /// </summary>
+    private Candidate2D BranchAndBound(
+        List<DiscreteScan2D> discreteScans,
+        SearchParameters searchParameters,
+        List<Candidate2D> candidates,
+        int candidateDepth,
+        double minScore)
+    {
+        if (candidateDepth == 0)
+        {
+            // Return the best candidate (first element after sorting by ScoreCandidates)
+            if (candidates.Count == 0)
+            {
+                return new Candidate2D(0, 0, 0, searchParameters);
+            }
+            return candidates[0];
+        }
+
+        var bestHighResolutionCandidate = new Candidate2D(0, 0, 0, searchParameters)
+        {
+            Score = minScore
+        };
+
+        foreach (var candidate in candidates)
+        {
+            if (candidate.Score <= minScore)
+            {
+                break;
+            }
+
+            // Generate higher resolution candidates
+            var higherResolutionCandidates = new List<Candidate2D>();
+            var halfWidth = 1 << (candidateDepth - 1);
+            var bounds = searchParameters.LinearBoundsList[candidate.ScanIndex];
+
+            foreach (var xOffset in new[] { 0, halfWidth })
+            {
+                if (candidate.XIndexOffset + xOffset > bounds.MaxX)
+                {
+                    break;
+                }
+
+                foreach (var yOffset in new[] { 0, halfWidth })
+                {
+                    if (candidate.YIndexOffset + yOffset > bounds.MaxY)
+                    {
+                        break;
+                    }
+
+                    higherResolutionCandidates.Add(new Candidate2D(
+                        candidate.ScanIndex,
+                        candidate.XIndexOffset + xOffset,
+                        candidate.YIndexOffset + yOffset,
+                        searchParameters));
+                }
+            }
+
+            // Score higher resolution candidates
+            ScoreCandidates(
+                _precomputationGridStack.Get(candidateDepth - 1),
+                discreteScans,
+                higherResolutionCandidates);
+
+            // Recursively search higher resolution
+            var bestCandidate = BranchAndBound(
+                discreteScans,
+                searchParameters,
+                higherResolutionCandidates,
+                candidateDepth - 1,
+                bestHighResolutionCandidate.Score);
+
+            // Clear to help GC - these lists accumulate in deep recursion
+            higherResolutionCandidates.Clear();
+
+            // Match C++: std::max(best_high_resolution_candidate, BranchAndBound(...))
+            // std::max uses operator> which compares scores, and returns FIRST element if equal
+            // Therefore, we should only update if strictly greater (not >=)
+            if (bestCandidate.Score > bestHighResolutionCandidate.Score)
+            {
+                bestHighResolutionCandidate = bestCandidate;
+            }
+        }
+
+        return bestHighResolutionCandidate;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/GridInterpolatorCache.cs

@@ -0,0 +1,355 @@
+/*
+ * 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.Mapping.D2D;
+using CeresSharp;
+using System.Collections.Concurrent;
+using System.Runtime.CompilerServices;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Cached interpolator resources for a grid.
+/// Contains the pre-computed ProbabilityGridAdapter and BiCubicInterpolator.
+/// </summary>
+internal sealed class CachedGridInterpolator : IDisposable
+{
+    public ProbabilityGridAdapter Adapter { get; }
+    public BiCubicInterpolator Interpolator { get; }
+
+    /// <summary>
+    /// Grid identity hash at cache time (for validation).
+    /// </summary>
+    public int GridHashCode { get; }
+
+    /// <summary>
+    /// Grid cell limits at cache time (for validation).
+    /// Only invalidate cache when grid SIZE changes (GrowLimits), not when cells are updated.
+    /// Using slightly stale interpolation data is acceptable for scan matching.
+    /// </summary>
+    public (int NumXCells, int NumYCells) CellLimits { get; }
+
+    /// <summary>
+    /// Grid resolution at cache time (for validation).
+    /// </summary>
+    public double Resolution { get; }
+
+    private bool _disposed;
+
+    public CachedGridInterpolator(
+        Grid2D grid,
+        ProbabilityGridAdapter adapter,
+        BiCubicInterpolator interpolator)
+    {
+        Adapter = adapter ?? throw new ArgumentNullException(nameof(adapter));
+        Interpolator = interpolator ?? throw new ArgumentNullException(nameof(interpolator));
+
+        // Store grid state for validation
+        // Only track size and resolution, NOT cell contents (KnownCellsBox)
+        // Reason: KnownCellsBox changes on every scan insertion, causing excessive cache invalidation
+        // Using slightly stale data is acceptable for scan matching optimization
+        GridHashCode = RuntimeHelpers.GetHashCode(grid);
+        var limits = grid.Limits.CellLimits;
+        CellLimits = (limits.NumXCells, limits.NumYCells);
+        Resolution = grid.Limits.Resolution;
+    }
+
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            Interpolator?.Dispose();
+            _disposed = true;
+        }
+    }
+}
+
+/// <summary>
+/// Thread-safe cache for BiCubicInterpolator resources.
+/// Caches ProbabilityGridAdapter and BiCubicInterpolator per Grid2D to avoid
+/// expensive re-computation of grid data on every scan match.
+///
+/// Performance: Creating BiCubicInterpolator requires PrecomputeGridData() which
+/// iterates all grid cells (~1000ms for 1000x1000 grid). Caching reduces this
+/// to near-zero for subsequent scan matches on the same grid.
+/// </summary>
+/// <remarks>
+/// Creates a new GridInterpolatorCache.
+/// </remarks>
+/// <param name="maxCacheSize">Maximum number of cached interpolators (default: 10).</param>
+internal sealed class GridInterpolatorCache(int maxCacheSize = 10) : IDisposable
+{
+    /// <summary>
+    /// Cache entry with weak reference to grid and strong reference to cached resources.
+    /// </summary>
+    private class CacheEntry(Grid2D grid, CachedGridInterpolator cachedInterpolator)
+    {
+        public WeakReference<Grid2D> GridRef { get; } = new WeakReference<Grid2D>(grid);
+        public CachedGridInterpolator CachedInterpolator { get; } = cachedInterpolator;
+        public DateTime LastAccessTime { get; set; } = DateTime.UtcNow;
+    }
+
+    // Cache keyed by grid identity hash code
+    private readonly ConcurrentDictionary<int, CacheEntry> _cache = new();
+
+    // Maximum cache size to prevent unbounded memory growth
+    private readonly int _maxCacheSize = maxCacheSize;
+
+    // Lock for cache cleanup and creation operations
+    private readonly Lock _cleanupLock = new();
+
+    // Statistics for debugging
+    private long _cacheHits;
+    private long _cacheMisses;
+
+    private bool _disposed;
+
+    /// <summary>
+    /// Gets or creates cached interpolator resources for the given grid.
+    /// Thread-safe: multiple threads can call this concurrently.
+    /// </summary>
+    /// <param name="grid">The grid to get interpolator for.</param>
+    /// <returns>Cached interpolator resources (do NOT dispose - owned by cache).</returns>
+    public CachedGridInterpolator GetOrCreate(Grid2D grid)
+    {
+        ArgumentNullException.ThrowIfNull(grid);
+
+        var gridHash = RuntimeHelpers.GetHashCode(grid);
+
+        // Fast path: try to get from cache
+        if (TryGetValidEntry(gridHash, grid, out var cachedInterpolator))
+        {
+            Interlocked.Increment(ref _cacheHits);
+            return cachedInterpolator;
+        }
+
+        // Slow path: need to create new interpolator
+        // Use lock to prevent multiple threads from creating interpolators for the same grid
+        lock (_cleanupLock)
+        {
+            // Double-check after acquiring lock
+            if (TryGetValidEntry(gridHash, grid, out cachedInterpolator))
+            {
+                Interlocked.Increment(ref _cacheHits);
+                return cachedInterpolator;
+            }
+
+            Interlocked.Increment(ref _cacheMisses);
+
+            // Remove invalid entry if exists
+            if (_cache.TryRemove(gridHash, out var removedEntry))
+            {
+                _ = removedEntry.CachedInterpolator?.CellLimits;
+                removedEntry.CachedInterpolator?.Dispose();
+            }
+
+            // Create new cached interpolator
+            var newInterpolator = CreateCachedInterpolator(grid);
+            var newEntry = new CacheEntry(grid, newInterpolator);
+
+            // Add to cache (should succeed since we removed invalid entry)
+            _cache[gridHash] = newEntry;
+
+            // Cleanup if needed
+            if (_cache.Count > _maxCacheSize)
+            {
+                CleanupOldEntriesLocked();
+            }
+
+            return newInterpolator;
+        }
+    }
+
+    /// <summary>
+    /// Tries to get a valid cached entry for the given grid.
+    /// </summary>
+    private bool TryGetValidEntry(int gridHash, Grid2D grid, out CachedGridInterpolator cachedInterpolator)
+    {
+        if (_cache.TryGetValue(gridHash, out var entry))
+        {
+            // Validate cached entry is still valid
+            if (entry.GridRef.TryGetTarget(out var cachedGrid) &&
+                ReferenceEquals(cachedGrid, grid) &&
+                IsValid(grid, entry.CachedInterpolator))
+            {
+                entry.LastAccessTime = DateTime.UtcNow;
+                cachedInterpolator = entry.CachedInterpolator;
+                return true;
+            }
+        }
+
+        cachedInterpolator = null!;
+        return false;
+    }
+
+    /// <summary>
+    /// Invalidates cache entry for the given grid.
+    /// Call this when the grid is modified.
+    /// </summary>
+    public void Invalidate(Grid2D grid)
+    {
+        if (grid == null) return;
+
+        var gridHash = RuntimeHelpers.GetHashCode(grid);
+        if (_cache.TryRemove(gridHash, out var entry))
+        {
+            entry.CachedInterpolator?.Dispose();
+        }
+    }
+
+    /// <summary>
+    /// Clears all cached entries.
+    /// </summary>
+    public void Clear()
+    {
+        lock (_cleanupLock)
+        {
+            foreach (var kvp in _cache)
+            {
+                kvp.Value.CachedInterpolator?.Dispose();
+            }
+            _cache.Clear();
+        }
+    }
+
+    /// <summary>
+    /// Gets the current cache size.
+    /// </summary>
+    public int Count => _cache.Count;
+
+    /// <summary>
+    /// Gets the number of cache hits.
+    /// </summary>
+    public long CacheHits => Interlocked.Read(ref _cacheHits);
+
+    /// <summary>
+    /// Gets the number of cache misses.
+    /// </summary>
+    public long CacheMisses => Interlocked.Read(ref _cacheMisses);
+
+    /// <summary>
+    /// Gets cache hit rate (0.0 to 1.0).
+    /// </summary>
+    public double HitRate
+    {
+        get
+        {
+            var hits = CacheHits;
+            var total = hits + CacheMisses;
+            return total > 0 ? (double)hits / total : 0.0;
+        }
+    }
+
+    private static CachedGridInterpolator CreateCachedInterpolator(Grid2D grid)
+    {
+        var adapter = new ProbabilityGridAdapter(grid);
+
+        var interpolator = new BiCubicInterpolator(
+            adapter.Data,
+            adapter.NumRows,
+            adapter.NumCols);
+
+        return new CachedGridInterpolator(grid, adapter, interpolator);
+    }
+
+    private static bool IsValid(Grid2D grid, CachedGridInterpolator cached)
+    {
+        // Check if grid hash matches (same grid instance)
+        if (RuntimeHelpers.GetHashCode(grid) != cached.GridHashCode)
+            return false;
+
+        // Check if grid SIZE has changed (GrowLimits was called)
+        // Only invalidate when grid grows - this is the critical structural change
+        var limits = grid.Limits.CellLimits;
+        if (limits.NumXCells != cached.CellLimits.NumXCells ||
+            limits.NumYCells != cached.CellLimits.NumYCells)
+        {
+            return false;
+        }
+
+        // Check if resolution changed (shouldn't happen normally)
+        if (Math.Abs(grid.Limits.Resolution - cached.Resolution) > 1e-9)
+            return false;
+
+        // NOTE: We intentionally do NOT check KnownCellsBox here
+        // KnownCellsBox changes on every scan insertion, causing excessive cache invalidation
+        // Using slightly stale interpolation data is acceptable for scan matching:
+        // - Existing cells: correspondence costs are similar
+        // - New cells: will return max correspondence cost via bounds check in Evaluate()
+
+        return true;
+    }
+
+    private void CleanupOldEntries()
+    {
+        lock (_cleanupLock)
+        {
+            CleanupOldEntriesLocked();
+        }
+    }
+
+    /// <summary>
+    /// Cleanup old entries. Caller must hold _cleanupLock.
+    /// </summary>
+    private void CleanupOldEntriesLocked()
+    {
+        if (_cache.Count <= _maxCacheSize)
+            return;
+
+        // Find entries to remove (oldest and entries with dead references)
+        var entriesToRemove = new List<int>();
+
+        foreach (var kvp in _cache)
+        {
+            // Remove entries with dead grid references
+            if (!kvp.Value.GridRef.TryGetTarget(out _))
+            {
+                entriesToRemove.Add(kvp.Key);
+            }
+        }
+
+        // If still need to remove more, remove oldest entries
+        if (_cache.Count - entriesToRemove.Count > _maxCacheSize)
+        {
+            var oldestEntries = _cache
+                .Where(kvp => !entriesToRemove.Contains(kvp.Key))
+                .OrderBy(kvp => kvp.Value.LastAccessTime)
+                .Take(_cache.Count - _maxCacheSize)
+                .Select(kvp => kvp.Key)
+                .ToList();
+
+            entriesToRemove.AddRange(oldestEntries);
+        }
+
+        // Remove entries
+        foreach (var key in entriesToRemove)
+        {
+            if (_cache.TryRemove(key, out var entry))
+            {
+                entry.CachedInterpolator?.Dispose();
+            }
+        }
+    }
+
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            Clear();
+            _disposed = true;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/InterpolatedTSDF2D.cs

@@ -0,0 +1,138 @@
+/*
+ * 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.D2D;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Interpolates between TSDF2D pixels with bilinear interpolation.
+/// This class works with Ceres autodiff by using double for interpolation.
+/// </summary>
+public class InterpolatedTSDF2D(TSDF2D tsdf)
+{
+    private readonly TSDF2D _tsdf = tsdf ?? throw new ArgumentNullException(nameof(tsdf));
+
+    /// <summary>
+    /// Returns the interpolated correspondence cost at (x,y).
+    /// Cells with at least one 'unknown' interpolation point result in
+    /// "MaxCorrespondenceCost()" with zero gradient.
+    /// </summary>
+    public double GetCorrespondenceCost(double x, double y)
+    {
+        ComputeInterpolationDataPoints(x, y, out double x1, out double y1, out double x2, out double y2);
+        
+        // Match C++: tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1))
+        // Use x1, y1 (center of lower pixel) instead of x, y (input coordinates)
+        var index1 = _tsdf.Limits.GetCellIndex(new Vector2(x1, y1));
+        
+        var w11 = GetWeightAt(index1);
+        var w12 = GetWeightAt(index1 + new Array2i(-1, 0));
+        var w21 = GetWeightAt(index1 + new Array2i(0, -1));
+        var w22 = GetWeightAt(index1 + new Array2i(-1, -1));
+
+        if (w11 == 0.0 || w12 == 0.0 || w21 == 0.0 || w22 == 0.0)
+        {
+            return _tsdf.MaxCorrespondenceCost;
+        }
+
+        var q11 = _tsdf.GetCorrespondenceCost(index1);
+        var q12 = _tsdf.GetCorrespondenceCost(index1 + new Array2i(-1, 0));
+        var q21 = _tsdf.GetCorrespondenceCost(index1 + new Array2i(0, -1));
+        var q22 = _tsdf.GetCorrespondenceCost(index1 + new Array2i(-1, -1));
+
+        return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22);
+    }
+
+    /// <summary>
+    /// Returns the interpolated weight at (x,y).
+    /// </summary>
+    public double GetWeight(double x, double y)
+    {
+        ComputeInterpolationDataPoints(x, y, out double x1, out double y1, out double x2, out double y2);
+        
+        // Match C++: tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1))
+        // Use x1, y1 (center of lower pixel) instead of x, y (input coordinates)
+        var index1 = _tsdf.Limits.GetCellIndex(new Vector2(x1, y1));
+        var q11 = GetWeightAt(index1);
+        var q12 = GetWeightAt(index1 + new Array2i(-1, 0));
+        var q21 = GetWeightAt(index1 + new Array2i(0, -1));
+        var q22 = GetWeightAt(index1 + new Array2i(-1, -1));
+
+        return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22);
+    }
+
+    private double GetWeightAt(Array2i index)
+    {
+        if (_tsdf.Limits.Contains(index))
+        {
+            return _tsdf.GetWeight(index);
+        }
+        return 0.0;
+    }
+
+    private void ComputeInterpolationDataPoints(double x, double y, out double x1, out double y1, out double x2, out double y2)
+    {
+        var lower = CenterOfLowerPixel(x, y);
+        x1 = lower.X;
+        y1 = lower.Y;
+        x2 = lower.X + _tsdf.Limits.Resolution;
+        y2 = lower.Y + _tsdf.Limits.Resolution;
+    }
+
+    private Vector2 CenterOfLowerPixel(double x, double y)
+    {
+        // Center of the cell containing (x, y)
+        var cellIndex = _tsdf.Limits.GetCellIndex(new Vector2(x, y));
+        var center = _tsdf.Limits.GetCellCenter(cellIndex);
+        
+        // Move to the next lower pixel center
+        if (center.X > x)
+        {
+            center.X -= _tsdf.Limits.Resolution;
+        }
+        if (center.Y > y)
+        {
+            center.Y -= _tsdf.Limits.Resolution;
+        }
+        
+        return center;
+    }
+
+    private static double InterpolateBilinear(double x, double y, double x1, double y1, double x2, double y2,
+                                       double q11, double q12, double q21, double q22)
+    {
+        // FIX: Guard against division by zero due to degenerate cell bounds
+        var dx = x2 - x1;
+        var dy = y2 - y1;
+        const double kEpsilon = 1e-10;
+        if (Math.Abs(dx) < kEpsilon || Math.Abs(dy) < kEpsilon)
+        {
+            // Degenerate case: return average of corner values
+            return (q11 + q12 + q21 + q22) * 0.25;
+        }
+
+        var normalizedX = (x - x1) / dx;
+        var normalizedY = (y - y1) / dy;
+
+        var q1 = (q12 - q11) * normalizedY + q11;
+        var q2 = (q22 - q21) * normalizedY + q21;
+        return (q2 - q1) * normalizedX + q1;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/OccupiedSpaceCostFunction2D.cs

@@ -0,0 +1,285 @@
+/*
+ * 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.Mapping.D2D;
+using CartographerSharp.Sensor;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Creates a cost function for matching the 'point_cloud' to the 'grid' with
+/// a 'pose'. The cost increases with poorer correspondence of the grid and the
+/// point observation (e.g. points falling into less occupied space).
+/// Match C++: cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc
+/// </summary>
+public class OccupiedSpaceCostFunction2D : IDisposable
+{
+    private readonly double _scalingFactor;
+    private readonly PointCloud _pointCloud;
+    private readonly Grid2D _grid;
+    private readonly MapLimits _limits;
+    private readonly BiCubicInterpolator _interpolator;
+    private readonly ProbabilityGridAdapter _adapter;
+
+    // Flag to track if we own the interpolator (should dispose) or borrowed from cache (should not dispose)
+    private readonly bool _ownsInterpolator;
+
+    /// <summary>
+    /// Creates an occupied space cost function for 2D scan matching.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="grid">Grid to match against.</param>
+    public OccupiedSpaceCostFunction2D(
+        double scalingFactor,
+        PointCloud pointCloud,
+        Grid2D grid)
+    {
+        _scalingFactor = scalingFactor;
+        _pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
+        _grid = grid ?? throw new ArgumentNullException(nameof(grid));
+        _limits = grid.Limits;
+
+        // Create adapter and interpolator
+        _adapter = new ProbabilityGridAdapter(grid);
+
+        _interpolator = new BiCubicInterpolator(
+            _adapter.Data,
+            _adapter.NumRows,
+            _adapter.NumCols
+        );
+
+        _ownsInterpolator = true; // We created it, we own it
+    }
+
+    /// <summary>
+    /// Creates an occupied space cost function using cached interpolator resources.
+    /// This constructor is much faster as it avoids PrecomputeGridData() (~1000ms savings).
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="grid">Grid to match against.</param>
+    /// <param name="cachedInterpolator">Cached interpolator resources (owned by cache, NOT disposed by this class).</param>
+    internal OccupiedSpaceCostFunction2D(
+        double scalingFactor,
+        PointCloud pointCloud,
+        Grid2D grid,
+        CachedGridInterpolator cachedInterpolator)
+    {
+        _scalingFactor = scalingFactor;
+        _pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
+        _grid = grid ?? throw new ArgumentNullException(nameof(grid));
+        _limits = grid.Limits;
+
+        ArgumentNullException.ThrowIfNull(cachedInterpolator);
+
+        // Use cached adapter and interpolator
+        _adapter = cachedInterpolator.Adapter;
+        _interpolator = cachedInterpolator.Interpolator;
+
+        _ownsInterpolator = false; // Borrowed from cache, do NOT dispose
+    }
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for occupied space matching.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="grid">Grid to match against.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        PointCloud pointCloud,
+        Grid2D grid)
+    {
+        var costFunction = new OccupiedSpaceCostFunction2D(scalingFactor, pointCloud, grid);
+        var dynamicCostFunction = new DynamicAutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: pointCloud.Count,
+            parameterBlockSizes: [3] // [x, y, theta]
+        );
+        return dynamicCostFunction;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// Match C++: OccupiedSpaceCostFunction2D::operator() in occupied_space_cost_function_2d.cc
+    /// </summary>
+    /// <param name="parameters">Pose parameters [x, y, theta].</param>
+    /// <param name="residuals">Output residuals (one per point).</param>
+    /// <returns>True on success (always returns true to match C++ behavior).</returns>
+    internal bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        try
+        {
+            // Match C++ behavior - validate inputs but don't return false for invalid inputs
+            if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
+            {
+                FillWithMaxCost(residuals);
+                return true;
+            }
+            if (residuals == null || residuals.Length < _pointCloud.Count)
+            {
+                return true;
+            }
+
+            var pose = parameters[0];
+            var translation = new Vector2(pose[0], pose[1]);
+            var rotation = pose[2];
+
+            // Create rotation matrix
+            // Match C++: Eigen::Rotation2D<T> rotation(pose[2]); rotation_matrix = rotation.toRotationMatrix();
+            var rotationMatrix = Matrix3x2.CreateRotation(rotation);
+
+            // Get grid parameters
+            var resolution = _limits.Resolution;
+            var max = _limits.Max;
+            var numRows = _adapter.NumRows;
+            var numCols = _adapter.NumCols;
+
+            // Check if grid is too small
+            if (numRows <= 0 || numCols <= 0)
+            {
+                FillWithMaxCost(residuals);
+                return true;
+            }
+
+            // Use max correspondence cost for out-of-bounds points (matching C++ behavior)
+            var kMaxCorrespondenceCost = ProbabilityValues.kMaxCorrespondenceCost;
+
+            // Match C++: for (size_t i = 0; i < point_cloud_.size(); ++i)
+            for (int i = 0; i < _pointCloud.Count; i++)
+            {
+                var point = _pointCloud[i];
+
+                // Match C++: const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())), ...);
+                //            const Eigen::Matrix<T, 3, 1> world = transform * point;
+                var localPoint = new Vector2(point.Position.X, point.Position.Y);
+                var worldPoint = Vector2.Transform(localPoint, rotationMatrix) + translation;
+
+                // COORDINATE SYSTEM MAPPING (verified correct):
+                // =============================================
+                // C++ code (occupied_space_cost_function_2d.cc lines 57-62):
+                //   interpolator.Evaluate(
+                //       (limits.max().x() - world[0]) / limits.resolution() - 0.5 + kPadding,  // row (1st arg)
+                //       (limits.max().y() - world[1]) / limits.resolution() - 0.5 + kPadding,  // col (2nd arg)
+                //       &residual[i]);
+                //
+                // C++ Ceres BiCubicInterpolator::Evaluate(row, col, value):
+                //   - First arg = row index
+                //   - Second arg = column index
+                //
+                // C# BiCubicInterpolator::Evaluate(x, y):
+                //   - x = "X coordinate in grid space (0 <= x < cols)" = column index
+                //   - y = "Y coordinate in grid space (0 <= y < rows)" = row index
+                //
+                // Therefore, to match C++ Evaluate(row, col), C# must call Evaluate(col, row) = Evaluate(x, y)
+                //
+                // actualRow    = (max.X - worldPoint.X) / resolution - 0.5  // matches C++ row formula
+                // actualColumn = (max.Y - worldPoint.Y) / resolution - 0.5  // matches C++ col formula
+                //
+                // C# call: Evaluate(actualColumn, actualRow) = Evaluate(col, row) ✓ CORRECT
+
+                double actualRow = (max.X - worldPoint.X) / resolution - 0.5;
+                double actualColumn = (max.Y - worldPoint.Y) / resolution - 0.5;
+
+                // Check for NaN/Infinity
+                double correspondenceCost;
+                if (double.IsNaN(actualColumn) || double.IsInfinity(actualColumn) ||
+                    double.IsNaN(actualRow) || double.IsInfinity(actualRow))
+                {
+                    correspondenceCost = kMaxCorrespondenceCost;
+                }
+                else
+                {
+                    // FIX: Simplified bounds checking to match C++ behavior more closely
+                    // C++ uses kPadding (INT_MAX/4) virtually - GetValue returns kMaxCorrespondenceCost
+                    // for anything outside actual grid cells.
+                    // C# uses actual array without virtual padding, so we check bounds explicitly.
+                    // BiCubicInterpolator needs 4x4 grid neighborhood (row-1 to row+2, col-1 to col+2)
+                    int minRow = (int)Math.Floor(actualRow - 1);
+                    int maxRow = (int)Math.Ceiling(actualRow + 2);
+                    int minCol = (int)Math.Floor(actualColumn - 1);
+                    int maxCol = (int)Math.Ceiling(actualColumn + 2);
+
+                    bool isOutOfBounds = minRow < 0 || maxRow >= numRows ||
+                                         minCol < 0 || maxCol >= numCols;
+
+                    if (isOutOfBounds)
+                    {
+                        // Out of bounds - return max cost (matches C++ GetValue behavior when
+                        // coordinates are outside kPadding range)
+                        correspondenceCost = kMaxCorrespondenceCost;
+                    }
+                    else
+                    {
+                        // In bounds - perform interpolation
+                        // Call Evaluate(x=column, y=row) to match C++ Evaluate(row, col)
+                        correspondenceCost = _interpolator.Evaluate(actualColumn, actualRow);
+
+                        // Validate interpolated value
+                        if (double.IsNaN(correspondenceCost) || double.IsInfinity(correspondenceCost))
+                        {
+                            correspondenceCost = kMaxCorrespondenceCost;
+                        }
+                    }
+                }
+
+                // Match C++: residual[i] = scaling_factor_ * residual[i];
+                residuals[i] = _scalingFactor * correspondenceCost;
+            }
+
+            // Match C++ behavior - always return true
+            return true;
+        }
+        catch (Exception)
+        {
+            FillWithMaxCost(residuals);
+            return true; // Match C++ behavior - always return true
+        }
+    }
+
+    /// <summary>
+    /// Fills residuals array with max correspondence cost.
+    /// </summary>
+    private void FillWithMaxCost(double[]? residuals)
+    {
+        if (residuals == null) return;
+
+        var maxCorrespondenceCost = _scalingFactor * ProbabilityValues.kMaxCorrespondenceCost;
+        int count = Math.Min(residuals.Length, _pointCloud.Count);
+        for (int i = 0; i < count; i++)
+        {
+            residuals[i] = maxCorrespondenceCost;
+        }
+    }
+
+    /// <summary>
+    /// Disposes managed resources.
+    /// Only disposes interpolator if we own it (not borrowed from cache).
+    /// </summary>
+    public void Dispose()
+    {
+        // Only dispose if we created the interpolator (not borrowed from cache)
+        if (_ownsInterpolator)
+        {
+            _interpolator?.Dispose();
+        }
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/PrecomputationGrid2D.cs

@@ -0,0 +1,217 @@
+/*
+ * 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.D2D;
+using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// A precomputed grid that contains in each cell (x0, y0) the maximum
+/// probability in the width x width area defined by x0 <= x < x0 + width and
+/// y0 <= y < y0 + width.
+/// </summary>
+internal class PrecomputationGrid2D
+{
+    private readonly Array2i _offset;
+    private readonly CellLimits _wideLimits;
+    private readonly double _minScore;
+    private readonly double _maxScore;
+    private readonly byte[] _cells;
+
+    /// <summary>
+    /// A collection of values which can be added and later removed, and the maximum
+    /// of the current values in the collection can be retrieved. All in O(1).
+    /// </summary>
+    private class SlidingWindowMaximum
+    {
+        private readonly LinkedList<double> _nonAscendingMaxima = new();
+
+        public void AddValue(double value)
+        {
+            while (_nonAscendingMaxima.Count > 0 && value > _nonAscendingMaxima.Last!.Value)
+            {
+                _nonAscendingMaxima.RemoveLast();
+            }
+            _nonAscendingMaxima.AddLast(value);
+        }
+
+        public void RemoveValue(double value)
+        {
+            // FIX: Match C++ DCHECK behavior - assert preconditions instead of silently returning
+            // C++ uses DCHECK (debug assertions) for performance:
+            //   DCHECK(!non_ascending_maxima_.empty());
+            //   DCHECK_LE(value, non_ascending_maxima_.front());
+            // Silently returning could hide bugs in the algorithm
+            System.Diagnostics.Debug.Assert(_nonAscendingMaxima.Count > 0,
+                "SlidingWindowMaximum.RemoveValue: list should not be empty");
+            System.Diagnostics.Debug.Assert(value <= _nonAscendingMaxima.First!.Value,
+                $"SlidingWindowMaximum.RemoveValue: value ({value}) should be <= front ({_nonAscendingMaxima.First.Value})");
+
+            if (value == _nonAscendingMaxima.First.Value)
+            {
+                _nonAscendingMaxima.RemoveFirst();
+            }
+        }
+
+        public double GetMaximum()
+        {
+            if (_nonAscendingMaxima.Count == 0)
+                throw new InvalidOperationException("SlidingWindowMaximum is empty");
+            return _nonAscendingMaxima.First!.Value;
+        }
+
+        public void CheckIsEmpty()
+        {
+            if (_nonAscendingMaxima.Count != 0)
+                throw new InvalidOperationException("SlidingWindowMaximum is not empty");
+        }
+    }
+
+    public PrecomputationGrid2D(
+        Grid2D grid,
+        CellLimits limits,
+        int width,
+        List<double> reusableIntermediateGrid)
+    {
+        if (width < 1)
+            throw new ArgumentException("width must be >= 1", nameof(width));
+        if (limits.NumXCells < 1 || limits.NumYCells < 1)
+            throw new ArgumentException("limits must have at least 1 cell in each dimension", nameof(limits));
+
+        _offset = new Array2i(-width + 1, -width + 1);
+        _wideLimits = new CellLimits(
+            limits.NumXCells + width - 1,
+            limits.NumYCells + width - 1);
+        _minScore = 1.0 - grid.MaxCorrespondenceCost;
+        _maxScore = 1.0 - grid.MinCorrespondenceCost;
+        _cells = new byte[_wideLimits.NumXCells * _wideLimits.NumYCells];
+
+        var stride = _wideLimits.NumXCells;
+
+        // First we compute the maximum probability for each (x0, y) achieved in the
+        // span defined by x0 <= x < x0 + width.
+        reusableIntermediateGrid.Clear();
+        reusableIntermediateGrid.Capacity = _wideLimits.NumXCells * limits.NumYCells;
+        for (int i = 0; i < reusableIntermediateGrid.Capacity; i++)
+        {
+            reusableIntermediateGrid.Add(0.0);
+        }
+
+        for (int y = 0; y < limits.NumYCells; y++)
+        {
+            var currentValues = new SlidingWindowMaximum();
+            currentValues.AddValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(0, y))));
+
+            for (int x = -width + 1; x < 0; x++)
+            {
+                reusableIntermediateGrid[x + width - 1 + y * stride] = currentValues.GetMaximum();
+                if (x + width < limits.NumXCells)
+                {
+                    currentValues.AddValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x + width, y))));
+                }
+            }
+
+            for (int x = 0; x < limits.NumXCells - width; x++)
+            {
+                reusableIntermediateGrid[x + width - 1 + y * stride] = currentValues.GetMaximum();
+                currentValues.RemoveValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x, y))));
+                currentValues.AddValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x + width, y))));
+            }
+
+            for (int x = Math.Max(limits.NumXCells - width, 0); x < limits.NumXCells; x++)
+            {
+                reusableIntermediateGrid[x + width - 1 + y * stride] = currentValues.GetMaximum();
+                currentValues.RemoveValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x, y))));
+            }
+            
+            currentValues.CheckIsEmpty();
+        }
+
+        // For each (x, y), we compute the maximum probability in the width x width
+        // region starting at each (x, y) and precompute the resulting bound on the
+        // score.
+        for (int x = 0; x < _wideLimits.NumXCells; x++)
+        {
+            var currentValues = new SlidingWindowMaximum();
+            currentValues.AddValue(reusableIntermediateGrid[x]);
+
+            for (int y = -width + 1; y < 0; y++)
+            {
+                _cells[x + (y + width - 1) * stride] = ComputeCellValue(currentValues.GetMaximum());
+                if (y + width < limits.NumYCells)
+                {
+                    currentValues.AddValue(reusableIntermediateGrid[x + (y + width) * stride]);
+                }
+            }
+
+            for (int y = 0; y < limits.NumYCells - width; y++)
+            {
+                _cells[x + (y + width - 1) * stride] = ComputeCellValue(currentValues.GetMaximum());
+                currentValues.RemoveValue(reusableIntermediateGrid[x + y * stride]);
+                currentValues.AddValue(reusableIntermediateGrid[x + (y + width) * stride]);
+            }
+
+            for (int y = Math.Max(limits.NumYCells - width, 0); y < limits.NumYCells; y++)
+            {
+                _cells[x + (y + width - 1) * stride] = ComputeCellValue(currentValues.GetMaximum());
+                currentValues.RemoveValue(reusableIntermediateGrid[x + y * stride]);
+            }
+            
+            currentValues.CheckIsEmpty();
+        }
+    }
+
+    /// <summary>
+    /// Returns a value between 0 and 255 to represent probabilities between
+    /// min_score and max_score.
+    /// </summary>
+    public int GetValue(Array2i xyIndex)
+    {
+        var localXYIndex = xyIndex - _offset;
+        
+        // Check bounds (similar to C++ unsigned cast trick)
+        if (localXYIndex.X < 0 || localXYIndex.Y < 0 ||
+            localXYIndex.X >= _wideLimits.NumXCells ||
+            localXYIndex.Y >= _wideLimits.NumYCells)
+        {
+            return 0;
+        }
+
+        var stride = _wideLimits.NumXCells;
+        return _cells[localXYIndex.X + localXYIndex.Y * stride];
+    }
+
+    /// <summary>
+    /// Maps values from [0, 255] to [min_score, max_score].
+    /// </summary>
+    public double ToScore(double value)
+    {
+        return _minScore + value * ((_maxScore - _minScore) / 255.0);
+    }
+
+    private byte ComputeCellValue(double probability)
+    {
+        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
+        // (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
+        var cellValue = (int)Math.Round((probability - _minScore) * (255.0 / (_maxScore - _minScore)), MidpointRounding.AwayFromZero);
+        // Match C++: CHECK_GE(cell_value, 0) and CHECK_LE(cell_value, 255)
+        cellValue = Math.Clamp(cellValue, 0, 255);
+        return (byte)cellValue;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/PrecomputationGridStack2D.cs

@@ -0,0 +1,62 @@
+/*
+ * 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.Mapping;
+using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Stack of precomputation grids at different resolutions for fast scan matching.
+/// </summary>
+internal class PrecomputationGridStack2D
+{
+    private readonly List<PrecomputationGrid2D> _precomputationGrids = [];
+    private readonly List<double> _reusableIntermediateGrid = [];
+
+    public PrecomputationGridStack2D(
+        Grid2D grid,
+        FastCorrelativeScanMatcherOptions2D options)
+    {
+        if (options.BranchAndBoundDepth < 1)
+            throw new ArgumentException("branch_and_bound_depth must be >= 1", nameof(options));
+
+        var maxWidth = 1 << (options.BranchAndBoundDepth - 1);
+        var limits = grid.Limits.CellLimits;
+        // Match C++: reserve capacity for precomputation_grids_
+        _precomputationGrids.Capacity = options.BranchAndBoundDepth;
+        // Match C++: reserve capacity for reusable_intermediate_grid
+        _reusableIntermediateGrid.Capacity = (limits.NumXCells + maxWidth - 1) * limits.NumYCells;
+
+        for (int i = 0; i < options.BranchAndBoundDepth; i++)
+        {
+            var width = 1 << i;
+            _precomputationGrids.Add(new PrecomputationGrid2D(
+                grid, limits, width, _reusableIntermediateGrid));
+        }
+    }
+
+    public PrecomputationGrid2D Get(int index)
+    {
+        if (index < 0 || index >= _precomputationGrids.Count)
+            throw new ArgumentOutOfRangeException(nameof(index));
+        
+        return _precomputationGrids[index];
+    }
+
+    public int MaxDepth => _precomputationGrids.Count - 1;
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/ProbabilityGridAdapter.cs

@@ -0,0 +1,132 @@
+/*
+ * 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.D2D;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Adapter to convert ProbabilityGrid to format suitable for BiCubicInterpolator.
+/// Provides grid data as 2D array with padding for boundary handling.
+/// </summary>
+internal class ProbabilityGridAdapter
+{
+    // CRITICAL: Match C++ behavior - use virtual padding like C++ (INT_MAX / 4)
+    // C++ uses: static constexpr int kPadding = INT_MAX / 4; (~536,870,912)
+    // This creates a virtual padding that doesn't require creating a real array for padding region
+    // The padding is used to offset grid coordinates, and GetValue handles out-of-bounds
+    public const int kPadding = int.MaxValue / 4;  // ~536,870,912 - matches C++ exactly
+    private readonly Grid2D _grid;
+    private readonly MapLimits _limits;
+    private readonly int _numRows;  // Virtual size: num_cells + 2 * kPadding
+    private readonly int _numCols;  // Virtual size: num_cells + 2 * kPadding
+    private readonly int _actualNumRows;  // Actual grid cells
+    private readonly int _actualNumCols;  // Actual grid cells
+    private readonly double[] _data;  // Only stores actual grid cells, not padding
+
+    public ProbabilityGridAdapter(Grid2D grid)
+    {
+        _grid = grid ?? throw new ArgumentNullException(nameof(grid));
+        _limits = grid.Limits;
+        
+        var cellLimits = _limits.CellLimits;
+        
+        // CRITICAL: Match C++ behavior - use virtual padding (INT_MAX / 4)
+        // C++: NumRows() = num_y_cells + 2 * kPadding (virtual, not real array)
+        // We need to create a virtual array for BiCubicInterpolator, but we can optimize
+        // by only storing actual grid cells and using GetValue for padding region
+        
+        _actualNumRows = cellLimits.NumYCells;
+        _actualNumCols = cellLimits.NumXCells;
+        
+        // Virtual size (matches C++): num_cells + 2 * kPadding
+        // Note: This can be very large, but we only create array for actual cells
+        // BiCubicInterpolator needs the virtual size, but we'll handle padding in GetValue
+        long numRowsLong = (long)_actualNumRows + 2L * kPadding;
+        long numColsLong = (long)_actualNumCols + 2L * kPadding;
+        
+        // Check for overflow (shouldn't happen with kPadding = INT_MAX/4)
+        if (numRowsLong > int.MaxValue || numColsLong > int.MaxValue)
+        {
+            var errorMsg = $"ProbabilityGridAdapter: Integer overflow detected! NumRows would be {numRowsLong}, NumCols would be {numColsLong}, but max int is {int.MaxValue}";
+            throw new ArgumentOutOfRangeException(nameof(grid), errorMsg);
+        }
+        
+        _numRows = (int)numRowsLong;
+        _numCols = (int)numColsLong;
+        
+        // Create array for actual grid cells (not including virtual padding)
+        long arraySizeLong = (long)_actualNumRows * _actualNumCols;
+        if (arraySizeLong > int.MaxValue)
+        {
+            var errorMsg = $"ProbabilityGridAdapter: Array size overflow! _actualNumRows={_actualNumRows}, _actualNumCols={_actualNumCols}, array size would be {arraySizeLong}, but max int is {int.MaxValue}";
+            throw new ArgumentOutOfRangeException(nameof(grid), errorMsg);
+        }
+        
+        _data = new double[_actualNumRows * _actualNumCols];
+        _grid.CopyCorrespondenceCostData(_data);
+    }
+
+    /// <summary>
+    /// Gets the number of rows for BiCubicInterpolator (actual size, not virtual).
+    /// BiCubicInterpolator needs actual array size, not virtual size with padding.
+    /// </summary>
+    public int NumRows => _actualNumRows;
+
+    /// <summary>
+    /// Gets the number of columns for BiCubicInterpolator (actual size, not virtual).
+    /// BiCubicInterpolator needs actual array size, not virtual size with padding.
+    /// </summary>
+    public int NumCols => _actualNumCols;
+    
+    /// <summary>
+    /// Gets the virtual number of rows (including padding) - for coordinate calculation only.
+    /// </summary>
+    public int VirtualNumRows => _numRows;
+
+    /// <summary>
+    /// Gets the virtual number of columns (including padding) - for coordinate calculation only.
+    /// </summary>
+    public int VirtualNumCols => _numCols;
+
+    /// <summary>
+    /// Gets the grid data array (row-major order).
+    /// </summary>
+    public double[] Data => _data;
+
+    /// <summary>
+    /// Gets the correspondence cost value at (row, col).
+    /// Returns kMaxCorrespondenceCost for out-of-bounds or padding regions.
+    /// </summary>
+    public double GetValue(int row, int col)
+    {
+        // CRITICAL: Match C++ behavior exactly
+        // C++: if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || column >= NumCols() - kPadding)
+        if (row < kPadding || col < kPadding || 
+            row >= _numRows - kPadding || col >= _numCols - kPadding)
+        {
+            // Out of bounds or padding region - return max correspondence cost
+            return ProbabilityValues.kMaxCorrespondenceCost;
+        }
+
+        // Convert from virtual coordinate space to actual grid cell coordinates
+        // C++: Eigen::Array2i(column - kPadding, row - kPadding)
+        var cellIndex = new Array2i(col - kPadding, row - kPadding);
+        
+        return _grid.GetCorrespondenceCost(cellIndex);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/RealTimeCorrelativeScanMatcher2D.cs

@@ -0,0 +1,540 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
+using ProbabilityGrid = CartographerSharp.Mapping.D2D.ProbabilityGrid;
+using TSDF2D = CartographerSharp.Mapping.D2D.TSDF2D;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// An implementation of "Real-Time Correlative Scan Matching" by Olson.
+/// The correlative scan matching algorithm is exhaustively evaluating the scan
+/// matching search space.
+/// </summary>
+public class RealTimeCorrelativeScanMatcher2D(RealTimeCorrelativeScanMatcherOptions options)
+{
+    private readonly int _numThreads = Math.Max(1, options.NumThreads);
+
+    /// <summary>
+    /// Aligns 'point_cloud' within the 'grid' given an
+    /// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
+    /// returns the score.
+    /// </summary>
+    public double Match(
+        Rigid2d initialPoseEstimate,
+        PointCloud pointCloud,
+        Grid2D grid,
+        out Rigid2d poseEstimate)
+    {
+        var initialAngle = initialPoseEstimate.Rotation; // Rotation is already the angle in radians
+
+        // Rotate point cloud to align with initial rotation
+        var rotationMatrix = Matrix3x2.CreateRotation(initialAngle);
+        var rotatedPointCloud = new PointCloud();
+        foreach (var point in pointCloud)
+        {
+            var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotationMatrix);
+            rotatedPointCloud.Add(new RangefinderPoint
+            {
+                Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
+            });
+        }
+
+        var searchParameters = new SearchParameters(
+            options.LinearSearchWindow,
+            options.AngularSearchWindow,
+            rotatedPointCloud,
+            grid.Limits.Resolution
+        );
+
+        var rotatedScans = ScanMatchingUtilities.GenerateRotatedScans(rotatedPointCloud, searchParameters);
+
+        var initialTranslation = new Vector2(initialPoseEstimate.Translation.X, initialPoseEstimate.Translation.Y);
+        var discreteScans = ScanMatchingUtilities.DiscretizeScans(grid.Limits, rotatedScans, initialTranslation);
+
+        var candidates = GenerateExhaustiveSearchCandidates(searchParameters);
+
+        ScoreCandidates(grid, discreteScans, candidates);
+
+        // Match C++: Find best candidate using std::max_element
+        var bestCandidate = candidates[0];
+        for (int i = 1; i < candidates.Count; i++)
+        {
+            if (candidates[i].Score > bestCandidate.Score)
+            {
+                bestCandidate = candidates[i];
+            }
+        }
+
+        // Match C++: Calculate final pose
+        var finalTranslation = new Vector2(
+            (initialPoseEstimate.Translation.X + bestCandidate.X),
+            (initialPoseEstimate.Translation.Y + bestCandidate.Y)
+        );
+        var finalAngle = initialAngle + bestCandidate.Orientation;
+        poseEstimate = new Rigid2d(finalTranslation, finalAngle);
+
+        return bestCandidate.Score;
+    }
+
+    /// <summary>
+    /// Computes the pose confidence by evaluating candidates around the estimated pose.
+    /// Match C++: LocalPose_Confidence method in RealTimeCorrelativeScanMatcher2D.
+    /// Returns confidence score as percentage (0-100).
+    /// </summary>
+    public double LocalPose_Confidence(
+        Rigid2d poseEstimated,
+        PointCloud pointCloud,
+        Grid2D grid)
+    {
+        var initialAngle = poseEstimated.Rotation;
+
+        // Rotate point cloud to align with estimated rotation
+        var rotationMatrix = Matrix3x2.CreateRotation(initialAngle);
+        var rotatedPointCloud = new PointCloud();
+        foreach (var point in pointCloud)
+        {
+            var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotationMatrix);
+            rotatedPointCloud.Add(new RangefinderPoint
+            {
+                Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
+            });
+        }
+
+        // Match C++: fixed parameters for confidence calculation
+        const int fixNumLinearPerturbations = 5;
+        const int fixNumAngularPerturbations = 25;
+        const double fixAngularPerturbationStepSize = 0.007;
+        const double fixResolution = 0.04;
+
+        var searchParameters = new SearchParameters(
+            fixNumLinearPerturbations,
+            fixNumAngularPerturbations,
+            fixAngularPerturbationStepSize,
+            fixResolution);
+
+        var rotatedScans = ScanMatchingUtilities.GenerateRotatedScans(rotatedPointCloud, searchParameters);
+        var initialTranslation = new Vector2(poseEstimated.Translation.X, poseEstimated.Translation.Y);
+        var discreteScans = ScanMatchingUtilities.DiscretizeScans(grid.Limits, rotatedScans, initialTranslation);
+
+        var candidates = GenerateExhaustiveSearchCandidatesForConfidence(searchParameters);
+        ScoreCandidates_Confidence(grid, discreteScans, candidates);
+
+        // Evaluate confidence from the set of candidates
+        double limitAngle = searchParameters.AngularPerturbationStepSize * (fixNumAngularPerturbations + 1);
+        double limitDist = searchParameters.Resolution * (fixNumLinearPerturbations + 1);
+
+        const double kMinScoreThreshold = 1e-10;
+        double maxOutCandidateScore = kMinScoreThreshold;
+        double maxInCandidateScore = kMinScoreThreshold;
+
+        foreach (var candidate in candidates)
+        {
+            if (Math.Abs(candidate.Orientation) <= limitAngle / 2.0 &&
+                Math.Abs(candidate.X) <= limitDist / 2.0 &&
+                Math.Abs(candidate.Y) <= limitDist / 2.0)
+            {
+                if (candidate.Score > maxInCandidateScore)
+                {
+                    maxInCandidateScore = candidate.Score;
+                }
+            }
+            else
+            {
+                if (candidate.Score > maxOutCandidateScore)
+                {
+                    maxOutCandidateScore = candidate.Score;
+                }
+            }
+        }
+
+        // Validate scores before division to avoid edge cases
+        if (maxInCandidateScore <= kMinScoreThreshold)
+        {
+            // No valid "in" candidates found - return neutral confidence
+            return 50.0;
+        }
+
+        double confidence = 1.0 - Math.Pow(maxOutCandidateScore / maxInCandidateScore, 10);
+        return confidence * 100.0;
+    }
+
+    /// <summary>
+    /// Scores candidates without applying cost weights (for confidence calculation).
+    /// Match C++: ScoreCandidates_Confidence method.
+    /// </summary>
+    private void ScoreCandidates_Confidence(Grid2D grid, List<DiscreteScan2D> discreteScans, List<Candidate2D> candidates)
+    {
+        for (int i = 0; i < candidates.Count; i++)
+        {
+            var candidate = candidates[i];
+
+            if (candidate.ScanIndex >= discreteScans.Count)
+            {
+                candidate.Score = 0.0;
+                candidates[i] = candidate;
+                continue;
+            }
+
+            var discreteScan = discreteScans[candidate.ScanIndex];
+            double candidateScore = 0.0;
+
+            if (grid is ProbabilityGrid probabilityGrid)
+            {
+                foreach (var xyIndex in discreteScan)
+                {
+                    var proposedXYIndex = new Array2i(
+                        xyIndex.X + candidate.XIndexOffset,
+                        xyIndex.Y + candidate.YIndexOffset
+                    );
+                    var probability = probabilityGrid.GetProbability(proposedXYIndex);
+                    candidateScore += probability;
+                }
+                if (discreteScan.Count > 0)
+                {
+                    candidateScore /= discreteScan.Count;
+                }
+            }
+            else if (grid.GetGridType() == CartographerSharp.Mapping.D2D.GridType.TSDF && grid is TSDF2D tsdfGrid)
+            {
+                double summedWeight = 0.0;
+                foreach (var xyIndex in discreteScan)
+                {
+                    var proposedXYIndex = new Array2i(
+                        xyIndex.X + candidate.XIndexOffset,
+                        xyIndex.Y + candidate.YIndexOffset
+                    );
+                    var (tsd, weight) = tsdfGrid.GetTSDAndWeight(proposedXYIndex);
+                    double maxCorrespondenceCost = tsdfGrid.MaxCorrespondenceCost;
+                    double normalizedTsdScore = (maxCorrespondenceCost - Math.Abs(tsd)) / maxCorrespondenceCost;
+                    candidateScore += normalizedTsdScore * weight;
+                    summedWeight += weight;
+                }
+                if (summedWeight == 0.0)
+                {
+                    candidateScore = 0.0;
+                }
+                else
+                {
+                    candidateScore /= summedWeight;
+                }
+            }
+
+            // NOTE: No cost weight penalty applied for confidence calculation (matches C++)
+            candidate.Score = candidateScore;
+            candidates[i] = candidate;
+        }
+    }
+
+    /// <summary>
+    /// Generates candidates for confidence calculation (simpler than ScoreCandidates).
+    /// </summary>
+    private static List<Candidate2D> GenerateExhaustiveSearchCandidatesForConfidence(SearchParameters searchParameters)
+    {
+        int numCandidates = 0;
+        for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
+        {
+            var bounds = searchParameters.LinearBoundsList[scanIndex];
+            int numLinearXCandidates = bounds.MaxX - bounds.MinX + 1;
+            int numLinearYCandidates = bounds.MaxY - bounds.MinY + 1;
+            numCandidates += numLinearXCandidates * numLinearYCandidates;
+        }
+
+        var candidates = new List<Candidate2D>(numCandidates);
+
+        for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
+        {
+            var bounds = searchParameters.LinearBoundsList[scanIndex];
+            for (int xIndexOffset = bounds.MinX; xIndexOffset <= bounds.MaxX; xIndexOffset++)
+            {
+                for (int yIndexOffset = bounds.MinY; yIndexOffset <= bounds.MaxY; yIndexOffset++)
+                {
+                    candidates.Add(new Candidate2D(scanIndex, xIndexOffset, yIndexOffset, searchParameters));
+                }
+            }
+        }
+
+        return candidates;
+    }
+
+    /// <summary>
+    /// Computes the score for each Candidate2D in a collection. The cost is
+    /// computed as the sum of probabilities or normalized TSD values.
+    /// </summary>
+    public void ScoreCandidates(Grid2D grid, List<DiscreteScan2D> discreteScans, List<Candidate2D> candidates)
+    {
+        int totalCandidates = candidates.Count;
+
+        // Use sequential processing if NumThreads <= 1 or too few candidates
+        if (_numThreads <= 1 || totalCandidates < _numThreads * 10)
+        {
+            int candidatesWithKnownCells = 0;
+            double maxScore = double.MinValue;
+            Candidate2D? bestCandidateWithKnownCells = null;
+            double maxScoreWithKnownCells = double.MinValue;
+
+            for (int i = 0; i < candidates.Count; i++)
+            {
+                ScoreSingleCandidate(grid, discreteScans, candidates, i,
+                    ref candidatesWithKnownCells, ref maxScore, ref bestCandidateWithKnownCells, ref maxScoreWithKnownCells);
+            }
+            return;
+        }
+
+        // Parallel processing using Thread with high priority
+        ScoreCandidatesParallel(grid, discreteScans, candidates, totalCandidates);
+    }
+
+    private void ScoreCandidatesParallel(
+        Grid2D grid,
+        List<DiscreteScan2D> discreteScans,
+        List<Candidate2D> candidates,
+        int totalCandidates)
+    {
+        // THREAD SAFETY NOTE:
+        // This uses partitioned writes pattern where each thread writes to non-overlapping indices.
+        // List<T> internally uses an array, and concurrent writes to different indices of an array
+        // are thread-safe as long as no reallocation occurs (no Add/Remove operations).
+        // Each thread processes a distinct chunk [startIndex, endIndex) with no overlap.
+
+        // Thread-safe shared state
+        int candidatesWithKnownCells = 0;
+        double maxScore = double.MinValue;
+        Candidate2D? bestCandidateWithKnownCells = null;
+        double maxScoreWithKnownCells = double.MinValue;
+        Lock lockObject = new();
+        
+        // Calculate chunk size
+        int chunkSize = Math.Max(1, totalCandidates / _numThreads);
+        int numThreads = Math.Min(_numThreads, totalCandidates);
+        
+        // Create and start threads with high priority
+        Thread[] threads = new Thread[numThreads];
+        
+        // Use CountdownEvent with using statement to ensure proper disposal
+        using CountdownEvent countdown = new(numThreads);
+        // Capture variables for thread closure to avoid closure issues
+        int capturedNumThreads = numThreads;
+        int capturedChunkSize = chunkSize;
+        int capturedTotalCandidates = totalCandidates;
+        
+        for (int threadIndex = 0; threadIndex < numThreads; threadIndex++)
+        {
+            // Capture loop variables to avoid closure issues
+            int capturedThreadIndex = threadIndex;
+            int capturedStartIndex = capturedThreadIndex * capturedChunkSize;
+            int capturedEndIndex = (capturedThreadIndex == capturedNumThreads - 1) 
+                ? capturedTotalCandidates 
+                : (capturedThreadIndex + 1) * capturedChunkSize;
+            
+            threads[capturedThreadIndex] = new Thread(() =>
+            {
+                Thread.BeginThreadAffinity();
+                try
+                {
+                    int localCandidatesWithKnownCells = 0;
+                    double localMaxScore = double.MinValue;
+                    Candidate2D? localBestCandidateWithKnownCells = null;
+                    double localMaxScoreWithKnownCells = double.MinValue;
+                    
+                    // Process candidates in this thread's chunk
+                    for (int i = capturedStartIndex; i < capturedEndIndex; i++)
+                    {
+                        ScoreSingleCandidate(grid, discreteScans, candidates, i,
+                            ref localCandidatesWithKnownCells, ref localMaxScore, 
+                            ref localBestCandidateWithKnownCells, ref localMaxScoreWithKnownCells);
+                    }
+                    
+                    // Merge thread-local results with shared state (thread-safe)
+                    lock (lockObject)
+                    {
+                        candidatesWithKnownCells += localCandidatesWithKnownCells;
+                        if (localMaxScore > maxScore)
+                        {
+                            maxScore = localMaxScore;
+                        }
+                        if (localBestCandidateWithKnownCells.HasValue && 
+                            localMaxScoreWithKnownCells > maxScoreWithKnownCells)
+                        {
+                            maxScoreWithKnownCells = localMaxScoreWithKnownCells;
+                            bestCandidateWithKnownCells = localBestCandidateWithKnownCells;
+                        }
+                    }
+                    countdown.Signal();
+                }
+                finally
+                {
+                    Thread.EndThreadAffinity();
+                }
+            })
+            {
+                IsBackground = false, // Foreground thread for high priority
+                Priority = ThreadPriority.Highest // Set thread priority to highest
+            };
+            
+            threads[capturedThreadIndex].Start();
+        }
+        
+        // Ensure all threads have finished (additional safety check)
+        foreach (var thread in threads)
+        {
+            if (thread.IsAlive)
+            {
+                thread.Join();
+            }
+        }
+        countdown.Wait();
+    }
+    
+    private void ScoreSingleCandidate(
+        Grid2D grid,
+        List<DiscreteScan2D> discreteScans,
+        List<Candidate2D> candidates,
+        int index,
+        ref int candidatesWithKnownCells,
+        ref double maxScore,
+        ref Candidate2D? bestCandidateWithKnownCells,
+        ref double maxScoreWithKnownCells)
+    {
+        var candidate = candidates[index];
+        
+        if (candidate.ScanIndex >= discreteScans.Count)
+        {
+            candidate.Score = 0.0;
+            candidates[index] = candidate;
+            return;
+        }
+
+        var discreteScan = discreteScans[candidate.ScanIndex];
+        double candidateScore = 0.0;
+
+        if (grid is ProbabilityGrid probabilityGrid)
+        {
+            // FIX: Match C++ behavior - no explicit bounds check needed
+            // ProbabilityGrid.GetProbability already returns kMinProbability for out-of-bounds cells
+            // (C++ probability_grid.cc line 79: if (!limits().Contains(cell_index)) return kMinProbability;)
+            foreach (var xyIndex in discreteScan)
+            {
+                var proposedXYIndex = new Array2i(
+                    xyIndex.X + candidate.XIndexOffset,
+                    xyIndex.Y + candidate.YIndexOffset
+                );
+
+                // Get probability - out-of-bounds/unknown cells will return kMinProbability (0.1)
+                var probability = probabilityGrid.GetProbability(proposedXYIndex);
+                candidateScore += probability;
+            }
+            candidateScore /= discreteScan.Count;
+            // Match C++ CHECK_GT(candidate_score, 0.f) - validate score is positive
+            // For ProbabilityGrid, scores should always be > 0 since probabilities are >= kMinProbability
+            System.Diagnostics.Debug.Assert(candidateScore > 0.0,
+                $"Candidate score must be positive for ProbabilityGrid, got {candidateScore}");
+        }
+        else if (grid.GetGridType() == CartographerSharp.Mapping.D2D.GridType.TSDF)
+        {
+            if (grid is TSDF2D tsdfGrid)
+            {
+                // Match C++: Use GetTSDAndWeight and compute normalized score with weighted average
+                double summedWeight = 0.0;
+                foreach (var xyIndex in discreteScan)
+                {
+                    var proposedXYIndex = new Array2i(
+                        xyIndex.X + candidate.XIndexOffset,
+                        xyIndex.Y + candidate.YIndexOffset
+                    );
+                    var (tsd, weight) = tsdfGrid.GetTSDAndWeight(proposedXYIndex);
+                    // Match C++: normalized_tsd_score = (max_correspondence_cost - abs(tsd)) / max_correspondence_cost
+                    double maxCorrespondenceCost = tsdfGrid.MaxCorrespondenceCost;
+                    double normalizedTsdScore = (maxCorrespondenceCost - Math.Abs(tsd)) / maxCorrespondenceCost;
+                    candidateScore += normalizedTsdScore * weight;
+                    summedWeight += weight;
+                }
+                // Match C++: if (summed_weight == 0.f) return 0.f; candidate_score /= summed_weight;
+                if (summedWeight == 0.0)
+                {
+                    candidateScore = 0.0;
+                }
+                else
+                {
+                    candidateScore /= summedWeight;
+                }
+            }
+        }
+
+        // Apply exponential penalty based on translation and rotation delta cost weights
+        var translationDistance = Math.Sqrt(candidate.X * candidate.X + candidate.Y * candidate.Y);
+        var rotationDelta = Math.Abs(candidate.Orientation);
+        var cost = translationDistance * options.TranslationDeltaCostWeight + 
+                   rotationDelta * options.RotationDeltaCostWeight;
+        candidateScore *= Math.Exp(-cost * cost);
+
+        candidate.Score = candidateScore;
+        candidates[index] = candidate;
+        
+        // Track candidates with known cells AFTER Score is set
+        // Unknown cells all return kMinProbability = 0.1, so scores > 0.1 indicate known cells
+        if (candidateScore > 0.1 + 1e-5)
+        {
+            candidatesWithKnownCells++;
+            if (candidateScore > maxScoreWithKnownCells)
+            {
+                maxScoreWithKnownCells = candidateScore;
+                bestCandidateWithKnownCells = candidate;
+            }
+        }
+        
+        // Track best candidate overall
+        if (candidateScore > maxScore)
+        {
+            maxScore = candidateScore;
+        }
+    }
+    
+    private static List<Candidate2D> GenerateExhaustiveSearchCandidates(SearchParameters searchParameters)
+    {
+        // Match C++: Calculate total number of candidates and reserve capacity
+        int numCandidates = 0;
+        for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
+        {
+            var bounds = searchParameters.LinearBoundsList[scanIndex];
+            int numLinearXCandidates = bounds.MaxX - bounds.MinX + 1;
+            int numLinearYCandidates = bounds.MaxY - bounds.MinY + 1;
+            numCandidates += numLinearXCandidates * numLinearYCandidates;
+        }
+
+        var candidates = new List<Candidate2D>(numCandidates); // Reserve capacity
+
+        for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
+        {
+            var bounds = searchParameters.LinearBoundsList[scanIndex];
+            for (int xIndexOffset = bounds.MinX; xIndexOffset <= bounds.MaxX; xIndexOffset++)
+            {
+                for (int yIndexOffset = bounds.MinY; yIndexOffset <= bounds.MaxY; yIndexOffset++)
+                {
+                    candidates.Add(new Candidate2D(scanIndex, xIndexOffset, yIndexOffset, searchParameters));
+                }
+            }
+        }
+
+        return candidates;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/RotationDeltaCostFunctor2D.cs

@@ -0,0 +1,75 @@
+/*
+ * 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 CeresSharp;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
+/// the solution's distance from 'target_angle'.
+/// </summary>
+public class RotationDeltaCostFunctor2D
+{
+    private readonly double _scalingFactor;
+    private readonly double _targetAngle;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for rotation delta.
+    /// </summary>
+    /// <param name="scalingFactor">Weight for the rotation cost.</param>
+    /// <param name="targetAngle">Target rotation angle in radians.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor, double targetAngle)
+    {
+        var functor = new RotationDeltaCostFunctor2D(scalingFactor, targetAngle);
+        return new AutoDiffCostFunction(
+            functor.Evaluate,
+            numResiduals: 1,
+            parameterBlockSizes: [3] // [x, y, theta]
+        );
+    }
+
+    private RotationDeltaCostFunctor2D(double scalingFactor, double targetAngle)
+    {
+        _scalingFactor = scalingFactor;
+        _targetAngle = targetAngle;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Pose parameters [x, y, theta].</param>
+    /// <param name="residuals">Output residual [dtheta].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 1)
+            return false;
+
+        var pose = parameters[0];
+        var theta = pose[2]; // rotation angle
+
+        // Match C++: residual[0] = scaling_factor_ * (pose[2] - angle_);
+        // C++ does NOT normalize angle difference - Ceres autodiff handles it
+        residuals[0] = _scalingFactor * (theta - _targetAngle);
+
+        return true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/TSDFMatchCostFunction2D.cs

@@ -0,0 +1,135 @@
+/*
+ * 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.Mapping.D2D;
+using CartographerSharp.Sensor;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Creates a cost function for matching the 'point_cloud' in the 'grid' at a 'pose'.
+/// The cost increases with the signed distance of the matched point location in the 'grid'.
+/// </summary>
+/// <remarks>
+/// Creates a TSDF match cost function for 2D scan matching.
+/// </remarks>
+/// <param name="residualScalingFactor">Scaling factor for residuals.</param>
+/// <param name="pointCloud">Point cloud to match.</param>
+/// <param name="tsdf">TSDF grid to match against.</param>
+public class TSDFMatchCostFunction2D(
+    double residualScalingFactor,
+    PointCloud _pointCloud,
+    TSDF2D tsdf) : IDisposable
+{
+    private readonly InterpolatedTSDF2D _interpolatedTSDF = new(tsdf);
+    // Cache tempResiduals array to avoid allocation on every Evaluate call
+    // Evaluate() is called many times during Ceres optimization (function + Jacobian)
+    private double[]? _tempResiduals;
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for TSDF matching.
+    /// </summary>
+    /// <param name="scalingFactor">Scaling factor.</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="tsdf">TSDF grid to match against.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        PointCloud pointCloud,
+        TSDF2D tsdf)
+    {
+        var costFunction = new TSDFMatchCostFunction2D(scalingFactor, pointCloud, tsdf);
+        return new DynamicAutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: pointCloud.Count,
+            parameterBlockSizes: [3] // [x, y, theta]
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Pose parameters [x, y, theta].</param>
+    /// <param name="residuals">Output residuals (one per point).</param>
+    /// <returns>True on success.</returns>
+    internal bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        // Return true with zero residuals for invalid inputs (consistent with OccupiedSpaceCostFunction2D)
+        // Returning false would tell Ceres the evaluation failed, causing it to reject the step
+        if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3 ||
+            residuals == null || residuals.Length < _pointCloud.Count)
+        {
+            if (residuals != null)
+                Array.Clear(residuals, 0, residuals.Length);
+            return true;
+        }
+
+        var pose = parameters[0];
+        var translation = new Vector2(pose[0], pose[1]);
+        var rotation = pose[2];
+
+        // Create rotation matrix
+        var rotationMatrix = Matrix3x2.CreateRotation(rotation);
+
+        // Reuse cached array to avoid allocation per Evaluate call
+        if (_tempResiduals == null || _tempResiduals.Length < _pointCloud.Count)
+            _tempResiduals = new double[_pointCloud.Count];
+
+        double summedWeight = 0.0;
+
+        for (int i = 0; i < _pointCloud.Count; i++)
+        {
+            var point = _pointCloud[i];
+
+            // Transform point from local frame to world frame
+            var localPoint = new Vector2(point.Position.X, point.Position.Y);
+            var worldPoint = Vector2.Transform(localPoint, rotationMatrix) + translation;
+
+            var pointWeight = _interpolatedTSDF.GetWeight(worldPoint.X, worldPoint.Y);
+            summedWeight += pointWeight;
+
+            _tempResiduals[i] = _pointCloud.Count * residualScalingFactor *
+                              _interpolatedTSDF.GetCorrespondenceCost(worldPoint.X, worldPoint.Y) *
+                              pointWeight;
+        }
+
+        if (summedWeight == 0.0)
+        {
+            // All weights are zero - return zero residuals (consistent with OccupiedSpaceCostFunction2D)
+            Array.Clear(residuals, 0, _pointCloud.Count);
+            return true;
+        }
+
+        // Normalize residuals by summed weight
+        for (int i = 0; i < _pointCloud.Count; i++)
+        {
+            residuals[i] = _tempResiduals[i] / summedWeight;
+        }
+
+        return true;
+    }
+
+    /// <summary>
+    /// Disposes managed resources.
+    /// </summary>
+    public void Dispose()
+    {
+        GC.SuppressFinalize(this);
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/ScanMatching/TranslationDeltaCostFunctor2D.cs

@@ -0,0 +1,79 @@
+/*
+ * 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  RobotNet10.Shared.Numbers;
+using CeresSharp;
+
+namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+
+/// <summary>
+/// Computes the cost of translating 'pose' to 'target_translation'.
+/// Cost increases with the solution's distance from 'target_translation'.
+/// </summary>
+public class TranslationDeltaCostFunctor2D
+{
+    private readonly double _scalingFactor;
+    private readonly double _targetX;
+    private readonly double _targetY;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for translation delta.
+    /// </summary>
+    /// <param name="scalingFactor">Weight for the translation cost.</param>
+    /// <param name="targetTranslation">Target translation (x, y).</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor, Vector2 targetTranslation)
+    {
+        var functor = new TranslationDeltaCostFunctor2D(scalingFactor, targetTranslation);
+        return new AutoDiffCostFunction(
+            functor.Evaluate,
+            numResiduals: 2,
+            parameterBlockSizes: [3] // [x, y, theta]
+        );
+    }
+
+    private TranslationDeltaCostFunctor2D(double scalingFactor, Vector2 targetTranslation)
+    {
+        _scalingFactor = scalingFactor;
+        _targetX = targetTranslation.X;
+        _targetY = targetTranslation.Y;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Pose parameters [x, y, theta].</param>
+    /// <param name="residuals">Output residuals [dx, dy].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 2)
+            return false;
+
+        var pose = parameters[0];
+        var x = pose[0];
+        var y = pose[1];
+        // theta (pose[2]) is not used for translation delta
+
+        residuals[0] = _scalingFactor * (x - _targetX);
+        residuals[1] = _scalingFactor * (y - _targetY);
+
+        return true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/TSDValueConverter.cs

@@ -0,0 +1,140 @@
+/*
+ * 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.
+ */
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Provides conversions between double and uint16 representations for
+/// truncated signed distance values and weights.
+/// </summary>
+public class TSDValueConverter
+{
+    private const double kMinWeight = 0.0;
+    private const ushort kUnknownTSDValue = 0;
+    private const ushort kUnknownWeightValue = 0;
+    private const ushort kUpdateMarker = (ushort)(1u << 15); // Highest bit
+
+    private readonly double _maxTSD;
+    private readonly double _minTSD;
+    private readonly double _maxWeight;
+    private readonly double _tsdResolution;
+    private readonly double _weightResolution;
+    private readonly double[] _valueToTSD;
+    private readonly double[] _valueToWeight;
+
+    public TSDValueConverter(double maxTSD, double maxWeight, ValueConversionTables conversionTables)
+    {
+        _maxTSD = maxTSD;
+        _minTSD = -maxTSD;
+        _maxWeight = maxWeight;
+        _tsdResolution = 32766.0 / (maxTSD - _minTSD);
+        _weightResolution = 32766.0 / (maxWeight - kMinWeight);
+
+        // Get conversion tables from ValueConversionTables
+        _valueToTSD = conversionTables.GetConversionTable(_minTSD, _minTSD, _maxTSD);
+        _valueToWeight = conversionTables.GetConversionTable(kMinWeight, kMinWeight, maxWeight);
+    }
+
+    /// <summary>
+    /// Converts a TSD to a ushort in the [1, 32767] range.
+    /// </summary>
+    public ushort TSDToValue(double tsd)
+    {
+        var clamped = ClampTSD(tsd);
+        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
+        // (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
+        var value = (int)Math.Round((clamped - _minTSD) * _tsdResolution, MidpointRounding.AwayFromZero) + 1;
+        return (ushort)Math.Clamp(value, 1, 32767);
+    }
+
+    /// <summary>
+    /// Converts a weight to a ushort in the [1, 32767] range.
+    /// </summary>
+    public ushort WeightToValue(double weight)
+    {
+        var clamped = ClampWeight(weight);
+        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
+        // (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
+        var value = (int)Math.Round((clamped - kMinWeight) * _weightResolution, MidpointRounding.AwayFromZero) + 1;
+        return (ushort)Math.Clamp(value, 1, 32767);
+    }
+
+    /// <summary>
+    /// Converts a ushort (which may or may not have the update marker set) to a
+    /// value in the range [min_tsd_, max_tsd_].
+    /// Match C++: return (*value_to_tsd_)[value];
+    /// Note: C++ conversion table has size 65536 (all possible ushort values),
+    /// and handles update marker by masking it out in PrecomputeValueToBoundedFloat.
+    /// </summary>
+    public double ValueToTSD(ushort value)
+    {
+        // Match C++: value_to_tsd_ has size 65536 (all possible ushort values)
+        // C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
+        // So we can safely access _valueToTSD[value] even if value has update marker set
+        if (value >= _valueToTSD.Length)
+        {
+            // Defensive check: should never happen if conversion table is correctly sized
+            return _minTSD;
+        }
+        return _valueToTSD[value];
+    }
+
+    /// <summary>
+    /// Converts a ushort (which may or may not have the update marker set) to a
+    /// value in the range [min_weight_, max_weight_].
+    /// Match C++: return (*value_to_weight_)[value];
+    /// Note: C++ conversion table has size 65536 (all possible ushort values),
+    /// and handles update marker by masking it out in PrecomputeValueToBoundedFloat.
+    /// </summary>
+    public double ValueToWeight(ushort value)
+    {
+        // Match C++: value_to_weight_ has size 65536 (all possible ushort values)
+        // C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
+        // So we can safely access _valueToWeight[value] even if value has update marker set
+        if (value >= _valueToWeight.Length)
+        {
+            // Defensive check: should never happen if conversion table is correctly sized
+            return kMinWeight;
+        }
+        return _valueToWeight[value];
+    }
+
+    public static ushort GetUnknownTSDValue() => kUnknownTSDValue;
+    public static ushort GetUnknownWeightValue() => kUnknownWeightValue;
+    public static ushort GetUpdateMarker() => kUpdateMarker;
+
+    public double GetMaxTSD() => _maxTSD;
+    public double GetMinTSD() => _minTSD;
+    public double GetMaxWeight() => _maxWeight;
+    public double GetMinWeight() => kMinWeight;
+
+    /// <summary>
+    /// Clamps TSD to be in the range [min_tsd_, max_tsd_].
+    /// </summary>
+    private double ClampTSD(double tsd)
+    {
+        return Math.Clamp(tsd, _minTSD, _maxTSD);
+    }
+
+    /// <summary>
+    /// Clamps weight to be in the range [min_weight_, max_weight_].
+    /// </summary>
+    private double ClampWeight(double weight)
+    {
+        return Math.Clamp(weight, kMinWeight, _maxWeight);
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/2D/TrajectoryBuilder2DAdapter.cs

@@ -0,0 +1,78 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+
+namespace CartographerSharp.Mapping.Internal.D2D;
+
+/// <summary>
+/// Adapter to make LocalTrajectoryBuilder2D implement TrajectoryBuilderInterface.
+/// </summary>
+internal class TrajectoryBuilder2DAdapter(LocalTrajectoryBuilder2D localBuilder) : ITrajectoryBuilder
+{
+    private readonly LocalTrajectoryBuilder2D _localBuilder = localBuilder ?? throw new ArgumentNullException(nameof(localBuilder));
+
+    public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
+    {
+        // LocalTrajectoryBuilder2D now returns ITrajectoryBuilder.MatchingResult directly
+        return _localBuilder.AddRangeData(sensorId, timedPointCloudData);
+    }
+
+    public void AddSensorData(string sensorId, ImuData imuData)
+    {
+        _localBuilder.AddImuData(imuData);
+    }
+
+    public void AddSensorData(string sensorId, OdometryData odometryData)
+    {
+        _localBuilder.AddOdometryData(odometryData);
+    }
+
+    public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
+    {
+        // Fixed frame pose data is typically used for external localization sources
+        // LocalTrajectoryBuilder2D does not directly handle FixedFramePoseData.
+        // If this adapter were wrapping another ITrajectoryBuilder, it would forward.
+        // For now, it's a no-op for LocalTrajectoryBuilder2D.
+    }
+
+    public void AddSensorData(string sensorId, LandmarkData landmarkData)
+    {
+        // Landmark data is used for landmark-based SLAM
+        // LocalTrajectoryBuilder2D does not directly handle LandmarkData.
+        // If this adapter were wrapping another ITrajectoryBuilder, it would forward.
+        // For now, it's a no-op for LocalTrajectoryBuilder2D.
+    }
+
+    public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
+    {
+        // LocalTrajectoryBuilder2D doesn't use this method
+        // Results are returned directly from AddRangeData
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        return _localBuilder.TryGetExtrapolatedPose(time);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        return _localBuilder.TryGetExtrapolatedPoseFilter(time);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/GlobalTrajectoryBuilder3D.cs

@@ -0,0 +1,162 @@
+/*
+ * 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.Mapping.Internal.D3D;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Wires up local SLAM (LocalTrajectoryBuilder3D) with the PoseGraph for 3D mapping.
+/// Handles sensor data, triggers local SLAM, and adds results to the pose graph.
+/// </summary>
+public class GlobalTrajectoryBuilder3D(
+    LocalTrajectoryBuilder3D? localTrajectoryBuilder,
+    int trajectoryId,
+    PoseGraph3D poseGraph,
+    MotionFilter? poseGraphOdometryMotionFilter = null) : ITrajectoryBuilder
+{
+    public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
+    {
+        if (localTrajectoryBuilder == null)
+        {
+            throw new InvalidOperationException("Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.");
+        }
+
+        var matchingResult = localTrajectoryBuilder.AddRangeData(sensorId, timedPointCloudData);
+        if (matchingResult == null)
+        {
+            // The range data has not been fully accumulated yet.
+            return null;
+        }
+
+        var result = matchingResult.Value;
+        ITrajectoryBuilder.InsertionResult? insertionResult = null;
+        
+        // If we have an insertion result, add node to pose graph
+        if (result.InsertionResult.HasValue)
+        {
+            var insertionResultValue = result.InsertionResult.Value;
+            if (insertionResultValue.ConstantData is null)
+                throw new InvalidOperationException($"insertionResult.ConstantData of sensorId {sensorId} is null");
+
+            // Cast submaps to Submap3D for PoseGraph3D.AddNode
+            var submaps3D = insertionResultValue.InsertionSubmaps.Cast<Mapping.D3D.Submap3D>().ToList();
+            
+            var nodeId = poseGraph.AddNode(
+                insertionResultValue.ConstantData,
+                trajectoryId,
+                submaps3D);
+
+            if (nodeId.TrajectoryId != trajectoryId)
+            {
+                throw new InvalidOperationException($"Node trajectory ID {nodeId.TrajectoryId} does not match expected {trajectoryId}");
+            }
+
+            // Update insertionResult with NodeId
+            insertionResult = new ITrajectoryBuilder.InsertionResult(
+                nodeId,
+                insertionResultValue.ConstantData,
+                insertionResultValue.InsertionSubmaps);
+            
+            // Update result with new insertionResult (including NodeId)
+            result = new ITrajectoryBuilder.MatchingResult(
+                trajectoryId,
+                result.Time,
+                result.LocalPose,
+                result.RangeDataInLocal,
+                insertionResult,
+                result.PoseConfidence,
+                result.CeresScore,
+                result.SamplePointCloudGlobal
+            );
+        }
+        
+        return result;
+    }
+
+    public void AddSensorData(string sensorId, ImuData imuData)
+    {
+        // Add to local trajectory builder if available
+        localTrajectoryBuilder?.AddImuData(imuData);
+        
+        // Always add to pose graph for global optimization
+        poseGraph.AddImuData(trajectoryId, imuData);
+    }
+
+    public void AddSensorData(string sensorId, OdometryData odometryData)
+    {
+        if (!odometryData.Pose.IsValid())
+        {
+            throw new ArgumentException($"Invalid odometry pose: {odometryData.Pose}", nameof(odometryData));
+        }
+
+        // Add to local trajectory builder if available
+        localTrajectoryBuilder?.AddOdometryData(odometryData);
+
+        // Apply motion filter if configured
+        if (poseGraphOdometryMotionFilter != null &&
+            poseGraphOdometryMotionFilter.IsSimilar(odometryData.Time, odometryData.Pose))
+        {
+            return; // Filtered out due to similar motion
+        }
+
+        // Add to pose graph
+        poseGraph.AddOdometryData(trajectoryId, odometryData);
+    }
+
+    public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
+    {
+        if (fixedFramePoseData.Pose.HasValue && !fixedFramePoseData.Pose.Value.IsValid())
+        {
+            throw new ArgumentException(
+                $"Invalid fixed frame pose: {fixedFramePoseData.Pose.Value}",
+                nameof(fixedFramePoseData));
+        }
+
+        poseGraph.AddFixedFramePoseData(trajectoryId, fixedFramePoseData);
+    }
+
+    public void AddSensorData(string sensorId, LandmarkData landmarkData)
+    {
+        poseGraph.AddLandmarkData(trajectoryId, landmarkData);
+    }
+
+    public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
+    {
+        if (localTrajectoryBuilder != null)
+        {
+            throw new InvalidOperationException(
+                "Can't add LocalSlamResultData with local_trajectory_builder_ present.");
+        }
+
+        // Add the local SLAM result directly to the pose graph
+        localSlamResultData.AddToPoseGraph(trajectoryId, poseGraph);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        return localTrajectoryBuilder?.TryGetExtrapolatedPose(time);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        return localTrajectoryBuilder?.TryGetExtrapolatedPoseFilter(time);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ImuIntegration.cs

@@ -0,0 +1,126 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Result of IMU integration.
+/// </summary>
+public struct IntegrateImuResult
+{
+    public Vector3 DeltaVelocity { get; set; }
+    public Vector3 DeltaTranslation { get; set; }
+    public Quaternion DeltaRotation { get; set; }
+
+    public IntegrateImuResult(Vector3 deltaVelocity, Vector3 deltaTranslation, Quaternion deltaRotation)
+    {
+        DeltaVelocity = deltaVelocity;
+        DeltaTranslation = deltaTranslation;
+        DeltaRotation = deltaRotation;
+    }
+}
+
+/// <summary>
+/// IMU integration utilities.
+/// </summary>
+public static class ImuIntegration
+{
+    /// <summary>
+    /// Integrates IMU data between start_time and end_time.
+    /// Returns delta_velocity, delta_translation, and delta_rotation.
+    /// </summary>
+    public static IntegrateImuResult IntegrateImu(
+        List<ImuData> imuData,
+        long startTime,
+        long endTime,
+        ref int imuIndex)
+    {
+        if (startTime > endTime)
+            throw new ArgumentException("startTime must be <= endTime");
+        
+        if (imuIndex < 0 || imuIndex >= imuData.Count)
+            throw new ArgumentOutOfRangeException(nameof(imuIndex));
+        
+        if (imuData[imuIndex].Time > startTime)
+            throw new ArgumentException("imuData[imuIndex].Time must be <= startTime");
+        
+        if (imuIndex + 1 < imuData.Count && imuData[imuIndex + 1].Time <= startTime)
+            throw new ArgumentException("imuData[imuIndex+1].Time must be > startTime");
+
+        var result = new IntegrateImuResult(
+            Vector3.Zero,
+            Vector3.Zero,
+            Quaternion.Identity);
+
+        long currentTime = startTime;
+
+        while (currentTime < endTime)
+        {
+            long nextImuTime = long.MaxValue;
+            if (imuIndex + 1 < imuData.Count)
+            {
+                nextImuTime = imuData[imuIndex + 1].Time;
+            }
+            
+            long nextTime = Math.Min(nextImuTime, endTime);
+            double deltaT = (nextTime - currentTime) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)
+
+            var currentImu = imuData[imuIndex];
+            
+            // Compute delta angle from angular velocity
+            var deltaAngle = currentImu.AngularVelocity * deltaT;
+            
+            // Convert angle-axis to quaternion (simplified - assumes small angles)
+            // For small angles: q ≈ [1, 0.5*angle.x, 0.5*angle.y, 0.5*angle.z]
+            var angleLength = deltaAngle.Length();
+            Quaternion deltaRotation;
+            if (angleLength < 1e-6)
+            {
+                deltaRotation = Quaternion.Identity;
+            }
+            else
+            {
+                var axis = Vector3.Normalize(deltaAngle);
+                deltaRotation = Quaternion.CreateFromAxisAngle(axis, angleLength);
+            }
+            
+            // Update cumulative rotation
+            result.DeltaRotation = Quaternion.Multiply(result.DeltaRotation, deltaRotation);
+            
+            // Integrate linear acceleration
+            // Rotate acceleration to current orientation frame
+            var rotatedAcceleration = Vector3.Transform(currentImu.LinearAcceleration, result.DeltaRotation);
+            var deltaVelocity = rotatedAcceleration * deltaT;
+            result.DeltaVelocity += deltaVelocity;
+            
+            // Integrate velocity to get translation
+            result.DeltaTranslation += result.DeltaVelocity * deltaT;
+
+            currentTime = nextTime;
+            if (currentTime == nextImuTime)
+            {
+                imuIndex++;
+            }
+        }
+
+        return result;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/LocalTrajectoryBuilder3D.cs

@@ -0,0 +1,713 @@
+/*
+ * 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.Mapping.D3D;
+using CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+using LocalTrajectoryBuilderOptions3D = CartographerSharp.Models.Mapping.LocalTrajectoryBuilderOptions3D;
+using RangeDataOperations = CartographerSharp.Sensor.RangeDataOperations;
+using Submap3D = CartographerSharp.Mapping.D3D.Submap3D;
+using PointCloudOperations = CartographerSharp.Sensor.PointCloudOperations;
+using FastCorrelativeScanMatcherOptions3D = CartographerSharp.Models.Mapping.FastCorrelativeScanMatcherOptions3D;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
+/// without loop closure for 3D.
+/// </summary>
+public class LocalTrajectoryBuilder3D : IDisposable
+{
+    private bool _disposed;
+    public struct InsertionResult(TrajectoryNode.Data? constantData, List<Submap3D> insertionSubmaps)
+    {
+        public TrajectoryNode.Data? ConstantData { get; set; } = constantData;
+        public List<Submap3D> InsertionSubmaps { get; set; } = insertionSubmaps ?? [];
+    }
+
+    private readonly LocalTrajectoryBuilderOptions3D _options;
+    private readonly ActiveSubmaps3D _activeSubmaps;
+    private readonly MotionFilter _motionFilter;
+    private readonly CeresScanMatcher3D? _ceresScanMatcher;
+    private PoseExtrapolator? _extrapolator;
+    // Range data accumulation - these are used when NumAccumulatedRangeData > 1
+    private int _numAccumulated = 0;
+    private readonly List<TimedPointCloudOriginData> _accumulatedPointCloudOriginData = [];
+    private long? _lastSensorTime;
+    private readonly RangeDataCollator _rangeDataCollator;
+
+    public LocalTrajectoryBuilder3D(
+        LocalTrajectoryBuilderOptions3D options,
+        List<string> expectedRangeSensorIds)
+    {
+        _options = options;
+        _activeSubmaps = new ActiveSubmaps3D(options.SubmapsOptions);
+        _motionFilter = new MotionFilter(options.MotionFilterOptions);
+        
+        // Initialize scan matchers from options
+        // Note: RealTimeCorrelativeScanMatcher3D is created per-submap in ScanMatch() 
+        // because it needs HybridGrid which is submap-specific and not available at construction time
+        
+        if (options.CeresScanMatcherOptions.HasValue)
+        {
+            _ceresScanMatcher = new CeresScanMatcher3D(options.CeresScanMatcherOptions.Value);
+        }
+        
+        _rangeDataCollator = new RangeDataCollator(expectedRangeSensorIds);
+    }
+
+    /// <summary>
+    /// Adds IMU data to the pose extrapolator.
+    /// Match C++ (local_trajectory_builder_3d.cc line 111-127)
+    /// </summary>
+    public void AddImuData(ImuData imuData)
+    {
+        if (_extrapolator != null)
+        {
+            _extrapolator.AddImuData(imuData);
+            return;
+        }
+
+        // Initialize extrapolator with IMU data and initial poses/data from options
+        var poseQueueDuration = TimeSpan.FromSeconds(_options.PoseExtrapolatorOptions.ConstantVelocity.PoseQueueDuration);
+
+        // Convert initial poses from proto
+        var initialPoses = new List<(long time, Rigid3d transform)>();
+        if (_options.InitialPoses != null)
+        {
+            foreach (var poseProto in _options.InitialPoses)
+            {
+                var transform = (Rigid3d)poseProto.Transform;
+                initialPoses.Add((poseProto.Time, transform));
+            }
+        }
+
+        // Convert initial IMU data from proto
+        var initialImuData = new List<ImuData>();
+        if (_options.InitialImuData != null)
+        {
+            foreach (var imuProto in _options.InitialImuData)
+            {
+                initialImuData.Add(ImuDataOperations.FromProto(imuProto));
+            }
+        }
+        // Add current IMU data to the list
+        initialImuData.Add(imuData);
+
+        // CRITICAL FIX: Match C++ CreateWithImuData behavior
+        // C++ passes ALL initial_imu_data and initial_poses to the extrapolator
+        // Initialize with first IMU data
+        _extrapolator = PoseExtrapolator.InitializeWithImu(
+            poseQueueDuration.Ticks,
+            _options.PoseExtrapolatorOptions.ConstantVelocity.ImuGravityTimeConstant,
+            initialImuData[0]  // Initialize with first IMU data
+        );
+
+        // Add remaining IMU data (skip the first one which was used for initialization)
+        for (int i = 1; i < initialImuData.Count; i++)
+        {
+            _extrapolator.AddImuData(initialImuData[i]);
+        }
+
+        // Add initial poses if available (match C++ line 126: initial_poses parameter)
+        foreach (var (time, transform) in initialPoses)
+        {
+            _extrapolator.AddPose(time, transform);
+        }
+    }
+
+    /// <summary>
+    /// Adds odometry data to the pose extrapolator.
+    /// </summary>
+    public void AddOdometryData(OdometryData odometryData)
+    {
+        _extrapolator?.AddOdometryData(odometryData);
+    }
+
+    /// <summary>
+    /// Tries to get the current pose from the extrapolator at the given time.
+    /// Returns null when extrapolator is not initialized or time is before the last pose time.
+    /// Used by ITrajectoryBuilder.TryGetExtrapolatedPose so callers (e.g. CartographerService) can read a live pose.
+    /// </summary>
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        if (_extrapolator == null)
+            return null;
+        if (time < _extrapolator.GetLastPoseTime())
+            return null;
+        try
+        {
+            return _extrapolator.ExtrapolatePose(time);
+        }
+        catch (ArgumentException)
+        {
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Tries to get the current pose with low-pass filter to reduce jitter during direction changes.
+    /// Match C++: ExtrapolatePose_filter - should be used for publishing pose to external systems.
+    /// </summary>
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        if (_extrapolator == null)
+            return null;
+        if (time < _extrapolator.GetLastPoseTime())
+            return null;
+        try
+        {
+            return _extrapolator.ExtrapolatePoseFilter(time);
+        }
+        catch (ArgumentException)
+        {
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Returns 'MatchingResult' when range data accumulation completed,
+    /// otherwise 'null'.
+    /// </summary>
+    public ITrajectoryBuilder.MatchingResult? AddRangeData(string sensorId, TimedPointCloudData rangeData)
+    {
+        // Check intensities consistency if enabled
+        if (_options.UseIntensities && rangeData.Intensities != null)
+        {
+            if (rangeData.Intensities.Count != rangeData.Ranges.Count)
+            {
+                throw new ArgumentException(
+                    "Passed point cloud has inconsistent number of intensities and ranges.");
+            }
+        }
+
+        var synchronizedData = _rangeDataCollator.AddRangeData(sensorId, rangeData);
+        if (synchronizedData.Ranges.Count == 0)
+        {
+            return null;
+        }
+
+        var time = synchronizedData.Time;
+        _lastSensorTime = time;
+
+        if (_extrapolator == null)
+        {
+            // Until we've initialized the extrapolator with our first IMU message, we
+            // cannot compute the orientation of the rangefinder.
+            return null;
+        }
+
+        // Validate time of first point
+        if (synchronizedData.Ranges.Count > 0)
+        {
+            // MEDIUM FIX: Use Math.Round for consistent precision with RangeDataCollator
+            var firstRangeTime = time + (long)Math.Round(synchronizedData.Ranges[0].PointTime.Time * TimeSpan.TicksPerSecond);
+            if (firstRangeTime < _extrapolator.GetLastPoseTime())
+            {
+                // Extrapolator is still initializing
+                return null;
+            }
+        }
+
+        // Apply voxel filter before accumulation (0.5 * voxel_filter_size)
+        var filteredRanges = Sensor.VoxelFilter.Filter(
+            synchronizedData.Ranges,
+            0.5 * _options.VoxelFilterSize);
+
+        // Create filtered synchronized data
+        var filteredSynchronizedData = new TimedPointCloudOriginData(
+            synchronizedData.Time,
+            synchronizedData.Origins,
+            filteredRanges);
+
+        // Range data accumulation: accumulate multiple range data if configured
+        if (_numAccumulated == 0)
+        {
+            _accumulatedPointCloudOriginData.Clear();
+        }
+
+        _accumulatedPointCloudOriginData.Add(filteredSynchronizedData);
+        _numAccumulated++;
+
+        if (_numAccumulated < _options.NumAccumulatedRangeData)
+        {
+            return null; // Need more accumulation
+        }
+
+        _numAccumulated = 0;
+
+        // Process accumulated range data
+        return ProcessAccumulatedRangeData(time);
+    }
+
+    /// <summary>
+    /// Processes accumulated range data and performs scan matching.
+    /// </summary>
+    private ITrajectoryBuilder.MatchingResult? ProcessAccumulatedRangeData(long time)
+    {
+        if (_accumulatedPointCloudOriginData.Count == 0)
+        {
+            return null;
+        }
+
+        // Collect detailed hit times with validation (per point)
+        bool warned = false;
+        var hitTimes = new List<long>();
+        long prevTimePoint = _extrapolator!.GetLastExtrapolatedTime();
+        
+        foreach (var pointCloudOriginData in _accumulatedPointCloudOriginData)
+        {
+            foreach (var hit in pointCloudOriginData.Ranges)
+            {
+                // Calculate absolute time for this hit point
+                // MEDIUM FIX: Use Math.Round for consistent precision with RangeDataCollator
+                var timePoint = pointCloudOriginData.Time +
+                    (long)Math.Round(hit.PointTime.Time * TimeSpan.TicksPerSecond);
+                
+                // Validate time doesn't jump backwards
+                if (timePoint < prevTimePoint)
+                {
+                    if (!warned)
+                    {
+                        // Log warning (could use proper logger here)
+                        System.Diagnostics.Debug.WriteLine(
+                            $"Warning: Timestamp of individual range data point jumps backwards " +
+                            $"from {prevTimePoint} to {timePoint}");
+                        warned = true;
+                    }
+                    timePoint = prevTimePoint;
+                }
+                
+                hitTimes.Add(timePoint);
+                prevTimePoint = timePoint;
+            }
+        }
+        // Add last sensor time
+        if (_accumulatedPointCloudOriginData.Count > 0)
+        {
+            hitTimes.Add(_accumulatedPointCloudOriginData[^1].Time);
+        }
+
+        // Extrapolate poses for all hit times
+        var extrapolationResult = _extrapolator!.ExtrapolatePosesWithGravity(hitTimes);
+        
+        // Build list of poses (one per hit time)
+        var hitPoses = new List<Rigid3f>();
+        foreach (var pose in extrapolationResult.PreviousPoses)
+        {
+            hitPoses.Add(pose);
+        }
+        hitPoses.Add(new Rigid3f(
+            (Vector3)extrapolationResult.CurrentPose.Translation, 
+            extrapolationResult.CurrentPose.Rotation));
+        
+        // Transform accumulated points using poses at their respective times
+        var accumulatedPoints = new List<RangefinderPoint>();
+        var accumulatedIntensities = _options.UseIntensities ? new List<double>() : null;
+        var misses = new PointCloud();
+        
+        int hitPoseIndex = 0;
+        bool warnedPosesExhausted = false;
+        bool warnedOriginsEmpty = false;
+        foreach (var pointCloudOriginData in _accumulatedPointCloudOriginData)
+        {
+            foreach (var hit in pointCloudOriginData.Ranges)
+            {
+                // MEDIUM FIX: Add warning log when hitPoses is exhausted
+                if (hitPoseIndex >= hitPoses.Count)
+                {
+                    if (!warnedPosesExhausted)
+                    {
+                        System.Diagnostics.Debug.WriteLine(
+                            $"Warning: hitPoses exhausted at index {hitPoseIndex}, expected {hitPoses.Count} poses. " +
+                            "This may indicate a mismatch between hit count and pose count.");
+                        warnedPosesExhausted = true;
+                    }
+                    break;
+                }
+
+                var poseAtTime = hitPoses[hitPoseIndex];
+                hitPoseIndex++;
+
+                // Transform hit point using pose at its time
+                var hitInLocal = poseAtTime.TransformPoint(hit.PointTime.Position);
+
+                // Get origin for this range
+                // MEDIUM FIX: Add warning log when origins collection is empty/insufficient
+                var originIndex = hit.OriginIndex;
+                Vector3 originInLocal;
+                if (originIndex < pointCloudOriginData.Origins.Count)
+                {
+                    originInLocal = poseAtTime.TransformPoint(pointCloudOriginData.Origins[originIndex]);
+                }
+                else
+                {
+                    if (!warnedOriginsEmpty)
+                    {
+                        System.Diagnostics.Debug.WriteLine(
+                            $"Warning: Origin index {originIndex} out of bounds (Origins.Count={pointCloudOriginData.Origins.Count}). " +
+                            "Using poseAtTime.Translation as fallback origin.");
+                        warnedOriginsEmpty = true;
+                    }
+                    originInLocal = poseAtTime.Translation;
+                }
+                
+                var delta = hitInLocal - originInLocal;
+                var rangeLength = delta.Length();
+                
+                if (rangeLength >= _options.MinRange)
+                {
+                    if (rangeLength <= _options.MaxRange)
+                    {
+                        accumulatedPoints.Add(new RangefinderPoint { Position = hitInLocal });
+                        if (_options.UseIntensities && accumulatedIntensities != null)
+                        {
+                            accumulatedIntensities.Add(hit.Intensity);
+                        }
+                    }
+                    else
+                    {
+                        // Miss beyond max range - insert ray cropped to max_range
+                        var missPoint = new RangefinderPoint
+                        {
+                            Position = originInLocal + delta / rangeLength * _options.MaxRange
+                        };
+                        misses.Add(missPoint);
+                    }
+                }
+            }
+        }
+        
+        // Create PointCloud with intensities if enabled
+        var accumulatedPointCloud = new PointCloud(
+            accumulatedPoints,
+            accumulatedIntensities ?? []);
+        
+        var origin = extrapolationResult.CurrentPose.Translation;
+
+        // Reset accumulation
+        _accumulatedPointCloudOriginData.Clear();
+
+        // Apply voxel filter to accumulated points and misses
+        var filteredReturns = Sensor.VoxelFilter.Filter(
+            accumulatedPointCloud, 
+            _options.VoxelFilterSize);
+        var filteredMisses = Sensor.VoxelFilter.Filter(
+            misses, 
+            _options.VoxelFilterSize);
+
+        // Create RangeData from accumulated and transformed points
+        // C++ line 260-263: filtered_range_data has origin in tracking frame (current_pose.translation())
+        // and points in local frame (hit_in_local from line 222-223)
+        // C++ line 276-278: Transform to local frame using current_pose.inverse()
+        var filteredRangeData = new RangeData(
+            (Vector3)extrapolationResult.CurrentPose.Translation, // origin in tracking frame
+            filteredReturns, // points in local frame
+            filteredMisses); // misses in local frame
+        
+        // Transform to local frame (C++ line 276-278: current_pose.inverse())
+        var filteredRangeDataInLocal = RangeDataOperations.Transform(
+            filteredRangeData,
+            new Rigid3f(
+                (Vector3)extrapolationResult.CurrentPose.Translation,
+                extrapolationResult.CurrentPose.Rotation).Inverse());
+
+        // Filter range data by max range (use maxRange from options)
+        var filteredRangeDataInTracking = Submap3D.FilterRangeDataByMaxRange(
+            filteredRangeDataInLocal,
+            _options.MaxRange
+        );
+
+        // Apply adaptive voxel filter using options
+        PointCloud highResolutionPointCloud;
+        if (_options.HighResolutionAdaptiveVoxelFilterOptions.HasValue)
+        {
+            highResolutionPointCloud = Sensor.AdaptiveVoxelFilter.Filter(
+                filteredRangeDataInTracking.Returns,
+                _options.HighResolutionAdaptiveVoxelFilterOptions.Value);
+        }
+        else
+        {
+            // Fallback to regular voxel filter
+            highResolutionPointCloud = Sensor.VoxelFilter.Filter(
+                filteredRangeDataInTracking.Returns,
+                _options.VoxelFilterSize);
+        }
+        
+        if (highResolutionPointCloud.Count == 0)
+        {
+            return null; // Empty point cloud
+        }
+        
+        PointCloud lowResolutionPointCloud;
+        if (_options.LowResolutionAdaptiveVoxelFilterOptions.HasValue)
+        {
+            lowResolutionPointCloud = Sensor.AdaptiveVoxelFilter.Filter(
+                filteredRangeDataInTracking.Returns,
+                _options.LowResolutionAdaptiveVoxelFilterOptions.Value);
+        }
+        else
+        {
+            // Fallback to regular voxel filter (typically 3x the high resolution size)
+            var lowResolutionVoxelFilterSize = _options.VoxelFilterSize * 3.0;
+            lowResolutionPointCloud = Sensor.VoxelFilter.Filter(
+                filteredRangeDataInTracking.Returns,
+                lowResolutionVoxelFilterSize);
+        }
+        
+        if (lowResolutionPointCloud.Count == 0)
+        {
+            return null; // Empty point cloud
+        }
+
+        // Get current pose and gravity alignment from extrapolation
+        var currentPose = extrapolationResult.CurrentPose;
+        var gravityAlignment = extrapolationResult.GravityFromTracking;
+
+        // Scan match
+        var poseEstimate = ScanMatch(
+            currentPose,
+            lowResolutionPointCloud,
+            highResolutionPointCloud
+        );
+
+        if (poseEstimate == null)
+        {
+            return null;
+        }
+
+        // Update extrapolator (called before InsertIntoSubmap to match C++ order)
+        _extrapolator!.AddPose(time, poseEstimate.Value);
+
+        // Transform range data to local frame
+        // C++ line 332-333: TransformRangeData(filtered_range_data_in_tracking, pose_estimate->cast<double>())
+        // pose_estimate is in tracking frame, so this transforms from tracking to local
+        var rangeDataInLocal = RangeDataOperations.Transform(
+            filteredRangeDataInTracking,
+            new Rigid3f((Vector3)poseEstimate.Value.Translation, poseEstimate.Value.Rotation)
+        );
+
+        // Insert into submap (motion filter is checked inside InsertIntoSubmap)
+        var localInsertionResult = InsertIntoSubmap(
+            time,
+            rangeDataInLocal,
+            filteredRangeDataInTracking,
+            highResolutionPointCloud,
+            lowResolutionPointCloud,
+            poseEstimate.Value,
+            gravityAlignment
+        );
+
+        // Convert LocalTrajectoryBuilder3D.InsertionResult to ITrajectoryBuilder.InsertionResult
+        ITrajectoryBuilder.InsertionResult? insertionResult = null;
+        if (localInsertionResult.HasValue)
+        {
+            var localInsertion = localInsertionResult.Value;
+            insertionResult = new ITrajectoryBuilder.InsertionResult(
+                nodeId: default, // NodeId will be assigned by PoseGraph
+                constantData: localInsertion.ConstantData,
+                insertionSubmaps: localInsertion.InsertionSubmaps.Cast<Submap>().ToList()
+            );
+        }
+
+        return new ITrajectoryBuilder.MatchingResult(
+            trajectoryId: 0,
+            time: time,
+            localPose: poseEstimate.Value,
+            rangeDataInLocal: rangeDataInLocal,
+            insertionResult: insertionResult,
+            poseConfidence: -1.0,
+            ceresScore: -1.0,
+            samplePointCloudGlobal: null // 3D builder doesn't generate sample point cloud yet
+        );
+    }
+
+    /// <summary>
+    /// Scan matches using the two point clouds and returns the observed pose, or
+    /// null on failure.
+    /// </summary>
+    private Rigid3d? ScanMatch(
+        Rigid3d posePrediction,
+        PointCloud lowResolutionPointCloudInTracking,
+        PointCloud highResolutionPointCloudInTracking)
+    {
+        var submaps = _activeSubmaps.Submaps();
+        if (submaps.Count == 0)
+        {
+            return posePrediction;
+        }
+
+        var matchingSubmap = submaps[0];
+        var initialCeresPose = matchingSubmap.LocalPose.Inverse() * posePrediction;
+
+        // Step 1: Real-time correlative scan matching (if enabled)
+        if (_options.UseOnlineCorrelativeScanMatching && 
+            _options.RealTimeCorrelativeScanMatcherOptions.HasValue)
+        {
+            // Convert RealTimeCorrelativeScanMatcherOptions to FastCorrelativeScanMatcherOptions3D
+            var rtOptions = _options.RealTimeCorrelativeScanMatcherOptions.Value;
+            var fastOptions = new FastCorrelativeScanMatcherOptions3D(
+                branchAndBoundDepth: 7, // Default depth
+                fullResolutionDepth: 0, // Default
+                minRotationalScore: 0.75f, // Default
+                minLowResolutionScore: 0.7, // Default
+                linearXySearchWindow: rtOptions.LinearSearchWindow,
+                linearZSearchWindow: rtOptions.LinearSearchWindow, // Use same as XY
+                angularSearchWindow: rtOptions.AngularSearchWindow
+            );
+            
+            // Create scan matcher per-submap (needs HybridGrid which is submap-specific)
+            var realTimeMatcher = new RealTimeCorrelativeScanMatcher3D(
+                matchingSubmap.HighResolutionHybridGrid,
+                matchingSubmap.LowResolutionHybridGrid,
+                null, // Rotational histogram not available here
+                fastOptions);
+            
+            // Create constant data for matching (simplified - only point clouds needed)
+            var constantData = new TrajectoryNode.Data
+            {
+                HighResolutionPointCloud = highResolutionPointCloudInTracking,
+                LowResolutionPointCloud = lowResolutionPointCloudInTracking,
+                RotationalScanMatcherHistogram = null,
+                GravityAlignment = Quaternion.Identity // Not critical for initial matching
+            };
+            
+            // Match with real-time correlative scan matcher
+            var matchingResult = realTimeMatcher.Match(
+                posePrediction,
+                matchingSubmap.LocalPose,
+                constantData,
+                minScore: 0.1);
+            
+            if (matchingResult.HasValue)
+            {
+                // Use matched pose as initial pose for Ceres
+                initialCeresPose = matchingSubmap.LocalPose.Inverse() * matchingResult.Value.PoseEstimate;
+            }
+        }
+
+        // Step 2: Ceres scan matching
+        if (_ceresScanMatcher == null)
+        {
+            return initialCeresPose;
+        }
+
+        var pointCloudsAndGrids = new List<PointCloudAndHybridGridsPointers>
+        {
+            new() {
+                PointCloud = highResolutionPointCloudInTracking,
+                HybridGrid = matchingSubmap.HighResolutionHybridGrid,
+                IntensityHybridGrid = _options.UseIntensities 
+                    ? matchingSubmap.HighResolutionIntensityHybridGrid 
+                    : null
+            },
+            new() {
+                PointCloud = lowResolutionPointCloudInTracking,
+                HybridGrid = matchingSubmap.LowResolutionHybridGrid,
+                IntensityHybridGrid = null
+            }
+        };
+
+        var targetTranslation = (matchingSubmap.LocalPose.Inverse() * posePrediction).Translation;
+
+        // FIX: SolverSummary holds unmanaged resources - must be disposed to prevent memory leak
+        CeresSharp.SolverSummary? summary = null;
+        try
+        {
+            _ceresScanMatcher.Match(
+                targetTranslation,
+                initialCeresPose,
+                pointCloudsAndGrids,
+                out var poseObservationInSubmap,
+                out summary
+            );
+
+            return matchingSubmap.LocalPose * poseObservationInSubmap;
+        }
+        finally
+        {
+            summary?.Dispose();
+        }
+    }
+
+    /// <summary>
+    /// Inserts range data into submaps.
+    /// </summary>
+    private InsertionResult? InsertIntoSubmap(
+        long time,
+        RangeData filteredRangeDataInLocal,
+        RangeData filteredRangeDataInTracking,
+        PointCloud highResolutionPointCloudInTracking,
+        PointCloud lowResolutionPointCloudInTracking,
+        Rigid3d poseEstimate,
+        Quaternion gravityAlignment)
+    {
+        // Check motion filter - skip insertion if motion is too small
+        if (_motionFilter.IsSimilar(time, poseEstimate))
+        {
+            return null;
+        }
+        
+        // Insert data into active submaps
+        // Compute localFromGravityAligned transform
+        var localFromGravityAligned = poseEstimate.Rotation * Quaternion.Inverse(gravityAlignment);
+        
+        // Compute rotational scan matcher histogram from gravity-aligned point cloud
+        var gravityAlignedPointCloud = PointCloudOperations.Transform(
+            filteredRangeDataInTracking.Returns,
+            new Rigid3f(Vector3.Zero, gravityAlignment));
+        
+        var rotationalScanMatcherHistogram = RotationalScanMatcher.ComputeHistogram(
+            gravityAlignedPointCloud,
+            _options.RotationalHistogramSize).ToList();
+        
+        _activeSubmaps.InsertData(
+            filteredRangeDataInLocal,
+            localFromGravityAligned,
+            rotationalScanMatcherHistogram
+        );
+
+        var submaps = _activeSubmaps.Submaps();
+        if (submaps.Count == 0)
+        {
+            return null;
+        }
+
+        // Create constant data with rotational histogram
+        var constantData = new TrajectoryNode.Data
+        {
+            Time = time,
+            GravityAlignment = gravityAlignment,
+            HighResolutionPointCloud = highResolutionPointCloudInTracking,
+            LowResolutionPointCloud = lowResolutionPointCloudInTracking,
+            RotationalScanMatcherHistogram = rotationalScanMatcherHistogram.ToArray(),
+            LocalPose = poseEstimate
+        };
+
+        return new InsertionResult(constantData, submaps);
+    }
+
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            _ceresScanMatcher?.Dispose();
+            _disposed = true;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/Optimization/AccelerationCostFunction3D.cs

@@ -0,0 +1,146 @@
+/*
+ * 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.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
+
+/// <summary>
+/// Penalizes differences between IMU data and optimized accelerations.
+/// Based on acceleration_cost_function_3d.h
+/// </summary>
+public class AccelerationCostFunction3D
+{
+    private readonly double _scalingFactor;
+    private readonly Vector3 _deltaVelocityImuFrame;
+    private readonly double _firstDeltaTimeSeconds;
+    private readonly double _secondDeltaTimeSeconds;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for acceleration constraint.
+    /// </summary>
+    /// <param name="scalingFactor">Scaling factor for the cost.</param>
+    /// <param name="deltaVelocityImuFrame">Delta velocity from IMU integration in IMU frame.</param>
+    /// <param name="firstDeltaTimeSeconds">Time duration of first interval in seconds.</param>
+    /// <param name="secondDeltaTimeSeconds">Time duration of second interval in seconds.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Vector3 deltaVelocityImuFrame,
+        double firstDeltaTimeSeconds,
+        double secondDeltaTimeSeconds)
+    {
+        var costFunction = new AccelerationCostFunction3D(
+            scalingFactor, 
+            deltaVelocityImuFrame, 
+            firstDeltaTimeSeconds, 
+            secondDeltaTimeSeconds);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 3, // [dx, dy, dz] - velocity difference error
+            parameterBlockSizes: [4, 3, 3, 3, 1, 4] // [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]]
+        );
+    }
+
+    private AccelerationCostFunction3D(
+        double scalingFactor,
+        Vector3 deltaVelocityImuFrame,
+        double firstDeltaTimeSeconds,
+        double secondDeltaTimeSeconds)
+    {
+        _scalingFactor = scalingFactor;
+        _deltaVelocityImuFrame = deltaVelocityImuFrame;
+        _firstDeltaTimeSeconds = firstDeltaTimeSeconds;
+        _secondDeltaTimeSeconds = secondDeltaTimeSeconds;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dz] (velocity difference error).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 6)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 3 || parameters[2].Length < 3 ||
+            parameters[3].Length < 3 || parameters[4].Length < 1 || parameters[5].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var middleRotation = parameters[0];    // [w, x, y, z]
+        var startPosition = parameters[1];     // [x, y, z]
+        var middlePosition = parameters[2];    // [x, y, z]
+        var endPosition = parameters[3];       // [x, y, z]
+        var gravityConstant = parameters[4][0]; // [g]
+        var imuCalibration = parameters[5];    // [w, x, y, z]
+
+        // Convert to quaternions
+        var middleRot = new Quaternion(
+            middleRotation[1], middleRotation[2], middleRotation[3], middleRotation[0]);
+        var imuCal = new Quaternion(
+            imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
+
+        // Convert positions to Vector3
+        var startPos = new Vector3(startPosition[0], startPosition[1], startPosition[2]);
+        var middlePos = new Vector3(middlePosition[0], middlePosition[1], middlePosition[2]);
+        var endPos = new Vector3(endPosition[0], endPosition[1], endPosition[2]);
+
+        // Compute IMU delta velocity in map frame
+        // Formula from C++: 
+        // imu_delta_velocity = middle_rotation * imu_calibration * delta_velocity_imu_frame - gravity_term
+        // where gravity_term = gravity_constant * 0.5 * (first_delta_time + second_delta_time) * UnitZ
+        
+        // Transform delta_velocity_imu_frame from IMU frame to map frame
+        // In Eigen: quaternion * vector rotates the vector
+        // In System.Numerics: Vector3.Transform(vector, quaternion) rotates the vector
+        // C++: middle_rotation * imu_calibration * delta_velocity
+        //     = middle_rotation * (imu_calibration * delta_velocity)
+        // Apply IMU calibration first, then middle rotation
+        var imuDeltaVelocityCalibrated = Vector3.Transform(_deltaVelocityImuFrame, imuCal);
+        var imuDeltaVelocityInMapFrame = Vector3.Transform(imuDeltaVelocityCalibrated, middleRot);
+        
+        // Subtract gravity contribution
+        // Gravity acts in positive Z direction in map frame (upward)
+        var gravityTerm = gravityConstant * 0.5 * (_firstDeltaTimeSeconds + _secondDeltaTimeSeconds) * Vector3.UnitZ;
+        var imuDeltaVelocity = imuDeltaVelocityInMapFrame - gravityTerm;
+
+        // Compute velocities from positions
+        // start_velocity = (middle_position - start_position) / first_delta_time
+        var startVelocity = (middlePos - startPos) / _firstDeltaTimeSeconds;
+
+        // end_velocity = (end_position - middle_position) / second_delta_time
+        var endVelocity = (endPos - middlePos) / _secondDeltaTimeSeconds;
+
+        // delta_velocity = end_velocity - start_velocity
+        var deltaVelocity = endVelocity - startVelocity;
+
+        // Error = IMU delta velocity - computed delta velocity
+        var error = imuDeltaVelocity - deltaVelocity;
+
+        // Scale error
+        residuals[0] = _scalingFactor * error.X;
+        residuals[1] = _scalingFactor * error.Y;
+        residuals[2] = _scalingFactor * error.Z;
+
+        return true;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/Optimization/RotationCostFunction3D.cs

@@ -0,0 +1,109 @@
+/*
+ * 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.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
+
+/// <summary>
+/// Penalizes differences between IMU data and optimized orientations.
+/// Based on rotation_cost_function_3d.h
+/// </summary>
+public class RotationCostFunction3D
+{
+    private readonly double _scalingFactor;
+    private readonly Quaternion _deltaRotationImuFrame;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for rotation constraint.
+    /// </summary>
+    /// <param name="scalingFactor">Scaling factor for the cost.</param>
+    /// <param name="deltaRotationImuFrame">Delta rotation from IMU integration in IMU frame.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Quaternion deltaRotationImuFrame)
+    {
+        var costFunction = new RotationCostFunction3D(scalingFactor, deltaRotationImuFrame);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 3, // [dx, dy, dz] - rotation error as angle-axis vector
+            parameterBlockSizes: [4, 4, 4] // [start_rotation[4], end_rotation[4], imu_calibration[4]]
+        );
+    }
+
+    private RotationCostFunction3D(double scalingFactor, Quaternion deltaRotationImuFrame)
+    {
+        _scalingFactor = scalingFactor;
+        _deltaRotationImuFrame = deltaRotationImuFrame;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [start_rotation[4], end_rotation[4], imu_calibration[4]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dz] (angle-axis error).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 3)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 4 || parameters[2].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var startRotation = parameters[0]; // [w, x, y, z] from Ceres
+        var endRotation = parameters[1];   // [w, x, y, z] from Ceres
+        var imuCalibration = parameters[2]; // [w, x, y, z] from Ceres
+
+        // Convert to quaternions
+        // C++ line 42-48: Eigen::Quaternion<T>(w, x, y, z)
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        // So we need to convert [w, x, y, z] to (x, y, z, w)
+        var start = new Quaternion(
+            startRotation[1], startRotation[2], startRotation[3], startRotation[0]);
+        var end = new Quaternion(
+            endRotation[1], endRotation[2], endRotation[3], endRotation[0]);
+        var imuCal = new Quaternion(
+            imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
+
+        // Compute error: end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
+        // C++ line 49-51: error = end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
+        // C++ line 52-54: residual = scaling_factor * error.vector() (x, y, z components of quaternion, not angle-axis)
+        var endConj = Quaternion.Conjugate(end);
+        var imuCalConj = Quaternion.Conjugate(imuCal);
+        var error = Quaternion.Multiply(
+            Quaternion.Multiply(
+                Quaternion.Multiply(
+                    Quaternion.Multiply(endConj, start),
+                    imuCal),
+                _deltaRotationImuFrame),
+            imuCalConj);
+
+        // C++ uses error.x(), error.y(), error.z() which are the vector (imaginary) parts of the quaternion
+        // NOT angle-axis representation. For small rotations, these are approximately the same, but we should match C++ exactly.
+        // Scale error using vector part of quaternion (x, y, z components)
+        residuals[0] = _scalingFactor * error.X;
+        residuals[1] = _scalingFactor * error.Y;
+        residuals[2] = _scalingFactor * error.Z;
+
+        return true;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/Optimization/SpaCostFunction3D.cs

@@ -0,0 +1,190 @@
+/*
+ * 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.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
+
+/// <summary>
+/// Sparse Pose Adjustment (SPA) cost function for 3D pose graph optimization.
+/// Computes the error between observed relative pose and computed relative pose.
+/// </summary>
+public class SpaCostFunction3D
+{
+    private readonly IPoseGraph.Constraint.Pose _observedRelativePose;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for SPA 3D.
+    /// </summary>
+    /// <param name="observedRelativePose">The observed relative pose constraint.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        var costFunction = new SpaCostFunction3D(observedRelativePose);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz] (quaternion rotation error as 3D vector)
+            parameterBlockSizes: [4, 3, 4, 3] // [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]]
+        );
+    }
+
+    private SpaCostFunction3D(IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        _observedRelativePose = observedRelativePose;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dz, dqx, dqy, dqz].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 4)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 3 || 
+            parameters[2].Length < 4 || parameters[3].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 6)
+            return false;
+
+        var submapRotation = parameters[0];
+        var submapTranslation = parameters[1];
+        var nodeRotation = parameters[2];
+        var nodeTranslation = parameters[3];
+
+        // Compute unscaled error
+        var unscaledError = ComputeUnscaledError(
+            _observedRelativePose.ZbarIj,
+            submapRotation,
+            submapTranslation,
+            nodeRotation,
+            nodeTranslation
+        );
+
+        // Scale error with weights
+        var scaledError = ScaleError(
+            unscaledError,
+            _observedRelativePose.TranslationWeight,
+            _observedRelativePose.RotationWeight
+        );
+
+        residuals[0] = scaledError[0];
+        residuals[1] = scaledError[1];
+        residuals[2] = scaledError[2];
+        residuals[3] = scaledError[3];
+        residuals[4] = scaledError[4];
+        residuals[5] = scaledError[5];
+
+        return true;
+    }
+
+    /// <summary>
+    /// Computes unscaled error between observed and computed relative pose.
+    /// Based on cost_helpers_impl.h ComputeUnscaledError for 3D.
+    /// </summary>
+    private static double[] ComputeUnscaledError(
+        Rigid3d observedRelativePose,
+        double[] submapRotation,
+        double[] submapTranslation,
+        double[] nodeRotation,
+        double[] nodeTranslation)
+    {
+        // submapRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
+        // submapTranslation = [x, y, z]
+        // nodeRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
+        // nodeTranslation = [x, y, z]
+
+        // IMPORTANT: System.Numerics.Quaternion constructor is (x, y, z, w), NOT (w, x, y, z)!
+        // Eigen::Quaternion uses (w, x, y, z), so we must reorder when creating System.Numerics.Quaternion.
+
+        // Compute R_i_inverse (inverse of submap rotation)
+        // C++: Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1], -start_rotation[2], -start_rotation[3])
+        var submapQuatInv = new Quaternion(
+            -submapRotation[1],  // -x
+            -submapRotation[2],  // -y
+            -submapRotation[3],  // -z
+            submapRotation[0]    // w
+        );
+
+        // Compute delta = node_translation - submap_translation
+        var delta = new Vector3(
+            (nodeTranslation[0] - submapTranslation[0]),
+            (nodeTranslation[1] - submapTranslation[1]),
+            (nodeTranslation[2] - submapTranslation[2])
+        );
+
+        // h_translation = R_i_inverse * delta
+        var hTranslation = Vector3.Transform(delta, submapQuatInv);
+
+        // Compute h_rotation_inverse = node_rotation_inverse * submap_rotation
+        // C++: Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2], -end_rotation[3]) *
+        //      Eigen::Quaternion<T>(start_rotation[0], start_rotation[1], start_rotation[2], start_rotation[3])
+        var nodeQuatInv = new Quaternion(
+            -nodeRotation[1],  // -x
+            -nodeRotation[2],  // -y
+            -nodeRotation[3],  // -z
+            nodeRotation[0]    // w
+        );
+        var submapQuat = new Quaternion(
+            submapRotation[1],  // x
+            submapRotation[2],  // y
+            submapRotation[3],  // z
+            submapRotation[0]   // w
+        );
+        var hRotationInverse = nodeQuatInv * submapQuat;
+
+        // Compute angle-axis difference: RotationQuaternionToAngleAxisVector(h_rotation_inverse * observed_rotation)
+        var observedQuat = observedRelativePose.Rotation;
+        var angleAxisDifference = TransformOperations.RotationQuaternionToAngleAxisVector(
+            hRotationInverse * observedQuat
+        );
+
+        // Error = observed - computed
+        return
+        [
+            observedRelativePose.Translation.X - hTranslation.X,
+            observedRelativePose.Translation.Y - hTranslation.Y,
+            observedRelativePose.Translation.Z - hTranslation.Z,
+            angleAxisDifference.X,
+            angleAxisDifference.Y,
+            angleAxisDifference.Z
+        ];
+    }
+
+    /// <summary>
+    /// Scales error with translation and rotation weights.
+    /// </summary>
+    private static double[] ScaleError(
+        double[] unscaledError,
+        double translationWeight,
+        double rotationWeight)
+    {
+        return
+        [
+            translationWeight * unscaledError[0],
+            translationWeight * unscaledError[1],
+            translationWeight * unscaledError[2],
+            rotationWeight * unscaledError[3],
+            rotationWeight * unscaledError[4],
+            rotationWeight * unscaledError[5]
+        ];
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/CeresScanMatcher3D.cs

@@ -0,0 +1,218 @@
+/*
+ * 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.Mapping;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using CeresSharp;
+using CeresSharp.Enums;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Point cloud and hybrid grids pointers structure.
+/// </summary>
+public struct PointCloudAndHybridGridsPointers
+{
+    public PointCloud? PointCloud { get; set; }
+    public Mapping.D3D.HybridGrid? HybridGrid { get; set; }
+    public Mapping.D3D.IntensityHybridGrid? IntensityHybridGrid { get; set; } // optional
+}
+
+/// <summary>
+/// This scan matcher uses Ceres to align scans with an existing 3D map.
+/// </summary>
+public class CeresScanMatcher3D : IDisposable
+{
+    private readonly CeresScanMatcherOptions3D _options;
+    private readonly SolverOptions _solverOptions;
+    private bool _disposed;
+
+    public CeresScanMatcher3D(CeresScanMatcherOptions3D options)
+    {
+        _options = options;
+
+        // Initialize CeresSharp solver options
+        _solverOptions = new SolverOptions
+        {
+            // Set linear solver type to DENSE_QR for 3D scan matching
+            LinearSolverType = LinearSolverType.DenseQr,
+
+            // Configure from CeresSolverOptions if available, otherwise use defaults
+            MaxNumIterations = options.CeresSolverOptions?.MaxNumIterations ?? 20, // Default for scan matching
+            NumThreads = options.CeresSolverOptions?.NumThreads ?? 1, // Single thread for scan matching
+            UseNonmonotonicSteps = options.CeresSolverOptions?.UseNonmonotonicSteps ?? false
+        };
+    }
+
+    /// <summary>
+    /// Aligns 'point_clouds' within the 'hybrid_grids' given an
+    /// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
+    /// 'summary'.
+    /// </summary>
+    public void Match(
+        Vector3 targetTranslation,
+        Rigid3d initialPoseEstimate,
+        List<PointCloudAndHybridGridsPointers> pointCloudsAndHybridGrids,
+        out Rigid3d poseEstimate,
+        out SolverSummary summary)
+    {
+        if (pointCloudsAndHybridGrids == null || pointCloudsAndHybridGrids.Count == 0)
+        {
+            poseEstimate = initialPoseEstimate;
+            using var emptyProblem = new Problem();
+            using var emptyOptions = new SolverOptions();
+            summary = emptyProblem.Solve(emptyOptions);
+            return;
+        }
+
+        // Validate weights
+        if (_options.OccupiedSpaceWeight.Count != pointCloudsAndHybridGrids.Count)
+        {
+            throw new ArgumentException(
+                $"OccupiedSpaceWeight count ({_options.OccupiedSpaceWeight.Count}) must match pointCloudsAndHybridGrids count ({pointCloudsAndHybridGrids.Count})",
+                nameof(pointCloudsAndHybridGrids));
+        }
+
+        for (int i = 0; i < _options.OccupiedSpaceWeight.Count; i++)
+        {
+            if (_options.OccupiedSpaceWeight[i] <= 0.0)
+            {
+                throw new ArgumentException($"OccupiedSpaceWeight[{i}] must be positive", nameof(_options));
+            }
+        }
+
+        if (_options.TranslationWeight <= 0.0)
+            throw new ArgumentException("TranslationWeight must be positive", nameof(_options));
+        if (_options.RotationWeight <= 0.0)
+            throw new ArgumentException("RotationWeight must be positive", nameof(_options));
+
+        // Initialize pose parameters
+        // For 3D: [translation[3], rotation[4]]
+        var translationParams = new double[3]
+        {
+            initialPoseEstimate.Translation.X,
+            initialPoseEstimate.Translation.Y,
+            initialPoseEstimate.Translation.Z
+        };
+        
+        var rotationParams = new double[4]
+        {
+            initialPoseEstimate.Rotation.W,
+            initialPoseEstimate.Rotation.X,
+            initialPoseEstimate.Rotation.Y,
+            initialPoseEstimate.Rotation.Z
+        };
+
+        // Create Ceres problem
+        using var problem = new Problem();
+        
+        // Add parameter blocks
+        problem.AddParameterBlock(translationParams, 3);
+        problem.AddParameterBlock(rotationParams, 4);
+        
+        // Set quaternion manifold (Ceres 2.2.0 uses Manifold instead of Parameterization)
+        // TODO: When OnlyOptimizeYaw is true, use a YawOnlyQuaternionManifold instead
+        // (C++ uses YawOnlyQuaternionPlus local parameterization for this case)
+        using var quaternionManifold = new QuaternionManifold();
+        problem.SetManifold(rotationParams, quaternionManifold);
+
+        // Add occupied space cost functions for each point cloud/grid pair
+        for (int i = 0; i < pointCloudsAndHybridGrids.Count; i++)
+        {
+            var pcAndGrid = pointCloudsAndHybridGrids[i];
+            if (pcAndGrid.PointCloud == null || pcAndGrid.HybridGrid == null)
+                continue;
+            
+            if (pcAndGrid.PointCloud.Count == 0)
+                continue;
+
+            var occupiedSpaceCost = OccupiedSpaceCostFunction3D.CreateAutoDiffCostFunction(
+                _options.OccupiedSpaceWeight[i] / Math.Sqrt(pcAndGrid.PointCloud.Count),
+                pcAndGrid.PointCloud,
+                pcAndGrid.HybridGrid
+            );
+            problem.AddResidualBlock(occupiedSpaceCost, null, [translationParams, rotationParams]);
+
+            // Add intensity cost function if intensity grid is available
+            if (pcAndGrid.IntensityHybridGrid != null && 
+                _options.IntensityCostFunctionOptions != null && 
+                _options.IntensityCostFunctionOptions.Count > i)
+            {
+                var intensityOptions = _options.IntensityCostFunctionOptions[i];
+                var intensityCost = IntensityCostFunction3D.CreateAutoDiffCostFunction(
+                    intensityOptions.Weight / Math.Sqrt(pcAndGrid.PointCloud.Count),
+                    intensityOptions.IntensityThreshold,
+                    pcAndGrid.PointCloud,
+                    pcAndGrid.IntensityHybridGrid
+                );
+                // Do NOT use 'using' here - Problem takes ownership of the loss function
+                // via MarkOwnedByProblem() and will manage its lifetime
+                var huberLoss = new HuberLoss(intensityOptions.HuberScale);
+                problem.AddResidualBlock(intensityCost, huberLoss, [translationParams, rotationParams]);
+            }
+        }
+
+        // Add translation delta cost function
+        var translationCost = TranslationDeltaCostFunctor3D.CreateAutoDiffCostFunction(
+            _options.TranslationWeight,
+            targetTranslation
+        );
+        problem.AddResidualBlock(translationCost, null, [translationParams]);
+
+        // Add rotation delta cost function
+        var rotationCost = RotationDeltaCostFunctor3D.CreateAutoDiffCostFunction(
+            _options.RotationWeight,
+            initialPoseEstimate.Rotation
+        );
+        problem.AddResidualBlock(rotationCost, null, [rotationParams]);
+
+        // Solve
+        summary = problem.Solve(_solverOptions);
+
+        // Extract result
+        var newTranslation = new Vector3(
+            translationParams[0],
+            translationParams[1],
+            translationParams[2]
+        );
+        
+        // rotationParams = [w, x, y, z] from Ceres
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        var newRotation = new Quaternion(
+            rotationParams[1], // x
+            rotationParams[2], // y
+            rotationParams[3], // z
+            rotationParams[0]  // w
+        );
+        // Normalize to ensure unit quaternion after Ceres optimization
+        // C++ uses EigenQuaternionParameterization which maintains unit norm,
+        // but CeresSharp may not have the same guarantee
+        newRotation = Quaternion.Normalize(newRotation);
+
+        poseEstimate = new Rigid3d(newTranslation, newRotation);
+    }
+
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            _solverOptions?.Dispose();
+            _disposed = true;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/IntensityCostFunction3D.cs

@@ -0,0 +1,141 @@
+/*
+ * Copyright 2019 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.Mapping.D3D;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
+/// 'translation' and 'rotation'. The cost increases when points fall into space
+/// for which different intensity has been observed, i.e. at voxels with different
+/// values. Only points up to a certain threshold are evaluated which is intended
+/// to ignore data from retroreflections.
+/// </summary>
+/// <remarks>
+/// Creates an intensity cost function for 3D scan matching.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor (typically intensity_weight / sqrt(point_cloud.size())).</param>
+/// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
+/// <param name="pointCloud">Point cloud to match (must have intensities).</param>
+/// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
+public class IntensityCostFunction3D(
+    double scalingFactor,
+    double intensityThreshold,
+    PointCloud pointCloud,
+    IntensityHybridGrid hybridGrid) : IDisposable
+{
+    private readonly PointCloud _pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
+    private readonly InterpolatedIntensityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
+    private static readonly int[] parameterBlockSizes = [3, 4];
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for intensity matching.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        double intensityThreshold,
+        PointCloud pointCloud,
+        IntensityHybridGrid hybridGrid)
+    {
+        var costFunction = new IntensityCostFunction3D(scalingFactor, intensityThreshold, pointCloud, hybridGrid);
+        return new DynamicAutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: pointCloud.Count,
+            parameterBlockSizes: parameterBlockSizes);
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
+    /// <param name="residuals">Output residuals (one per point).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 2)
+            return false;
+        if (parameters[0].Length < 3 || parameters[1].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < _pointCloud.Count)
+            return false;
+
+        var translation = parameters[0];
+        var rotation = parameters[1]; // [w, x, y, z] from Ceres
+        
+        // Create transform from translation and rotation
+        // C++ line 48-50: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
+        // where rotation = [w, x, y, z]
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        var transform = new Rigid3d(
+            new Vector3(translation[0], translation[1], translation[2]),
+            new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
+        );
+
+        // Transform each point and compute residual
+        for (int i = 0; i < _pointCloud.Count; i++)
+        {
+            var point = _pointCloud[i];
+            
+            // Get intensity from point cloud if available, otherwise use 0
+            double intensity = 0.0;
+            if (_pointCloud.Intensities.Count > 0 && i < _pointCloud.Intensities.Count)
+            {
+                intensity = _pointCloud.Intensities[i];
+            }
+            
+            // Ignore points with intensity above threshold (retroreflections)
+            if (intensity > intensityThreshold)
+            {
+                residuals[i] = 0.0;
+                continue;
+            }
+            
+            // Transform point from local frame to world frame
+            var worldPoint = transform * point.Position;
+            
+            // Get interpolated intensity value at world point
+            var interpolatedIntensity = _interpolatedGrid.GetInterpolatedValue(
+                worldPoint.X,
+                worldPoint.Y,
+                worldPoint.Z
+            );
+            
+            // Residual = scaling_factor * (interpolated_intensity - intensity)
+            residuals[i] = scalingFactor * (interpolatedIntensity - intensity);
+        }
+
+        return true;
+    }
+
+    /// <summary>
+    /// Disposes managed resources.
+    /// </summary>
+    public void Dispose()
+    {
+        // InterpolatedIntensityGrid doesn't need disposal, but we implement IDisposable for consistency
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/InterpolatedGrid.cs

@@ -0,0 +1,238 @@
+/*
+ * 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.D3D;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Interpolates between HybridGrid voxels using tricubic interpolation.
+/// This class is designed to work with Ceres autodiff, so the interpolation
+/// scheme must be continuously differentiable.
+/// </summary>
+public class InterpolatedProbabilityGrid(HybridGrid _hybridGrid)
+{
+    /// <summary>
+    /// Returns the interpolated value at (x, y, z) of the HybridGrid.
+    /// Uses tricubic interpolation (piecewise cubic polynomials).
+    /// </summary>
+    public double GetInterpolatedValue(double x, double y, double z)
+    {
+        ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
+
+        var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
+        var q111 = GetValue(index1);
+        var q112 = GetValue(index1 + new Array3i(0, 0, 1));
+        var q121 = GetValue(index1 + new Array3i(0, 1, 0));
+        var q122 = GetValue(index1 + new Array3i(0, 1, 1));
+        var q211 = GetValue(index1 + new Array3i(1, 0, 0));
+        var q212 = GetValue(index1 + new Array3i(1, 0, 1));
+        var q221 = GetValue(index1 + new Array3i(1, 1, 0));
+        var q222 = GetValue(index1 + new Array3i(1, 1, 1));
+
+        var normalizedX = (x - x1) / (x2 - x1);
+        var normalizedY = (y - y1) / (y2 - y1);
+        var normalizedZ = (z - z1) / (z2 - z1);
+
+        // Compute powers: t^2 and t^3
+        var normalizedXx = normalizedX * normalizedX;
+        var normalizedXxx = normalizedX * normalizedXx;
+        var normalizedYy = normalizedY * normalizedY;
+        var normalizedYyy = normalizedY * normalizedYy;
+        var normalizedZz = normalizedZ * normalizedZ;
+        var normalizedZzz = normalizedZ * normalizedZz;
+
+        // Interpolate in z, then y, then x
+        // Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
+        var q11 = (q111 - q112) * normalizedZzz * 2.0 +
+                  (q112 - q111) * normalizedZz * 3.0 + q111;
+        var q12 = (q121 - q122) * normalizedZzz * 2.0 +
+                  (q122 - q121) * normalizedZz * 3.0 + q121;
+        var q21 = (q211 - q212) * normalizedZzz * 2.0 +
+                  (q212 - q211) * normalizedZz * 3.0 + q211;
+        var q22 = (q221 - q222) * normalizedZzz * 2.0 +
+                  (q222 - q221) * normalizedZz * 3.0 + q221;
+        var q1 = (q11 - q12) * normalizedYyy * 2.0 +
+                 (q12 - q11) * normalizedYy * 3.0 + q11;
+        var q2 = (q21 - q22) * normalizedYyy * 2.0 +
+                 (q22 - q21) * normalizedYy * 3.0 + q21;
+        return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
+    }
+
+    /// <summary>
+    /// Computes interpolation data points (corners of the voxel containing the point).
+    /// </summary>
+    private void ComputeInterpolationDataPoints(
+        double x, double y, double z,
+        out double x1, out double y1, out double z1,
+        out double x2, out double y2, out double z2)
+    {
+        var lower = CenterOfLowerVoxel(x, y, z);
+        x1 = lower.X;
+        y1 = lower.Y;
+        z1 = lower.Z;
+        x2 = lower.X + _hybridGrid.Resolution;
+        y2 = lower.Y + _hybridGrid.Resolution;
+        z2 = lower.Z + _hybridGrid.Resolution;
+    }
+
+    /// <summary>
+    /// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
+    /// For each dimension, the largest voxel index so that the corresponding center
+    /// is at most the given coordinate.
+    /// </summary>
+    private Vector3 CenterOfLowerVoxel(double x, double y, double z)
+    {
+        // Center of the cell containing (x, y, z)
+        var center = _hybridGrid.GetCenterOfCell(
+            _hybridGrid.GetCellIndex(new Vector3(x, y, z))
+        );
+        
+        // Move to the next lower voxel center
+        var resolution = _hybridGrid.Resolution;
+        if (center.X > x)
+        {
+            center.X -= resolution;
+        }
+        if (center.Y > y)
+        {
+            center.Y -= resolution;
+        }
+        if (center.Z > z)
+        {
+            center.Z -= resolution;
+        }
+        return center;
+    }
+
+    /// <summary>
+    /// Gets the probability value at the given cell index.
+    /// </summary>
+    private double GetValue(Array3i index)
+    {
+        // HybridGrid.GetProbability already returns probability in range [0, 1]
+        // It internally calls ProbabilityValues.ValueToProbability which does the conversion
+        // DO NOT divide by ushort.MaxValue - that was a bug!
+        return _hybridGrid.GetProbability(index);
+    }
+}
+
+/// <summary>
+/// Interpolates between IntensityHybridGrid voxels using tricubic interpolation.
+/// </summary>
+public class InterpolatedIntensityGrid(IntensityHybridGrid _hybridGrid)
+{
+    /// <summary>
+    /// Returns the interpolated value at (x, y, z) of the IntensityHybridGrid.
+    /// Uses tricubic interpolation (piecewise cubic polynomials).
+    /// </summary>
+    public double GetInterpolatedValue(double x, double y, double z)
+    {
+        ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
+
+        var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
+        var q111 = GetValue(index1);
+        var q112 = GetValue(index1 + new Array3i(0, 0, 1));
+        var q121 = GetValue(index1 + new Array3i(0, 1, 0));
+        var q122 = GetValue(index1 + new Array3i(0, 1, 1));
+        var q211 = GetValue(index1 + new Array3i(1, 0, 0));
+        var q212 = GetValue(index1 + new Array3i(1, 0, 1));
+        var q221 = GetValue(index1 + new Array3i(1, 1, 0));
+        var q222 = GetValue(index1 + new Array3i(1, 1, 1));
+
+        var normalizedX = (x - x1) / (x2 - x1);
+        var normalizedY = (y - y1) / (y2 - y1);
+        var normalizedZ = (z - z1) / (z2 - z1);
+
+        // Compute powers: t^2 and t^3
+        var normalizedXx = normalizedX * normalizedX;
+        var normalizedXxx = normalizedX * normalizedXx;
+        var normalizedYy = normalizedY * normalizedY;
+        var normalizedYyy = normalizedY * normalizedYy;
+        var normalizedZz = normalizedZ * normalizedZ;
+        var normalizedZzz = normalizedZ * normalizedZz;
+
+        // Interpolate in z, then y, then x
+        // Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
+        var q11 = (q111 - q112) * normalizedZzz * 2.0 +
+                  (q112 - q111) * normalizedZz * 3.0 + q111;
+        var q12 = (q121 - q122) * normalizedZzz * 2.0 +
+                  (q122 - q121) * normalizedZz * 3.0 + q121;
+        var q21 = (q211 - q212) * normalizedZzz * 2.0 +
+                  (q212 - q211) * normalizedZz * 3.0 + q211;
+        var q22 = (q221 - q222) * normalizedZzz * 2.0 +
+                  (q222 - q221) * normalizedZz * 3.0 + q221;
+        var q1 = (q11 - q12) * normalizedYyy * 2.0 +
+                 (q12 - q11) * normalizedYy * 3.0 + q11;
+        var q2 = (q21 - q22) * normalizedYyy * 2.0 +
+                 (q22 - q21) * normalizedYy * 3.0 + q21;
+        return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
+    }
+
+    /// <summary>
+    /// Computes interpolation data points (corners of the voxel containing the point).
+    /// </summary>
+    private void ComputeInterpolationDataPoints(
+        double x, double y, double z,
+        out double x1, out double y1, out double z1,
+        out double x2, out double y2, out double z2)
+    {
+        var lower = CenterOfLowerVoxel(x, y, z);
+        x1 = lower.X;
+        y1 = lower.Y;
+        z1 = lower.Z;
+        x2 = lower.X + _hybridGrid.Resolution;
+        y2 = lower.Y + _hybridGrid.Resolution;
+        z2 = lower.Z + _hybridGrid.Resolution;
+    }
+
+    /// <summary>
+    /// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
+    /// </summary>
+    private Vector3 CenterOfLowerVoxel(double x, double y, double z)
+    {
+        // Center of the cell containing (x, y, z)
+        var center = _hybridGrid.GetCenterOfCell(
+            _hybridGrid.GetCellIndex(new Vector3(x, y, z))
+        );
+        
+        // Move to the next lower voxel center
+        var resolution = _hybridGrid.Resolution;
+        if (center.X > x)
+        {
+            center.X -= resolution;
+        }
+        if (center.Y > y)
+        {
+            center.Y -= resolution;
+        }
+        if (center.Z > z)
+        {
+            center.Z -= resolution;
+        }
+        return center;
+    }
+
+    /// <summary>
+    /// Gets the intensity value at the given cell index.
+    /// </summary>
+    private double GetValue(Array3i index)
+    {
+        return _hybridGrid.GetIntensity(index);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/OccupiedSpaceCostFunction3D.cs

@@ -0,0 +1,121 @@
+/*
+ * 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.Mapping.D3D;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
+/// 'translation' and 'rotation'. The cost increases when points fall into less
+/// occupied space, i.e. at voxels with lower values.
+/// </summary>
+/// <remarks>
+/// Creates an occupied space cost function for 3D scan matching.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
+/// <param name="pointCloud">Point cloud to match.</param>
+/// <param name="hybridGrid">Hybrid grid to match against.</param>
+public class OccupiedSpaceCostFunction3D(
+    double scalingFactor,
+    PointCloud _pointCloud,
+    HybridGrid hybridGrid) : IDisposable
+{
+    private readonly InterpolatedProbabilityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for occupied space matching.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="hybridGrid">Hybrid grid to match against.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        PointCloud pointCloud,
+        HybridGrid hybridGrid)
+    {
+        var costFunction = new OccupiedSpaceCostFunction3D(scalingFactor, pointCloud, hybridGrid);
+        return new DynamicAutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: pointCloud.Count,
+            parameterBlockSizes: [3, 4] // [translation[3], rotation[4]]
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
+    /// <param name="residuals">Output residuals (one per point).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 2)
+            return false;
+        if (parameters[0].Length < 3 || parameters[1].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < _pointCloud.Count)
+            return false;
+
+        var translation = parameters[0];
+        var rotation = parameters[1]; // [w, x, y, z] from Ceres
+        
+        // Create transform from translation and rotation
+        // C++ line 52-53: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
+        // where rotation = [w, x, y, z]
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        var transform = new Rigid3d(
+            new Vector3(translation[0], translation[1], translation[2]),
+            new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
+        );
+
+        // Transform each point and compute residual
+        for (int i = 0; i < _pointCloud.Count; i++)
+        {
+            var point = _pointCloud[i];
+            
+            // Transform point from local frame to world frame
+            var worldPoint = transform * point.Position;
+            
+            // Get interpolated probability value at world point
+            var probability = _interpolatedGrid.GetInterpolatedValue(
+                worldPoint.X,
+                worldPoint.Y,
+                worldPoint.Z
+            );
+            
+            // Residual = scaling_factor * (1 - probability)
+            // Higher probability (occupied space) = lower residual = better match
+            residuals[i] = scalingFactor * (1.0 - probability);
+        }
+
+        return true;
+    }
+
+    /// <summary>
+    /// Disposes managed resources.
+    /// </summary>
+    public void Dispose()
+    {
+        // InterpolatedProbabilityGrid doesn't need disposal, but we implement IDisposable for consistency
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/PrecomputationGrid3D.cs

@@ -0,0 +1,152 @@
+/*
+ * 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.D3D;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Precomputation grid for 3D scan matching using 8-bit values instead of 16-bit.
+/// This is used for branch-and-bound algorithm in Fast Correlative Scan Matcher.
+/// </summary>
+/// <remarks>
+/// Creates a new PrecomputationGrid3D with the specified resolution.
+/// </remarks>
+public class PrecomputationGrid3D(double resolution) : HybridGridBase<byte>(resolution)
+{
+    /// <summary>
+    /// Minimum probability value.
+    /// </summary>
+    public const double kMinProbability = 0.1;
+
+    /// <summary>
+    /// Maximum probability value.
+    /// </summary>
+    public const double kMaxProbability = 0.9;
+
+    /// <summary>
+    /// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
+    /// </summary>
+    public static double ToProbability(double value)
+    {
+        return kMinProbability +
+               value * ((kMaxProbability - kMinProbability) / 255.0);
+    }
+
+    /// <summary>
+    /// Gets the value at the given cell index.
+    /// </summary>
+    public new byte GetValue(Array3i index)
+    {
+        return base.GetValue(index);
+    }
+
+    /// <summary>
+    /// Sets the value at the given cell index.
+    /// </summary>
+    public void SetValue(Array3i index, byte value)
+    {
+        ref var cell = ref GetMutableValue(index);
+        cell = value;
+    }
+}
+
+/// <summary>
+/// Converts a HybridGrid to a PrecomputationGrid3D representing the same data,
+/// but only using 8 bit instead of 2 x 16 bit.
+/// </summary>
+public static class PrecomputationGrid3DOperations
+{
+    /// <summary>
+    /// Converts a HybridGrid to a PrecomputationGrid3D.
+    /// </summary>
+    public static PrecomputationGrid3D ConvertToPrecomputationGrid(Mapping.D3D.HybridGrid hybridGrid)
+    {
+        var result = new PrecomputationGrid3D(hybridGrid.Resolution);
+        
+        // Iterate through all cells in the hybrid grid
+        foreach (var (index, value) in hybridGrid)
+        {
+            // Convert probability (ushort) to byte [0, 255]
+            var probability = ProbabilityValues.ValueToProbability(value);
+            var cellValue = (int)Math.Round(
+                (probability - PrecomputationGrid3D.kMinProbability) *
+                (255.0 / (PrecomputationGrid3D.kMaxProbability - PrecomputationGrid3D.kMinProbability))
+            );
+            cellValue = Math.Max(0, Math.Min(255, cellValue));
+            result.SetValue(index, (byte)cellValue);
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Returns a grid of the same resolution containing the maximum value of
+    /// original voxels in 'grid'. This maximum is over the 8 voxels that have
+    /// any combination of index components optionally increased by 'shift'.
+    /// </summary>
+    public static PrecomputationGrid3D PrecomputeGrid(
+        PrecomputationGrid3D grid,
+        bool halfResolution,
+        Array3i shift)
+    {
+        var result = new PrecomputationGrid3D(grid.Resolution);
+        
+        // Iterate through all cells in the input grid
+        foreach (var (index, value) in grid)
+        {
+            // Update 8 values in the resulting grid
+            for (int i = 0; i < 8; i++)
+            {
+                var octant = HybridGridBase<byte>.GetOctant(i);
+                // Element-wise multiplication: shift * octant
+                var shiftOctant = new Array3i(
+                    shift.X * octant.X,
+                    shift.Y * octant.Y,
+                    shift.Z * octant.Z
+                );
+                var cellIndex = index - shiftOctant;
+                
+                if (halfResolution)
+                {
+                    // Convert to half resolution index
+                    cellIndex = CellIndexAtHalfResolution(cellIndex);
+                }
+                
+                // Take maximum value
+                var currentValue = result.GetValue(cellIndex);
+                var newValue = (byte)Math.Max(value, currentValue);
+                result.SetValue(cellIndex, newValue);
+            }
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Computes the half resolution index corresponding to the full resolution
+    /// 'cell_index'. Uses bit shift to round towards negative infinity.
+    /// </summary>
+    private static Array3i CellIndexAtHalfResolution(Array3i cellIndex)
+    {
+        return new Array3i(
+            cellIndex.X >> 1, // Divide by 2, rounding towards negative infinity
+            cellIndex.Y >> 1,
+            cellIndex.Z >> 1
+        );
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/PrecomputationGridStack3D.cs

@@ -0,0 +1,90 @@
+/*
+ * 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;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Stack of precomputation grids for branch-and-bound algorithm.
+/// </summary>
+public class PrecomputationGridStack3D
+{
+    private readonly List<PrecomputationGrid3D> _precomputationGrids;
+
+    /// <summary>
+    /// Creates a precomputation grid stack from a hybrid grid.
+    /// </summary>
+    public PrecomputationGridStack3D(
+        Mapping.D3D.HybridGrid hybridGrid,
+        FastCorrelativeScanMatcherOptions3D options)
+    {
+        if (options.BranchAndBoundDepth < 1)
+            throw new ArgumentException("branch_and_bound_depth must be >= 1", nameof(options));
+        if (options.FullResolutionDepth < 1)
+            throw new ArgumentException("full_resolution_depth must be >= 1", nameof(options));
+
+        _precomputationGrids = new List<PrecomputationGrid3D>(options.BranchAndBoundDepth)
+        {
+            // First grid: convert from hybrid grid
+            PrecomputationGrid3DOperations.ConvertToPrecomputationGrid(hybridGrid)
+        };
+        
+        var lastWidth = new Array3i(1, 1, 1);
+        
+        // Create grids for each depth
+        for (int depth = 1; depth < options.BranchAndBoundDepth; depth++)
+        {
+            var halfResolution = depth >= options.FullResolutionDepth;
+            var nextWidth = new Array3i(1 << depth, 1 << depth, 1 << depth);
+            
+            var fullVoxelsPerHighResolutionVoxel = 1 << Math.Max(0, depth - options.FullResolutionDepth);
+            // Element-wise division: (nextWidth - lastWidth + (fullVoxelsPerHighResolutionVoxel - 1)) / fullVoxelsPerHighResolutionVoxel
+            var numerator = nextWidth - lastWidth + new Array3i(fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1);
+            var shift = new Array3i(
+                numerator.X / fullVoxelsPerHighResolutionVoxel,
+                numerator.Y / fullVoxelsPerHighResolutionVoxel,
+                numerator.Z / fullVoxelsPerHighResolutionVoxel
+            );
+            
+            _precomputationGrids.Add(
+                PrecomputationGrid3DOperations.PrecomputeGrid(
+                    _precomputationGrids[^1],
+                    halfResolution,
+                    shift
+                )
+            );
+            
+            lastWidth = nextWidth;
+        }
+    }
+
+    /// <summary>
+    /// Gets the precomputation grid at the specified depth.
+    /// </summary>
+    public PrecomputationGrid3D Get(int depth)
+    {
+        if (depth < 0 || depth >= _precomputationGrids.Count)
+            throw new ArgumentOutOfRangeException(nameof(depth));
+        return _precomputationGrids[depth];
+    }
+
+    /// <summary>
+    /// Gets the maximum depth (0-based).
+    /// </summary>
+    public int MaxDepth => _precomputationGrids.Count - 1;
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/RealTimeCorrelativeScanMatcher3D.cs

@@ -0,0 +1,632 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+using InterpolatedProbabilityGrid = CartographerSharp.Mapping.Internal.D3D.ScanMatching.InterpolatedProbabilityGrid;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Result of fast correlative scan matching for 3D.
+/// </summary>
+public struct FastCorrelativeScanMatcher3DResult(double score, Rigid3d poseEstimate, double rotationalScore, double lowResolutionScore)
+{
+    public double Score { get; set; } = score;
+    public Rigid3d PoseEstimate { get; set; } = poseEstimate;
+    public double RotationalScore { get; set; } = rotationalScore;
+    public double LowResolutionScore { get; set; } = lowResolutionScore;
+}
+
+/// <summary>
+/// Discrete scan structure for 3D scan matching.
+/// </summary>
+internal struct DiscreteScan3D
+{
+    public Rigid3f Pose { get; set; }
+    public List<List<Array3i>> CellIndicesPerDepth { get; set; }
+    public double RotationalScore { get; set; }
+}
+
+/// <summary>
+/// Candidate structure for branch-and-bound algorithm.
+/// </summary>
+internal struct Candidate3D(int scanIndex, Array3i offset) : IComparable<Candidate3D>
+{
+    public int ScanIndex { get; set; } = scanIndex;
+    public Array3i Offset { get; set; } = offset;
+    public double Score { get; set; } = double.NegativeInfinity;
+    public double LowResolutionScore { get; set; } = 0.0;
+
+    public static Candidate3D Unsuccessful()
+    {
+        return new Candidate3D(0, Array3i.Zero);
+    }
+
+    public readonly int CompareTo(Candidate3D other)
+    {
+        return Score.CompareTo(other.Score);
+    }
+
+    public static bool operator <(Candidate3D left, Candidate3D right)
+    {
+        return left.Score < right.Score;
+    }
+
+    public static bool operator >(Candidate3D left, Candidate3D right)
+    {
+        return left.Score > right.Score;
+    }
+}
+
+/// <summary>
+/// WARNING: NAMING MISMATCH WITH C++
+///
+/// This class is actually an implementation of FastCorrelativeScanMatcher3D (branch-and-bound algorithm),
+/// NOT RealTimeCorrelativeScanMatcher3D (exhaustive search).
+///
+/// C++ differences:
+/// - real_time_correlative_scan_matcher_3d.cc: Uses exhaustive search with 6 nested loops over
+///   a SMALL search window (linear and angular). Simple O(n^6) brute force.
+/// - fast_correlative_scan_matcher_3d.cc: Uses branch-and-bound optimization with precomputation
+///   grids for efficient search over LARGE windows. This is what this class implements.
+///
+/// The class name was incorrectly chosen. For constraint building (loop closure), this branch-and-bound
+/// implementation is actually correct since it can search over large windows efficiently.
+/// For real-time scan matching in LocalTrajectoryBuilder3D, the exhaustive search version should be
+/// used (smaller window, simpler, more predictable performance).
+///
+/// TODO: Consider renaming to FastCorrelativeScanMatcher3D and implementing a proper
+/// RealTimeCorrelativeScanMatcher3D for local SLAM if needed.
+/// </summary>
+public class RealTimeCorrelativeScanMatcher3D(
+    Mapping.D3D.HybridGrid _hybridGrid,
+    Mapping.D3D.HybridGrid? lowResolutionHybridGrid,
+    double[]? rotationalScanMatcherHistogram,
+    FastCorrelativeScanMatcherOptions3D options)
+{
+    private readonly double _resolution = _hybridGrid.Resolution;
+    private readonly int _widthInVoxels = 256;
+    private readonly PrecomputationGridStack3D _precomputationGridStack = new(_hybridGrid, options);
+    private readonly RotationalScanMatcher _rotationalScanMatcher = new(rotationalScanMatcherHistogram);
+
+    /// <summary>
+    /// Search parameters for branch-and-bound algorithm.
+    /// </summary>
+    private struct SearchParameters
+    {
+        public int LinearXyWindowSize { get; set; }  // voxels
+        public int LinearZWindowSize { get; set; }   // voxels
+        public double AngularSearchWindow { get; set; }  // radians
+        public Func<Rigid3f, double>? LowResolutionMatcher { get; set; }
+    }
+
+    /// <summary>
+    /// Creates a low resolution matcher function.
+    /// </summary>
+    private static Func<Rigid3f, double>? CreateLowResolutionMatcher(
+        Mapping.D3D.HybridGrid? lowResolutionGrid,
+        PointCloud? points)
+    {
+        if (lowResolutionGrid == null || points == null || points.Count == 0)
+            return null;
+
+        return pose =>
+        {
+            double score = 0.0;
+            var transformedPoints = PointCloudOperations.Transform(points, pose);
+            var interpolatedGrid = new InterpolatedProbabilityGrid(lowResolutionGrid);
+            
+            foreach (var point in transformedPoints)
+            {
+                // Use interpolated grid for better score
+                var probability = interpolatedGrid.GetInterpolatedValue(
+                    point.Position.X,
+                    point.Position.Y,
+                    point.Position.Z);
+                score += probability;
+            }
+            return score / points.Count;
+        };
+    }
+
+    /// <summary>
+    /// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
+    /// given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
+    /// returned if a score above 'min_score' (excluding equality) is possible.
+    /// </summary>
+    public FastCorrelativeScanMatcher3DResult? Match(
+        Rigid3d globalNodePose,
+        Rigid3d globalSubmapPose,
+        TrajectoryNode.Data constantData,
+        double minScore)
+    {
+        ArgumentNullException.ThrowIfNull(constantData);
+
+        var pointCloud = constantData.HighResolutionPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0)
+        {
+            return null;
+        }
+
+        var lowResolutionMatcher = CreateLowResolutionMatcher(
+            lowResolutionHybridGrid,
+            constantData.LowResolutionPointCloud);
+        
+        var searchParameters = new SearchParameters
+        {
+            LinearXyWindowSize = (int)Math.Round(options.LinearXySearchWindow / _resolution),
+            LinearZWindowSize = (int)Math.Round(options.LinearZSearchWindow / _resolution),
+            AngularSearchWindow = options.AngularSearchWindow,
+            LowResolutionMatcher = lowResolutionMatcher
+        };
+
+        return MatchWithSearchParameters(
+            searchParameters,
+            new Rigid3f(globalNodePose.Translation, globalNodePose.Rotation),
+            new Rigid3f(globalSubmapPose.Translation, globalSubmapPose.Rotation),
+            pointCloud,
+            constantData.RotationalScanMatcherHistogram?.ToArray(),
+            constantData.GravityAlignment,
+            minScore);
+    }
+
+    /// <summary>
+    /// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
+    /// given rotations which are expected to be approximately gravity aligned.
+    /// 'Result' is only returned if a score above 'min_score' (excluding equality)
+    /// is possible.
+    /// </summary>
+    public FastCorrelativeScanMatcher3DResult? MatchFullSubmap(
+        Quaternion globalNodeRotation,
+        Quaternion globalSubmapRotation,
+        TrajectoryNode.Data constantData,
+        double minScore)
+    {
+        ArgumentNullException.ThrowIfNull(constantData);
+
+        var pointCloud = constantData.HighResolutionPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0)
+        {
+            return null;
+        }
+
+        // Compute max point distance to determine search window
+        double maxPointDistance = 0.0;
+        foreach (var point in pointCloud)
+        {
+            maxPointDistance = Math.Max(maxPointDistance, point.Position.Length());
+        }
+
+        var linearWindowSize = (_widthInVoxels + 1) / 2 +
+                              (int)Math.Round(maxPointDistance / _resolution + 0.5);
+
+        var lowResolutionMatcher = CreateLowResolutionMatcher(
+            lowResolutionHybridGrid,
+            constantData.LowResolutionPointCloud);
+
+        var searchParameters = new SearchParameters
+        {
+            LinearXyWindowSize = linearWindowSize,
+            LinearZWindowSize = linearWindowSize,
+            AngularSearchWindow = Math.PI,
+            LowResolutionMatcher = lowResolutionMatcher
+        };
+
+        var globalNodePose = Rigid3f.FromRotation(globalNodeRotation);
+        var globalSubmapPose = Rigid3f.FromRotation(globalSubmapRotation);
+
+        return MatchWithSearchParameters(
+            searchParameters,
+            globalNodePose,
+            globalSubmapPose,
+            pointCloud,
+            constantData.RotationalScanMatcherHistogram?.ToArray(),
+            constantData.GravityAlignment,
+            minScore);
+    }
+
+    /// <summary>
+    /// Matches with given search parameters.
+    /// </summary>
+    private FastCorrelativeScanMatcher3DResult? MatchWithSearchParameters(
+        SearchParameters searchParameters,
+        Rigid3f globalNodePose,
+        Rigid3f globalSubmapPose,
+        PointCloud pointCloud,
+        double[]? rotationalScanMatcherHistogram,
+        Quaternion gravityAlignment,
+        double minScore)
+    {
+        var discreteScans = GenerateDiscreteScans(
+            searchParameters,
+            pointCloud,
+            rotationalScanMatcherHistogram,
+            gravityAlignment,
+            globalNodePose,
+            globalSubmapPose);
+
+        var lowestResolutionCandidates = ComputeLowestResolutionCandidates(
+            searchParameters,
+            discreteScans);
+
+        var bestCandidate = BranchAndBound(
+            searchParameters,
+            discreteScans,
+            lowestResolutionCandidates,
+            _precomputationGridStack.MaxDepth,
+            minScore);
+
+        if (bestCandidate.Score > minScore)
+        {
+            var pose = GetPoseFromCandidate(discreteScans, bestCandidate);
+            return new FastCorrelativeScanMatcher3DResult(
+                bestCandidate.Score,
+                new Rigid3d(pose.Translation, pose.Rotation),
+                discreteScans[bestCandidate.ScanIndex].RotationalScore,
+                bestCandidate.LowResolutionScore);
+        }
+
+        return null;
+    }
+
+    /// <summary>
+    /// Discretizes a scan at different resolutions for branch-and-bound.
+    /// </summary>
+    private DiscreteScan3D DiscretizeScan(
+        SearchParameters searchParameters,
+        PointCloud pointCloud,
+        Rigid3f pose,
+        double rotationalScore)
+    {
+        var cellIndicesPerDepth = new List<List<Array3i>>();
+        var originalGrid = _precomputationGridStack.Get(0);
+        
+        // Transform point cloud
+        var transformedPoints = PointCloudOperations.Transform(pointCloud, pose);
+        
+        // Get full resolution cell indices
+        var fullResolutionCellIndices = new List<Array3i>();
+        foreach (var point in transformedPoints)
+        {
+            fullResolutionCellIndices.Add(originalGrid.GetCellIndex(point.Position));
+        }
+
+        var fullResolutionDepth = Math.Min(
+            options.FullResolutionDepth,
+            options.BranchAndBoundDepth);
+        
+        if (fullResolutionDepth < 1)
+            fullResolutionDepth = 1;
+
+        // Add full resolution indices for each depth up to full_resolution_depth
+        for (int i = 0; i < fullResolutionDepth; i++)
+        {
+            cellIndicesPerDepth.Add([.. fullResolutionCellIndices]);
+        }
+
+        var lowResolutionDepth = options.BranchAndBoundDepth - fullResolutionDepth;
+        if (lowResolutionDepth < 0)
+            lowResolutionDepth = 0;
+
+        var searchWindowStart = new Array3i(
+            -searchParameters.LinearXyWindowSize,
+            -searchParameters.LinearXyWindowSize,
+            -searchParameters.LinearZWindowSize);
+
+        // Add low resolution indices
+        for (int i = 0; i < lowResolutionDepth; i++)
+        {
+            var reductionExponent = i + 1;
+            var lowResolutionSearchWindowStart = new Array3i(
+                searchWindowStart.X >> reductionExponent,
+                searchWindowStart.Y >> reductionExponent,
+                searchWindowStart.Z >> reductionExponent);
+
+            var lowResolutionIndices = new List<Array3i>();
+            foreach (var cellIndex in fullResolutionCellIndices)
+            {
+                var cellAtStart = cellIndex + searchWindowStart;
+                var lowResolutionCellAtStart = new Array3i(
+                    cellAtStart.X >> reductionExponent,
+                    cellAtStart.Y >> reductionExponent,
+                    cellAtStart.Z >> reductionExponent);
+                lowResolutionIndices.Add(
+                    lowResolutionCellAtStart - lowResolutionSearchWindowStart);
+            }
+            cellIndicesPerDepth.Add(lowResolutionIndices);
+        }
+
+        return new DiscreteScan3D
+        {
+            Pose = pose,
+            CellIndicesPerDepth = cellIndicesPerDepth,
+            RotationalScore = rotationalScore
+        };
+    }
+
+    /// <summary>
+    /// Generates discrete scans for different rotation angles.
+    /// </summary>
+    private List<DiscreteScan3D> GenerateDiscreteScans(
+        SearchParameters searchParameters,
+        PointCloud pointCloud,
+        double[]? rotationalScanMatcherHistogram,
+        Quaternion gravityAlignment,
+        Rigid3f globalNodePose,
+        Rigid3f globalSubmapPose)
+    {
+        var result = new List<DiscreteScan3D>();
+
+        // Compute max scan range
+        double maxScanRange = 3.0 * _resolution;
+        foreach (var point in pointCloud)
+        {
+            var range = point.Position.Length();
+            maxScanRange = Math.Max(range, maxScanRange);
+        }
+
+        const double kSafetyMargin = 1.0 - 1e-2;
+        var angularStepSize = kSafetyMargin * Math.Acos(
+            1.0 - MathUtils.Pow2(_resolution) / (2.0 * MathUtils.Pow2(maxScanRange)));
+
+        var angularWindowSize = (int)Math.Round(
+            searchParameters.AngularSearchWindow / angularStepSize);
+
+        var angles = new List<double>();
+        for (int rz = -angularWindowSize; rz <= angularWindowSize; rz++)
+        {
+            angles.Add(rz * angularStepSize);
+        }
+
+        var nodeToSubmap = globalSubmapPose.Inverse() * globalNodePose;
+        var initialAngle = TransformOperations.GetYaw(
+            nodeToSubmap.Rotation * Quaternion.Inverse(gravityAlignment));
+
+        var scores = _rotationalScanMatcher.Match(
+            rotationalScanMatcherHistogram ?? [],
+            initialAngle,
+            angles);
+
+        for (int i = 0; i < angles.Count; i++)
+        {
+            if (scores[i] < options.MinRotationalScore)
+                continue;
+
+            var angleAxis = new Vector3(0.0f, 0.0f, angles[i]);
+            // Apply rotation between translation and rotation of initial_pose
+            var pose = new Rigid3f(
+                nodeToSubmap.Translation,
+                Quaternion.Inverse(globalSubmapPose.Rotation) *
+                    TransformOperations.AngleAxisVectorToRotationQuaternion(angleAxis) *
+                    globalNodePose.Rotation);
+
+            result.Add(DiscretizeScan(searchParameters, pointCloud, pose, scores[i]));
+        }
+
+        return result;
+    }
+
+    /// <summary>
+    /// Generates candidates at the lowest resolution.
+    /// </summary>
+    private List<Candidate3D> GenerateLowestResolutionCandidates(
+        SearchParameters searchParameters,
+        int numDiscreteScans)
+    {
+        var linearStepSize = 1 << _precomputationGridStack.MaxDepth;
+        var numLowestResolutionLinearXyCandidates =
+            (2 * searchParameters.LinearXyWindowSize + linearStepSize) / linearStepSize;
+        var numLowestResolutionLinearZCandidates =
+            (2 * searchParameters.LinearZWindowSize + linearStepSize) / linearStepSize;
+        var numCandidates = numDiscreteScans *
+                           MathUtils.Power(numLowestResolutionLinearXyCandidates, 2) *
+                           numLowestResolutionLinearZCandidates;
+
+        var candidates = new List<Candidate3D>((int)numCandidates);
+        for (int scanIndex = 0; scanIndex < numDiscreteScans; scanIndex++)
+        {
+            for (int z = -searchParameters.LinearZWindowSize;
+                 z <= searchParameters.LinearZWindowSize;
+                 z += linearStepSize)
+            {
+                for (int y = -searchParameters.LinearXyWindowSize;
+                     y <= searchParameters.LinearXyWindowSize;
+                     y += linearStepSize)
+                {
+                    for (int x = -searchParameters.LinearXyWindowSize;
+                         x <= searchParameters.LinearXyWindowSize;
+                         x += linearStepSize)
+                    {
+                        candidates.Add(new Candidate3D(scanIndex, new Array3i(x, y, z)));
+                    }
+                }
+            }
+        }
+
+        return candidates;
+    }
+
+    /// <summary>
+    /// Scores candidates at a given depth.
+    /// </summary>
+    private void ScoreCandidates(
+        int depth,
+        List<DiscreteScan3D> discreteScans,
+        List<Candidate3D> candidates)
+    {
+        var reductionExponent = Math.Max(0, depth - options.FullResolutionDepth + 1);
+        
+        for (int i = 0; i < candidates.Count; i++)
+        {
+            var candidate = candidates[i];
+            int sum = 0;
+            var discreteScan = discreteScans[candidate.ScanIndex];
+            var offset = new Array3i(
+                candidate.Offset.X >> reductionExponent,
+                candidate.Offset.Y >> reductionExponent,
+                candidate.Offset.Z >> reductionExponent);
+
+            if (depth >= discreteScan.CellIndicesPerDepth.Count)
+                continue;
+
+            var grid = _precomputationGridStack.Get(depth);
+            foreach (var cellIndex in discreteScan.CellIndicesPerDepth[depth])
+            {
+                var proposedCellIndex = cellIndex + offset;
+                sum += grid.GetValue(proposedCellIndex);
+            }
+
+            var newScore = PrecomputationGrid3D.ToProbability(
+                sum / discreteScan.CellIndicesPerDepth[depth].Count);
+            // Create new candidate with updated score
+            var updatedCandidate = new Candidate3D(candidate.ScanIndex, candidate.Offset)
+            {
+                Score = newScore,
+                LowResolutionScore = candidate.LowResolutionScore
+            };
+            candidates[i] = updatedCandidate;
+        }
+
+        // Sort candidates by score (descending)
+        candidates.Sort((a, b) => b.Score.CompareTo(a.Score));
+    }
+
+    /// <summary>
+    /// Computes candidates at the lowest resolution.
+    /// </summary>
+    private List<Candidate3D> ComputeLowestResolutionCandidates(
+        SearchParameters searchParameters,
+        List<DiscreteScan3D> discreteScans)
+    {
+        var lowestResolutionCandidates = GenerateLowestResolutionCandidates(
+            searchParameters,
+            discreteScans.Count);
+        
+        ScoreCandidates(
+            _precomputationGridStack.MaxDepth,
+            discreteScans,
+            lowestResolutionCandidates);
+        
+        return lowestResolutionCandidates;
+    }
+
+    /// <summary>
+    /// Gets pose from candidate.
+    /// </summary>
+    private Rigid3f GetPoseFromCandidate(
+        List<DiscreteScan3D> discreteScans,
+        Candidate3D candidate)
+    {
+        var translation = (_resolution) * candidate.Offset.ToVector3();
+        return Rigid3f.FromTranslation(translation) * discreteScans[candidate.ScanIndex].Pose;
+    }
+
+    /// <summary>
+    /// Branch-and-bound algorithm to find best candidate.
+    /// </summary>
+    private Candidate3D BranchAndBound(
+        SearchParameters searchParameters,
+        List<DiscreteScan3D> discreteScans,
+        List<Candidate3D> candidates,
+        int candidateDepth,
+        double minScore)
+    {
+        if (candidateDepth == 0)
+        {
+            foreach (var candidate in candidates)
+            {
+                if (candidate.Score <= minScore)
+                {
+                    // Return if candidate is bad because following candidates won't be better
+                    return Candidate3D.Unsuccessful();
+                }
+
+                if (searchParameters.LowResolutionMatcher == null)
+                    continue;
+
+                var lowResolutionScore = searchParameters.LowResolutionMatcher(
+                    GetPoseFromCandidate(discreteScans, candidate));
+                
+                if (lowResolutionScore >= options.MinLowResolutionScore)
+                {
+                    // Found best candidate that passes matching function
+                    var bestCandidate = candidate;
+                    bestCandidate.LowResolutionScore = lowResolutionScore;
+                    return bestCandidate;
+                }
+            }
+            // All candidates have good scores but none passes matching function
+            return Candidate3D.Unsuccessful();
+        }
+
+        var bestHighResolutionCandidate = Candidate3D.Unsuccessful();
+        bestHighResolutionCandidate.Score = minScore;
+
+        foreach (var candidate in candidates)
+        {
+            if (candidate.Score <= minScore)
+                break;
+
+            var higherResolutionCandidates = new List<Candidate3D>();
+            var halfWidth = 1 << (candidateDepth - 1);
+
+            for (int z = 0; z <= halfWidth; z += halfWidth)
+            {
+                if (candidate.Offset.Z + z > searchParameters.LinearZWindowSize)
+                    break;
+
+                for (int y = 0; y <= halfWidth; y += halfWidth)
+                {
+                    if (candidate.Offset.Y + y > searchParameters.LinearXyWindowSize)
+                        break;
+
+                    for (int x = 0; x <= halfWidth; x += halfWidth)
+                    {
+                        if (candidate.Offset.X + x > searchParameters.LinearXyWindowSize)
+                            break;
+
+                        higherResolutionCandidates.Add(new Candidate3D(
+                            candidate.ScanIndex,
+                            candidate.Offset + new Array3i(x, y, z)));
+                    }
+                }
+            }
+
+            ScoreCandidates(candidateDepth - 1, discreteScans, higherResolutionCandidates);
+            
+            // C++ line 433-437: std::max(best_high_resolution_candidate, BranchAndBound(...))
+            // This ensures we always get the candidate with the highest score (or equal)
+            var bestCandidate = BranchAndBound(
+                searchParameters,
+                discreteScans,
+                higherResolutionCandidates,
+                candidateDepth - 1,
+                bestHighResolutionCandidate.Score);
+
+            // Use >= to match std::max behavior (prefer new candidate if score is equal or greater)
+            if (bestCandidate.Score >= bestHighResolutionCandidate.Score)
+            {
+                bestHighResolutionCandidate = bestCandidate;
+            }
+        }
+
+        return bestHighResolutionCandidate;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/RotationDeltaCostFunctor3D.cs

@@ -0,0 +1,100 @@
+/*
+ * 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 CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
+/// Cost increases with the solution's distance from 'target_rotation'.
+/// </summary>
+/// <remarks>
+/// Creates a rotation delta cost functor for 3D.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor.</param>
+/// <param name="targetRotation">Target rotation to match.</param>
+public class RotationDeltaCostFunctor3D(double scalingFactor, Quaternion targetRotation)
+{
+    private readonly double[] _targetRotationInverse =
+        [
+            targetRotation.W,
+            -targetRotation.X,
+            -targetRotation.Y,
+            -targetRotation.Z
+        ]; // [w, x, y, z]
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for rotation delta.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="targetRotation">Target rotation to match.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Quaternion targetRotation)
+    {
+        var functor = new RotationDeltaCostFunctor3D(scalingFactor, targetRotation);
+        return new DynamicAutoDiffCostFunction(
+            functor.Evaluate,
+            numResiduals: 3, // [x, y, z] - imaginary part of delta quaternion
+            parameterBlockSizes: [4] // [w, x, y, z] - quaternion
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// Computes delta = target_rotation_inverse * rotation_quaternion
+    /// Returns the imaginary part (x, y, z) of the delta quaternion.
+    /// </summary>
+    /// <param name="parameters">Rotation quaternion [w, x, y, z].</param>
+    /// <param name="residuals">Output residuals [x, y, z] - imaginary part of delta.</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 1 || parameters[0].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var rotation = parameters[0];
+        
+        // Compute quaternion product: target_rotation_inverse * rotation
+        // delta = q_inv * q = [w1, x1, y1, z1] * [w2, x2, y2, z2]
+        // delta.w = w1*w2 - x1*x2 - y1*y2 - z1*z2
+        // delta.x = w1*x2 + x1*w2 + y1*z2 - z1*y2
+        // delta.y = w1*y2 - x1*z2 + y1*w2 + z1*x2
+        // delta.z = w1*z2 + x1*y2 - y1*x2 + z1*w2
+        var w1 = _targetRotationInverse[0];
+        var x1 = _targetRotationInverse[1];
+        var y1 = _targetRotationInverse[2];
+        var z1 = _targetRotationInverse[3];
+        
+        var w2 = rotation[0];
+        var x2 = rotation[1];
+        var y2 = rotation[2];
+        var z2 = rotation[3];
+        
+        // Compute delta quaternion (only need imaginary part for residual)
+        // The squared norm of the imaginary component is sin(phi/2)^2
+        residuals[0] = scalingFactor * (w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2);
+        residuals[1] = scalingFactor * (w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2);
+        residuals[2] = scalingFactor * (w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2);
+        
+        return true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/RotationalScanMatcher.cs

@@ -0,0 +1,312 @@
+/*
+ * 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.Sensor;
+using RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Rotational scan matcher for 3D scan matching.
+/// Computes histogram-based rotational matching scores.
+/// Match C++ RotationalScanMatcher (rotational_scan_matcher.cc)
+/// </summary>
+public class RotationalScanMatcher(double[]? _histogram)
+{
+    // Constants from C++ (rotational_scan_matcher.cc lines 31-33)
+    private const float kMinDistance = 0.2f;
+    private const float kMaxDistance = 0.9f;
+    private const float kSliceHeight = 0.2f;
+
+    /// <summary>
+    /// Rotates the given 'histogram' by the given 'angle'. This might lead to
+    /// rotations of a fractional bucket which is handled by linearly interpolating.
+    /// Match C++ RotateHistogram (rotational_scan_matcher.cc lines 141-162)
+    /// </summary>
+    public static double[] RotateHistogram(double[] histogram, double angle)
+    {
+        if (histogram == null || histogram.Length == 0)
+            return histogram ?? [];
+
+        var numBuckets = histogram.Length;
+        // C++: rotate_by_buckets = -angle * histogram.size() / M_PI
+        var rotateByBuckets = -angle * numBuckets / Math.PI;
+        var fullBuckets = (int)Math.Round(rotateByBuckets - 0.5);
+        var fraction = rotateByBuckets - fullBuckets;
+
+        // Normalize full_buckets to be non-negative
+        while (fullBuckets < 0)
+        {
+            fullBuckets += numBuckets;
+        }
+
+        // Create two rotated histograms for interpolation
+        var rotatedHistogram0 = new double[numBuckets];
+        var rotatedHistogram1 = new double[numBuckets];
+
+        for (int i = 0; i < numBuckets; i++)
+        {
+            rotatedHistogram0[i] = histogram[(i + fullBuckets) % numBuckets];
+            rotatedHistogram1[i] = histogram[(i + 1 + fullBuckets) % numBuckets];
+        }
+
+        // Linear interpolation: fraction * rotated_histogram_1 + (1 - fraction) * rotated_histogram_0
+        var result = new double[numBuckets];
+        for (int i = 0; i < numBuckets; i++)
+        {
+            result[i] = fraction * rotatedHistogram1[i] + (1.0 - fraction) * rotatedHistogram0[i];
+        }
+
+        return result;
+    }
+
+    /// <summary>
+    /// Computes the histogram for a gravity aligned 'point_cloud'.
+    /// Match C++ ComputeHistogram (rotational_scan_matcher.cc lines 164-176)
+    ///
+    /// Algorithm:
+    /// 1. Divide points into horizontal slices by Z coordinate
+    /// 2. For each slice, compute centroid and sort points by angle around centroid
+    /// 3. Compute angle differences between consecutive points
+    /// 4. Weight values by orthogonality to centroid direction (reject ceiling/floor angles)
+    /// </summary>
+    public static double[] ComputeHistogram(PointCloud pointCloud, int histogramSize)
+    {
+        if (pointCloud == null || pointCloud.Count == 0)
+            return new double[histogramSize];
+
+        var histogram = new double[histogramSize];
+
+        // Step 1: Divide points into slices by Z (C++ lines 167-171)
+        var slices = new Dictionary<int, List<RangefinderPoint>>();
+        foreach (var point in pointCloud)
+        {
+            var sliceIndex = (int)Math.Round(point.Position.Z / kSliceHeight);
+            if (!slices.TryGetValue(sliceIndex, out var slice))
+            {
+                slice = [];
+                slices[sliceIndex] = slice;
+            }
+            slice.Add(point);
+        }
+
+        // Step 2: Process each slice (C++ lines 172-174)
+        foreach (var slice in slices.Values)
+        {
+            AddPointCloudSliceToHistogram(SortSlice(slice), histogram);
+        }
+
+        return histogram;
+    }
+
+    /// <summary>
+    /// Computes the centroid of a point cloud slice.
+    /// Match C++ ComputeCentroid (rotational_scan_matcher.cc lines 52-59)
+    /// </summary>
+    private static Vector3 ComputeCentroid(List<RangefinderPoint> slice)
+    {
+        if (slice.Count == 0)
+            return Vector3.Zero;
+
+        var sum = Vector3.Zero;
+        foreach (var point in slice)
+        {
+            sum += point.Position;
+        }
+        return sum / slice.Count;
+    }
+
+    /// <summary>
+    /// Sorts points in a slice by angle around the centroid.
+    /// Match C++ SortSlice (rotational_scan_matcher.cc lines 94-119)
+    /// </summary>
+    private static List<RangefinderPoint> SortSlice(List<RangefinderPoint> slice)
+    {
+        if (slice.Count == 0)
+            return [];
+
+        var centroid = ComputeCentroid(slice);
+
+        // Create list of (angle, point) pairs
+        var byAngle = new List<(double angle, RangefinderPoint point)>();
+        foreach (var point in slice)
+        {
+            var delta = new Vector2(
+                point.Position.X - centroid.X,
+                point.Position.Y - centroid.Y);
+
+            if (delta.Length() < kMinDistance)
+                continue;
+
+            var angle = Math.Atan2(delta.Y, delta.X);
+            byAngle.Add((angle, point));
+        }
+
+        // Sort by angle
+        byAngle.Sort((a, b) => a.angle.CompareTo(b.angle));
+
+        // Return sorted points
+        return byAngle.Select(p => p.point).ToList();
+    }
+
+    /// <summary>
+    /// Adds histogram values for a sorted point cloud slice.
+    /// Match C++ AddPointCloudSliceToHistogram (rotational_scan_matcher.cc lines 61-89)
+    /// </summary>
+    private static void AddPointCloudSliceToHistogram(List<RangefinderPoint> sortedSlice, double[] histogram)
+    {
+        if (sortedSlice.Count == 0)
+            return;
+
+        var centroid = ComputeCentroid(sortedSlice);
+        var lastPointPosition = sortedSlice[0].Position;
+
+        foreach (var point in sortedSlice)
+        {
+            // Compute delta between consecutive points (2D only, XY plane)
+            var delta = new Vector2(
+                point.Position.X - lastPointPosition.X,
+                point.Position.Y - lastPointPosition.Y);
+
+            // Direction from centroid to current point
+            var direction = new Vector2(
+                point.Position.X - centroid.X,
+                point.Position.Y - centroid.Y);
+
+            var distance = delta.Length();
+            if (distance < kMinDistance || direction.Length() < kMinDistance)
+            {
+                continue;
+            }
+
+            if (distance > kMaxDistance)
+            {
+                lastPointPosition = point.Position;
+                continue;
+            }
+
+            // Compute angle of the delta vector
+            var angle = (float)Math.Atan2(delta.Y, delta.X);
+
+            // Weight: orthogonality to centroid direction (reject ceiling/floor angles)
+            // Value is higher when delta is perpendicular to direction
+            var deltaNorm = Vector2.Normalize(delta);
+            var directionNorm = Vector2.Normalize(direction);
+            var dotProduct = Vector2.Dot(deltaNorm, directionNorm);
+            var value = Math.Max(0.0, 1.0 - Math.Abs(dotProduct));
+
+            AddValueToHistogram(angle, value, histogram);
+        }
+    }
+
+    /// <summary>
+    /// Adds a value to the histogram at the given angle.
+    /// Match C++ AddValueToHistogram (rotational_scan_matcher.cc lines 35-50)
+    /// </summary>
+    private static void AddValueToHistogram(float angle, double value, double[] histogram)
+    {
+        // Map the angle to [0, pi), i.e. a vector and its inverse are considered to
+        // represent the same angle.
+        while (angle > Math.PI)
+        {
+            angle -= (float)Math.PI;
+        }
+        while (angle < 0)
+        {
+            angle += (float)Math.PI;
+        }
+
+        var zeroToOne = angle / Math.PI;
+        var bucket = Math.Clamp(
+            (int)Math.Round(histogram.Length * zeroToOne - 0.5),
+            0,
+            histogram.Length - 1);
+
+        histogram[bucket] += value;
+    }
+
+    /// <summary>
+    /// Matches two histograms and returns a normalized score.
+    /// Match C++ MatchHistograms (rotational_scan_matcher.cc lines 121-132)
+    /// </summary>
+    private static double MatchHistograms(double[] submapHistogram, double[] scanHistogram)
+    {
+        // We compute the dot product of normalized histograms as a measure of similarity.
+        var scanNorm = ComputeNorm(scanHistogram);
+        var submapNorm = ComputeNorm(submapHistogram);
+        var normalization = scanNorm * submapNorm;
+
+        if (normalization < 1e-3)
+        {
+            return 1.0; // Both histograms are nearly zero, consider them similar
+        }
+
+        var dotProduct = 0.0;
+        for (int i = 0; i < scanHistogram.Length && i < submapHistogram.Length; i++)
+        {
+            dotProduct += scanHistogram[i] * submapHistogram[i];
+        }
+
+        return dotProduct / normalization;
+    }
+
+    /// <summary>
+    /// Computes the L2 norm of a histogram.
+    /// </summary>
+    private static double ComputeNorm(double[] histogram)
+    {
+        var sumSquares = 0.0;
+        foreach (var val in histogram)
+        {
+            sumSquares += val * val;
+        }
+        return Math.Sqrt(sumSquares);
+    }
+
+    /// <summary>
+    /// Scores how well 'histogram' rotated by 'initial_angle' can be understood as
+    /// further rotated by certain 'angles' relative to the 'nodes'. Each angle
+    /// results in a score between 0 (worst) and 1 (best).
+    /// Match C++ Match (rotational_scan_matcher.cc lines 178-189)
+    /// </summary>
+    public List<double> Match(double[] histogram, double initialAngle, List<double> angles)
+    {
+        if (_histogram == null || _histogram.Length == 0)
+        {
+            // Return zero scores if no reference histogram
+            return [.. angles.Select(_ => 0.0)];
+        }
+
+        if (histogram == null || histogram.Length != _histogram.Length)
+        {
+            return [.. angles.Select(_ => 0.0)];
+        }
+
+        var scores = new List<double>();
+
+        foreach (var angle in angles)
+        {
+            var totalAngle = initialAngle + angle;
+            var rotatedHistogram = RotateHistogram(histogram, totalAngle);
+
+            // Use MatchHistograms which normalizes by the product of norms
+            var score = MatchHistograms(_histogram, rotatedHistogram);
+            scores.Add(score);
+        }
+
+        return scores;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/TranslationDeltaCostFunctor3D.cs

@@ -0,0 +1,74 @@
+/*
+ * 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 CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes the cost of translating 'translation' to 'target_translation'.
+/// Cost increases with the solution's distance from 'target_translation'.
+/// </summary>
+/// <remarks>
+/// Creates a translation delta cost functor for 3D.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor.</param>
+/// <param name="targetTranslation">Target translation to match.</param>
+public class TranslationDeltaCostFunctor3D(double scalingFactor, Vector3 targetTranslation)
+{
+    private readonly double _targetX = targetTranslation.X;
+    private readonly double _targetY = targetTranslation.Y;
+    private readonly double _targetZ = targetTranslation.Z;
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for translation delta.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="targetTranslation">Target translation to match.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Vector3 targetTranslation)
+    {
+        var functor = new TranslationDeltaCostFunctor3D(scalingFactor, targetTranslation);
+        return new DynamicAutoDiffCostFunction(
+            functor.Evaluate,
+            numResiduals: 3, // [x, y, z]
+            parameterBlockSizes: [3] // [x, y, z]
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Translation parameters [x, y, z].</param>
+    /// <param name="residuals">Output residuals [x, y, z].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var translation = parameters[0];
+        residuals[0] = scalingFactor * (translation[0] - _targetX);
+        residuals[1] = scalingFactor * (translation[1] - _targetY);
+        residuals[2] = scalingFactor * (translation[2] - _targetZ);
+        return true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/TrajectoryBuilder3DAdapter.cs

@@ -0,0 +1,80 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Adapter to make LocalTrajectoryBuilder3D implement TrajectoryBuilderInterface.
+/// </summary>
+internal class TrajectoryBuilder3DAdapter(LocalTrajectoryBuilder3D localBuilder) : ITrajectoryBuilder
+{
+    private readonly LocalTrajectoryBuilder3D _localBuilder = localBuilder ?? throw new ArgumentNullException(nameof(localBuilder));
+
+    public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
+    {
+        // LocalTrajectoryBuilder3D now returns ITrajectoryBuilder.MatchingResult directly
+        return _localBuilder.AddRangeData(sensorId, timedPointCloudData);
+    }
+
+    public void AddSensorData(string sensorId, ImuData imuData)
+    {
+        _localBuilder.AddImuData(imuData);
+    }
+
+    public void AddSensorData(string sensorId, OdometryData odometryData)
+    {
+        _localBuilder.AddOdometryData(odometryData);
+    }
+
+    public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
+    {
+        // Fixed frame pose data is typically used for external localization sources
+        // Forward to the wrapped trajectory builder if it supports it
+        // In this adapter, we assume LocalTrajectoryBuilder3D does not directly handle this,
+        // so we do nothing or could potentially pass it to a different component if available.
+        // For now, it remains unimplemented for _localBuilder.
+    }
+
+    public void AddSensorData(string sensorId, LandmarkData landmarkData)
+    {
+        // Landmark data is used for landmark-based SLAM
+        // Forward to the wrapped trajectory builder if it supports it
+        // In this adapter, we assume LocalTrajectoryBuilder3D does not directly handle this,
+        // so we do nothing or could potentially pass it to a different component if available.
+        // For now, it remains unimplemented for _localBuilder.
+    }
+
+    public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
+    {
+        // LocalTrajectoryBuilder3D doesn't use this method
+        // Results are returned directly from AddRangeData
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        return _localBuilder.TryGetExtrapolatedPose(time);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        return _localBuilder.TryGetExtrapolatedPoseFilter(time);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/ConnectedComponents.cs

@@ -0,0 +1,192 @@
+/*
+ * 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.Internal;
+
+/// <summary>
+/// A class that tracks the connectivity structure between trajectories.
+///
+/// Connectivity includes both the count ("How many times have I _directly_
+/// connected trajectories i and j?") and the transitive connectivity.
+///
+/// Uses Union-Find (disjoint set forest) algorithm for efficient transitive
+/// connectivity tracking.
+///
+/// Match C++ ConnectedComponents (connected_components.cc)
+/// </summary>
+public class ConnectedComponents
+{
+    private readonly object _lock = new();
+
+    // Tracks transitive connectivity using a disjoint set forest, i.e. each
+    // entry points towards the representative for the given trajectory.
+    private readonly Dictionary<int, int> _forest = new();
+
+    // Tracks the number of direct connections between a pair of trajectories.
+    private readonly Dictionary<(int, int), int> _connectionMap = new();
+
+    /// <summary>
+    /// Add a trajectory which is initially connected to only itself.
+    /// </summary>
+    public void Add(int trajectoryId)
+    {
+        lock (_lock)
+        {
+            // Use TryAdd to avoid overwriting existing entries
+            _forest.TryAdd(trajectoryId, trajectoryId);
+        }
+    }
+
+    /// <summary>
+    /// Connect two trajectories. If either trajectory is untracked, it will be
+    /// tracked. This function is invariant to the order of its arguments. Repeated
+    /// calls to Connect increment the connectivity count.
+    /// </summary>
+    public void Connect(int trajectoryIdA, int trajectoryIdB)
+    {
+        lock (_lock)
+        {
+            Union(trajectoryIdA, trajectoryIdB);
+            var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+            if (!_connectionMap.TryGetValue(sortedPair, out var count))
+            {
+                count = 0;
+            }
+            _connectionMap[sortedPair] = count + 1;
+        }
+    }
+
+    /// <summary>
+    /// Determines if two trajectories have been (transitively) connected. If
+    /// either trajectory is not being tracked, returns false, except when it is
+    /// the same trajectory, where it returns true. This function is invariant to
+    /// the order of its arguments.
+    /// </summary>
+    public bool TransitivelyConnected(int trajectoryIdA, int trajectoryIdB)
+    {
+        if (trajectoryIdA == trajectoryIdB)
+        {
+            return true;
+        }
+
+        lock (_lock)
+        {
+            if (!_forest.ContainsKey(trajectoryIdA) || !_forest.ContainsKey(trajectoryIdB))
+            {
+                return false;
+            }
+            return FindSet(trajectoryIdA) == FindSet(trajectoryIdB);
+        }
+    }
+
+    /// <summary>
+    /// Return the number of _direct_ connections between 'trajectoryIdA' and
+    /// 'trajectoryIdB'. If either trajectory is not being tracked, returns 0.
+    /// This function is invariant to the order of its arguments.
+    /// </summary>
+    public int ConnectionCount(int trajectoryIdA, int trajectoryIdB)
+    {
+        lock (_lock)
+        {
+            var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+            return _connectionMap.TryGetValue(sortedPair, out var count) ? count : 0;
+        }
+    }
+
+    /// <summary>
+    /// The trajectory IDs, grouped by connectivity.
+    /// </summary>
+    public List<List<int>> Components()
+    {
+        lock (_lock)
+        {
+            // Map from cluster exemplar -> growing cluster
+            var map = new Dictionary<int, List<int>>();
+            foreach (var entry in _forest)
+            {
+                var representative = FindSet(entry.Key);
+                if (!map.TryGetValue(representative, out var component))
+                {
+                    component = [];
+                    map[representative] = component;
+                }
+                component.Add(entry.Key);
+            }
+
+            return [.. map.Values];
+        }
+    }
+
+    /// <summary>
+    /// The list of trajectory IDs that belong to the same connected component as
+    /// 'trajectoryId'.
+    /// </summary>
+    public List<int> GetComponent(int trajectoryId)
+    {
+        lock (_lock)
+        {
+            if (!_forest.ContainsKey(trajectoryId))
+            {
+                return [trajectoryId];
+            }
+
+            var setId = FindSet(trajectoryId);
+            var trajectoryIds = new List<int>();
+            foreach (var entry in _forest)
+            {
+                if (FindSet(entry.Key) == setId)
+                {
+                    trajectoryIds.Add(entry.Key);
+                }
+            }
+            return trajectoryIds;
+        }
+    }
+
+    /// <summary>
+    /// Find the representative and compresses the path to it.
+    /// Must be called with lock held.
+    /// </summary>
+    private int FindSet(int trajectoryId)
+    {
+        if (!_forest.TryGetValue(trajectoryId, out var parent))
+        {
+            return trajectoryId;
+        }
+
+        if (trajectoryId != parent)
+        {
+            // Path compression for efficiency
+            _forest[trajectoryId] = FindSet(parent);
+        }
+        return _forest[trajectoryId];
+    }
+
+    /// <summary>
+    /// Union two sets.
+    /// Must be called with lock held.
+    /// </summary>
+    private void Union(int trajectoryIdA, int trajectoryIdB)
+    {
+        // Add trajectories if not already tracked
+        _forest.TryAdd(trajectoryIdA, trajectoryIdA);
+        _forest.TryAdd(trajectoryIdB, trajectoryIdB);
+
+        var representativeA = FindSet(trajectoryIdA);
+        var representativeB = FindSet(trajectoryIdB);
+        _forest[representativeA] = representativeB;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Constraints/ConstraintBuilder2D.cs

@@ -0,0 +1,838 @@
+/*
+ * 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;
+using CartographerSharp.Mapping.Internal.D2D.ScanMatching;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using Submap2D = CartographerSharp.Mapping.D2D.Submap2D;
+using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
+
+namespace CartographerSharp.Mapping.Internal.Constraints;
+
+/// <summary>
+/// Result of constraint building.
+/// </summary>
+public record struct ConstraintBuilder2DResult(List<IPoseGraph.Constraint> Constraints);
+
+/// <summary>
+/// Callback for constraint building completion.
+/// </summary>
+public delegate void ConstraintBuilder2DCallback(ConstraintBuilder2DResult result);
+
+/// <summary>
+/// Builds constraints for the pose graph by matching nodes against submaps.
+/// Matches C++ ConstraintBuilder2D (xloc) including localization and manual compute APIs.
+/// </summary>
+public class ConstraintBuilder2D : IDisposable
+{
+    private bool _disposed;
+    private readonly ConstraintBuilderOptions _options;
+    private readonly Lock _mutex = new();
+
+    private readonly CeresScanMatcher2D _ceresScanMatcher;
+    private readonly FastCorrelativeScanMatcherOptions2D? _fastCorrelativeScanMatcherOptions;
+    private readonly Dictionary<SubmapId, FixedRatioSampler> _perSubmapSampler = [];
+
+    // Match C++: localization_mode_, is_search_for_relocalization_
+    private bool _localizationMode;
+    private bool _isSearchForRelocalization;
+
+    // Match C++: num_started_nodes_, num_finished_nodes_ (constraint_builder_2d.h line 177-179)
+    private int _numStartedNodes;
+    private int _numFinishedNodes;
+
+    // Constraint task progress counters (for MapSaveProcessor progress tracking)
+    private int _numConstraintTasksDispatched;
+    private int _numConstraintTasksFinished;
+
+    // Match C++: thread_pool_ (constraint_builder_2d.cc line 63)
+    private readonly Common.Threading.ThreadPoolInterface _threadPool;
+
+    // Match C++: finish_node_task_, when_done_task_ (constraint_builder_2d.h line 181-183)
+    private Common.Threading.Task _finishNodeTask;
+    private Common.Threading.Task _whenDoneTask;
+
+    // Match C++: SubmapScanMatcher struct (constraint_builder_2d.h line 140-145)
+    // Stores the grid, fast correlative scan matcher, and creation task handle
+    private class SubmapScanMatcher
+    {
+        public Grid2D? Grid { get; set; }
+        public FastCorrelativeScanMatcher2D? FastCorrelativeScanMatcher { get; set; }
+        public WeakReference<Common.Threading.Task>? CreationTaskHandle { get; set; }
+    }
+
+    // Match C++: submap_scan_matchers_ (constraint_builder_2d.h line 191-192)
+    private readonly Dictionary<SubmapId, SubmapScanMatcher> _submapScanMatchers = [];
+
+    // Match C++: constraints_ deque (constraint_builder_2d.h line 188)
+    // We use a list of nullable constraints since computation may fail
+    private readonly List<IPoseGraph.Constraint?> _pendingConstraints = [];
+
+    // Match C++: when_done_ callback (constraint_builder_2d.h line 170-171)
+    private ConstraintBuilder2DCallback? _whenDoneCallback;
+
+    // === MEMORY OPTIMIZATION: Limit concurrent MatchFullSubmap calls ===
+    // Each MatchFullSubmap can allocate 150-250MB, running 10+ concurrently causes 3-4GB spikes
+    // Limit to 2 concurrent calls to prevent memory exhaustion while still allowing parallelism
+    private readonly SemaphoreSlim _matchFullSubmapSemaphore = new(16, 16);
+    private int _activeMatchFullSubmapCount;
+
+    // Match C++: Constructor accepts thread_pool (constraint_builder_2d.cc line 59-66)
+    public ConstraintBuilder2D(ConstraintBuilderOptions options, Common.Threading.ThreadPoolInterface threadPool)
+    {
+        _options = options;
+        _threadPool = threadPool;
+        _fastCorrelativeScanMatcherOptions = options.FastCorrelativeScanMatcherOptions ??
+            new FastCorrelativeScanMatcherOptions2D(linearSearchWindow: 7.0, angularSearchWindow: Math.PI / 6.0, branchAndBoundDepth: 7);
+        var ceresOptions = options.CeresScanMatcherOptions ?? new CeresScanMatcherOptions2D(20.0, 0.1, 0.1);
+        _ceresScanMatcher = new CeresScanMatcher2D(ceresOptions);
+
+        // Match C++ (constraint_builder_2d.cc line 64-65): Initialize task objects
+        _finishNodeTask = new Common.Threading.Task();
+        _whenDoneTask = new Common.Threading.Task();
+    }
+
+    /// <summary>
+    /// Match C++: MaybeAddConstraint - one initial_relative_pose, Match() then Ceres.
+    /// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
+    /// </summary>
+    public void MaybeAddConstraint(
+        SubmapId submapId,
+        NodeId nodeId,
+        Submap2D submap,
+        TrajectoryNode node,
+        Rigid2d initialRelativePose,
+        ConstraintBuilder2DCallback? callback = null)
+    {
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(node);
+        if (node.ConstantData == null) return;
+
+        if (initialRelativePose.Translation.Length() > _options.MaxConstraintDistance)
+            return;
+
+        if (!GetOrCreateSampler(submapId).Pulse())
+            return;
+
+        var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0) return;
+
+        var grid = submap.Grid;
+        if (grid == null) return;
+
+        // Match C++ (constraint_builder_2d.cc line 92-111)
+        lock (_mutex)
+        {
+            if (_whenDoneCallback != null)
+            {
+                // LOG(WARNING): MaybeAddConstraint was called while WhenDone was scheduled
+            }
+
+            // Add placeholder for constraint result
+            var constraintIndex = _pendingConstraints.Count;
+            _pendingConstraints.Add(null);
+            _numConstraintTasksDispatched++;
+
+            // Get or create scan matcher (may schedule async construction)
+            var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);
+
+            // Schedule constraint computation task
+            var constraintTask = new Common.Threading.Task();
+            constraintTask.SetWorkItem(() =>
+            {
+                ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
+                    matchFullSubmap: false, [initialRelativePose], constraintIndex);
+                Interlocked.Increment(ref _numConstraintTasksFinished);
+            });
+
+            // Add dependency on scan matcher construction (match C++ line 108)
+            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
+
+            var constraintTaskHandle = _threadPool.Schedule(constraintTask);
+
+            // Add dependency to finish_node_task (match C++ line 111)
+            _finishNodeTask.AddDependency(constraintTaskHandle);
+        }
+    }
+
+    /// <summary>
+    /// Match C++: MaybeAddLocalizationConstraint - list of initial_relative_poses, LocalizationMatch then Ceres.
+    /// Only adds a constraint when IsSearchingForRelocalization is true; then clears the flag on success.
+    /// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
+    /// </summary>
+    public void MaybeAddLocalizationConstraint(
+        SubmapId submapId,
+        NodeId nodeId,
+        Submap2D submap,
+        TrajectoryNode node,
+        IReadOnlyList<Rigid2d> initialRelativePoses,
+        ConstraintBuilder2DCallback? callback = null)
+    {
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(node);
+        if (node.ConstantData == null) return;
+        if (initialRelativePoses == null || initialRelativePoses.Count == 0) return;
+
+        var filtered = initialRelativePoses
+            .Where(p => p.Translation.Length() <= _options.MaxConstraintDistance)
+            .ToList();
+        if (filtered.Count == 0) return;
+
+        _localizationMode = true;
+
+        var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0) return;
+
+        var grid = submap.Grid;
+        if (grid == null) return;
+
+        // Match C++ (constraint_builder_2d.cc line 138-157)
+        lock (_mutex)
+        {
+            if (_whenDoneCallback != null)
+            {
+                // LOG(WARNING): MaybeAddConstraint was called while WhenDone was scheduled
+            }
+
+            var constraintIndex = _pendingConstraints.Count;
+            _pendingConstraints.Add(null);
+            _numConstraintTasksDispatched++;
+
+            var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);
+
+            var constraintTask = new Common.Threading.Task();
+            constraintTask.SetWorkItem(() =>
+            {
+                ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
+                    matchFullSubmap: true, filtered, constraintIndex);
+                Interlocked.Increment(ref _numConstraintTasksFinished);
+            });
+
+            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
+            var constraintTaskHandle = _threadPool.Schedule(constraintTask);
+            _finishNodeTask.AddDependency(constraintTaskHandle);
+        }
+    }
+
+    /// <summary>
+    /// Match C++: MaybeAddGlobalConstraint - full submap match (MatchFullSubmap then Ceres).
+    /// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
+    /// </summary>
+    public void MaybeAddGlobalConstraint(
+        SubmapId submapId,
+        NodeId nodeId,
+        Submap2D submap,
+        TrajectoryNode node,
+        ConstraintBuilder2DCallback? callback = null)
+    {
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(node);
+        if (node.ConstantData == null) return;
+
+        var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0) return;
+
+        var grid = submap.Grid;
+        if (grid == null) return;
+
+        // Match C++ (constraint_builder_2d.cc line 160-182)
+        lock (_mutex)
+        {
+            if (_whenDoneCallback != null)
+            {
+                // LOG(WARNING): MaybeAddGlobalConstraint was called while WhenDone was scheduled
+            }
+
+            var constraintIndex = _pendingConstraints.Count;
+            _pendingConstraints.Add(null);
+            _numConstraintTasksDispatched++;
+
+            var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);
+
+            var constraintTask = new Common.Threading.Task();
+            constraintTask.SetWorkItem(() =>
+            {
+                ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
+                    matchFullSubmap: true, [Rigid2d.Identity], constraintIndex);
+                Interlocked.Increment(ref _numConstraintTasksFinished);
+            });
+
+            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
+            var constraintTaskHandle = _threadPool.Schedule(constraintTask);
+            _finishNodeTask.AddDependency(constraintTaskHandle);
+        }
+    }
+
+    /// <summary>
+    /// Match C++: manualComputeGlobalConstraint - MatchFullSubmap, Ceres, then MatchWithCustomizeParameters(0.1, 0.1, 0.01) for score.
+    /// </summary>
+    public (double Score, IPoseGraph.Constraint? Constraint) ManualComputeGlobalConstraint(
+        SubmapId submapId,
+        Submap2D submap,
+        NodeId nodeId,
+        TrajectoryNode.Data constantData,
+        double minScore)
+    {
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(constantData);
+        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0)
+            return (0, null);
+
+        var grid = submap.Grid;
+        if (grid == null) return (0, null);
+
+        var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
+        var submapPose = ComputeSubmapPose(submap);
+        if (!fastMatcher.MatchFullSubmap(pointCloud, 0, out _, out var poseEstimate))
+            return (0, null);
+
+        _ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary1);
+        ceresSummary1?.Dispose();
+        if (fastMatcher.MatchWithCustomizeParameters(0.1, 0.1, 0.01f, poseEstimate, pointCloud, 0, out double score, out poseEstimate))
+        {
+            // Re-calculated score
+        }
+
+        var constraintTransform = submapPose.Inverse() * poseEstimate;
+        // Match C++: include score and state (constraint_builder_2d.cc)
+        var constraint = new IPoseGraph.Constraint(
+            submapId, nodeId,
+            new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
+            IPoseGraph.Constraint.Tag.InterSubmap,
+            score,
+            IPoseGraph.Constraint.State.Enabled);
+        return (score, constraint);
+    }
+
+    /// <summary>
+    /// Match C++: manualComputeRelocalizationConstraint - try LocalizationMatch on each submap/pose, pick best, Ceres, return constraint.
+    /// </summary>
+    public (double Score, IPoseGraph.Constraint? Constraint) ManualComputeRelocalizationConstraint(
+        IReadOnlyList<SubmapId> submapIds,
+        IReadOnlyList<Submap2D> submaps,
+        IReadOnlyList<Rigid2d> relativePoses,
+        NodeId nodeId,
+        TrajectoryNode.Data constantData,
+        double minScore)
+    {
+        if (submapIds == null || submaps == null || relativePoses == null || submapIds.Count != submaps.Count || submapIds.Count != relativePoses.Count)
+            return (0, null);
+        ArgumentNullException.ThrowIfNull(constantData);
+        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0) return (0, null);
+
+        double bestScore = 0;
+        Rigid2d bestPoseEstimate = Rigid2d.Identity;
+        Submap2D? bestSubmap = null;
+        SubmapId bestSubmapId = default;
+
+        for (int i = 0; i < submaps.Count; i++)
+        {
+            var submap = submaps[i];
+            var grid = submap.Grid;
+            if (grid == null) continue;
+
+            var fastMatcher = GetOrCreateFastMatcherSync(submapIds[i], grid);
+            var localizationInitialPose = ComputeSubmapPose(submap) * relativePoses[i];
+
+            if (!fastMatcher.LocalizationMatch(localizationInitialPose, pointCloud, minScore, out var score, out var poseEstimate))
+                continue;
+
+            if (score > bestScore)
+            {
+                bestScore = score;
+                bestPoseEstimate = poseEstimate;
+                bestSubmap = submap;
+                bestSubmapId = submapIds[i];
+            }
+        }
+
+        if (bestSubmap == null) return (0, null);
+
+        _ceresScanMatcher.Match(bestPoseEstimate.Translation, bestPoseEstimate, pointCloud, bestSubmap.Grid!, out bestPoseEstimate, out var ceresSummary2);
+        ceresSummary2?.Dispose();
+        var constraintTransform = ComputeSubmapPose(bestSubmap).Inverse() * bestPoseEstimate;
+        // Match C++: include score and state (constraint_builder_2d.cc)
+        var constraint = new IPoseGraph.Constraint(
+            bestSubmapId, nodeId,
+            new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
+            IPoseGraph.Constraint.Tag.InterSubmap,
+            bestScore,
+            IPoseGraph.Constraint.State.Enabled);
+        return (bestScore, constraint);
+    }
+
+    /// <summary>
+    /// Match C++: manualComputeConstraintScore - MatchWithCustomizeParameters(1.5, 1.5, 0.05) from initial_pose.
+    /// </summary>
+    public double ManualComputeConstraintScore(
+        SubmapId submapId,
+        Submap2D submap,
+        NodeId nodeId,
+        TrajectoryNode.Data constantData,
+        double minScore,
+        Rigid3d initialPose)
+    {
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(constantData);
+        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0) return 0;
+
+        var grid = submap.Grid;
+        if (grid == null) return 0;
+
+        var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
+        var poseEstimate = TransformOperations.Project2D(initialPose);
+        fastMatcher.MatchWithCustomizeParameters(1.5, 1.5, 0.05f, poseEstimate, pointCloud, 0, out var constraintScore, out _);
+        return constraintScore;
+    }
+
+    /// <summary>
+    /// Match C++: manualComputeScanMatcher - Ceres then MatchWithCustomizeParameters(0.2, 0.2, 0.01), output pose_manual_estimate.
+    /// </summary>
+    public double ManualComputeScanMatcher(
+        SubmapId submapId,
+        Submap2D submap,
+        NodeId nodeId,
+        TrajectoryNode.Data constantData,
+        double minScore,
+        Rigid3d initialPose,
+        out Rigid3d poseManualEstimate)
+    {
+        poseManualEstimate = default;
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(constantData);
+        var pointCloud = constantData.FilteredGravityAlignedPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0) return 0;
+
+        var grid = submap.Grid;
+        if (grid == null) return 0;
+
+        var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
+        var poseEstimate = TransformOperations.Project2D(initialPose);
+
+        _ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary3);
+        ceresSummary3?.Dispose();
+
+        if (fastMatcher.MatchWithCustomizeParameters(0.2, 0.2, 0.01f, poseEstimate, pointCloud, 0, out double score, out poseEstimate))
+        {
+            poseManualEstimate = TransformOperations.Embed3D(poseEstimate);
+            return score;
+        }
+        poseManualEstimate = TransformOperations.Embed3D(poseEstimate);
+        return score;
+    }
+
+    /// <summary>
+    /// Match C++: NotifyEndOfNode - must be called after all computations for one node have been added.
+    /// Match C++ (constraint_builder_2d.cc line 403-415)
+    /// </summary>
+    public void NotifyEndOfNode()
+    {
+        lock (_mutex)
+        {
+            // Set work item for finish_node_task to increment num_finished_nodes
+            _finishNodeTask.SetWorkItem(() =>
+            {
+                lock (_mutex)
+                {
+                    _numFinishedNodes++;
+                }
+            });
+
+            // Schedule finish_node_task
+            var finishNodeTaskHandle = _threadPool.Schedule(_finishNodeTask);
+
+            // Create new finish_node_task for next node
+            _finishNodeTask = new Common.Threading.Task();
+
+            // Add dependency to when_done_task
+            _whenDoneTask.AddDependency(finishNodeTaskHandle);
+
+            _numStartedNodes++;
+        }
+    }
+
+    /// <summary>
+    /// Match C++ WhenDone: Registers callback to be called after all computations finish.
+    /// Match C++ (constraint_builder_2d.cc line 417-427)
+    /// </summary>
+    public void WhenDone(ConstraintBuilder2DCallback callback)
+    {
+        lock (_mutex)
+        {
+            if (_whenDoneCallback != null)
+            {
+                throw new InvalidOperationException("WhenDone() called while another WhenDone() was pending");
+            }
+
+            _whenDoneCallback = callback;
+
+            // Set work item for when_done_task to run callback
+            _whenDoneTask.SetWorkItem(RunWhenDoneCallback);
+
+            // Schedule when_done_task (it will wait for all dependencies)
+            _threadPool.Schedule(_whenDoneTask);
+
+            // Create new when_done_task for next cycle
+            _whenDoneTask = new Common.Threading.Task();
+        }
+    }
+
+    /// <summary>
+    /// Match C++ RunWhenDoneCallback (constraint_builder_2d.cc line 596-617)
+    /// </summary>
+    private void RunWhenDoneCallback()
+    {
+        List<IPoseGraph.Constraint> result = [];
+        ConstraintBuilder2DCallback? callback;
+
+        lock (_mutex)
+        {
+            if (_whenDoneCallback == null)
+            {
+                throw new InvalidOperationException("RunWhenDoneCallback called without callback set");
+            }
+
+            // Collect all non-null constraints
+            foreach (var constraint in _pendingConstraints)
+            {
+                if (constraint != null)
+                {
+                    result.Add(constraint.Value);
+                }
+            }
+
+            // Clear pending constraints
+            _pendingConstraints.Clear();
+
+            // Take callback and clear
+            callback = _whenDoneCallback;
+            _whenDoneCallback = null;
+        }
+
+        // Invoke callback outside lock
+        callback(new ConstraintBuilder2DResult(result));
+    }
+
+    public List<IPoseGraph.Constraint> GetConstraints()
+    {
+        lock (_mutex)
+        {
+            return [.. _pendingConstraints.Where(c => c != null).Select(c => c!.Value)];
+        }
+    }
+
+    /// <summary>
+    /// Match C++: GetNumFinishedNodes().
+    /// </summary>
+    public int GetNumFinishedNodes()
+    {
+        lock (_mutex) return _numFinishedNodes;
+    }
+
+    /// <summary>
+    /// Match C++: DeleteScanMatcher(submap_id).
+    /// </summary>
+    public void DeleteScanMatcher(SubmapId submapId)
+    {
+        lock (_mutex)
+        {
+            _submapScanMatchers.Remove(submapId);
+            _perSubmapSampler.Remove(submapId);
+        }
+    }
+
+    /// <summary>
+    /// Match C++: GetNumStartedNodes().
+    /// </summary>
+    public int GetNumStartedNodes()
+    {
+        lock (_mutex) return _numStartedNodes;
+    }
+
+    /// <summary>
+    /// Gets the total number of constraint tasks dispatched for computation.
+    /// </summary>
+    public int GetNumConstraintTasksDispatched()
+    {
+        lock (_mutex) return _numConstraintTasksDispatched;
+    }
+
+    /// <summary>
+    /// Gets the number of constraint tasks that have finished computation.
+    /// </summary>
+    public int GetNumConstraintTasksFinished()
+    {
+        return Interlocked.CompareExchange(ref _numConstraintTasksFinished, 0, 0);
+    }
+
+    /// <summary>
+    /// Match C++: IsSearchingForRelocalization().
+    /// </summary>
+    public bool IsSearchingForRelocalization => _isSearchForRelocalization;
+
+    /// <summary>
+    /// Match C++: ToggleSearchingForRelocalization(enable).
+    /// </summary>
+    public void ToggleSearchingForRelocalization(bool enable)
+    {
+        _isSearchForRelocalization = enable;
+    }
+
+    public void Clear()
+    {
+        lock (_mutex)
+        {
+            _pendingConstraints.Clear();
+        }
+    }
+
+    private FixedRatioSampler GetOrCreateSampler(SubmapId submapId)
+    {
+        lock (_mutex)
+        {
+            if (!_perSubmapSampler.TryGetValue(submapId, out var sampler))
+            {
+                sampler = new FixedRatioSampler(_options.SamplingRatio);
+                _perSubmapSampler[submapId] = sampler;
+            }
+            return sampler;
+        }
+    }
+
+    /// <summary>
+    /// Match C++ DispatchScanMatcherConstruction (constraint_builder_2d.cc line 429-450)
+    /// Creates or returns existing SubmapScanMatcher, scheduling async construction if needed.
+    /// MUST be called with _mutex held.
+    /// </summary>
+    private SubmapScanMatcher DispatchScanMatcherConstruction(SubmapId submapId, Grid2D grid)
+    {
+        // Check if scan matcher already exists
+        if (_submapScanMatchers.TryGetValue(submapId, out var existingMatcher))
+        {
+            return existingMatcher;
+        }
+
+        // Create new scan matcher entry
+        var submapScanMatcher = new SubmapScanMatcher
+        {
+            Grid = grid
+        };
+        _submapScanMatchers[submapId] = submapScanMatcher;
+
+        var scanMatcherOptions = _fastCorrelativeScanMatcherOptions ??
+            new FastCorrelativeScanMatcherOptions2D(7.0, Math.PI / 6.0, 7);
+
+        // Schedule async construction of FastCorrelativeScanMatcher2D
+        var scanMatcherTask = new Common.Threading.Task();
+        scanMatcherTask.SetWorkItem(() =>
+        {
+            // Create the scan matcher (this may be expensive)
+            var gridLimits = grid.Limits.CellLimits;
+            var matcher = new FastCorrelativeScanMatcher2D(grid, scanMatcherOptions);
+            lock (_mutex)
+            {
+                submapScanMatcher.FastCorrelativeScanMatcher = matcher;
+            }
+        });
+
+        submapScanMatcher.CreationTaskHandle = _threadPool.Schedule(scanMatcherTask);
+
+        return submapScanMatcher;
+    }
+
+    /// <summary>
+    /// Gets the FastCorrelativeScanMatcher for a submap (for manual compute methods).
+    /// This blocks until the scan matcher is ready.
+    /// </summary>
+    private FastCorrelativeScanMatcher2D GetOrCreateFastMatcherSync(SubmapId submapId, Grid2D grid)
+    {
+        SubmapScanMatcher? scanMatcher;
+        lock (_mutex)
+        {
+            if (!_submapScanMatchers.TryGetValue(submapId, out scanMatcher))
+            {
+                // Create synchronously for manual methods
+                var options = _fastCorrelativeScanMatcherOptions ??
+                    new FastCorrelativeScanMatcherOptions2D(7.0, Math.PI / 6.0, 7);
+                var matcher = new FastCorrelativeScanMatcher2D(grid, options);
+                scanMatcher = new SubmapScanMatcher
+                {
+                    Grid = grid,
+                    FastCorrelativeScanMatcher = matcher
+                };
+                _submapScanMatchers[submapId] = scanMatcher;
+                return matcher;
+            }
+        }
+
+        // Wait for async construction if needed
+        if (scanMatcher.FastCorrelativeScanMatcher == null &&
+            scanMatcher.CreationTaskHandle != null &&
+            scanMatcher.CreationTaskHandle.TryGetTarget(out var task))
+        {
+            while (task.GetState() != Common.Threading.TaskState.Completed)
+            {
+                System.Threading.Thread.Sleep(1);
+            }
+        }
+
+        lock (_mutex)
+        {
+            return scanMatcher.FastCorrelativeScanMatcher!;
+        }
+    }
+
+    /// <summary>
+    /// Single internal compute: handles MaybeAddConstraint (matchFullSubmap=false, single pose),
+    /// MaybeAddLocalizationConstraint (matchFullSubmap=true, localizationMode, many poses),
+    /// MaybeAddGlobalConstraint (matchFullSubmap=true, single Identity pose).
+    /// Match C++ ComputeConstraint (constraint_builder_2d.cc line 452-594)
+    /// </summary>
+    private void ComputeConstraint(
+        SubmapId submapId,
+        NodeId nodeId,
+        Submap2D submap,
+        Grid2D grid,
+        PointCloud pointCloud,
+        bool matchFullSubmap,
+        IReadOnlyList<Rigid2d> initialRelativePoses,
+        int constraintIndex)
+    {
+        // Get the scan matcher (should be ready by now due to task dependency)
+        FastCorrelativeScanMatcher2D? fastMatcher;
+        lock (_mutex)
+        {
+            if (!_submapScanMatchers.TryGetValue(submapId, out var scanMatcher) ||
+                scanMatcher.FastCorrelativeScanMatcher == null)
+            {
+                return; // Scan matcher not ready (shouldn't happen with proper dependencies)
+            }
+            fastMatcher = scanMatcher.FastCorrelativeScanMatcher;
+        }
+
+        var submapPose = ComputeSubmapPose(submap);
+        double score = 0;
+        Rigid2d poseEstimate = Rigid2d.Identity;
+
+        if (matchFullSubmap)
+        {
+            if (_localizationMode)
+            {
+                lock (_mutex)
+                {
+                    if (!_isSearchForRelocalization)
+                        return;
+                }
+
+                double bestScore = 0;
+                Rigid2d bestPoseEstimate = Rigid2d.Identity;
+                foreach (var rel in initialRelativePoses)
+                {
+                    var localizationInitialPose = submapPose * rel;
+                    if (fastMatcher.LocalizationMatch(localizationInitialPose, pointCloud, _options.GlobalLocalizationMinScore, out score, out poseEstimate))
+                    {
+                        if (score > bestScore)
+                        {
+                            bestScore = score;
+                            bestPoseEstimate = poseEstimate;
+                        }
+                    }
+                }
+
+                if (bestScore < _options.GlobalLocalizationMinScore)
+                    return;
+
+                _isSearchForRelocalization = false;
+                score = bestScore;
+                poseEstimate = bestPoseEstimate;
+            }
+            else
+            {
+                // === MEMORY OPTIMIZATION: Limit concurrent MatchFullSubmap calls ===
+                // Each call allocates 150-250MB, running many concurrently causes GB-level spikes
+                _matchFullSubmapSemaphore.Wait();
+                _ = Interlocked.Increment(ref _activeMatchFullSubmapCount);
+                try
+                {
+                    // === DEBUG: Track memory before/after MatchFullSubmap ===
+                    var ramBeforeMatch = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);
+
+                    if (!fastMatcher.MatchFullSubmap(pointCloud, _options.GlobalLocalizationMinScore, out score, out poseEstimate))
+                    {
+                        var ramAfterFail = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);
+                        return;
+                    }
+
+                    var ramAfterMatch = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);
+
+                    if (score <= _options.GlobalLocalizationMinScore)
+                        return;
+                }
+                finally
+                {
+                    Interlocked.Decrement(ref _activeMatchFullSubmapCount);
+                    _matchFullSubmapSemaphore.Release();
+                }
+            }
+        }
+        else
+        {
+            var initialPose = submapPose * initialRelativePoses[0];
+            if (!fastMatcher.Match(initialPose, pointCloud, _options.MinScore, out score, out poseEstimate))
+                return;
+            if (score <= _options.MinScore)
+                return;
+        }
+
+        _ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary4);
+        ceresSummary4?.Dispose();
+
+        var constraintTransform = submapPose.Inverse() * poseEstimate;
+        // Match C++: include score and state (constraint_builder_2d.cc lines 567-574)
+        var constraint = new IPoseGraph.Constraint(
+            submapId, nodeId,
+            new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
+            IPoseGraph.Constraint.Tag.InterSubmap,
+            score,  // CRITICAL FIX: include score from scan matching
+            IPoseGraph.Constraint.State.Enabled);  // Match C++: Constraint::ENABLED
+
+        // Store constraint at the pre-allocated index
+        lock (_mutex)
+        {
+            _pendingConstraints[constraintIndex] = constraint;
+        }
+    }
+
+    private static Rigid2d ComputeSubmapPose(Submap2D submap)
+    {
+        return TransformOperations.Project2D(submap.LocalPose);
+    }
+
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            _ceresScanMatcher?.Dispose();
+            _matchFullSubmapSemaphore?.Dispose();
+            _disposed = true;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Constraints/ConstraintBuilder3D.cs

@@ -0,0 +1,580 @@
+/*
+ * 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;
+using CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Numbers;
+using Submap3D = CartographerSharp.Mapping.D3D.Submap3D;
+
+namespace CartographerSharp.Mapping.Internal.Constraints;
+
+/// <summary>
+/// Result of constraint building for 3D.
+/// </summary>
+public record struct ConstraintBuilder3DResult(List<IPoseGraph.Constraint> Constraints);
+
+/// <summary>
+/// Callback for constraint building completion.
+/// </summary>
+public delegate void ConstraintBuilder3DCallback(ConstraintBuilder3DResult result);
+
+/// <summary>
+/// Builds constraints for the 3D pose graph by matching nodes against submaps.
+/// Match C++ ConstraintBuilder3D (constraint_builder_3d.h/cc)
+/// </summary>
+public class ConstraintBuilder3D : IDisposable
+{
+    private bool _disposed;
+    private readonly ConstraintBuilderOptions _options;
+    private readonly object _mutex = new();
+    private readonly Dictionary<SubmapId, FixedRatioSampler> _perSubmapSampler = [];
+
+    // Scan matchers
+    private readonly CeresScanMatcher3D? _ceresScanMatcher;
+
+    // Match C++: num_started_nodes_, num_finished_nodes_ (constraint_builder_3d.h line 157-159)
+    private int _numStartedNodes;
+    private int _numFinishedNodes;
+
+    // Match C++: thread_pool_ (constraint_builder_3d.cc line 63)
+    private readonly Common.Threading.ThreadPoolInterface _threadPool;
+
+    // Match C++: finish_node_task_, when_done_task_ (constraint_builder_3d.h line 161-163)
+    private Common.Threading.Task _finishNodeTask;
+    private Common.Threading.Task _whenDoneTask;
+
+    /// <summary>
+    /// Submap scan matcher structure.
+    /// Match C++ SubmapScanMatcher (constraint_builder_3d.h line 117-123)
+    /// </summary>
+    private class SubmapScanMatcher
+    {
+        public Mapping.D3D.HybridGrid? HighResolutionHybridGrid { get; set; }
+        public Mapping.D3D.HybridGrid? LowResolutionHybridGrid { get; set; }
+        public Mapping.D3D.IntensityHybridGrid? HighResolutionIntensityHybridGrid { get; set; }
+        public RealTimeCorrelativeScanMatcher3D? FastCorrelativeScanMatcher { get; set; }
+        public WeakReference<Common.Threading.Task>? CreationTaskHandle { get; set; }
+    }
+
+    // Match C++: submap_scan_matchers_ (constraint_builder_3d.h line 171-172)
+    private readonly Dictionary<SubmapId, SubmapScanMatcher> _submapScanMatchers = [];
+
+    // Match C++: constraints_ deque (constraint_builder_3d.h line 168)
+    private readonly List<IPoseGraph.Constraint?> _pendingConstraints = [];
+
+    // Match C++: when_done_ callback (constraint_builder_3d.h line 150-151)
+    private ConstraintBuilder3DCallback? _whenDoneCallback;
+
+    // Match C++: Constructor accepts thread_pool (constraint_builder_3d.cc line 61-68)
+    public ConstraintBuilder3D(ConstraintBuilderOptions options, Common.Threading.ThreadPoolInterface threadPool)
+    {
+        _options = options;
+        _threadPool = threadPool;
+
+        // Initialize Ceres scan matcher if options are provided
+        if (options.CeresScanMatcherOptions3D != null)
+        {
+            _ceresScanMatcher = new CeresScanMatcher3D(options.CeresScanMatcherOptions3D.Value);
+        }
+
+        // Match C++ (constraint_builder_3d.cc line 66-67): Initialize task objects
+        _finishNodeTask = new Common.Threading.Task();
+        _whenDoneTask = new Common.Threading.Task();
+    }
+
+    /// <summary>
+    /// Schedules exploring a new constraint between 'submap' identified by
+    /// 'submap_id', and the point cloud for 'node_id'.
+    /// Match C++ MaybeAddConstraint (constraint_builder_3d.cc line 79-114)
+    /// </summary>
+    public void MaybeAddConstraint(
+        SubmapId submapId,
+        NodeId nodeId,
+        Submap3D submap,
+        TrajectoryNode node,
+        Rigid3d globalNodePose,
+        Rigid3d globalSubmapPose)
+    {
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(node);
+        if (node.ConstantData == null)
+            return;
+
+        // Check distance threshold
+        var distance = (globalNodePose.Translation - globalSubmapPose.Translation).Length();
+        if (distance > _options.MaxConstraintDistance)
+            return;
+
+        // Check sampling ratio
+        if (!GetOrCreateSampler(submapId).Pulse())
+            return;
+
+        // Get point cloud from node
+        var pointCloud = node.ConstantData.HighResolutionPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0)
+            return;
+
+        // Match C++ (constraint_builder_3d.cc line 95-113)
+        lock (_mutex)
+        {
+            if (_whenDoneCallback != null)
+            {
+                // LOG(WARNING): MaybeAddConstraint was called while WhenDone was scheduled
+            }
+
+            // Add placeholder for constraint result
+            var constraintIndex = _pendingConstraints.Count;
+            _pendingConstraints.Add(null);
+
+            // Get or create scan matcher (may schedule async construction)
+            var scanMatcher = DispatchScanMatcherConstruction(submapId, submap);
+            if (scanMatcher == null)
+                return;
+
+            // Schedule constraint computation task
+            var constraintTask = new Common.Threading.Task();
+            constraintTask.SetWorkItem(() =>
+            {
+                ComputeConstraint(submapId, nodeId, false, node.ConstantData,
+                    globalNodePose, globalSubmapPose, scanMatcher, constraintIndex);
+            });
+
+            // Add dependency on scan matcher construction (match C++ line 110)
+            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
+
+            var constraintTaskHandle = _threadPool.Schedule(constraintTask);
+
+            // Add dependency to finish_node_task (match C++ line 113)
+            _finishNodeTask.AddDependency(constraintTaskHandle);
+        }
+    }
+
+    /// <summary>
+    /// Schedules exploring a new global constraint (full submap matching).
+    /// Match C++ MaybeAddGlobalConstraint (constraint_builder_3d.cc line 116-142)
+    /// </summary>
+    public void MaybeAddGlobalConstraint(
+        SubmapId submapId,
+        NodeId nodeId,
+        Submap3D submap,
+        TrajectoryNode node,
+        Quaternion globalNodeRotation,
+        Quaternion globalSubmapRotation)
+    {
+        ArgumentNullException.ThrowIfNull(submap);
+        ArgumentNullException.ThrowIfNull(node);
+        if (node.ConstantData == null)
+            return;
+
+        // Get point cloud from node
+        var pointCloud = node.ConstantData.HighResolutionPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0)
+            return;
+
+        // Match C++ (constraint_builder_3d.cc line 121-141)
+        lock (_mutex)
+        {
+            if (_whenDoneCallback != null)
+            {
+                // LOG(WARNING): MaybeAddGlobalConstraint was called while WhenDone was scheduled
+            }
+
+            var constraintIndex = _pendingConstraints.Count;
+            _pendingConstraints.Add(null);
+
+            var scanMatcher = DispatchScanMatcherConstruction(submapId, submap);
+            if (scanMatcher == null)
+                return;
+
+            // Create poses with only rotation (yaw is ignored for global matching)
+            var globalNodePose = Rigid3d.FromRotation(globalNodeRotation);
+            var globalSubmapPose = Rigid3d.FromRotation(globalSubmapRotation);
+
+            var constraintTask = new Common.Threading.Task();
+            constraintTask.SetWorkItem(() =>
+            {
+                ComputeConstraint(submapId, nodeId, true, node.ConstantData,
+                    globalNodePose, globalSubmapPose, scanMatcher, constraintIndex);
+            });
+
+            constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
+            var constraintTaskHandle = _threadPool.Schedule(constraintTask);
+            _finishNodeTask.AddDependency(constraintTaskHandle);
+        }
+    }
+
+    /// <summary>
+    /// Must be called after all computations related to one node have been added.
+    /// Match C++ NotifyEndOfNode (constraint_builder_3d.cc line 144-156)
+    /// </summary>
+    public void NotifyEndOfNode()
+    {
+        lock (_mutex)
+        {
+            // Set work item for finish_node_task to increment num_finished_nodes
+            _finishNodeTask.SetWorkItem(() =>
+            {
+                lock (_mutex)
+                {
+                    _numFinishedNodes++;
+                }
+            });
+
+            // Schedule finish_node_task
+            var finishNodeTaskHandle = _threadPool.Schedule(_finishNodeTask);
+
+            // Create new finish_node_task for next node
+            _finishNodeTask = new Common.Threading.Task();
+
+            // Add dependency to when_done_task
+            _whenDoneTask.AddDependency(finishNodeTaskHandle);
+
+            _numStartedNodes++;
+        }
+    }
+
+    /// <summary>
+    /// Registers the callback to be called with the results, after all
+    /// computations triggered by MaybeAdd*Constraint have finished.
+    /// Match C++ WhenDone (constraint_builder_3d.cc line 158-168)
+    /// </summary>
+    public void WhenDone(ConstraintBuilder3DCallback callback)
+    {
+        lock (_mutex)
+        {
+            if (_whenDoneCallback != null)
+            {
+                throw new InvalidOperationException("WhenDone() called while another WhenDone() was pending");
+            }
+
+            _whenDoneCallback = callback;
+
+            // Set work item for when_done_task to run callback
+            _whenDoneTask.SetWorkItem(RunWhenDoneCallback);
+
+            // Schedule when_done_task (it will wait for all dependencies)
+            _threadPool.Schedule(_whenDoneTask);
+
+            // Create new when_done_task for next cycle
+            _whenDoneTask = new Common.Threading.Task();
+        }
+    }
+
+    /// <summary>
+    /// Match C++ RunWhenDoneCallback (constraint_builder_3d.cc line 307-333)
+    /// </summary>
+    private void RunWhenDoneCallback()
+    {
+        List<IPoseGraph.Constraint> result = [];
+        ConstraintBuilder3DCallback? callback;
+
+        lock (_mutex)
+        {
+            if (_whenDoneCallback == null)
+            {
+                throw new InvalidOperationException("RunWhenDoneCallback called without callback set");
+            }
+
+            // Collect all non-null constraints
+            foreach (var constraint in _pendingConstraints)
+            {
+                if (constraint != null)
+                {
+                    result.Add(constraint.Value);
+                }
+            }
+
+            // Clear pending constraints
+            _pendingConstraints.Clear();
+
+            // Take callback and clear
+            callback = _whenDoneCallback;
+            _whenDoneCallback = null;
+        }
+
+        // Invoke callback outside lock
+        callback(new ConstraintBuilder3DResult(result));
+    }
+
+    /// <summary>
+    /// Returns the number of consecutive finished nodes.
+    /// </summary>
+    public int GetNumFinishedNodes()
+    {
+        lock (_mutex) return _numFinishedNodes;
+    }
+
+    /// <summary>
+    /// Returns the number of started nodes.
+    /// </summary>
+    public int GetNumStartedNodes()
+    {
+        lock (_mutex) return _numStartedNodes;
+    }
+
+    /// <summary>
+    /// Delete data related to 'submap_id'.
+    /// </summary>
+    public void DeleteScanMatcher(SubmapId submapId)
+    {
+        lock (_mutex)
+        {
+            _submapScanMatchers.Remove(submapId);
+            _perSubmapSampler.Remove(submapId);
+        }
+    }
+
+    /// <summary>
+    /// Returns the computed constraints.
+    /// </summary>
+    public List<IPoseGraph.Constraint> GetConstraints()
+    {
+        lock (_mutex)
+        {
+            return _pendingConstraints.Where(c => c != null).Select(c => c!.Value).ToList();
+        }
+    }
+
+    /// <summary>
+    /// Clears all constraints.
+    /// </summary>
+    public void Clear()
+    {
+        lock (_mutex)
+        {
+            _pendingConstraints.Clear();
+        }
+    }
+
+    private FixedRatioSampler GetOrCreateSampler(SubmapId submapId)
+    {
+        lock (_mutex)
+        {
+            if (!_perSubmapSampler.TryGetValue(submapId, out var sampler))
+            {
+                sampler = new FixedRatioSampler(_options.SamplingRatio);
+                _perSubmapSampler[submapId] = sampler;
+            }
+            return sampler;
+        }
+    }
+
+    /// <summary>
+    /// Dispatches scan matcher construction for a submap.
+    /// Match C++ DispatchScanMatcherConstruction (constraint_builder_3d.cc line 170-198)
+    /// MUST be called with _mutex held.
+    /// </summary>
+    private SubmapScanMatcher? DispatchScanMatcherConstruction(SubmapId submapId, Submap3D submap)
+    {
+        // Check if scan matcher already exists
+        if (_submapScanMatchers.TryGetValue(submapId, out var existingMatcher))
+        {
+            return existingMatcher;
+        }
+
+        // Create new scan matcher entry
+        var scanMatcher = new SubmapScanMatcher
+        {
+            HighResolutionHybridGrid = submap.HighResolutionHybridGrid,
+            LowResolutionHybridGrid = submap.LowResolutionHybridGrid,
+            HighResolutionIntensityHybridGrid = submap.HighResolutionIntensityHybridGrid
+        };
+
+        if (scanMatcher.HighResolutionHybridGrid == null)
+        {
+            return null;
+        }
+
+        _submapScanMatchers[submapId] = scanMatcher;
+
+        var fastOptions = _options.FastCorrelativeScanMatcherOptions3D ??
+            new FastCorrelativeScanMatcherOptions3D();
+
+        // Get rotational scan matcher histogram from submap if available
+        double[]? histogram = null;
+        if (submap is Mapping.D3D.Submap3D submap3D)
+        {
+            var histogramList = submap3D.RotationalScanMatcherHistogram;
+            if (histogramList != null && histogramList.Count > 0)
+            {
+                histogram = histogramList.ToArray();
+            }
+        }
+
+        // Capture values for closure
+        var highResGrid = scanMatcher.HighResolutionHybridGrid;
+        var lowResGrid = scanMatcher.LowResolutionHybridGrid;
+
+        // Schedule async construction of FastCorrelativeScanMatcher
+        var scanMatcherTask = new Common.Threading.Task();
+        scanMatcherTask.SetWorkItem(() =>
+        {
+            var matcher = new RealTimeCorrelativeScanMatcher3D(
+                highResGrid,
+                lowResGrid,
+                histogram,
+                fastOptions);
+
+            lock (_mutex)
+            {
+                scanMatcher.FastCorrelativeScanMatcher = matcher;
+            }
+        });
+
+        scanMatcher.CreationTaskHandle = _threadPool.Schedule(scanMatcherTask);
+
+        return scanMatcher;
+    }
+
+    /// <summary>
+    /// Computes a constraint between a node and submap.
+    /// Match C++ ComputeConstraint (constraint_builder_3d.cc line 200-305)
+    /// </summary>
+    private void ComputeConstraint(
+        SubmapId submapId,
+        NodeId nodeId,
+        bool matchFullSubmap,
+        TrajectoryNode.Data constantData,
+        Rigid3d globalNodePose,
+        Rigid3d globalSubmapPose,
+        SubmapScanMatcher scanMatcher,
+        int constraintIndex)
+    {
+        // Get the scan matcher (should be ready by now due to task dependency)
+        RealTimeCorrelativeScanMatcher3D? fastMatcher;
+        lock (_mutex)
+        {
+            if (scanMatcher.FastCorrelativeScanMatcher == null)
+            {
+                return; // Scan matcher not ready (shouldn't happen with proper dependencies)
+            }
+            fastMatcher = scanMatcher.FastCorrelativeScanMatcher;
+        }
+
+        if (scanMatcher.HighResolutionHybridGrid == null)
+            return;
+
+        var pointCloud = constantData.HighResolutionPointCloud;
+        if (pointCloud == null || pointCloud.Count == 0)
+            return;
+
+        // Step 1: Fast correlative scan matching for initial estimate
+        FastCorrelativeScanMatcher3DResult? matchResult;
+        if (matchFullSubmap)
+        {
+            matchResult = fastMatcher.MatchFullSubmap(
+                globalNodePose.Rotation,
+                globalSubmapPose.Rotation,
+                constantData,
+                _options.GlobalLocalizationMinScore);
+        }
+        else
+        {
+            matchResult = fastMatcher.Match(
+                globalNodePose,
+                globalSubmapPose,
+                constantData,
+                _options.MinScore);
+        }
+
+        if (matchResult == null)
+            return; // Score too low
+
+        var poseEstimate = matchResult.Value.PoseEstimate;
+
+        // Step 2: Refine with Ceres scan matcher
+        if (_ceresScanMatcher != null)
+        {
+            var pointCloudsAndGrids = new List<PointCloudAndHybridGridsPointers>();
+
+            // Add high resolution point cloud and grid
+            if (scanMatcher.HighResolutionHybridGrid != null)
+            {
+                pointCloudsAndGrids.Add(new PointCloudAndHybridGridsPointers
+                {
+                    PointCloud = pointCloud,
+                    HybridGrid = scanMatcher.HighResolutionHybridGrid,
+                    IntensityHybridGrid = scanMatcher.HighResolutionIntensityHybridGrid
+                });
+            }
+
+            // Add low resolution point cloud and grid if available
+            if (scanMatcher.LowResolutionHybridGrid != null && constantData.LowResolutionPointCloud != null)
+            {
+                pointCloudsAndGrids.Add(new PointCloudAndHybridGridsPointers
+                {
+                    PointCloud = constantData.LowResolutionPointCloud,
+                    HybridGrid = scanMatcher.LowResolutionHybridGrid,
+                    IntensityHybridGrid = null
+                });
+            }
+
+            if (pointCloudsAndGrids.Count > 0)
+            {
+                CeresSharp.SolverSummary? summary = null;
+                try
+                {
+                    _ceresScanMatcher.Match(
+                        poseEstimate.Translation,
+                        poseEstimate,
+                        pointCloudsAndGrids,
+                        out poseEstimate,
+                        out summary
+                    );
+                }
+                finally
+                {
+                    summary?.Dispose();
+                }
+            }
+        }
+
+        // Step 3: Create constraint
+        // CRITICAL FIX: Match C++ (constraint_builder_3d.cc line 303-304)
+        // constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate
+        // poseEstimate is in global frame, constraint must be relative to submap's local frame
+        var constraintTransform = globalSubmapPose.Inverse() * poseEstimate;
+        var constraint = new IPoseGraph.Constraint(
+            submapId,
+            nodeId,
+            new IPoseGraph.Constraint.Pose(
+                constraintTransform,
+                _options.LoopClosureTranslationWeight,
+                _options.LoopClosureRotationWeight
+            ),
+            IPoseGraph.Constraint.Tag.InterSubmap,
+            matchResult.Value.Score,
+            IPoseGraph.Constraint.State.Enabled
+        );
+
+        // Store constraint at the pre-allocated index
+        lock (_mutex)
+        {
+            _pendingConstraints[constraintIndex] = constraint;
+        }
+    }
+
+    public void Dispose()
+    {
+        if (!_disposed)
+        {
+            _ceresScanMatcher?.Dispose();
+            _disposed = true;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/GlobalTrajectoryBuilder.cs

@@ -0,0 +1,36 @@
+/*
+ * 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.
+ */
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Corresponds to C++ <c>internal/global_trajectory_builder.cc</c> (single file in <c>internal/</c>).
+/// <para>
+/// C++ has one template class <c>GlobalTrajectoryBuilder&lt;LocalTrajectoryBuilder, PoseGraph&gt;</c>
+/// and two factory functions: <c>CreateGlobalTrajectoryBuilder2D</c>, <c>CreateGlobalTrajectoryBuilder3D</c>.
+/// </para>
+/// <para>
+/// C# has no templates, so the logic is split into two classes that mirror the template instantiations:
+/// <list type="bullet">
+///   <item><c>GlobalTrajectoryBuilder2D</c> in <c>Internal/2D/GlobalTrajectoryBuilder2D.cs</c> — corresponds to <c>GlobalTrajectoryBuilder&lt;LocalTrajectoryBuilder2D, PoseGraph2D&gt;</c></item>
+///   <item><c>GlobalTrajectoryBuilder3D</c> in <c>Internal/3D/GlobalTrajectoryBuilder3D.cs</c> — corresponds to <c>GlobalTrajectoryBuilder&lt;LocalTrajectoryBuilder3D, PoseGraph3D&gt;</c></item>
+/// </list>
+/// Creation is performed in <see cref="MapBuilder.AddTrajectoryBuilder"/>, equivalent to C++ MapBuilder::AddTrajectory calling CreateGlobalTrajectoryBuilder2D/3D.
+/// </para>
+/// </summary>
+public static class GlobalTrajectoryBuilder
+{
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/MotionFilter.cs

@@ -0,0 +1,67 @@
+/*
+ * 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.Mapping;
+using CartographerSharp.Transform;
+using static CartographerSharp.Transform.TransformOperations;
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Takes poses as input and filters them to get fewer poses.
+/// </summary>
+public class MotionFilter(MotionFilterOptions options)
+{
+    private int _numTotal;
+    private int _numDifferent;
+    private long _lastTime; // Universal Time Scale ticks
+    private Rigid3d _lastPose = Rigid3d.Identity;
+
+    /// <summary>
+    /// If the accumulated motion (linear, rotational, or time) is above the
+    /// threshold, returns false. Otherwise the relative motion is accumulated and
+    /// true is returned.
+    /// </summary>
+    public bool IsSimilar(long time, Rigid3d pose)
+    {
+        _numTotal++;
+        
+        // Match C++ logic: Check all conditions in one if statement for short-circuit evaluation
+        // C++: if (num_total_ > 1 && time - last_time_ <= ... && translation <= ... && rotation <= ...)
+        if (_numTotal > 1)
+        {
+            var timeDelta = (time - _lastTime) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)
+            var translationDelta = (pose.Translation - _lastPose.Translation).Length();
+            var rotationDelta = GetAngle(pose.Inverse() * _lastPose);
+            
+            // Match C++: All conditions must be true (using && for short-circuit evaluation)
+            if (timeDelta <= options.MaxTimeSeconds &&
+                translationDelta <= options.MaxDistanceMeters &&
+                rotationDelta <= options.MaxAngleRadians)
+            {
+                // Motion is similar - return true without updating last_time_ and last_pose_
+                // (matches C++ behavior where last_time_ and last_pose_ are only updated when returning false)
+                return true;
+            }
+        }
+        
+        // Motion is NOT similar - update last_time_ and last_pose_ (match C++ lines 57-59)
+        _lastTime = time;
+        _lastPose = pose;
+        _numDifferent++;
+        return false;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Optimization/CostHelpers.cs

@@ -0,0 +1,199 @@
+/*
+ * 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.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Helper functions for cost function computation.
+/// </summary>
+internal static class CostHelpers
+{
+    /// <summary>
+    /// Computes spherical linear interpolation of unit quaternions.
+    /// </summary>
+    public static Quaternion SlerpQuaternions(Quaternion start, Quaternion end, double factor)
+    {
+        // Normalize quaternions
+        start = Quaternion.Normalize(start);
+        end = Quaternion.Normalize(end);
+
+        // Compute dot product
+        var cosTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
+        // Clamp to [-1, 1] to handle floating-point errors that could cause Math.Acos to return NaN
+        var absCosTheta = Math.Min(1.0, Math.Abs(cosTheta));
+
+        // If quaternions are nearly collinear, use linear interpolation
+        const double kEpsilon = 1e-6;
+        double prevScale = 1.0 - factor;
+        double nextScale = factor;
+
+        if (absCosTheta < 1.0 - kEpsilon)
+        {
+            var theta = Math.Acos(absCosTheta);
+            var sinTheta = Math.Sin(theta);
+            if (sinTheta > kEpsilon)
+            {
+                prevScale = Math.Sin((1.0 - factor) * theta) / sinTheta;
+                nextScale = Math.Sin(factor * theta) / sinTheta;
+            }
+        }
+
+        if (cosTheta < 0.0)
+        {
+            nextScale = -nextScale;
+        }
+
+        // Quaternion constructor is (x, y, z, w), matching C++ output format [w, x, y, z]
+        // but converting to C# Quaternion format (x, y, z, w)
+        var result = new Quaternion(
+            prevScale * start.X + nextScale * end.X,
+            prevScale * start.Y + nextScale * end.Y,
+            prevScale * start.Z + nextScale * end.Z,
+            prevScale * start.W + nextScale * end.W
+        );
+        // Normalize to ensure unit quaternion (Eigen SLERP automatically normalizes)
+        return Quaternion.Normalize(result);
+    }
+
+    /// <summary>
+    /// Interpolates 3D nodes.
+    /// </summary>
+    public static (Quaternion rotation, Vector3 translation) InterpolateNodes3D(
+        double[] prevNodeRotation, // [w, x, y, z]
+        double[] prevNodeTranslation, // [x, y, z]
+        double[] nextNodeRotation, // [w, x, y, z]
+        double[] nextNodeTranslation, // [x, y, z]
+        double interpolationParameter)
+    {
+        // Match C++: prev_node_rotation is [w, x, y, z]
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        var prevQuaternion = new Quaternion(
+            prevNodeRotation[1], // x
+            prevNodeRotation[2], // y
+            prevNodeRotation[3], // z
+            prevNodeRotation[0]  // w
+        );
+        var nextQuaternion = new Quaternion(
+            nextNodeRotation[1], // x
+            nextNodeRotation[2], // y
+            nextNodeRotation[3], // z
+            nextNodeRotation[0]  // w
+        );
+
+        // Interpolate rotation using SLERP
+        var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);
+
+        // Interpolate translation linearly
+        var interpolatedTranslation = new Vector3(
+            (prevNodeTranslation[0] + interpolationParameter * (nextNodeTranslation[0] - prevNodeTranslation[0])),
+            (prevNodeTranslation[1] + interpolationParameter * (nextNodeTranslation[1] - prevNodeTranslation[1])),
+            (prevNodeTranslation[2] + interpolationParameter * (nextNodeTranslation[2] - prevNodeTranslation[2]))
+        );
+
+        return (interpolatedRotation, interpolatedTranslation);
+    }
+
+    /// <summary>
+    /// Interpolates 2D nodes embedded in 3D space.
+    /// </summary>
+    public static (Quaternion rotation, Vector3 translation) InterpolateNodes2D(
+        double[] prevNodePose, // [x, y, theta]
+        Quaternion prevNodeGravityAlignment,
+        double[] nextNodePose, // [x, y, theta]
+        Quaternion nextNodeGravityAlignment,
+        double interpolationParameter)
+    {
+        // Embed 2D pose into 3D with gravity alignment
+        // Equivalent to: Embed3D(prev_node_pose) * Rigid3d::Rotation(prev_node_gravity_alignment)
+        var prevRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, prevNodePose[2]);
+        var prevQuaternion = Quaternion.Normalize(prevRotation2D * prevNodeGravityAlignment);
+
+        var nextRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, nextNodePose[2]);
+        var nextQuaternion = Quaternion.Normalize(nextRotation2D * nextNodeGravityAlignment);
+
+        // Interpolate rotation using SLERP
+        var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);
+
+        // Interpolate translation linearly (2D, z=0)
+        var interpolatedTranslation = new Vector3(
+            (prevNodePose[0] + interpolationParameter * (nextNodePose[0] - prevNodePose[0])),
+            (prevNodePose[1] + interpolationParameter * (nextNodePose[1] - prevNodePose[1])),
+            0.0
+        );
+
+        return (interpolatedRotation, interpolatedTranslation);
+    }
+
+    /// <summary>
+    /// Computes unscaled error for 3D poses.
+    /// Error = observed_relative_pose - computed_relative_pose
+    /// </summary>
+    public static double[] ComputeUnscaledError3D(
+        Rigid3d observedRelativePose,
+        Quaternion startRotation,
+        Vector3 startTranslation,
+        Quaternion endRotation,
+        Vector3 endTranslation)
+    {
+        // Compute relative transform: start^-1 * end
+        var startInverse = Quaternion.Inverse(startRotation);
+        var deltaTranslation = endTranslation - startTranslation;
+        var rotatedDelta = Vector3.Transform(deltaTranslation, startInverse);
+
+        // Compute h_rotation_inverse = (end^-1) * start (matching C++ implementation)
+        // This is equivalent to: endRotation.Inverse() * startRotation
+        var endInverse = Quaternion.Inverse(endRotation);
+        var hRotationInverse = endInverse * startRotation;
+
+        // Error rotation: h_rotation_inverse * observed_relative_rotation
+        var errorRotation = hRotationInverse * observedRelativePose.Rotation;
+
+        // Convert rotation error to angle-axis
+        var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(errorRotation);
+
+        return
+        [
+            observedRelativePose.Translation.X - rotatedDelta.X,
+            observedRelativePose.Translation.Y - rotatedDelta.Y,
+            observedRelativePose.Translation.Z - rotatedDelta.Z,
+            angleAxis.X,
+            angleAxis.Y,
+            angleAxis.Z
+        ];
+    }
+
+    /// <summary>
+    /// Scales error with translation and rotation weights.
+    /// </summary>
+    public static double[] ScaleError3D(
+        double[] unscaledError,
+        double translationWeight,
+        double rotationWeight)
+    {
+        return
+        [
+            translationWeight * unscaledError[0],
+            translationWeight * unscaledError[1],
+            translationWeight * unscaledError[2],
+            rotationWeight * unscaledError[3],
+            rotationWeight * unscaledError[4],
+            rotationWeight * unscaledError[5]
+        ];
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Optimization/LandmarkCostFunction2D.cs

@@ -0,0 +1,124 @@
+/*
+ * 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 CeresSharp;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Cost function measuring the weighted error between the observed pose given by
+/// the landmark measurement and the linearly interpolated pose of embedded in 3D
+/// space node poses.
+/// </summary>
+public class LandmarkCostFunction2D
+{
+    private readonly IPoseGraph.LandmarkNode.LandmarkObservation _observation;
+    private readonly NodeSpec2D _prevNode;
+    private readonly NodeSpec2D _nextNode;
+    private readonly double _interpolationParameter;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for landmark constraints.
+    /// </summary>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec2D prevNode,
+        NodeSpec2D nextNode)
+    {
+        var costFunction = new LandmarkCostFunction2D(observation, prevNode, nextNode);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz]
+            parameterBlockSizes: [3, 3, 4, 3] // [prev_node[3], next_node[3], landmark_rotation[4], landmark_translation[3]]
+        );
+    }
+
+    private LandmarkCostFunction2D(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec2D prevNode,
+        NodeSpec2D nextNode)
+    {
+        _observation = observation;
+        _prevNode = prevNode;
+        _nextNode = nextNode;
+
+        // Compute interpolation parameter
+        _interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
+            _observation.Time,
+            _prevNode.Time,
+            _nextNode.Time
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 4)
+            return false;
+        if (parameters[0].Length < 3 || parameters[1].Length < 3 ||
+            parameters[2].Length < 4 || parameters[3].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 6)
+            return false;
+
+        var prevNodePose = parameters[0]; // [x, y, theta]
+        var nextNodePose = parameters[1]; // [x, y, theta]
+        var landmarkRotation = parameters[2]; // [w, x, y, z]
+        var landmarkTranslation = parameters[3]; // [x, y, z]
+
+        // Interpolate node poses
+        var (interpolatedRotation, interpolatedTranslation) = CostHelpers.InterpolateNodes2D(
+            prevNodePose,
+            _prevNode.GravityAlignment,
+            nextNodePose,
+            _nextNode.GravityAlignment,
+            _interpolationParameter
+        );
+
+        // Landmark pose parameters
+        var landmarkRotationQuat = OptimizationHelpers.ParametersToQuaternion(landmarkRotation);
+        var landmarkTranslationVec = OptimizationHelpers.ParametersToVector3(landmarkTranslation);
+
+        // The landmark cost function computes error between:
+        // - observed: landmark_to_tracking_transform (from observation)
+        // - computed: (interpolated_tracking_pose^-1 * landmark_pose)
+        // Error = observed - computed
+        // This is equivalent to: landmark_to_tracking_transform - (interpolated_pose^-1 * landmark_pose)
+        var unscaledError = CostHelpers.ComputeUnscaledError3D(
+            _observation.LandmarkToTrackingTransform,
+            interpolatedRotation,
+            interpolatedTranslation,
+            landmarkRotationQuat,
+            landmarkTranslationVec
+        );
+
+        // Scale error
+        var scaledError = CostHelpers.ScaleError3D(
+            unscaledError,
+            _observation.TranslationWeight,
+            _observation.RotationWeight
+        );
+
+        for (int i = 0; i < 6; i++)
+        {
+            residuals[i] = scaledError[i];
+        }
+
+        return true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Optimization/LandmarkCostFunction3D.cs

@@ -0,0 +1,122 @@
+/*
+ * 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.Mapping.Internal.D3D.Optimization;
+using CeresSharp;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Cost function measuring the weighted error between the observed pose given by
+/// the landmark measurement and the linearly interpolated pose.
+/// </summary>
+public class LandmarkCostFunction3D
+{
+    private readonly IPoseGraph.LandmarkNode.LandmarkObservation _observation;
+    private readonly NodeSpec3D _prevNode;
+    private readonly NodeSpec3D _nextNode;
+    private readonly double _interpolationParameter;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for landmark constraints in 3D.
+    /// </summary>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec3D prevNode,
+        NodeSpec3D nextNode)
+    {
+        var costFunction = new LandmarkCostFunction3D(observation, prevNode, nextNode);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz]
+            parameterBlockSizes: [4, 3, 4, 3, 4, 3] // [prev_rotation[4], prev_translation[3], next_rotation[4], next_translation[3], landmark_rotation[4], landmark_translation[3]]
+        );
+    }
+
+    private LandmarkCostFunction3D(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec3D prevNode,
+        NodeSpec3D nextNode)
+    {
+        _observation = observation;
+        _prevNode = prevNode;
+        _nextNode = nextNode;
+
+        // Compute interpolation parameter
+        _interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
+            _observation.Time,
+            _prevNode.Time,
+            _nextNode.Time
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 6)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 3 ||
+            parameters[2].Length < 4 || parameters[3].Length < 3 ||
+            parameters[4].Length < 4 || parameters[5].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 6)
+            return false;
+
+        var prevNodeRotation = parameters[0]; // [w, x, y, z]
+        var prevNodeTranslation = parameters[1]; // [x, y, z]
+        var nextNodeRotation = parameters[2]; // [w, x, y, z]
+        var nextNodeTranslation = parameters[3]; // [x, y, z]
+        var landmarkRotation = parameters[4]; // [w, x, y, z]
+        var landmarkTranslation = parameters[5]; // [x, y, z]
+
+        // Interpolate node poses
+        var (interpolatedRotationQuat, interpolatedTranslationVec) = CostHelpers.InterpolateNodes3D(
+            prevNodeRotation,
+            prevNodeTranslation,
+            nextNodeRotation,
+            nextNodeTranslation,
+            _interpolationParameter
+        );
+
+        var landmarkRotationQuat = OptimizationHelpers.ParametersToQuaternion(landmarkRotation);
+        var landmarkTranslationVec = OptimizationHelpers.ParametersToVector3(landmarkTranslation);
+
+        // Compute error
+        var unscaledError = CostHelpers.ComputeUnscaledError3D(
+            _observation.LandmarkToTrackingTransform,
+            interpolatedRotationQuat,
+            interpolatedTranslationVec,
+            landmarkRotationQuat,
+            landmarkTranslationVec
+        );
+
+        // Scale error
+        var scaledError = CostHelpers.ScaleError3D(
+            unscaledError,
+            _observation.TranslationWeight,
+            _observation.RotationWeight
+        );
+
+        for (int i = 0; i < 6; i++)
+        {
+            residuals[i] = scaledError[i];
+        }
+
+        return true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Optimization/OptimizationHelpers.cs

@@ -0,0 +1,164 @@
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Helper utilities for optimization problems.
+/// Provides common operations for pose parameter conversion and angle normalization.
+/// </summary>
+public static class OptimizationHelpers
+{
+    /// <summary>
+    /// Normalizes angle difference to [-pi, pi].
+    /// Uses modulo-based approach for efficiency with large angles.
+    /// </summary>
+    /// <param name="angle">The angle to normalize.</param>
+    /// <returns>Normalized angle in [-pi, pi].</returns>
+    public static double NormalizeAngleDifference(double angle)
+    {
+        // Use modulo for efficiency - handles large angles in O(1)
+        const double twoPi = 2.0 * Math.PI;
+        angle = angle % twoPi;
+        if (angle > Math.PI)
+            angle -= twoPi;
+        else if (angle < -Math.PI)
+            angle += twoPi;
+        return angle;
+    }
+
+    /// <summary>
+    /// Converts Rigid2d pose to parameter array [x, y, theta].
+    /// </summary>
+    /// <param name="pose">The 2D pose.</param>
+    /// <returns>Parameter array [x, y, theta].</returns>
+    public static double[] Rigid2dToParameters(Rigid2d pose) => [ pose.Translation.X, pose.Translation.Y, pose.Rotation ];
+
+    /// <summary>
+    /// Converts parameter array [x, y, theta] to Rigid2d pose.
+    /// </summary>
+    /// <param name="parameters">Parameter array [x, y, theta].</param>
+    /// <returns>The 2D pose.</returns>
+    public static Rigid2d ParametersToRigid2d(double[] parameters)
+    {
+        if (parameters == null || parameters.Length < 3)
+            throw new ArgumentException("Parameters array must have at least 3 elements", nameof(parameters));
+
+        return new Rigid2d(
+            new Vector2(parameters[0], parameters[1]),
+            parameters[2]
+        );
+    }
+
+    /// <summary>
+    /// Converts Rigid3d pose to parameter arrays (rotation and translation).
+    /// </summary>
+    /// <param name="pose">The 3D pose.</param>
+    /// <returns>Tuple of (rotation[4], translation[3]).</returns>
+    public static (double[] rotation, double[] translation) Rigid3dToParameters(Rigid3d pose)
+    {
+        var rotation = new double[4]
+        {
+            pose.Rotation.W,
+            pose.Rotation.X,
+            pose.Rotation.Y,
+            pose.Rotation.Z
+        };
+        var translation = new double[3]
+        {
+            pose.Translation.X,
+            pose.Translation.Y,
+            pose.Translation.Z
+        };
+        return (rotation, translation);
+    }
+
+    /// <summary>
+    /// Converts parameter arrays to Rigid3d pose.
+    /// </summary>
+    /// <param name="rotation">Rotation parameters [w, x, y, z].</param>
+    /// <param name="translation">Translation parameters [x, y, z].</param>
+    /// <returns>The 3D pose.</returns>
+    public static Rigid3d ParametersToRigid3d(double[] rotation, double[] translation)
+    {
+        if (rotation == null || rotation.Length < 4)
+            throw new ArgumentException("Rotation array must have at least 4 elements", nameof(rotation));
+        if (translation == null || translation.Length < 3)
+            throw new ArgumentException("Translation array must have at least 3 elements", nameof(translation));
+
+        // Convert from [w, x, y, z] to (x, y, z, w) for System.Numerics.Quaternion
+        return new Rigid3d(
+            new Vector3(translation[0], translation[1], translation[2]),
+            new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
+        );
+    }
+
+    /// <summary>
+    /// Converts Quaternion to parameter array [w, x, y, z].
+    /// </summary>
+    /// <param name="quaternion">The quaternion.</param>
+    /// <returns>Parameter array [w, x, y, z].</returns>
+    public static double[] QuaternionToParameters(Quaternion quaternion) => [ quaternion.W, quaternion.X, quaternion.Y, quaternion.Z ];
+
+    /// <summary>
+    /// Converts parameter array [w, x, y, z] to System.Numerics.Quaternion (x, y, z, w).
+    /// Match C++: Eigen::Quaternion<T> uses (w, x, y, z) format.
+    /// System.Numerics.Quaternion uses (x, y, z, w) format.
+    /// </summary>
+    /// <param name="parameters">Parameter array [w, x, y, z].</param>
+    /// <returns>The quaternion.</returns>
+    public static Quaternion ParametersToQuaternion(double[] parameters)
+    {
+        if (parameters == null || parameters.Length < 4)
+            throw new ArgumentException("Parameters array must have at least 4 elements", nameof(parameters));
+
+        // Convert from [w, x, y, z] to (x, y, z, w)
+        return new Quaternion(
+            parameters[1], // x
+            parameters[2], // y
+            parameters[3], // z
+            parameters[0]  // w
+        );
+    }
+
+    /// <summary>
+    /// Converts Vector3 to parameter array [x, y, z].
+    /// </summary>
+    /// <param name="vector">The vector.</param>
+    /// <returns>Parameter array [x, y, z].</returns>
+    public static double[] Vector3ToParameters(Vector3 vector) => [ vector.X, vector.Y, vector.Z ];
+
+    /// <summary>
+    /// Converts parameter array [x, y, z] to Vector3.
+    /// </summary>
+    /// <param name="parameters">Parameter array [x, y, z].</param>
+    /// <returns>The vector.</returns>
+    public static Vector3 ParametersToVector3(double[] parameters)
+    {
+        if (parameters == null || parameters.Length < 3)
+            throw new ArgumentException("Parameters array must have at least 3 elements", nameof(parameters));
+
+        return new Vector3(
+            parameters[0],
+            parameters[1],
+            parameters[2]
+        );
+    }
+
+    /// <summary>
+    /// Computes interpolation parameter for time-based interpolation.
+    /// </summary>
+    /// <param name="observationTime">The observation time.</param>
+    /// <param name="prevTime">The previous node time.</param>
+    /// <param name="nextTime">The next node time.</param>
+    /// <returns>Interpolation parameter in [0, 1].</returns>
+    public static double ComputeInterpolationParameter(long observationTime, long prevTime, long nextTime)
+    {
+        var timeDiff = nextTime - prevTime;
+        if (timeDiff == 0)
+            return 0.0;
+        // Cast to double to avoid integer division
+        return (double)(observationTime - prevTime) / timeDiff;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Optimization/SpaCostFunction2D.cs

@@ -0,0 +1,187 @@
+/*
+ * 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.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Sparse Pose Adjustment (SPA) cost function for 2D pose graph optimization.
+/// Computes the error between observed relative pose and computed relative pose.
+/// </summary>
+public class SpaCostFunction2D
+{
+    private readonly IPoseGraph.Constraint.Pose _observedRelativePose;
+    private readonly Rigid2d _observedRelativePose2D;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for SPA.
+    /// </summary>
+    /// <param name="observedRelativePose">The observed relative pose constraint.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        var costFunction = new SpaCostFunction2D(observedRelativePose);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 3, // [dx, dy, dtheta]
+            parameterBlockSizes: [3, 3] // [start_pose[3], end_pose[3]]
+        );
+    }
+
+    private SpaCostFunction2D(IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        _observedRelativePose = observedRelativePose;
+        // Project 3D pose to 2D
+        _observedRelativePose2D = TransformOperations.Project2D(observedRelativePose.ZbarIj);
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// Match C++ spa_cost_function_2d.h operator() implementation.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [start_pose[3], end_pose[3]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dtheta].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 2)
+        {
+            return false;
+        }
+        if (parameters[0].Length < 3 || parameters[1].Length < 3)
+        {
+            return false;
+        }
+        if (residuals == null || residuals.Length < 3)
+        {
+            return false;
+        }
+
+        var startPose = parameters[0];
+        var endPose = parameters[1];
+        
+        // Validate parameters for NaN/Infinity
+        for (int i = 0; i < 3; i++)
+        {
+            if (double.IsNaN(startPose[i]) || double.IsInfinity(startPose[i]))
+            {
+                return false;
+            }
+            if (double.IsNaN(endPose[i]) || double.IsInfinity(endPose[i]))
+            {
+                return false;
+            }
+        }
+        
+        // NOTE: Weight validation removed to match C++ behavior.
+        // C++ does not validate weights - Ceres handles invalid weights internally.
+        // Validation was causing constraints to be incorrectly rejected.
+
+        // NOTE: Pose explosion handling REMOVED to match C++ behavior.
+        // The original C++ spa_cost_function_2d.h does NOT have any pose distance checks.
+        // Returning zero residuals was causing optimization to skip constraints incorrectly,
+        // leading to optimization failures and incorrect pose graph results.
+        // If poses diverge, Ceres will handle it through its own convergence criteria.
+
+        // Compute unscaled error (match C++ cost_helpers_impl.h ComputeUnscaledError)
+        var unscaledError = ComputeUnscaledError(
+            _observedRelativePose2D,
+            startPose,
+            endPose
+        );
+
+        // Scale error with weights (match C++ ScaleError)
+        var translationWeight = _observedRelativePose.TranslationWeight;
+        var rotationWeight = _observedRelativePose.RotationWeight;
+
+        var scaledError = ScaleError(
+            unscaledError,
+            translationWeight,
+            rotationWeight
+        );
+
+        residuals[0] = scaledError[0];
+        residuals[1] = scaledError[1];
+        residuals[2] = scaledError[2];
+
+        return true;
+    }
+
+    /// <summary>
+    /// Computes unscaled error between observed and computed relative pose.
+    /// Match C++: Uses direct formula for numerical stability with Ceres autodiff.
+    /// </summary>
+    private static double[] ComputeUnscaledError(
+        Rigid2d observedRelativePose,
+        double[] startPose,
+        double[] endPose)
+    {
+        // Match C++ implementation in cost_helpers_impl.h
+        // startPose = [x1, y1, theta1]
+        // endPose = [x2, y2, theta2]
+        // observedRelativePose = relative pose from start to end (in start frame)
+
+        var cosThetaI = Math.Cos(startPose[2]);
+        var sinThetaI = Math.Sin(startPose[2]);
+        var deltaX = endPose[0] - startPose[0];
+        var deltaY = endPose[1] - startPose[1];
+        
+        // Compute h = relative pose from start to end (in start frame)
+        // h[0] = cos_theta_i * delta_x + sin_theta_i * delta_y
+        // h[1] = -sin_theta_i * delta_x + cos_theta_i * delta_y
+        // h[2] = end[2] - start[2]
+        var h0 = cosThetaI * deltaX + sinThetaI * deltaY;
+        var h1 = -sinThetaI * deltaX + cosThetaI * deltaY;
+        var h2 = endPose[2] - startPose[2];
+
+        // Error = observed - computed
+        var translationErrorX = observedRelativePose.Translation.X - h0;
+        var translationErrorY = observedRelativePose.Translation.Y - h1;
+        
+        // Rotation error (normalize angle difference)
+        var rotationError = OptimizationHelpers.NormalizeAngleDifference(
+            observedRelativePose.Rotation - h2
+        );
+
+        return
+        [
+            translationErrorX,
+            translationErrorY,
+            rotationError
+        ];
+    }
+
+    /// <summary>
+    /// Scales error with translation and rotation weights.
+    /// </summary>
+    private static double[] ScaleError(
+        double[] unscaledError,
+        double translationWeight,
+        double rotationWeight)
+    {
+        return
+        [
+            translationWeight * unscaledError[0],
+            translationWeight * unscaledError[1],
+            rotationWeight * unscaledError[2]
+        ];
+    }
+
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/RangeDataCollator.cs

@@ -0,0 +1,270 @@
+/*
+ * 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.Sensor;
+using System.Globalization;
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Synchronizes TimedPointCloudData from different sensors. Input needs only be
+/// monotonous in 'TimedPointCloudData::time', output is monotonous in per-point
+/// timing. Up to one message per sensor is buffered, so a delay of the period of
+/// the slowest sensor may be introduced, which can be alleviated by passing
+/// subdivisions.
+/// </summary>
+public class RangeDataCollator(IEnumerable<string> expectedRangeSensorIds)
+{
+    private const double kDefaultIntensityValue = 0.0;
+
+    private readonly HashSet<string> _expectedSensorIds = [.. expectedRangeSensorIds];
+    private readonly Dictionary<string, TimedPointCloudData> _idToPendingData = [];
+    private long _currentStart = long.MinValue; // Universal Time Scale ticks
+    private long _currentEnd = long.MinValue; // Universal Time Scale ticks
+
+    // Debug: Track per-sensor timestamps to detect out-of-order data
+    private readonly Dictionary<string, long> _lastSensorTimestamp = [];
+    private static readonly object _collatorLogLock = new();
+    private static readonly string _collatorLogPath = "collator.log";
+    private long _lastOutputTime = long.MinValue;
+
+    private static void LogCollator(string message)
+    {
+        lock (_collatorLogLock)
+        {
+            try
+            {
+                var timestamp = DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff", CultureInfo.InvariantCulture);
+                var line = $"{timestamp}|{message}";
+                File.AppendAllText(_collatorLogPath, line + Environment.NewLine);
+            }
+            catch { /* Ignore logging errors */ }
+        }
+    }
+
+    /// <summary>
+    /// If timed_point_cloud_data has incomplete intensity data, we will fill the
+    /// missing intensities with kDefaultIntensityValue.
+    /// </summary>
+    public TimedPointCloudOriginData AddRangeData(string sensorId, TimedPointCloudData timedPointCloudData)
+    {
+        if (!_expectedSensorIds.Contains(sensorId))
+        {
+            throw new ArgumentException($"Unexpected sensor ID: {sensorId}", nameof(sensorId));
+        }
+
+        // DEBUG: Timestamp validation - check if input is monotonic per sensor
+        var currentTime = timedPointCloudData.Time;
+        var tickMs = currentTime / TimeSpan.TicksPerMillisecond;
+        if (_lastSensorTimestamp.TryGetValue(sensorId, out var lastTime))
+        {
+            if (currentTime < lastTime)
+            {
+                var diffMs = (lastTime - currentTime) / (double)TimeSpan.TicksPerMillisecond;
+                LogCollator($"WARNING|sensor={sensorId}|TIME_REVERSAL|prev_tick={lastTime / TimeSpan.TicksPerMillisecond}|curr_tick={tickMs}|diff={diffMs:F3}ms");
+            }
+            else
+            {
+                var deltaMs = (currentTime - lastTime) / (double)TimeSpan.TicksPerMillisecond;
+                // Log normal data flow (can comment out for less verbose logging)
+                // LogCollator($"INFO|sensor={sensorId}|tick={tickMs}|delta={deltaMs:F3}ms|points={timedPointCloudData.Ranges.Count}");
+            }
+        }
+        _lastSensorTimestamp[sensorId] = currentTime;
+
+        // Fill missing intensities
+        // Match C++: timed_point_cloud_data.intensities.resize(
+        //     timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
+        // This resizes to exactly ranges.size(), filling with kDefaultIntensityValue if needed,
+        // or truncating if intensities is larger than ranges
+        if (timedPointCloudData.Intensities.Count != timedPointCloudData.Ranges.Count)
+        {
+            var intensities = new List<double>(timedPointCloudData.Intensities);
+            // Resize to match ranges.Count exactly
+            if (intensities.Count < timedPointCloudData.Ranges.Count)
+            {
+                // Fill missing with default value
+                while (intensities.Count < timedPointCloudData.Ranges.Count)
+                {
+                    intensities.Add(kDefaultIntensityValue);
+                }
+            }
+            else if (intensities.Count > timedPointCloudData.Ranges.Count)
+            {
+                // Truncate if larger
+                intensities.RemoveRange(timedPointCloudData.Ranges.Count, intensities.Count - timedPointCloudData.Ranges.Count);
+            }
+            timedPointCloudData.Intensities = intensities;
+        }
+
+        if (_idToPendingData.TryGetValue(sensorId, out TimedPointCloudData value))
+        {
+            _currentStart = _currentEnd;
+            _currentEnd = value.Time;
+            var result = CropAndMerge();
+            _idToPendingData[sensorId] = timedPointCloudData;
+            return result;
+        }
+
+        _idToPendingData[sensorId] = timedPointCloudData;
+        
+        if (_expectedSensorIds.Count != _idToPendingData.Count)
+        {
+            return new TimedPointCloudOriginData(0, [], []);
+        }
+
+        _currentStart = _currentEnd;
+        // We have messages from all sensors, move forward to oldest.
+        var oldestTimestamp = _idToPendingData.Values.Min(d => d.Time);
+        _currentEnd = oldestTimestamp;
+        return CropAndMerge();
+    }
+
+    private TimedPointCloudOriginData CropAndMerge()
+    {
+        var result = new TimedPointCloudOriginData(_currentEnd, [], []);
+
+        // DEBUG: Check if output time is monotonic
+        var outputTickMs = _currentEnd / TimeSpan.TicksPerMillisecond;
+        if (_lastOutputTime != long.MinValue && _currentEnd < _lastOutputTime)
+        {
+            var diffMs = (_lastOutputTime - _currentEnd) / (double)TimeSpan.TicksPerMillisecond;
+            LogCollator($"WARNING|OUTPUT_TIME_REVERSAL|prev_output={_lastOutputTime / TimeSpan.TicksPerMillisecond}|curr_output={outputTickMs}|diff={diffMs:F3}ms|start={_currentStart / TimeSpan.TicksPerMillisecond}");
+        }
+        _lastOutputTime = _currentEnd;
+
+        var warnedForDroppedPoints = false;
+
+        // Use ToList() to create a snapshot for iteration, but we'll modify _idToPendingData during iteration
+        var sensorIds = _idToPendingData.Keys.ToList();
+        
+        foreach (var sensorId in sensorIds)
+        {
+            if (!_idToPendingData.TryGetValue(sensorId, out var data))
+            {
+                continue; // Already removed
+            }
+            
+            var ranges = data.Ranges;
+            var intensities = data.Intensities;
+
+            // Find overlap range (matching C++ line 69-80)
+            var overlapBegin = 0;
+            while (overlapBegin < ranges.Count)
+            {
+                // Convert seconds to ticks: use double for precision, then round to long
+                // This matches C++: data.time + common::FromSeconds((*overlap_begin).time)
+                var pointTime = data.Time + (long)Math.Round(ranges[overlapBegin].Time * TimeSpan.TicksPerSecond);
+                if (pointTime >= _currentStart)
+                {
+                    break;
+                }
+                overlapBegin++;
+            }
+
+            var overlapEnd = overlapBegin;
+            while (overlapEnd < ranges.Count)
+            {
+                // Convert seconds to ticks: use double for precision, then round to long
+                // This matches C++: data.time + common::FromSeconds((*overlap_end).time)
+                var pointTime = data.Time + (long)Math.Round(ranges[overlapEnd].Time * TimeSpan.TicksPerSecond);
+                if (pointTime > _currentEnd)
+                {
+                    break;
+                }
+                overlapEnd++;
+            }
+
+            if (overlapBegin > 0 && !warnedForDroppedPoints)
+            {
+                // Log warning about dropped points (matching C++ line 81-84)
+                warnedForDroppedPoints = true;
+            }
+
+            // Copy overlapping range (matching C++ line 88-106)
+            if (overlapBegin < overlapEnd)
+            {
+                var originIndex = result.Origins.Count;
+                result.Origins.Add(data.Origin);
+                
+                // CRITICAL FIX: Apply time correction to point_time.time (match C++ line 91-103)
+                // C++: const double time_correction = static_cast<double>(common::ToSeconds(data.time - current_end_));
+                // C++: point.point_time.time += time_correction;
+                // Time correction converts the difference between data.Time and currentEnd from ticks to seconds
+                var timeCorrection = ((data.Time - _currentEnd) / 10_000_000.0); // Convert ticks to seconds (10 million ticks per second)
+                
+                for (int i = overlapBegin; i < overlapEnd; i++)
+                {
+                    // Apply time correction to point time
+                    // Create new TimedRangefinderPoint with corrected time
+                    var correctedPointTime = ranges[i].Time + timeCorrection;
+                    var correctedPoint = new TimedRangefinderPoint(
+                        ranges[i].Position,
+                        correctedPointTime);
+                    
+                    var rangeMeasurement = new TimedPointCloudOriginData.RangeMeasurement(
+                        correctedPoint,
+                        intensities[i],
+                        originIndex);
+                    result.Ranges.Add(rangeMeasurement);
+                }
+            }
+
+            // CRITICAL FIX: Drop buffered points until overlap_end (matching C++ line 108-121)
+            // This prevents reprocessing of already-processed points
+            if (overlapEnd == ranges.Count)
+            {
+                // All points processed, remove entry
+                _idToPendingData.Remove(sensorId);
+            }
+            else if (overlapEnd == 0)
+            {
+                // No points processed, keep entry as is
+                // Continue to next sensor
+            }
+            else
+            {
+                // Some points processed, keep only unprocessed points
+                var remainingRanges = new TimedPointCloud();
+                var remainingIntensities = new List<double>();
+                
+                for (int i = overlapEnd; i < ranges.Count; i++)
+                {
+                    remainingRanges.Add(ranges[i]);
+                    remainingIntensities.Add(intensities[i]);
+                }
+                
+                _idToPendingData[sensorId] = new TimedPointCloudData(
+                    data.Time,
+                    data.Origin,
+                    remainingRanges,
+                    remainingIntensities
+                );
+            }
+        }
+
+        // CRITICAL FIX: Sort ranges by time (match C++ line 124-128)
+        // C++: std::sort(result.ranges.begin(), result.ranges.end(),
+        //                [](const auto& a, const auto& b) { return a.point_time.time < b.point_time.time; });
+        // This ensures output is monotonous in per-point timing as documented
+        if (result.Ranges.Count > 0)
+        {
+            result.Ranges = [.. result.Ranges.OrderBy(r => r.PointTime.Time)];
+        }
+
+        return result;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/TrajectoryConnectivityState.cs

@@ -0,0 +1,117 @@
+/*
+ * Copyright 2017 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.Internal;
+
+/// <summary>
+/// Tracks the connectivity state between trajectories and the last time a global
+/// constraint connected two trajectories.
+///
+/// Compared to ConnectedComponents it tracks additionally the last time that a global
+/// constraint connected two trajectories.
+///
+/// Match C++ TrajectoryConnectivityState (trajectory_connectivity_state.cc)
+/// </summary>
+public class TrajectoryConnectivityState
+{
+    // ConnectedComponents is thread safe
+    private readonly ConnectedComponents _connectedComponents = new();
+
+    // Tracks the last time a direct connection between two trajectories has
+    // been added. The exception is when a connection between two trajectories
+    // connects two formerly unconnected connected components. In this case all
+    // bipartite trajectories entries for these components are updated with the
+    // new connection time.
+    private readonly Dictionary<(int, int), long> _lastConnectionTimeMap = new();
+
+    /// <summary>
+    /// Add a trajectory which is initially connected to only itself.
+    /// </summary>
+    public void Add(int trajectoryId)
+    {
+        _connectedComponents.Add(trajectoryId);
+    }
+
+    /// <summary>
+    /// Connect two trajectories. If either trajectory is untracked, it will be
+    /// tracked. This function is invariant to the order of its arguments. Repeated
+    /// calls to Connect increment the connectivity count and update the last
+    /// connected time.
+    /// </summary>
+    public void Connect(int trajectoryIdA, int trajectoryIdB, long time)
+    {
+        if (TransitivelyConnected(trajectoryIdA, trajectoryIdB))
+        {
+            // The trajectories are transitively connected, i.e. they belong to the same
+            // connected component. In this case we only update the last connection time
+            // of those two trajectories.
+            var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+            if (!_lastConnectionTimeMap.TryGetValue(sortedPair, out var existing) || existing < time)
+            {
+                _lastConnectionTimeMap[sortedPair] = time;
+            }
+        }
+        else
+        {
+            // The connection between these two trajectories is about to join two
+            // connected components. Here we update all bipartite trajectory pairs for
+            // the two connected components with the connection time. This is to quickly
+            // change to a more efficient loop closure search (by constraining the
+            // search window) when connected components are joined.
+            var componentA = _connectedComponents.GetComponent(trajectoryIdA);
+            var componentB = _connectedComponents.GetComponent(trajectoryIdB);
+            foreach (var idA in componentA)
+            {
+                foreach (var idB in componentB)
+                {
+                    var idPair = (Math.Min(idA, idB), Math.Max(idA, idB));
+                    _lastConnectionTimeMap[idPair] = time;
+                }
+            }
+        }
+        _connectedComponents.Connect(trajectoryIdA, trajectoryIdB);
+    }
+
+    /// <summary>
+    /// Determines if two trajectories have been (transitively) connected. If
+    /// either trajectory is not being tracked, returns false, except when it is
+    /// the same trajectory, where it returns true. This function is invariant to
+    /// the order of its arguments.
+    /// </summary>
+    public bool TransitivelyConnected(int trajectoryIdA, int trajectoryIdB)
+    {
+        return _connectedComponents.TransitivelyConnected(trajectoryIdA, trajectoryIdB);
+    }
+
+    /// <summary>
+    /// The trajectory IDs, grouped by connectivity.
+    /// </summary>
+    public List<List<int>> Components()
+    {
+        return _connectedComponents.Components();
+    }
+
+    /// <summary>
+    /// Returns the last connection time between the two trajectories.
+    /// If either of the trajectories is untracked or they have never been
+    /// connected returns 0 (beginning of time).
+    /// </summary>
+    public long LastConnectionTime(int trajectoryIdA, int trajectoryIdB)
+    {
+        var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+        return _lastConnectionTimeMap.TryGetValue(sortedPair, out var t) ? t : 0L;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/WorkItem.cs

@@ -0,0 +1,40 @@
+/*
+ * 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.Internal;
+
+/// <summary>
+/// Result of processing a work item, indicating what should happen next.
+/// Similar to Cartographer C++ WorkItem::Result.
+/// </summary>
+public enum WorkItemResult
+{
+    /// <summary>
+    /// Do not run optimization after this work item.
+    /// </summary>
+    DoNotRunOptimization,
+    
+    /// <summary>
+    /// Run optimization after this work item.
+    /// </summary>
+    RunOptimization
+}
+
+/// <summary>
+/// Represents a work item to be processed by the pose graph.
+/// Similar to Cartographer C++ WorkItem.
+/// </summary>
+public record WorkItem(Func<WorkItemResult> Action);

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/WorkQueue.cs

@@ -0,0 +1,304 @@
+/*
+ * 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.Collections.Concurrent;
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Thread-safe work queue for serializing pose graph operations.
+/// Similar to Cartographer C++ WorkQueue.
+/// 
+/// Operations are added to the queue non-blocking, and processed
+/// sequentially by a background thread to ensure thread-safety.
+/// </summary>
+public class WorkQueue : IDisposable
+{
+    private readonly ConcurrentQueue<WorkItem> _queue = new();
+    private readonly Lock _lock = new();
+    private readonly CancellationTokenSource _cancellationTokenSource = new();
+    private readonly AutoResetEvent _newItemEvent = new(false);
+    private readonly ManualResetEvent _optimizationDoneEvent = new(false);
+    private Thread? _processingThread;
+    private bool _running = true;
+    private bool _disposed = false;
+
+    /// <summary>
+    /// Event raised when work queue needs optimization.
+    /// IMPORTANT: Handler MUST call NotifyOptimizationDone() when optimization completes,
+    /// otherwise work queue will be blocked forever.
+    /// </summary>
+    public event EventHandler? OptimizationNeeded;
+    
+    /// <summary>
+    /// Gets whether the work queue is empty.
+    /// </summary>
+    public bool IsEmpty => _queue.IsEmpty;
+    
+    /// <summary>
+    /// Gets the number of items in the work queue.
+    /// </summary>
+    public int Count => _queue.Count;
+    
+    public WorkQueue()
+    {
+        StartProcessing();
+    }
+    
+    /// <summary>
+    /// Starts background thread to process work queue with high priority.
+    /// </summary>
+    private void StartProcessing()
+    {
+        _processingThread = new Thread(() => ProcessWorkQueue(_cancellationTokenSource.Token))
+        {
+            // IMPORTANT (Linux RT): do NOT run Highest priority here.
+            // This thread should never starve the sensor/scan-matching pipeline.
+            Priority = ThreadPriority.Highest,
+            IsBackground = true,
+            Name = "CartographerWorkQueue"
+        };
+        _processingThread.Start();
+    }
+    
+    /// <summary>
+    /// Adds a work item to the queue. Non-blocking and thread-safe.
+    /// </summary>
+    public void AddWorkItem(WorkItem workItem)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, this);
+
+        if (!_running)
+        {
+            throw new InvalidOperationException("WorkQueue is not running");
+        }
+
+        _queue.Enqueue(workItem);
+        // Wake processing thread if it is waiting.
+        _newItemEvent.Set();
+    }
+
+    /// <summary>
+    /// Notifies work queue that optimization has completed.
+    /// This allows the work queue processing thread to resume.
+    /// Match C++ behavior: After HandleWorkQueue completes (which runs optimization),
+    /// DrainWorkQueue is called again to continue processing.
+    /// </summary>
+    public void NotifyOptimizationDone()
+    {
+        _optimizationDoneEvent.Set();
+    }
+    
+    /// <summary>
+    /// Processes work items until queue is empty or optimization is needed.
+    /// Called by background thread.
+    /// Match C++ behavior: When optimization is needed, STOP processing and WAIT
+    /// until optimization completes (signaled via NotifyOptimizationDone).
+    /// </summary>
+    private void ProcessWorkQueue(CancellationToken cancellationToken)
+    {
+        Thread.BeginThreadAffinity();
+        try
+        {
+            while (!cancellationToken.IsCancellationRequested && _running)
+            {
+                bool processedAny = false;
+                bool optimizationNeeded = false;
+
+                while (_queue.TryDequeue(out var workItem))
+                {
+                    if (cancellationToken.IsCancellationRequested)
+                    {
+                        break;
+                    }
+
+                    var result = workItem.Action();
+
+                    if (result == WorkItemResult.RunOptimization)
+                    {
+                        optimizationNeeded = true;
+                        // Stop processing to allow optimization to run
+                        break;
+                    }
+
+                    processedAny = true;
+                }
+
+                // Match C++: When optimization needed, STOP work queue and WAIT for optimization to complete
+                if (optimizationNeeded)
+                {
+                    // Reset event before invoking (in case it was set previously)
+                    _optimizationDoneEvent.Reset();
+
+                    // Invoke optimization handler synchronously
+                    // CRITICAL: Handler MUST call NotifyOptimizationDone() when done
+                    OptimizationNeeded?.Invoke(this, EventArgs.Empty);
+
+                    // WAIT for optimization to complete before continuing work queue
+                    // This matches C++ behavior where DrainWorkQueue() stops until HandleWorkQueue completes
+                    _optimizationDoneEvent.WaitOne();
+
+                    // After optimization completes, continue processing work queue
+                    continue;
+                }
+
+                // Avoid busy-spinning (especially harmful on RT kernels). If we didn't
+                // process anything and no optimization was requested, wait for new work.
+                if (!processedAny)
+                {
+                    // Wakeups happen via AddWorkItem(). Also periodically wake to observe cancellation.
+                    // Check cancellation before waiting
+                    if (cancellationToken.IsCancellationRequested || !_running)
+                    {
+                        break;
+                    }
+                    _newItemEvent.WaitOne(TimeSpan.FromMilliseconds(50));
+                }
+            }
+        }
+        catch
+        {
+            throw;
+        }
+        finally
+        {
+            Thread.EndThreadAffinity();
+        }
+    }
+    
+    /// <summary>
+    /// Drains the work queue synchronously (for testing or final processing).
+    /// Match C++: Process work items until queue is empty or optimization is needed.
+    ///
+    /// If processing thread is alive, waits for queue to empty.
+    /// If processing thread is dead, processes remaining items.
+    /// </summary>
+    public void DrainWorkQueue()
+    {
+        // If processing thread is still alive, just wait for it to drain the queue
+        if (_processingThread?.IsAlive == true)
+        {
+            // Wait for queue to be empty (processing thread will handle it)
+            while (!_queue.IsEmpty)
+            {
+                Thread.Sleep(10);
+            }
+            return;
+        }
+
+        // Processing thread is dead, we need to drain the queue ourselves
+        bool processWorkQueue = true;
+        while (processWorkQueue)
+        {
+            if (!_queue.TryDequeue(out var workItem))
+            {
+                // Queue is empty
+                return;
+            }
+
+            var result = workItem.Action();
+            // Match C++: Continue processing if kDoNotRunOptimization, stop if kRunOptimization
+            processWorkQueue = result == WorkItemResult.DoNotRunOptimization;
+
+            if (result == WorkItemResult.RunOptimization)
+            {
+                // Signal optimization needed (caller should handle this)
+                OptimizationNeeded?.Invoke(this, EventArgs.Empty);
+                // Stop processing to allow optimization to run
+                // Caller should call DrainWorkQueue() again after optimization
+                return;
+            }
+        }
+    }
+    
+    /// <summary>
+    /// Waits for queue to be empty (with timeout).
+    /// </summary>
+    public void WaitForQueueToEmptyAsync(TimeSpan timeout)
+    {
+        var startTime = DateTime.UtcNow;
+        
+        while (DateTime.UtcNow - startTime < timeout)
+        {
+            if (_queue.IsEmpty)
+            {
+                // Give a small delay to ensure no new items are being added
+                Thread.Sleep(10);
+                
+                if (_queue.IsEmpty)
+                {
+                    return;
+                }
+            }
+            
+            Thread.Sleep(10);
+        }
+        
+        var remainingCount = _queue.Count;
+        if (remainingCount > 0)
+        {
+            throw new TimeoutException($"Work queue not empty after timeout. Remaining items: {remainingCount}");
+        }
+    }
+    
+    public void Stop()
+    {
+        lock (_lock)
+        {
+            _running = false;
+        }
+    }
+    
+    public void Dispose()
+    {
+        if (_disposed)
+        {
+            return;
+        }
+        
+        _running = false;
+        _cancellationTokenSource.Cancel();
+        
+        _newItemEvent.Set(); // wake thread so it can exit promptly
+        
+        // Wait for thread to finish (with timeout)
+        if (_processingThread != null)
+        {
+            // Check if thread is already finished before joining
+            if (_processingThread.IsAlive)
+            {
+                if (!_processingThread.Join(TimeSpan.FromSeconds(5)))
+                {
+                    // Thread didn't finish in time, but continue cleanup
+                }
+            }
+        }
+        
+        // Clear event handlers to prevent memory leaks
+        OptimizationNeeded = null;
+        
+        // Drain remaining work items to prevent memory leaks
+        DrainWorkQueue();
+
+        _newItemEvent.Dispose();
+        _optimizationDoneEvent.Dispose();
+        _cancellationTokenSource.Dispose();
+        _disposed = true;
+
+        GC.SuppressFinalize(this);
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/MapBuilder.cs

@@ -0,0 +1,721 @@
+/*
+ * 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.IO;
+using CartographerSharp.Mapping.Internal;
+using CartographerSharp.Mapping.Internal.D2D;
+using CartographerSharp.Mapping.Internal.D3D;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Transform;
+using ThreadPool = CartographerSharp.Common.Threading.ThreadPool;
+
+namespace CartographerSharp.Mapping;
+
+using CartographerSharp.Common.Math;
+using CartographerSharp.Mapping.D2D;
+using CartographerSharp.Models.Transform;
+using System;
+using System.IO;
+using System.IO.Compression;
+using  RobotNet10.Shared.Numbers;
+
+
+/// <summary>
+/// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
+/// and a PoseGraph for loop closure.
+/// </summary>
+public class MapBuilder : IMapBuilder
+{
+    private readonly MapBuilderOptions _options;
+    private readonly ThreadPool _threadPool;
+    private readonly IPoseGraph _poseGraph;
+    private readonly List<ITrajectoryBuilder> _trajectoryBuilders = [];
+    private readonly List<TrajectoryBuilderOptionsWithSensorIds> _allTrajectoryBuilderOptions = [];
+    // CRITICAL FIX: Map trajectoryId to builder because trajectoryId may not match index in _trajectoryBuilders
+    // when trajectories are loaded from map (they don't have builders)
+    private readonly Dictionary<int, ITrajectoryBuilder> _trajectoryIdToBuilder = [];
+    private bool _disposed = false;
+
+    public MapBuilder(MapBuilderOptions options)
+    {
+        _options = options;
+        
+        if (options.UseTrajectoryBuilder2D == options.UseTrajectoryBuilder3D)
+        {
+            throw new ArgumentException("Exactly one of UseTrajectoryBuilder2D or UseTrajectoryBuilder3D must be true");
+        }
+
+        _threadPool = new ThreadPool(options.NumBackgroundThreads);
+
+        if (options.UseTrajectoryBuilder2D)
+        {
+            // Match C++: Pass thread_pool to PoseGraph2D (map_builder.cc line 86-90)
+            _poseGraph = new PoseGraph2D(options.PoseGraphOptions, _threadPool);
+        }
+        else if (options.UseTrajectoryBuilder3D)
+        {
+            // Match C++: Pass thread_pool to PoseGraph3D (map_builder.cc line 92-97)
+            _poseGraph = new PoseGraph3D(options.PoseGraphOptions, optimizationProblem: null, threadPool: _threadPool);
+        }
+        else
+        {
+            throw new ArgumentException("Exactly one of UseTrajectoryBuilder2D or UseTrajectoryBuilder3D must be true");
+        }
+    }
+
+    public int AddTrajectoryBuilder(
+        HashSet<ITrajectoryBuilder.SensorId> expectedSensorIds,
+        TrajectoryBuilderOptions trajectoryOptions)
+    {
+        // CRITICAL FIX: Use _allTrajectoryBuilderOptions.Count instead of _trajectoryBuilders.Count
+        // because AddTrajectoryForDeserialization() adds to _allTrajectoryBuilderOptions but not to _trajectoryBuilders.
+        // This ensures new trajectory IDs don't conflict with trajectories loaded from map.
+        var trajectoryId = _allTrajectoryBuilderOptions.Count;
+        
+        // Select range sensor IDs
+        var rangeSensorIds = expectedSensorIds
+            .Where(s => s.Type == ITrajectoryBuilder.SensorId.SensorType.Range)
+            .Select(s => s.Id)
+            .ToList();
+
+        ITrajectoryBuilder trajectoryBuilder;
+        
+        if (_options.UseTrajectoryBuilder2D)
+        {
+            // Create LocalTrajectoryBuilder2D with options
+            // Note: TrajectoryBuilder2DOptions is JsonElement?, need to deserialize if present
+            LocalTrajectoryBuilderOptions2D? localOptions = null;
+            if (trajectoryOptions.TrajectoryBuilder2DOptions.HasValue)
+            {
+                localOptions = System.Text.Json.JsonSerializer.Deserialize<LocalTrajectoryBuilderOptions2D>(
+                    trajectoryOptions.TrajectoryBuilder2DOptions.Value.GetRawText());
+            }
+            
+            var localTrajectoryBuilder = new LocalTrajectoryBuilder2D(
+                localOptions ?? new LocalTrajectoryBuilderOptions2D(),
+                rangeSensorIds
+            );
+
+            // Set initial pose if provided in trajectory options
+            if (trajectoryOptions.InitialTrajectoryPose.HasValue)
+            {
+                var initialTrajectoryPose = trajectoryOptions.InitialTrajectoryPose.Value;
+                var initialPose = (Rigid3d)initialTrajectoryPose.RelativePose;
+            }
+
+            // Create motion filter for odometry if configured
+            MotionFilter? motionFilter = null;
+            if (trajectoryOptions.PoseGraphOdometryMotionFilter.HasValue)
+            {
+                motionFilter = new MotionFilter(trajectoryOptions.PoseGraphOdometryMotionFilter.Value);
+            }
+
+            // Create GlobalTrajectoryBuilder2D wrapping the local builder
+            trajectoryBuilder = new GlobalTrajectoryBuilder2D(
+                localTrajectoryBuilder,
+                trajectoryId,
+                (PoseGraph2D)_poseGraph,
+                motionFilter
+            );
+        }
+        else if (_options.UseTrajectoryBuilder3D)
+        {
+            // Create LocalTrajectoryBuilder3D with options
+            // Note: TrajectoryBuilder3DOptions is JsonElement?, need to deserialize if present
+            LocalTrajectoryBuilderOptions3D? localOptions = null;
+            if (trajectoryOptions.TrajectoryBuilder3DOptions.HasValue)
+            {
+                localOptions = System.Text.Json.JsonSerializer.Deserialize<LocalTrajectoryBuilderOptions3D>(
+                    trajectoryOptions.TrajectoryBuilder3DOptions.Value.GetRawText());
+            }
+            
+            var localTrajectoryBuilder = new LocalTrajectoryBuilder3D(
+                localOptions ?? new LocalTrajectoryBuilderOptions3D(),
+                rangeSensorIds
+            );
+
+            // Create motion filter for odometry if configured
+            MotionFilter? motionFilter = null;
+            if (trajectoryOptions.PoseGraphOdometryMotionFilter.HasValue)
+            {
+                motionFilter = new MotionFilter(trajectoryOptions.PoseGraphOdometryMotionFilter.Value);
+            }
+
+            // Create GlobalTrajectoryBuilder3D wrapping the local builder
+            trajectoryBuilder = new GlobalTrajectoryBuilder3D(
+                localTrajectoryBuilder,
+                trajectoryId,
+                (PoseGraph3D)_poseGraph,
+                motionFilter
+            );
+        }
+        else
+        {
+            throw new ArgumentException("Exactly one of UseTrajectoryBuilder2D or UseTrajectoryBuilder3D must be true");
+        }
+
+        _trajectoryBuilders.Add(trajectoryBuilder);
+        
+        // CRITICAL FIX: Map trajectoryId to builder for GetTrajectoryBuilder() lookup
+        _trajectoryIdToBuilder[trajectoryId] = trajectoryBuilder;
+
+        // Store options
+        var optionsWithSensorIds = new TrajectoryBuilderOptionsWithSensorIds
+        {
+            TrajectoryBuilderOptions = trajectoryOptions,
+            SensorIds = [.. expectedSensorIds.Select(s => SensorIdOperations.ToProto(s))]
+        };
+        _allTrajectoryBuilderOptions.Add(optionsWithSensorIds);
+
+        // Match C++ MaybeAddPureLocalizationTrimmer (map_builder.cc 151-152)
+        if (trajectoryOptions.PureLocalizationTrimmer.HasValue)
+        {
+            var trimmerOpts = trajectoryOptions.PureLocalizationTrimmer.Value;
+            _poseGraph.AddTrimmer(new PureLocalizationTrimmer(trajectoryId, trimmerOpts.MaxSubmapsToKeep));
+        }
+
+        // Set initial trajectory pose if provided (localization: loadFrozenState = true)
+        if (trajectoryOptions.InitialTrajectoryPose.HasValue)
+        {
+            var initialPose = trajectoryOptions.InitialTrajectoryPose.Value;
+            var relativePose = (Rigid3d)initialPose.RelativePose;
+            _poseGraph.SetInitialTrajectoryPose(
+                trajectoryId,
+                initialPose.ToTrajectoryId,
+                relativePose,
+                initialPose.Timestamp
+            );
+            // Match C++ (map_builder.cc 164-170): when localization_mode, set pose graph mode and enable_matching_score
+            if (_poseGraph is PoseGraph2D poseGraph2D)
+            {
+                poseGraph2D.SetLocalizationMode(true);
+                // C++: if (trajectory_options.enable_matching_score()) pose_graph_->set_enable_matching_score(true);
+                // C#: SetEnableMatchingScore can be added to PoseGraph2D when TrajectoryBuilderOptions has it.
+            }
+            // Original Cartographer: no SetInitialPose on GlobalTrajectoryBuilder. Initial pose is in pose graph (SetInitialTrajectoryPose); relocalization constraints via SetLocalizationInitialPoses. MCL refines pose before adding trajectory.
+        }
+
+        return trajectoryId;
+    }
+
+    /// <summary>
+    /// Starts relocalization (match C++ MapBuilder::StartRelocalization).
+    /// Sets pose graph localization callback and initial poses so constraint builder
+    /// uses them when finding constraints (MaybeAddLocalizationConstraint).
+    /// </summary>
+    public void StartRelocalization(IReadOnlyList<Rigid3d> initialPoses, Action<int, long, Rigid3d>? callback)
+    {
+        if (_poseGraph is PoseGraph2D poseGraph2D)
+        {
+            poseGraph2D.SetLocalizationCallback(
+                callback != null ? (int tid, long t, Rigid3d p) => callback(tid, t, p) : null);
+            poseGraph2D.SetLocalizationInitialPoses(initialPoses ?? []);
+        }
+        // PoseGraph3D: add SetLocalizationCallback/SetLocalizationInitialPoses if needed for 3D
+    }
+
+    public int AddTrajectoryForDeserialization(
+        TrajectoryBuilderOptionsWithSensorIds optionsWithSensorIdsProto)
+    {
+        // CRITICAL FIX: Use _allTrajectoryBuilderOptions.Count to ensure trajectory IDs are sequential
+        // and don't conflict with trajectories created via AddTrajectoryBuilder()
+        var trajectoryId = _allTrajectoryBuilderOptions.Count;
+        _allTrajectoryBuilderOptions.Add(optionsWithSensorIdsProto);
+        // No trajectory builder is created for deserialization
+        return trajectoryId;
+    }
+
+    public ITrajectoryBuilder? GetTrajectoryBuilder(int trajectoryId)
+    {
+        // CRITICAL FIX: Use dictionary lookup instead of index because trajectoryId may not match
+        // index in _trajectoryBuilders when trajectories are loaded from map (they don't have builders)
+        if (_trajectoryIdToBuilder.TryGetValue(trajectoryId, out var builder))
+        {
+            return builder;
+        }
+        return null;
+    }
+
+    public void FinishTrajectory(int trajectoryId)
+    {
+        // CRITICAL FIX: Don't check _trajectoryBuilders.Count because trajectoryId may not match index
+        // when trajectories are loaded from map. Just call FinishTrajectory on pose graph,
+        // which will handle the check internally.
+        if (trajectoryId >= 0)
+        {
+            _poseGraph.FinishTrajectory(trajectoryId);
+        }
+    }
+
+    public string SubmapToProto(SubmapId submapId, out SubmapQuery.Response response)
+    {
+        // Convert submap to proto format for visualization and serialization
+        var submapData = _poseGraph.GetSubmapData(submapId);
+        
+        if (submapData.Submap == null)
+        {
+            response = new SubmapQuery.Response(0, []);
+            return "Submap not found";
+        }
+
+        if (submapData.Submap is Mapping.D2D.Submap2D submap2D)
+        {
+            // Get submap version (number of range data inserted)
+            int version = submap2D.NumRangeData;
+
+            var textures = new List<SubmapQuery.Texture>();
+            
+            // Extract grid data if available
+            var grid = submap2D.Grid;
+            if (grid != null)
+            {
+                // Compute cropped grid to minimal size
+                var croppedGrid = grid.ComputeCroppedGrid();
+                var limits = croppedGrid.Limits;
+                
+                var width = limits.CellLimits.NumXCells;
+                var height = limits.CellLimits.NumYCells;
+                var resolution = limits.Resolution;
+                
+                // Compute slice pose (origin of the texture)
+                // C++: local_pose.inverse() * transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.));
+                // This means the slice pose is relative to the submap frame.
+                var max = limits.Max;
+                var sliceTranslation = new Vector3(max.X, max.Y, 0.0);
+                var slicePose = submap2D.LocalPose.Inverse() * new Rigid3d(sliceTranslation, Quaternion.Identity);
+                
+                // Texture format is 2 bytes per pixel: Value, Alpha
+                var pixels = new List<byte>(width * height * 2);
+                var probabilityGrid = (ProbabilityGrid)croppedGrid;
+
+                // Iterate cells and convert to pixels
+                for (int y = 0; y < height; y++)
+                {
+                    for (int x = 0; x < width; x++)
+                    {
+                        var cellIndex = new Array2i(x, y);
+                        if (probabilityGrid.IsKnown(cellIndex))
+                        {
+                            double probability = probabilityGrid.GetProbability(cellIndex);
+                            // C++: const int delta = 128 - ProbabilityToLogOddsInteger(probability);
+                            byte logOddsInteger = SubmapProbabilityUtils.ProbabilityToLogOddsInteger(probability);
+                            int delta = 128 - logOddsInteger;
+                            
+                            byte value = (byte)(delta > 0 ? delta : 0);
+                            byte alpha = (byte)(delta > 0 ? 0 : -delta);
+                            
+                            pixels.Add(value);
+                            pixels.Add(alpha);
+                        }
+                        else
+                        {
+                            pixels.Add(0); // Unknown
+                            pixels.Add(0);
+                        }
+                    }
+                }
+                
+                // Compress data
+                byte[] compressedBytes;
+                using (var memoryStream = new MemoryStream())
+                {
+                    using (var gzipStream = new GZipStream(memoryStream, CompressionMode.Compress))
+                    {
+                        var pixelArray = pixels.ToArray();
+                        gzipStream.Write(pixelArray, 0, pixelArray.Length);
+                    }
+                    compressedBytes = memoryStream.ToArray();
+                }
+                
+                textures.Add(new SubmapQuery.Texture(
+                    [.. compressedBytes],
+                    width,
+                    height,
+                    resolution,
+                    (Rigid3dProto)slicePose
+                ));
+            }
+
+            response = new SubmapQuery.Response(version, textures);
+            return string.Empty; // Empty string indicates success
+        }
+        else if (submapData.Submap is Mapping.D3D.Submap3D)
+        {
+            // 3D submaps would require voxel grid conversion
+            // This is more complex and typically involves:
+            // 1. Extracting hybrid grid data
+            // 2. Converting TSDF or probability values
+            // 3. Generating 3D texture or point cloud representation
+            
+            response = new SubmapQuery.Response(0, []);
+            return "3D submap conversion not yet fully implemented";
+        }
+        
+        response = new SubmapQuery.Response(0, []);
+        return "Unknown submap type";
+    }
+
+    public void SerializeState(bool includeUnfinishedSubmaps, IO.IProtoStreamWriter writer)
+    {
+        ArgumentNullException.ThrowIfNull(writer);
+
+        var trajectoryBuilderOptions = GetAllTrajectoryBuilderOptions();
+        MappingStateSerialization.WritePbStream(
+            _poseGraph,
+            trajectoryBuilderOptions,
+            writer,
+            includeUnfinishedSubmaps
+        );
+    }
+
+    public bool SerializeStateToFile(bool includeUnfinishedSubmaps, string filename)
+    {
+        try
+        {
+            using var writer = new ProtoStreamWriter(filename);
+            SerializeState(includeUnfinishedSubmaps, writer);
+            return writer.Close();
+        }
+        catch (Exception)
+        {
+            return false;
+        }
+    }
+
+    public Dictionary<int, int> LoadState(IO.IProtoStreamReader reader, bool loadFrozenState)
+    {
+        ArgumentNullException.ThrowIfNull(reader);
+
+        var deserializer = new ProtoStreamDeserializer(reader);
+
+        // Create a copy of the pose_graph_proto, such that we can re-write the trajectory ids.
+        var poseGraphProto = deserializer.PoseGraph;
+        var allBuilderOptionsProto = deserializer.AllTrajectoryBuilderOptions;
+
+        var trajectoryRemapping = new Dictionary<int, int>();
+        
+        // Create trajectories and build remapping
+        for (int i = 0; i < poseGraphProto.Trajectories.Count; ++i)
+        {
+            var trajectoryProto = poseGraphProto.Trajectories[i];
+            var optionsWithSensorIdsProto = allBuilderOptionsProto.OptionsWithSensorIds[i];
+            
+            var newTrajectoryId = AddTrajectoryForDeserialization(optionsWithSensorIdsProto);
+            
+            if (trajectoryRemapping.ContainsKey(trajectoryProto.TrajectoryId))
+            {
+                throw new InvalidOperationException(
+                    $"Duplicate trajectory ID: {trajectoryProto.TrajectoryId}");
+            }
+            
+            trajectoryRemapping[trajectoryProto.TrajectoryId] = newTrajectoryId;
+            
+            // Update trajectory ID in the proto (we need to modify the list element)
+            var updatedTrajectory = trajectoryProto;
+            updatedTrajectory.TrajectoryId = newTrajectoryId;
+            poseGraphProto.Trajectories[i] = updatedTrajectory;
+            
+            if (loadFrozenState)
+            {
+                _poseGraph.FreezeTrajectory(newTrajectoryId);
+            }
+        }
+
+        // Apply the calculated remapping to constraints in the pose graph proto.
+        for (int i = 0; i < poseGraphProto.Constraints.Count; ++i)
+        {
+            var constraintProto = poseGraphProto.Constraints[i];
+            
+            var updatedSubmapId = constraintProto.SubmapId;
+            updatedSubmapId.TrajectoryId = trajectoryRemapping[constraintProto.SubmapId.TrajectoryId];
+            
+            var updatedNodeId = constraintProto.NodeId;
+            updatedNodeId.TrajectoryId = trajectoryRemapping[constraintProto.NodeId.TrajectoryId];
+            
+            var updatedConstraint = constraintProto;
+            updatedConstraint.SubmapId = updatedSubmapId;
+            updatedConstraint.NodeId = updatedNodeId;
+            poseGraphProto.Constraints[i] = updatedConstraint;
+        }
+
+        // Build submap poses map
+        var submapPoses = new MapById<SubmapId, Rigid3d>();
+        foreach (var trajectoryProto in poseGraphProto.Trajectories)
+        {
+            foreach (var submapProto in trajectoryProto.Submaps)
+            {
+                var submapId = new SubmapId(trajectoryProto.TrajectoryId, submapProto.SubmapIndex);
+                var pose = (Rigid3d)submapProto.Pose;
+                submapPoses.Insert(submapId, pose);
+            }
+        }
+
+        // Build node poses map
+        var nodePoses = new MapById<NodeId, Rigid3d>();
+        foreach (var trajectoryProto in poseGraphProto.Trajectories)
+        {
+            foreach (var nodeProto in trajectoryProto.Nodes)
+            {
+                var nodeId = new NodeId(trajectoryProto.TrajectoryId, nodeProto.NodeIndex);
+                var pose = (Rigid3d)nodeProto.Pose;
+                nodePoses.Insert(nodeId, pose);
+            }
+        }
+
+        // Set global poses of landmarks.
+        if (poseGraphProto.LandmarkPoses != null)
+        {
+            foreach (var landmark in poseGraphProto.LandmarkPoses)
+            {
+                _poseGraph.SetLandmarkPose(
+                    landmark.LandmarkId,
+                    (Rigid3d)landmark.GlobalPose,
+                    true);
+            }
+        }
+
+        // Check format version for 3D
+        if (_options.UseTrajectoryBuilder3D)
+        {
+            const uint FormatVersionWithoutSubmapHistograms = 1;
+            if (deserializer.Header.FormatVersion == FormatVersionWithoutSubmapHistograms)
+            {
+                throw new NotSupportedException(
+                    "The pbstream file contains submaps without rotational histograms. " +
+                    "This can be converted with the 'pbstream migrate' tool, see the " +
+                    "Cartographer documentation for details.");
+            }
+        }
+
+        // Read and process serialized data
+        while (deserializer.ReadNextSerializedData(out var protoNullable))
+        {
+            if (!protoNullable.HasValue)
+                break;
+                
+            var proto = protoNullable.Value;
+
+            // Handle different data types
+            if (proto.PoseGraph.HasValue)
+            {
+                // Found multiple serialized `PoseGraph`. Serialized stream likely corrupt!
+                // Log error but continue
+                continue;
+            }
+            
+            if (proto.AllTrajectoryBuilderOptions.HasValue)
+            {
+                // Found multiple serialized `AllTrajectoryBuilderOptions`. Serialized stream likely corrupt!
+                // Log error but continue
+                continue;
+            }
+            
+            if (proto.Submap.HasValue)
+            {
+                var submapProto = proto.Submap.Value;
+                var oldTrajectoryId = submapProto.SubmapId.TrajectoryId;
+                var newTrajectoryId = trajectoryRemapping[oldTrajectoryId];
+                
+                var submapId = new SubmapId(newTrajectoryId, submapProto.SubmapId.SubmapIndex);
+                var globalPose = submapPoses[submapId];
+                
+                var updatedSubmap = submapProto;
+                var updatedSubmapId = submapProto.SubmapId;
+                updatedSubmapId.TrajectoryId = newTrajectoryId;
+                updatedSubmap.SubmapId = updatedSubmapId;
+                
+                _poseGraph.AddSubmapFromProto(globalPose, updatedSubmap);
+            }
+            else if (proto.Node.HasValue)
+            {
+                var nodeProto = proto.Node.Value;
+                var oldTrajectoryId = nodeProto.NodeId.TrajectoryId;
+                var newTrajectoryId = trajectoryRemapping[oldTrajectoryId];
+                
+                var nodeId = new NodeId(newTrajectoryId, nodeProto.NodeId.NodeIndex);
+                var nodePose = nodePoses[nodeId];
+                
+                var updatedNode = nodeProto;
+                var updatedNodeId = nodeProto.NodeId;
+                updatedNodeId.TrajectoryId = newTrajectoryId;
+                updatedNode.NodeId = updatedNodeId;
+                
+                _poseGraph.AddNodeFromProto(nodePose, updatedNode);
+            }
+            else if (proto.SerializedTrajectoryData.HasValue)
+            {
+                var trajectoryDataProto = proto.SerializedTrajectoryData.Value;
+                var oldTrajectoryId = trajectoryDataProto.TrajectoryId;
+                var newTrajectoryId = trajectoryRemapping[oldTrajectoryId];
+                
+                var updatedTrajectoryData = new TrajectoryData(
+                    newTrajectoryId,
+                    trajectoryDataProto.GravityConstant,
+                    trajectoryDataProto.ImuCalibration,
+                    trajectoryDataProto.FixedFrameOriginInMap
+                );
+                
+                _poseGraph.SetTrajectoryDataFromProto(updatedTrajectoryData);
+            }
+            else if (proto.ImuData.HasValue)
+            {
+                if (!loadFrozenState)
+                {
+                    var imuDataProto = proto.ImuData.Value;
+                    var oldTrajectoryId = imuDataProto.TrajectoryId;
+                    var newTrajectoryId = trajectoryRemapping[oldTrajectoryId];
+                    
+                    var imuData = Sensor.ImuDataOperations.FromProto(imuDataProto.ImuDataValue);
+                    _poseGraph.AddImuData(newTrajectoryId, imuData);
+                }
+            }
+            else if (proto.OdometryData.HasValue)
+            {
+                if (!loadFrozenState)
+                {
+                    var odometryDataProto = proto.OdometryData.Value;
+                    var oldTrajectoryId = odometryDataProto.TrajectoryId;
+                    var newTrajectoryId = trajectoryRemapping[oldTrajectoryId];
+                    
+                    var odometryData = Sensor.OdometryDataOperations.FromProto(odometryDataProto.OdometryDataValue);
+                    _poseGraph.AddOdometryData(newTrajectoryId, odometryData);
+                }
+            }
+            else if (proto.FixedFramePoseData.HasValue)
+            {
+                if (!loadFrozenState)
+                {
+                    var fixedFramePoseDataProto = proto.FixedFramePoseData.Value;
+                    var oldTrajectoryId = fixedFramePoseDataProto.TrajectoryId;
+                    var newTrajectoryId = trajectoryRemapping[oldTrajectoryId];
+                    
+                    var fixedFramePoseData = Sensor.FixedFramePoseDataOperations.FromProto(fixedFramePoseDataProto.FixedFramePoseDataValue);
+                    _poseGraph.AddFixedFramePoseData(newTrajectoryId, fixedFramePoseData);
+                }
+            }
+            else if (proto.LandmarkData.HasValue)
+            {
+                if (!loadFrozenState)
+                {
+                    var landmarkDataProto = proto.LandmarkData.Value;
+                    var oldTrajectoryId = landmarkDataProto.TrajectoryId;
+                    var newTrajectoryId = trajectoryRemapping[oldTrajectoryId];
+                    
+                    var landmarkData = Sensor.LandmarkDataOperations.FromProto(landmarkDataProto.LandmarkDataValue);
+                    _poseGraph.AddLandmarkData(newTrajectoryId, landmarkData);
+                }
+            }
+        }
+
+        if (loadFrozenState)
+        {
+            // Add information about which nodes belong to which submap.
+            // This is required, even without constraints.
+            foreach (var constraintProto in poseGraphProto.Constraints)
+            {
+                if (constraintProto.ConstraintTag != Models.Mapping.PoseGraph.Constraint.Tag.IntraSubmap)
+                {
+                    continue;
+                }
+                
+                var nodeId = new NodeId(
+                    constraintProto.NodeId.TrajectoryId,
+                    constraintProto.NodeId.NodeIndex);
+                var submapId = new SubmapId(
+                    constraintProto.SubmapId.TrajectoryId,
+                    constraintProto.SubmapId.SubmapIndex);
+                
+                _poseGraph.AddNodeToSubmap(nodeId, submapId);
+            }
+        }
+        else
+        {
+            // When loading unfrozen trajectories, 'AddSerializedConstraints' will
+            // take care of adding information about which nodes belong to which submap.
+            // Use the static method from ConstraintOperations class
+            var constraints = ConstraintOperations.FromProto(poseGraphProto.Constraints);
+            _poseGraph.AddSerializedConstraints(constraints);
+        }
+
+        // Match C++: apply transform_to_map when loading state (map_builder.cc LoadState)
+        if (poseGraphProto.TransformToMap != null)
+        {
+            _poseGraph.SetTransformToMap((Rigid3d)poseGraphProto.TransformToMap);
+        }
+
+        if (!reader.Eof)
+        {
+            throw new InvalidOperationException("Reader is not at end of file after deserialization");
+        }
+
+        return trajectoryRemapping;
+    }
+
+    public Dictionary<int, int> LoadStateFromFile(string filename, bool loadFrozenState)
+    {
+        const string suffix = ".pbstream";
+        if (filename.Length >= suffix.Length &&
+            filename[^suffix.Length..] != suffix)
+        {
+            // Log warning: The file containing the state should be a .pbstream file.
+        }
+
+        using var reader = new ProtoStreamReader(filename);
+        return LoadState(reader, loadFrozenState);
+    }
+
+    public int NumTrajectoryBuilders => _trajectoryBuilders.Count;
+
+    public IPoseGraph PoseGraph => _poseGraph;
+
+    public List<TrajectoryBuilderOptionsWithSensorIds> GetAllTrajectoryBuilderOptions()
+    {
+        return [.. _allTrajectoryBuilderOptions];
+    }
+    
+    /// <summary>
+    /// Disposes resources, including pose graph and thread pool.
+    /// </summary>
+    public void Dispose()
+    {
+        if (_disposed)
+        {
+            return;
+        }
+        
+        // Dispose pose graph first (waits for optimization threads)
+        if (_poseGraph is IDisposable disposablePoseGraph)
+        {
+            disposablePoseGraph.Dispose();
+        }
+
+        // Dispose trajectory builders (they hold CeresScanMatcher instances with native resources)
+        foreach (var builder in _trajectoryBuilders)
+        {
+            if (builder is IDisposable disposableBuilder)
+            {
+                disposableBuilder.Dispose();
+            }
+        }
+
+        // Dispose thread pool
+        _threadPool?.Dispose();
+        
+        _disposed = true;
+        
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/MapById.cs

@@ -0,0 +1,577 @@
+/*
+ * 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.Collections;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Reminiscent of Dictionary, but indexed by 'IdType' which can be 'NodeId' or 'SubmapId'.
+/// Note: This container will only ever contain non-empty trajectories. Trimming
+/// the last remaining node of a trajectory drops the trajectory.
+/// </summary>
+public class MapById<TId, TData> : IEnumerable<MapById<TId, TData>.IdDataReference>
+    where TId : struct, IIdType
+{
+    private class MapByIndex
+    {
+        public bool CanAppend { get; set; } = true;
+        public SortedDictionary<int, TData> Data { get; } = [];
+    }
+
+    /// <summary>
+    /// Reference to an ID and its associated data.
+    /// </summary>
+    public readonly struct IdDataReference(TId id, TData data)
+    {
+        public TId Id { get; } = id;
+        public TData Data { get; } = data;
+    }
+
+    private readonly SortedDictionary<int, MapByIndex> _trajectories = [];
+
+    /// <summary>
+    /// Appends data to a 'trajectory_id', creating trajectories as needed.
+    /// </summary>
+    public TId Append(int trajectoryId, TData data)
+    {
+        if (trajectoryId < 0)
+        {
+            throw new ArgumentException("Trajectory ID must be non-negative", nameof(trajectoryId));
+        }
+
+        if (!_trajectories.TryGetValue(trajectoryId, out var trajectory))
+        {
+            trajectory = new MapByIndex();
+            _trajectories[trajectoryId] = trajectory;
+        }
+
+        if (!trajectory.CanAppend)
+        {
+            throw new InvalidOperationException($"Cannot append to trajectory {trajectoryId} that has been modified. Trajectory has {trajectory.Data.Count} existing entries.");
+        }
+
+        var index = trajectory.Data.Count == 0 
+            ? 0 
+            : trajectory.Data.Keys.Max() + 1;
+        
+        trajectory.Data[index] = data;
+        
+        // Create ID instance
+        if (typeof(TId) == typeof(NodeId))
+        {
+            return (TId)(object)new NodeId(trajectoryId, index);
+        }
+        else if (typeof(TId) == typeof(SubmapId))
+        {
+            return (TId)(object)new SubmapId(trajectoryId, index);
+        }
+        throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+    }
+
+    /// <summary>
+    /// Inserts data (which must not exist already) into a trajectory.
+    /// </summary>
+    public void Insert(TId id, TData data)
+    {
+        var trajectoryId = id.GetTrajectoryId();
+        var index = id.GetIndex();
+
+        if (trajectoryId < 0 || index < 0)
+        {
+            throw new ArgumentException("ID components must be non-negative", nameof(id));
+        }
+
+        if (!_trajectories.TryGetValue(trajectoryId, out var trajectory))
+        {
+            trajectory = new MapByIndex();
+            _trajectories[trajectoryId] = trajectory;
+        }
+
+        trajectory.CanAppend = false;
+        
+        if (trajectory.Data.ContainsKey(index))
+        {
+            throw new ArgumentException($"Data already exists for ID {id}", nameof(id));
+        }
+
+        trajectory.Data[index] = data;
+    }
+
+    /// <summary>
+    /// Removes the data for 'id' which must exist.
+    /// </summary>
+    public void Trim(TId id)
+    {
+        var trajectoryId = id.GetTrajectoryId();
+        var index = id.GetIndex();
+
+        if (!_trajectories.TryGetValue(trajectoryId, out var trajectory))
+        {
+            throw new KeyNotFoundException($"Trajectory {trajectoryId} not found");
+        }
+        
+        if (!trajectory.Data.TryGetValue(index, out _))
+        {
+            throw new KeyNotFoundException($"ID {id} not found");
+        }
+
+        // Check if this is the highest index in the trajectory
+        // Since SortedDictionary maintains sorted order, we can check if index equals the max key
+        var maxKey = trajectory.Data.Keys.Max();
+        if (index == maxKey)
+        {
+            // We are removing the data with the highest index from this trajectory.
+            // We assume that we will never append to it anymore.
+            trajectory.CanAppend = false;
+        }
+
+        trajectory.Data.Remove(index);
+        
+        if (trajectory.Data.Count == 0)
+        {
+            _trajectories.Remove(trajectoryId);
+        }
+    }
+
+    /// <summary>
+    /// Checks if the container contains the given ID.
+    /// </summary>
+    public bool Contains(TId id)
+    {
+        var trajectoryId = id.GetTrajectoryId();
+        var index = id.GetIndex();
+        
+        return _trajectories.TryGetValue(trajectoryId, out var trajectory) &&
+               trajectory.Data.ContainsKey(index);
+    }
+
+    /// <summary>
+    /// Gets the data for the given ID.
+    /// </summary>
+    public TData this[TId id]
+    {
+        get
+        {
+            var trajectoryId = id.GetTrajectoryId();
+            var index = id.GetIndex();
+            
+            if (!_trajectories.TryGetValue(trajectoryId, out var trajectory))
+            {
+                throw new KeyNotFoundException($"Trajectory {trajectoryId} not found");
+            }
+            
+            if (!trajectory.Data.TryGetValue(index, out var data))
+            {
+                throw new KeyNotFoundException($"ID {id} not found");
+            }
+            
+            return data;
+        }
+        set
+        {
+            var trajectoryId = id.GetTrajectoryId();
+            var index = id.GetIndex();
+            
+            if (!_trajectories.TryGetValue(trajectoryId, out var trajectory))
+            {
+                trajectory = new MapByIndex();
+                _trajectories[trajectoryId] = trajectory;
+            }
+            
+            trajectory.Data[index] = value;
+        }
+    }
+
+    /// <summary>
+    /// Returns an iterator to the first element in the trajectory.
+    /// </summary>
+    public IEnumerable<IdDataReference> BeginOfTrajectory(int trajectoryId)
+    {
+        if (!_trajectories.TryGetValue(trajectoryId, out var trajectory))
+        {
+            yield break;
+        }
+
+        // Create snapshot to avoid concurrent modification exception
+        foreach (var kvp in trajectory.Data.ToList())
+        {
+            TId id;
+            if (typeof(TId) == typeof(NodeId))
+            {
+                id = (TId)(object)new NodeId(trajectoryId, kvp.Key);
+            }
+            else if (typeof(TId) == typeof(SubmapId))
+            {
+                id = (TId)(object)new SubmapId(trajectoryId, kvp.Key);
+            }
+            else
+            {
+                throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+            }
+            yield return new IdDataReference(id, kvp.Value);
+        }
+    }
+
+    /// <summary>
+    /// Returns the size of a trajectory, or 0 if it doesn't exist.
+    /// </summary>
+    public int SizeOfTrajectoryOrZero(int trajectoryId)
+    {
+        return _trajectories.TryGetValue(trajectoryId, out var trajectory)
+            ? trajectory.Data.Count
+            : 0;
+    }
+
+    /// <summary>
+    /// Returns the first element in the trajectory whose time is not before 'time',
+    /// or null if the trajectory is empty or all elements are before 'time'.
+    /// Match C++ MapById::lower_bound(trajectory_id, time) using internal::GetTime(data).
+    /// </summary>
+    /// <param name="trajectoryId">Trajectory ID.</param>
+    /// <param name="time">Time (e.g. common::Time in C++).</param>
+    /// <param name="getTime">Function to get time from TData (e.g. node => node.Time).</param>
+    /// <returns>First IdDataReference with getTime(data) >= time, or null if none.</returns>
+    public IdDataReference? LowerBound(int trajectoryId, long time, Func<TData, long> getTime)
+    {
+        if (getTime == null)
+        {
+            throw new ArgumentNullException(nameof(getTime));
+        }
+
+        if (!_trajectories.TryGetValue(trajectoryId, out var trajectory) || trajectory.Data.Count == 0)
+        {
+            return null;
+        }
+
+        // SortedDictionary orders by key (index); get ordered indices
+        var indices = trajectory.Data.Keys.ToList();
+
+        // If last element's time is before 'time', return null (past end)
+        var lastIndex = indices[^1];
+        var lastData = trajectory.Data[lastIndex];
+        if (getTime(lastData) < time)
+        {
+            return null;
+        }
+
+        // Binary search: first index i such that getTime(Data[indices[i]]) >= time
+        int left = 0;
+        int right = indices.Count - 1;
+        while (left < right)
+        {
+            int mid = left + (right - left) / 2;
+            var midIndex = indices[mid];
+            var midData = trajectory.Data[midIndex];
+            if (getTime(midData) < time)
+            {
+                left = mid + 1;
+            }
+            else
+            {
+                right = mid;
+            }
+        }
+
+        var index = indices[left];
+        var data = trajectory.Data[index];
+        TId id;
+        if (typeof(TId) == typeof(NodeId))
+        {
+            id = (TId)(object)new NodeId(trajectoryId, index);
+        }
+        else if (typeof(TId) == typeof(SubmapId))
+        {
+            id = (TId)(object)new SubmapId(trajectoryId, index);
+        }
+        else
+        {
+            throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+        }
+        return new IdDataReference(id, data);
+    }
+
+    /// <summary>
+    /// Gets the last element in a trajectory (matching C++ std::prev(end_it)).
+    /// Returns null if trajectory doesn't exist or is empty.
+    /// CRITICAL FIX: Use Last() on SortedDictionary.Keys instead of Max() for better performance.
+    /// </summary>
+    public IdDataReference? GetLastOfTrajectory(int trajectoryId)
+    {
+        if (!_trajectories.TryGetValue(trajectoryId, out var trajectory) || trajectory.Data.Count == 0)
+        {
+            return null;
+        }
+
+        // CRITICAL FIX: SortedDictionary maintains sorted order, so use Last() instead of Max()
+        // Max() iterates through all keys, while Last() is O(1) for SortedDictionary
+        // However, SortedDictionary.Keys doesn't have Last() directly, so we need to use Max()
+        // But we can optimize by checking if Count > 0 first (already done above)
+        var lastKey = trajectory.Data.Keys.Max(); // This is O(n) but necessary for SortedDictionary
+        var lastData = trajectory.Data[lastKey];
+        
+        TId id;
+        if (typeof(TId) == typeof(NodeId))
+        {
+            id = (TId)(object)new NodeId(trajectoryId, lastKey);
+        }
+        else if (typeof(TId) == typeof(SubmapId))
+        {
+            id = (TId)(object)new SubmapId(trajectoryId, lastKey);
+        }
+        else
+        {
+            throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+        }
+        
+        return new IdDataReference(id, lastData);
+    }
+
+    /// <summary>
+    /// Returns the total count of all elements.
+    /// </summary>
+    public int Count
+    {
+        get
+        {
+            return _trajectories.Values.Sum(t => t.Data.Count);
+        }
+    }
+
+    /// <summary>
+    /// Checks if the container is empty.
+    /// </summary>
+    public bool IsEmpty => _trajectories.Count == 0;
+
+    /// <summary>
+    /// Gets all trajectory IDs.
+    /// </summary>
+    public IEnumerable<int> TrajectoryIds => _trajectories.Keys;
+
+    public IEnumerator<IdDataReference> GetEnumerator()
+    {
+        // Create snapshot to avoid concurrent modification exception
+        // This prevents "Collection was modified after the enumerator was instantiated" error
+        // when the collection is modified during iteration (e.g., from another thread)
+        List<IdDataReference> snapshot = new List<IdDataReference>();
+
+        foreach (var trajectoryKvp in _trajectories.ToList())
+        {
+            var trajectoryId = trajectoryKvp.Key;
+            foreach (var dataKvp in trajectoryKvp.Value.Data.ToList())
+            {
+                TId id;
+                if (typeof(TId) == typeof(NodeId))
+                {
+                    id = (TId)(object)new NodeId(trajectoryId, dataKvp.Key);
+                }
+                else if (typeof(TId) == typeof(SubmapId))
+                {
+                    id = (TId)(object)new SubmapId(trajectoryId, dataKvp.Key);
+                }
+                else
+                {
+                    throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+                }
+                snapshot.Add(new IdDataReference(id, dataKvp.Value));
+            }
+        }
+
+        return snapshot.GetEnumerator();
+    }
+
+    /// <summary>
+    /// Enumerates all elements without creating a snapshot.
+    /// ONLY safe to use when the caller holds the lock that protects this collection
+    /// from concurrent modification (e.g., inside lock(_dataLock)).
+    /// Avoids O(N) allocation that GetEnumerator() incurs.
+    /// </summary>
+    public IEnumerable<IdDataReference> EnumerateUnsafe()
+    {
+        foreach (var trajectoryKvp in _trajectories)
+        {
+            var trajectoryId = trajectoryKvp.Key;
+            foreach (var dataKvp in trajectoryKvp.Value.Data)
+            {
+                TId id;
+                if (typeof(TId) == typeof(NodeId))
+                {
+                    id = (TId)(object)new NodeId(trajectoryId, dataKvp.Key);
+                }
+                else if (typeof(TId) == typeof(SubmapId))
+                {
+                    id = (TId)(object)new SubmapId(trajectoryId, dataKvp.Key);
+                }
+                else
+                {
+                    throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+                }
+                yield return new IdDataReference(id, dataKvp.Value);
+            }
+        }
+    }
+
+    IEnumerator IEnumerable.GetEnumerator()
+    {
+        return GetEnumerator();
+    }
+
+    #region Copy-on-Write Support Methods
+
+    /// <summary>
+    /// Creates a shallow clone of this MapById.
+    /// The new container has its own data structures but references the same TData objects.
+    /// For value types (structs), this effectively creates independent copies.
+    /// Used for Copy-on-Write optimization pattern.
+    /// </summary>
+    /// <returns>A new MapById with the same data.</returns>
+    public MapById<TId, TData> ShallowClone()
+    {
+        var clone = new MapById<TId, TData>();
+        foreach (var trajectoryKvp in _trajectories)
+        {
+            var trajectoryId = trajectoryKvp.Key;
+            foreach (var dataKvp in trajectoryKvp.Value.Data)
+            {
+                TId id;
+                if (typeof(TId) == typeof(NodeId))
+                {
+                    id = (TId)(object)new NodeId(trajectoryId, dataKvp.Key);
+                }
+                else if (typeof(TId) == typeof(SubmapId))
+                {
+                    id = (TId)(object)new SubmapId(trajectoryId, dataKvp.Key);
+                }
+                else
+                {
+                    throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+                }
+                clone.Insert(id, dataKvp.Value);
+            }
+        }
+        return clone;
+    }
+
+    /// <summary>
+    /// Clears all data from this MapById.
+    /// </summary>
+    public void Clear()
+    {
+        _trajectories.Clear();
+    }
+
+    /// <summary>
+    /// Replaces all data in this MapById with data from another MapById.
+    /// This is an atomic operation for Copy-on-Write pattern - clears existing data
+    /// and copies all entries from the source.
+    /// </summary>
+    /// <param name="source">The source MapById to copy from.</param>
+    public void ReplaceWith(MapById<TId, TData> source)
+    {
+        _trajectories.Clear();
+        foreach (var trajectoryKvp in source._trajectories)
+        {
+            var trajectoryId = trajectoryKvp.Key;
+            foreach (var dataKvp in trajectoryKvp.Value.Data)
+            {
+                TId id;
+                if (typeof(TId) == typeof(NodeId))
+                {
+                    id = (TId)(object)new NodeId(trajectoryId, dataKvp.Key);
+                }
+                else if (typeof(TId) == typeof(SubmapId))
+                {
+                    id = (TId)(object)new SubmapId(trajectoryId, dataKvp.Key);
+                }
+                else
+                {
+                    throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+                }
+                Insert(id, dataKvp.Value);
+            }
+        }
+    }
+
+    /// <summary>
+    /// Gets all IDs that exist in this MapById but not in another MapById.
+    /// Useful for finding nodes added during Copy-on-Write preparation phase.
+    /// </summary>
+    /// <param name="other">The other MapById to compare against.</param>
+    /// <returns>IDs that exist in this but not in other.</returns>
+    public IEnumerable<TId> GetIdsDifference(MapById<TId, TData> other)
+    {
+        foreach (var trajectoryKvp in _trajectories)
+        {
+            var trajectoryId = trajectoryKvp.Key;
+            foreach (var dataKvp in trajectoryKvp.Value.Data)
+            {
+                TId id;
+                if (typeof(TId) == typeof(NodeId))
+                {
+                    id = (TId)(object)new NodeId(trajectoryId, dataKvp.Key);
+                }
+                else if (typeof(TId) == typeof(SubmapId))
+                {
+                    id = (TId)(object)new SubmapId(trajectoryId, dataKvp.Key);
+                }
+                else
+                {
+                    throw new NotSupportedException($"ID type {typeof(TId)} is not supported");
+                }
+
+                if (!other.Contains(id))
+                {
+                    yield return id;
+                }
+            }
+        }
+    }
+
+    /// <summary>
+    /// Estimates memory usage in bytes for this MapById.
+    /// Used for memory overhead analysis.
+    /// </summary>
+    /// <param name="dataSize">Size of each TData element in bytes.</param>
+    /// <returns>Estimated memory usage in bytes.</returns>
+    public long EstimateMemoryUsageBytes(int dataSize)
+    {
+        // Base overhead for MapById object + _trajectories SortedDictionary
+        long baseOverhead = 64;
+
+        // Per-trajectory overhead: MapByIndex object + SortedDictionary
+        long perTrajectoryOverhead = 48;
+
+        // Per-entry overhead: SortedDictionary node (~40 bytes) + key (4 bytes)
+        long perEntryOverhead = 44;
+
+        long total = baseOverhead;
+        total += _trajectories.Count * perTrajectoryOverhead;
+        total += Count * (perEntryOverhead + dataSize);
+
+        return total;
+    }
+
+    #endregion
+}
+
+/// <summary>
+/// Interface for ID types used with MapById.
+/// </summary>
+public interface IIdType
+{
+    int GetTrajectoryId();
+    int GetIndex();
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/PoseExtrapolator.cs

@@ -0,0 +1,856 @@
+/*
+ * Copyright 2017 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Keep poses for a certain duration to estimate linear and angular velocity.
+/// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
+/// available to improve the extrapolation.
+/// Match C++: cartographer/mapping/pose_extrapolator.h/cc
+/// </summary>
+public class PoseExtrapolator(long poseQueueDuration, double imuGravityTimeConstant) : IPoseExtrapolator
+{
+    private struct TimedPose(long time, Rigid3d pose)
+    {
+        public long Time { get; set; } = time;
+        public Rigid3d Pose { get; set; } = pose;
+    }
+
+    // Use List instead of Queue for random access (similar to std::deque)
+    private readonly List<TimedPose> _timedPoseQueue = [];
+    private Vector3 _linearVelocityFromPoses = Vector3.Zero;
+    private Vector3 _angularVelocityFromPoses = Vector3.Zero;
+
+    private readonly List<ImuData> _imuData = [];
+    private readonly List<OdometryData> _odometryData = [];
+    private Vector3 _linearVelocityFromOdometry = Vector3.Zero;
+    private Vector3 _angularVelocityFromOdometry = Vector3.Zero;
+    private Vector3 _instantAngularVelocityFromOdometry = Vector3.Zero;
+
+    // Lock for thread-safe access to _imuData and _odometryData
+    private readonly Lock _dataLock = new();
+
+    private readonly double _gravityTimeConstant = imuGravityTimeConstant;
+    private ImuTracker? _imuTracker;
+    private ImuTracker? _odometryImuTracker;
+    private ImuTracker? _extrapolationImuTracker;
+
+    // Match C++: cached_extrapolated_pose_ and cached_extrapolated_pose_filter
+    private TimedPose? _cachedExtrapolatedPose;
+    private TimedPose? _cachedExtrapolatedPoseFilter;
+
+    // Odometry trajectory-based extrapolation: reference odometry pose at last AddPose time
+    // and cached rotation from odom frame to global frame
+    private OdometryData? _odometryAtLastPose;
+    private Quaternion _odomToGlobalRotation = Quaternion.Identity;
+
+
+    /// <summary>
+    /// Match C++: InitializeWithImu factory method
+    /// </summary>
+    public static PoseExtrapolator InitializeWithImu(
+        long poseQueueDuration,
+        double imuGravityTimeConstant,
+        ImuData imuData)
+    {
+        var extrapolator = new PoseExtrapolator(poseQueueDuration, imuGravityTimeConstant);
+        extrapolator.AddImuData(imuData);
+
+        // Initialize ImuTracker with first IMU data
+        extrapolator._imuTracker = new ImuTracker(imuGravityTimeConstant, imuData.Time);
+        extrapolator._imuTracker.AddImuLinearAccelerationObservation(imuData.LinearAcceleration);
+        extrapolator._imuTracker.AddImuAngularVelocityObservation(imuData.AngularVelocity);
+        extrapolator._imuTracker.Advance(imuData.Time);
+
+        // Add initial pose with rotation from IMU tracker
+        extrapolator.AddPose(
+            imuData.Time,
+            Rigid3d.FromRotation(extrapolator._imuTracker.Orientation));
+
+        return extrapolator;
+    }
+
+    /// <summary>
+    /// Returns diagnostic info about current odom/IMU data status for debugging.
+    /// </summary>
+    public (int OdomCount, int ImuCount, long LastOdomTime, long LastPoseTime, Vector3 LinVelOdom, Vector3 LinVelPose) GetDiagnostics()
+    {
+        lock (_dataLock)
+        {
+            return (
+                OdomCount: _odometryData.Count,
+                ImuCount: _imuData.Count,
+                LastOdomTime: _odometryData.Count > 0 ? _odometryData[^1].Time : 0,
+                LastPoseTime: _timedPoseQueue.Count > 0 ? _timedPoseQueue[^1].Time : 0,
+                LinVelOdom: _linearVelocityFromOdometry,
+                LinVelPose: _linearVelocityFromPoses
+            );
+        }
+    }
+
+    /// <summary>
+    /// Returns detailed extrapolation breakdown for diagnostics.
+    /// Match C++: velocity × dt extrapolation.
+    /// </summary>
+    public string GetExtrapolationBreakdown(long time)
+    {
+        if (_timedPoseQueue.Count == 0) return "NO_POSES";
+
+        var newestTimedPose = _timedPoseQueue[^1];
+        var extrapolationDelta = (time - newestTimedPose.Time) / 10_000_000.0;
+
+        lock (_dataLock)
+        {
+            var angVelOdom = _angularVelocityFromOdometry;
+            var basePose = newestTimedPose.Pose.Translation;
+
+            if (_odometryData.Count >= 2 && _odometryAtLastPose.HasValue)
+            {
+                var odomRef = _odometryAtLastPose.Value;
+                var idx = FindOdometryIndexBeforeTime(time);
+                var odomWindow = (_odometryData[^1].Time - _odometryData[0].Time) / 10_000_000.0;
+
+                if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
+                {
+                    var odomAtTime = _odometryData[idx];
+                    var displacementOdom = odomAtTime.Pose.Translation - odomRef.Pose.Translation;
+                    var displacementGlobal = Vector3.Transform(displacementOdom, _odomToGlobalRotation);
+                    var tailDt = (time - odomAtTime.Time) / 10_000_000.0;
+                    return $"PATH=odom_trajectory, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
+                           $"displ=({displacementGlobal.X:F4},{displacementGlobal.Y:F4}), tailDt={tailDt:F4}s, " +
+                           $"odomWindow={odomWindow:F3}s, odomCount={_odometryData.Count}, " +
+                           $"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
+                }
+                else
+                {
+                    var vel = _linearVelocityFromOdometry;
+                    return $"PATH=odom_vel_fallback, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
+                           $"vel=({vel.X:F4},{vel.Y:F4}), odomWindow={odomWindow:F3}s, odomCount={_odometryData.Count}, " +
+                           $"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
+                }
+            }
+            else
+            {
+                var vel = _linearVelocityFromPoses;
+                return $"PATH=pose_vel, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
+                       $"vel=({vel.X:F4},{vel.Y:F4}), odomCount={_odometryData.Count}, " +
+                       $"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
+            }
+        }
+    }
+
+    /// <summary>
+    /// Returns the time span (ms) covered by the current odometry data window [oldest, newest].
+    /// Large values relative to scan period indicate odom accumulated during a processing
+    /// delay (e.g., grid resize blocking the SLAM thread in InsertIntoSubmap).
+    /// </summary>
+    public double GetOdometryWindowMs()
+    {
+        lock (_dataLock)
+        {
+            if (_odometryData.Count < 2) return 0;
+            return (_odometryData[^1].Time - _odometryData[0].Time) / 10_000.0;
+        }
+    }
+
+    /// <summary>
+    /// Match C++: GetLastPoseTime
+    /// </summary>
+    public long GetLastPoseTime()
+    {
+        return _timedPoseQueue.Count > 0 ? _timedPoseQueue[^1].Time : long.MinValue;
+    }
+
+    /// <summary>
+    /// Match C++: GetLastExtrapolatedTime
+    /// </summary>
+    public long GetLastExtrapolatedTime()
+    {
+        return _extrapolationImuTracker?.Time ?? long.MinValue;
+    }
+
+    /// <summary>
+    /// Match C++: AddPose (pose_extrapolator.cc:70-91)
+    /// </summary>
+    public void AddPose(long time, Rigid3d pose)
+    {
+        // Match C++ lines 72-79: Initialize ImuTracker if needed
+        if (_imuTracker == null)
+        {
+            long trackerStart = time;
+            lock (_dataLock)
+            {
+                if (_imuData.Count > 0)
+                {
+                    trackerStart = Math.Min(trackerStart, _imuData[0].Time);
+                }
+            }
+            _imuTracker = new ImuTracker(_gravityTimeConstant, trackerStart);
+        }
+
+        // Match C++ line 80: Add pose to queue
+        _timedPoseQueue.Add(new TimedPose(time, pose));
+
+        // Match C++ lines 81-84: Trim queue to keep at least 2 poses
+        while (_timedPoseQueue.Count > 2 &&
+               _timedPoseQueue[1].Time <= time - poseQueueDuration)
+        {
+            _timedPoseQueue.RemoveAt(0);
+        }
+
+        // Match C++ line 85: Update velocities from poses
+        UpdateVelocitiesFromPoses();
+
+        // Match C++ lines 86-88: Advance IMU tracker and trim data
+        // FIX: Extended lock scope to include ImuTracker copies to prevent race condition with AddOdometryData
+        lock (_dataLock)
+        {
+            AdvanceImuTracker(time, _imuTracker);
+            TrimImuData();
+            TrimOdometryData();
+
+            // Save reference odometry pose for trajectory-based extrapolation
+            if (_odometryData.Count > 0)
+            {
+                _odometryAtLastPose = _odometryData[0];
+                _odomToGlobalRotation = pose.Rotation * Quaternion.Inverse(_odometryData[0].Pose.Rotation);
+            }
+            else
+            {
+                _odometryAtLastPose = null;
+            }
+
+            // Match C++ lines 89-90: Create copies of ImuTracker for odometry and extrapolation
+            // These must be created inside lock to prevent race condition with AddOdometryData
+            _odometryImuTracker = new ImuTracker(_imuTracker);
+            _extrapolationImuTracker = new ImuTracker(_imuTracker);
+        }
+
+        // Invalidate cache when new pose is added
+        _cachedExtrapolatedPose = null;
+
+    }
+
+    /// <summary>
+    /// Match C++: AddImuData (pose_extrapolator.cc:93-98)
+    /// </summary>
+    public void AddImuData(ImuData imuData)
+    {
+        // Match C++ lines 94-95: CHECK that IMU time >= last pose time
+        if (_timedPoseQueue.Count > 0 && imuData.Time < _timedPoseQueue[^1].Time)
+        {
+            var timeDiffMs = (_timedPoseQueue[^1].Time - imuData.Time) / TimeSpan.TicksPerMillisecond;
+            if(timeDiffMs > 5)
+            {
+                throw new ArgumentException($"IMU data time ({imuData.Time}) must be >= last pose time ({_timedPoseQueue[^1].Time}), diff={timeDiffMs}ms");
+            }
+            else
+            {
+                return; // Ignore slightly out-of-order IMU data
+            }
+        }
+
+        lock (_dataLock)
+        {
+            _imuData.Add(imuData);
+            TrimImuData();
+        }
+    }
+
+    /// <summary>
+    /// Match C++: AddOdometryData (pose_extrapolator.cc:100-144)
+    /// CRITICAL FIX: Do NOT reset odometry_imu_tracker_ - let it accumulate state
+    /// </summary>
+    public void AddOdometryData(OdometryData odometryData)
+    {
+        // Match C++ lines 105-106: CHECK that odometry time >= last pose time
+        if (_timedPoseQueue.Count > 0 && odometryData.Time < _timedPoseQueue[^1].Time)
+        {
+            var timeDiffMs = (_timedPoseQueue[^1].Time - odometryData.Time) / TimeSpan.TicksPerMillisecond;
+            if(timeDiffMs > 5)
+            {
+                throw new ArgumentException($"Odometry data time ({odometryData.Time}) must be >= last pose time ({_timedPoseQueue[^1].Time}), diff={timeDiffMs}ms");
+            }
+            else
+            {
+                return; // Ignore slightly out-of-order odometry data
+            }
+        }
+
+        // FIX: Use single lock scope to prevent race condition with AddPose modifying _odometryImuTracker
+        // Between two separate lock blocks, _odometryImuTracker could be replaced by AddPose from SLAM thread
+        lock (_dataLock)
+        {
+            // Match C++ line 107-108: Add to queue and trim
+            _odometryData.Add(odometryData);
+            TrimOdometryData();
+
+            // Match C++ lines 109-111: Need at least 2 odometry data points
+            if (_odometryData.Count < 2)
+            {
+                return;
+            }
+
+            // Match C++: Use front() and back() of the queue (full window since last AddPose)
+            var odometryDataOldest = _odometryData[0];
+            var odometryDataNewest = _odometryData[^1];
+
+            // Match C++ lines 116-117: Compute time delta
+            var odometryTimeDelta = (odometryDataOldest.Time - odometryDataNewest.Time) / 10_000_000.0;
+
+            // Safeguard: Skip if time delta is too small (< 1ms) to avoid noisy velocity estimates
+            if (Math.Abs(odometryTimeDelta) < 0.001)
+            {
+                return;
+            }
+
+            // Match C++ lines 118-119: Compute pose delta
+            var odometryPoseDelta = odometryDataNewest.Pose.Inverse() * odometryDataOldest.Pose;
+
+            // Match C++ lines 120-123: Compute angular velocity from odometry
+            var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(odometryPoseDelta.Rotation);
+            _angularVelocityFromOdometry = new Vector3(
+                angleAxis.X / odometryTimeDelta,
+                angleAxis.Y / odometryTimeDelta,
+                angleAxis.Z / odometryTimeDelta);
+
+            // Instantaneous angular velocity from last 2 samples for tail-gap extrapolation
+            if (_odometryData.Count >= 2)
+            {
+                var prevOdom = _odometryData[^2];
+                var currOdom = _odometryData[^1];
+                var dtInstant = (currOdom.Time - prevOdom.Time) / 10_000_000.0;
+                if (dtInstant > 0.001)
+                {
+                    var instantDelta = Quaternion.Inverse(prevOdom.Pose.Rotation) * currOdom.Pose.Rotation;
+                    var instantAxis = TransformOperations.RotationQuaternionToAngleAxisVector(instantDelta);
+                    _instantAngularVelocityFromOdometry = instantAxis / dtInstant;
+                }
+            }
+
+            // Match C++ lines 124-126: Return if no poses yet
+            if (_timedPoseQueue.Count == 0)
+            {
+                return;
+            }
+
+            // Match C++ lines 127-129: Compute linear velocity in tracking frame
+            var linearVelocityInTrackingFrameAtNewestOdometryTime = new Vector3(
+                odometryPoseDelta.Translation.X / odometryTimeDelta,
+                odometryPoseDelta.Translation.Y / odometryTimeDelta,
+                odometryPoseDelta.Translation.Z / odometryTimeDelta);
+
+            // Match C++ lines 130-133: Compute orientation at newest odometry time
+            // FIX: _odometryImuTracker is now accessed within the same lock scope
+            if (_odometryImuTracker != null)
+            {
+                var newestTimedPose = _timedPoseQueue[^1];
+                var orientationAtNewestOdometryTime =
+                    newestTimedPose.Pose.Rotation *
+                    ExtrapolateRotation(odometryDataNewest.Time, _odometryImuTracker);
+
+                // Match C++ lines 134-136: Transform to global frame
+                _linearVelocityFromOdometry = Vector3.Transform(
+                    linearVelocityInTrackingFrameAtNewestOdometryTime,
+                    orientationAtNewestOdometryTime);
+            }
+            else
+            {
+                // Fallback: use pose rotation directly without IMU extrapolation
+                var newestTimedPose = _timedPoseQueue[^1];
+                _linearVelocityFromOdometry = Vector3.Transform(
+                    linearVelocityInTrackingFrameAtNewestOdometryTime,
+                    newestTimedPose.Pose.Rotation);
+            }
+        }
+    }
+
+    /// <summary>
+    /// Match C++: ExtrapolatePose (pose_extrapolator.cc:195-208)
+    /// </summary>
+    public Rigid3d ExtrapolatePose(long time)
+    {
+        if (_timedPoseQueue.Count == 0)
+        {
+            return Rigid3d.Identity;
+        }
+
+        // Match C++ line 196-197
+        var newestTimedPose = _timedPoseQueue[^1];
+        if (time < newestTimedPose.Time)
+        {
+            var timeDiffMs = (newestTimedPose.Time - time) / TimeSpan.TicksPerMillisecond;
+            throw new ArgumentException($"time ({time}) must be >= newest pose time ({newestTimedPose.Time}), diff={timeDiffMs}ms", nameof(time));
+        }
+
+        // Match C++ line 198: if (cached_extrapolated_pose_.time != time)
+        if (_cachedExtrapolatedPose.HasValue && _cachedExtrapolatedPose.Value.Time == time)
+        {
+            return _cachedExtrapolatedPose.Value.Pose;
+        }
+
+        // Match C++ lines 199-200: Compute translation
+        var translation = ExtrapolateTranslation(time) + newestTimedPose.Pose.Translation;
+
+        // Match C++ lines 201-203: Compute rotation
+        Quaternion rotation;
+        lock (_dataLock)
+        {
+            if (_extrapolationImuTracker == null)
+            {
+                rotation = newestTimedPose.Pose.Rotation;
+            }
+            else if (_imuData.Count == 0)
+            {
+                // No IMU: prefer trajectory-based rotation from odometry
+                var odomRotation = ExtrapolateRotationFromOdometry(time);
+                rotation = odomRotation.HasValue
+                    ? newestTimedPose.Pose.Rotation * odomRotation.Value
+                    : newestTimedPose.Pose.Rotation * ExtrapolateRotation(time, _extrapolationImuTracker);
+            }
+            else
+            {
+                rotation = newestTimedPose.Pose.Rotation *
+                           ExtrapolateRotation(time, _extrapolationImuTracker);
+            }
+        }
+
+        // Match C++ lines 204-205: Cache result
+        _cachedExtrapolatedPose = new TimedPose(time, new Rigid3d(translation, rotation));
+        return _cachedExtrapolatedPose.Value.Pose;
+    }
+
+    /// <summary>
+    /// Match C++: ExtrapolatePose_filter (pose_extrapolator.cc:146-193)
+    /// Returns filtered pose with low-pass filter to reduce jitter during direction changes.
+    /// </summary>
+    public Rigid3d ExtrapolatePoseFilter(long time)
+    {
+        if (_timedPoseQueue.Count == 0)
+        {
+            return Rigid3d.Identity;
+        }
+
+        var newestTimedPose = _timedPoseQueue[^1];
+        if (time < newestTimedPose.Time)
+        {
+            var timeDiffMs = (newestTimedPose.Time - time) / TimeSpan.TicksPerMillisecond;
+            throw new ArgumentException($"time ({time}) must be >= newest pose time ({newestTimedPose.Time}), diff={timeDiffMs}ms", nameof(time));
+        }
+
+        // Match C++ line 149: if (cached_extrapolated_pose_.time != time)
+        if (!_cachedExtrapolatedPose.HasValue || _cachedExtrapolatedPose.Value.Time != time)
+        {
+            // Match C++ lines 150-156: Compute translation and rotation, update cached_extrapolated_pose_
+            var translation = ExtrapolateTranslation(time) + newestTimedPose.Pose.Translation;
+
+            Quaternion rotation;
+            lock (_dataLock)
+            {
+                if (_extrapolationImuTracker == null || _imuTracker == null)
+                {
+                    rotation = newestTimedPose.Pose.Rotation;
+                }
+                else if (_imuData.Count == 0)
+                {
+                    // No IMU: prefer trajectory-based rotation from odometry
+                    var odomRotation = ExtrapolateRotationFromOdometry(time);
+                    rotation = odomRotation.HasValue
+                        ? newestTimedPose.Pose.Rotation * odomRotation.Value
+                        : newestTimedPose.Pose.Rotation * ExtrapolateRotation(time, _extrapolationImuTracker);
+                }
+                else
+                {
+                    rotation = newestTimedPose.Pose.Rotation *
+                               ExtrapolateRotation(time, _extrapolationImuTracker);
+                }
+            }
+
+            _cachedExtrapolatedPose = new TimedPose(time, new Rigid3d(translation, rotation));
+
+            // Match C++ lines 159-190: Update cached_extrapolated_pose_filter
+            var extrapolationDeltaFilterTime = _cachedExtrapolatedPoseFilter.HasValue
+                ? (time - _cachedExtrapolatedPoseFilter.Value.Time) / 10_000_000.0
+                : double.PositiveInfinity;
+
+            Vector3 translationFilter;
+
+            if (extrapolationDeltaFilterTime < 0.1 && _cachedExtrapolatedPoseFilter.HasValue)
+            {
+                // Match C++ lines 163-171: Apply low-pass filter
+                Vector3 linearVelocity;
+                lock (_dataLock)
+                {
+                    linearVelocity = _odometryData.Count < 2
+                        ? _linearVelocityFromPoses
+                        : _linearVelocityFromOdometry;
+                }
+
+                translationFilter = _cachedExtrapolatedPoseFilter.Value.Pose.Translation +
+                    new Vector3(
+                        extrapolationDeltaFilterTime * linearVelocity.X,
+                        extrapolationDeltaFilterTime * linearVelocity.Y,
+                        extrapolationDeltaFilterTime * linearVelocity.Z);
+
+                // Match C++ lines 173-180: Check delta and blend
+                var deltaTrans = translationFilter - _cachedExtrapolatedPose.Value.Pose.Translation;
+
+                if (deltaTrans.Length() < 0.03)
+                {
+                    translationFilter = 0.7 * translationFilter + 0.3 * _cachedExtrapolatedPose.Value.Pose.Translation;
+                }
+                else
+                {
+                    translationFilter = _cachedExtrapolatedPose.Value.Pose.Translation;
+                }
+            }
+            else
+            {
+                // Match C++ lines 187-189: No filter, use extrapolated pose directly
+                translationFilter = _cachedExtrapolatedPose.Value.Pose.Translation;
+            }
+
+            // Match C++ lines 182-183, 188-189: Cache filtered pose
+            _cachedExtrapolatedPoseFilter = new TimedPose(time,
+                new Rigid3d(translationFilter, _cachedExtrapolatedPose.Value.Pose.Rotation));
+        }
+
+        // Match C++ line 192: return cached_extrapolated_pose_filter.pose
+        return _cachedExtrapolatedPoseFilter!.Value.Pose;
+    }
+
+    /// <summary>
+    /// Match C++: ExtrapolatePosesWithGravity (pose_extrapolator.cc:306-320)
+    /// </summary>
+    public ExtrapolationResult ExtrapolatePosesWithGravity(List<long> times)
+    {
+        var previousPoses = new List<Rigid3f>();
+
+        for (int i = 0; i < times.Count - 1; i++)
+        {
+            var pose = ExtrapolatePose(times[i]);
+            previousPoses.Add(new Rigid3f(
+                new Vector3(pose.Translation.X, pose.Translation.Y, pose.Translation.Z),
+                pose.Rotation));
+        }
+
+        var currentPose = ExtrapolatePose(times[^1]);
+
+        Vector3 currentVelocity;
+        lock (_dataLock)
+        {
+            currentVelocity = _odometryData.Count < 2
+                ? _linearVelocityFromPoses
+                : _linearVelocityFromOdometry;
+        }
+
+        return new ExtrapolationResult
+        {
+            PreviousPoses = previousPoses,
+            CurrentPose = currentPose,
+            CurrentVelocity = currentVelocity,
+            GravityFromTracking = EstimateGravityOrientation(times[^1])
+        };
+    }
+
+    /// <summary>
+    /// Match C++: EstimateGravityOrientation (pose_extrapolator.cc:210-215)
+    /// </summary>
+    public Quaternion EstimateGravityOrientation(long time)
+    {
+        // Match C++ line 212: ImuTracker imu_tracker = *imu_tracker_;
+        // C++ assumes imu_tracker_ is initialized (via AddPose or InitializeWithImu)
+        if (_imuTracker == null)
+        {
+            throw new InvalidOperationException("ImuTracker not initialized. Call AddPose or InitializeWithImu first.");
+        }
+
+        var imuTracker = new ImuTracker(_imuTracker);
+
+        // Match C++ line 213: AdvanceImuTracker(time, &imu_tracker);
+        lock (_dataLock)
+        {
+            AdvanceImuTracker(time, imuTracker);
+        }
+
+        // Match C++ line 214: return imu_tracker.orientation();
+        return imuTracker.Orientation;
+    }
+
+    /// <summary>
+    /// Match C++: TrimImuData (pose_extrapolator.cc:243-248)
+    /// </summary>
+    private void TrimImuData()
+    {
+        while (_imuData.Count > 1 && _timedPoseQueue.Count > 0 &&
+               _imuData[1].Time <= _timedPoseQueue[^1].Time)
+        {
+            _imuData.RemoveAt(0);
+        }
+    }
+
+    /// <summary>
+    /// Match C++: TrimOdometryData (pose_extrapolator.cc:250-255)
+    /// </summary>
+    private void TrimOdometryData()
+    {
+        while (_odometryData.Count > 2 && _timedPoseQueue.Count > 0 &&
+               _odometryData[1].Time <= _timedPoseQueue[^1].Time)
+        {
+            _odometryData.RemoveAt(0);
+        }
+    }
+
+    /// <summary>
+    /// Match C++: UpdateVelocitiesFromPoses (pose_extrapolator.cc:217-241)
+    /// </summary>
+    private void UpdateVelocitiesFromPoses()
+    {
+        if (_timedPoseQueue.Count < 2)
+        {
+            return;
+        }
+
+        var newestTimedPose = _timedPoseQueue[^1];
+        var newestTime = newestTimedPose.Time;
+        var oldestTimedPose = _timedPoseQueue[0];
+        var oldestTime = oldestTimedPose.Time;
+
+        var queueDelta = (newestTime - oldestTime) / 10_000_000.0;
+        var requiredQueueDelta = poseQueueDuration / 10_000_000.0;
+
+        if (queueDelta < requiredQueueDelta)
+        {
+            return;
+        }
+
+        var newestPose = newestTimedPose.Pose;
+        var oldestPose = oldestTimedPose.Pose;
+        var translationDelta = newestPose.Translation - oldestPose.Translation;
+
+        _linearVelocityFromPoses = new Vector3(
+            translationDelta.X / queueDelta,
+            translationDelta.Y / queueDelta,
+            translationDelta.Z / queueDelta);
+
+        var rotationDelta = Quaternion.Inverse(oldestPose.Rotation) * newestPose.Rotation;
+        var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(rotationDelta);
+
+        _angularVelocityFromPoses = new Vector3(
+            angleAxis.X / queueDelta,
+            angleAxis.Y / queueDelta,
+            angleAxis.Z / queueDelta);
+    }
+
+    /// <summary>
+    /// Match C++: AdvanceImuTracker (pose_extrapolator.cc:257-286)
+    /// </summary>
+    private void AdvanceImuTracker(long time, ImuTracker imuTracker)
+    {
+        if (time < imuTracker.Time)
+        {
+            var timeDiffMs = (imuTracker.Time - time) / TimeSpan.TicksPerMillisecond;
+            throw new ArgumentException($"time ({time}) must be >= imuTracker time ({imuTracker.Time}), diff={timeDiffMs}ms", nameof(time));
+        }
+
+        if (_imuData.Count == 0 || time < _imuData[0].Time)
+        {
+            imuTracker.Advance(time);
+            imuTracker.AddImuLinearAccelerationObservation(Vector3.UnitZ);
+            var angularVel = _odometryData.Count < 2
+                ? _angularVelocityFromPoses
+                : _angularVelocityFromOdometry;
+            imuTracker.AddImuAngularVelocityObservation(angularVel);
+            return;
+        }
+
+        if (imuTracker.Time < _imuData[0].Time)
+        {
+            imuTracker.Advance(_imuData[0].Time);
+        }
+
+        int startIndex = 0;
+        for (int i = 0; i < _imuData.Count; i++)
+        {
+            if (_imuData[i].Time >= imuTracker.Time)
+            {
+                startIndex = i;
+                break;
+            }
+        }
+
+        for (int i = startIndex; i < _imuData.Count && _imuData[i].Time < time; i++)
+        {
+            if (_imuData[i].Time >= imuTracker.Time)
+            {
+                imuTracker.Advance(_imuData[i].Time);
+                imuTracker.AddImuLinearAccelerationObservation(_imuData[i].LinearAcceleration);
+                imuTracker.AddImuAngularVelocityObservation(_imuData[i].AngularVelocity);
+            }
+        }
+
+        if (time >= imuTracker.Time)
+        {
+            imuTracker.Advance(time);
+        }
+    }
+
+    /// <summary>
+    /// Match C++: ExtrapolateRotation (pose_extrapolator.cc:288-294)
+    /// </summary>
+    private Quaternion ExtrapolateRotation(long time, ImuTracker imuTracker)
+    {
+        // Match C++ line 290: CHECK_GE(time, imu_tracker->time());
+        if (time < imuTracker.Time)
+        {
+            var timeDiffMs = (imuTracker.Time - time) / TimeSpan.TicksPerMillisecond;
+            throw new ArgumentException($"time ({time}) must be >= imuTracker time ({imuTracker.Time}), diff={timeDiffMs}ms", nameof(time));
+        }
+
+        // Match C++ line 291: AdvanceImuTracker(time, imu_tracker);
+        AdvanceImuTracker(time, imuTracker);
+
+        // Match C++ lines 292-293: return last_orientation.inverse() * imu_tracker->orientation();
+        var lastOrientation = _imuTracker!.Orientation;
+        return Quaternion.Inverse(lastOrientation) * imuTracker.Orientation;
+    }
+
+    /// <summary>
+    /// Find the index of the latest odometry entry with Time &lt;= requested time.
+    /// Returns -1 if no suitable entry found.
+    /// Must be called under _dataLock.
+    /// </summary>
+    private int FindOdometryIndexBeforeTime(long time)
+    {
+        for (int i = _odometryData.Count - 1; i >= 0; i--)
+        {
+            if (_odometryData[i].Time <= time)
+                return i;
+        }
+        return -1;
+    }
+
+    /// <summary>
+    /// Extrapolate rotation using the actual odometry trajectory, mirroring ExtrapolateTranslation.
+    /// Returns the relative rotation delta (compatible with ExtrapolateRotation return value).
+    /// Returns null when no odometry data is available, signaling fallback to ImuTracker path.
+    /// Must be called under _dataLock.
+    /// </summary>
+    private Quaternion? ExtrapolateRotationFromOdometry(long time)
+    {
+        if (_odometryData.Count < 2 || !_odometryAtLastPose.HasValue)
+        {
+            return null;
+        }
+
+        var odomRef = _odometryAtLastPose.Value;
+
+        // Find latest odometry entry at or before requested time
+        var idx = FindOdometryIndexBeforeTime(time);
+
+        Quaternion deltaRotation;
+        long lastCoveredTime;
+
+        if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
+        {
+            // Exact rotation delta from odometry trajectory
+            var odomAtTime = _odometryData[idx];
+            deltaRotation = Quaternion.Inverse(odomRef.Pose.Rotation) * odomAtTime.Pose.Rotation;
+            lastCoveredTime = odomAtTime.Time;
+        }
+        else
+        {
+            deltaRotation = Quaternion.Identity;
+            lastCoveredTime = _timedPoseQueue[^1].Time;
+        }
+
+        // Instantaneous angular velocity extrapolation for the small gap after last odometry sample
+        if (time > lastCoveredTime)
+        {
+            var dtRemaining = (time - lastCoveredTime) / 10_000_000.0;
+            var tailAngleAxis = _instantAngularVelocityFromOdometry * dtRemaining;
+            var tailRotation = TransformOperations.AngleAxisVectorToRotationQuaternion(tailAngleAxis);
+            deltaRotation = Quaternion.Normalize(deltaRotation * tailRotation);
+        }
+
+        return deltaRotation;
+    }
+
+    /// <summary>
+    /// Extrapolate translation using the actual odometry trajectory for exact displacement,
+    /// with constant-velocity extrapolation only for the small gap after the last odometry sample.
+    /// Falls back to pose-based constant velocity when no odometry data is available.
+    /// </summary>
+    private Vector3 ExtrapolateTranslation(long time)
+    {
+        var newestTimedPose = _timedPoseQueue[^1];
+
+        lock (_dataLock)
+        {
+            if (_odometryData.Count < 2 || !_odometryAtLastPose.HasValue)
+            {
+                // No odometry: fall back to constant-velocity from poses
+                var dtFallback = (time - newestTimedPose.Time) / 10_000_000.0;
+                return new Vector3(
+                    dtFallback * _linearVelocityFromPoses.X,
+                    dtFallback * _linearVelocityFromPoses.Y,
+                    dtFallback * _linearVelocityFromPoses.Z);
+            }
+
+            var odomRef = _odometryAtLastPose.Value;
+
+            // Find latest odometry entry at or before requested time
+            var idx = FindOdometryIndexBeforeTime(time);
+
+            Vector3 displacementGlobal;
+            long lastCoveredTime;
+
+            if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
+            {
+                // Compute exact displacement from odometry trajectory (in odom frame)
+                var odomAtTime = _odometryData[idx];
+                var displacementOdom = odomAtTime.Pose.Translation - odomRef.Pose.Translation;
+
+                // Transform odom-frame displacement to global frame
+                displacementGlobal = Vector3.Transform(displacementOdom, _odomToGlobalRotation);
+                lastCoveredTime = odomAtTime.Time;
+            }
+            else
+            {
+                // No odometry data between reference and requested time
+                displacementGlobal = Vector3.Zero;
+                lastCoveredTime = newestTimedPose.Time;
+            }
+
+            // Constant-velocity extrapolation for the small gap after last odometry sample
+            if (time > lastCoveredTime)
+            {
+                var dtRemaining = (time - lastCoveredTime) / 10_000_000.0;
+                displacementGlobal += new Vector3(
+                    dtRemaining * _linearVelocityFromOdometry.X,
+                    dtRemaining * _linearVelocityFromOdometry.Y,
+                    dtRemaining * _linearVelocityFromOdometry.Z);
+            }
+
+            return displacementGlobal;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/PoseGraph.cs

@@ -0,0 +1,343 @@
+/*
+ * 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;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Base class for pose graph implementations.
+/// </summary>
+public abstract class PoseGraph : IPoseGraph
+{
+    /// <summary>
+    /// Initial trajectory pose information.
+    /// </summary>
+    public struct InitialTrajectoryPose(int toTrajectoryId, Rigid3d relativePose, long time)
+    {
+        public int ToTrajectoryId { get; set; } = toTrajectoryId;
+        public Rigid3d RelativePose { get; set; } = relativePose;
+        public long Time { get; set; } = time;
+    }
+
+    protected PoseGraph()
+    {
+    }
+
+    /// <summary>
+    /// Gets the total number of work items added to the work queue.
+    /// </summary>
+    public abstract int WorkItemsAdded { get; }
+
+    /// <summary>
+    /// Gets the total number of work items completed by the work queue.
+    /// </summary>
+    public abstract int WorkItemsCompleted { get; }
+
+    /// <summary>
+    /// Gets the number of work items currently pending in the work queue.
+    /// </summary>
+    public abstract int WorkItemsPending { get; }
+
+    /// <summary>
+    /// Gets the current number of items in the work queue.
+    /// </summary>
+    public abstract int WorkQueueCount { get; }
+
+    /// <summary>
+    /// Gets the number of nodes started in the constraint builder.
+    /// </summary>
+    public abstract int ConstraintBuilderNodesStarted { get; }
+
+    /// <summary>
+    /// Gets the number of nodes finished in the constraint builder.
+    /// </summary>
+    public abstract int ConstraintBuilderNodesFinished { get; }
+
+    /// <summary>
+    /// Gets the total number of trajectory nodes in the pose graph.
+    /// Used for progress tracking during optimization.
+    /// </summary>
+    public abstract int TrajectoryNodesCount { get; }
+
+    /// <summary>
+    /// Gets the total number of constraint tasks dispatched for scan matching.
+    /// </summary>
+    public abstract int ConstraintTasksTotal { get; }
+
+    /// <summary>
+    /// Gets the number of constraint tasks that have finished scan matching.
+    /// </summary>
+    public abstract int ConstraintTasksFinished { get; }
+
+    /// <summary>
+    /// Inserts an IMU measurement.
+    /// </summary>
+    public abstract void AddImuData(int trajectoryId, ImuData imuData);
+
+    /// <summary>
+    /// Inserts an odometry measurement.
+    /// </summary>
+    public abstract void AddOdometryData(int trajectoryId, OdometryData odometryData);
+
+    /// <summary>
+    /// Inserts a fixed frame pose measurement.
+    /// </summary>
+    public abstract void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData);
+
+    /// <summary>
+    /// Inserts landmarks observations.
+    /// </summary>
+    public abstract void AddLandmarkData(int trajectoryId, LandmarkData landmarkData);
+
+    /// <summary>
+    /// Drains the work queue to ensure all pending operations are completed.
+    /// This should be called before finishing a trajectory to avoid race conditions.
+    /// Default implementation does nothing (for pose graphs without work queues).
+    /// </summary>
+    public virtual void DrainWorkQueue()
+    {
+        // Default implementation: do nothing (for pose graphs without work queues)
+    }
+
+    /// <summary>
+    /// Finishes the given trajectory.
+    /// </summary>
+    public abstract void FinishTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Freezes a trajectory. Poses in this trajectory will not be optimized.
+    /// </summary>
+    public abstract void FreezeTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Adds a 'submap' from a proto with the given 'global_pose' to the
+    /// appropriate trajectory.
+    /// </summary>
+    public abstract void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap);
+
+    /// <summary>
+    /// Adds a 'node' from a proto with the given 'global_pose' to the
+    /// appropriate trajectory.
+    /// </summary>
+    public abstract void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node);
+
+    /// <summary>
+    /// Sets the trajectory data from a proto.
+    /// </summary>
+    public abstract void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data);
+
+    /// <summary>
+    /// Adds information that 'node_id' was inserted into 'submap_id'. The submap
+    /// has to be deserialized first.
+    /// </summary>
+    public abstract void AddNodeToSubmap(NodeId nodeId, SubmapId submapId);
+
+    /// <summary>
+    /// Adds serialized constraints. The corresponding trajectory nodes and submaps
+    /// have to be deserialized before calling this function.
+    /// </summary>
+    public abstract void AddSerializedConstraints(List<IPoseGraph.Constraint> constraints);
+
+    /// <summary>
+    /// Adds a 'trimmer'. It will be used after all data added before it has been
+    /// included in the pose graph.
+    /// </summary>
+    public abstract void AddTrimmer(PoseGraphTrimmer trimmer);
+
+    /// <summary>
+    /// Gets the current trajectory clusters.
+    /// </summary>
+    public abstract List<List<int>> GetConnectedTrajectories();
+
+    /// <summary>
+    /// Returns the IMU data.
+    /// </summary>
+    public abstract Dictionary<int, List<ImuData>> GetImuData();
+
+    /// <summary>
+    /// Returns the odometry data.
+    /// </summary>
+    public abstract Dictionary<int, List<OdometryData>> GetOdometryData();
+
+    /// <summary>
+    /// Returns the fixed frame pose data.
+    /// </summary>
+    public abstract Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData();
+
+    /// <summary>
+    /// Returns the landmark data.
+    /// </summary>
+    public abstract Dictionary<string, IPoseGraph.LandmarkNode> GetLandmarkNodes();
+
+    /// <summary>
+    /// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
+    /// respect to 'to_trajectory_id' at time 'time'.
+    /// </summary>
+    public abstract void SetInitialTrajectoryPose(
+        int fromTrajectoryId,
+        int toTrajectoryId,
+        Rigid3d pose,
+        long time);
+
+    /// <summary>
+    /// Sets localization initial poses for relocalizing against the map.
+    /// Match C++: SetLocalizationInitialPoses (pose_graph.h:139)
+    /// C++ signature: const std::vector&lt;transform::Rigid3d&gt; &amp; (const reference)
+    /// C# equivalent: IReadOnlyList&lt;Rigid3d&gt; (read-only collection)
+    /// </summary>
+    public abstract void SetLocalizationInitialPoses(IReadOnlyList<Rigid3d> localizationInitialPoses);
+
+    public abstract void RunFinalOptimization();
+    public abstract MapById<SubmapId, IPoseGraph.SubmapData> GetAllSubmapData();
+    public abstract IPoseGraph.SubmapData GetSubmapData(SubmapId submapId);
+    public abstract MapById<SubmapId, IPoseGraph.SubmapPose> GetAllSubmapPoses();
+    public abstract Rigid3d GetLocalToGlobalTransform(int trajectoryId);
+    public abstract MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
+    public abstract MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses();
+    public abstract Dictionary<int, IPoseGraph.TrajectoryState> GetTrajectoryStates();
+    public abstract Dictionary<string, Rigid3d> GetLandmarkPoses();
+    public abstract void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false);
+    public abstract void DeleteTrajectory(int trajectoryId);
+    public abstract bool IsTrajectoryFinished(int trajectoryId);
+    public abstract bool IsTrajectoryFrozen(int trajectoryId);
+    public abstract Dictionary<int, IPoseGraph.TrajectoryData> GetTrajectoryData();
+    public abstract List<IPoseGraph.Constraint> Constraints();
+    public abstract Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps);
+    public abstract void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback);
+    public abstract void SetTransformToMap(Rigid3d transform);
+    public abstract Rigid3d GetTransformToMap();
+
+    // Manual compute methods (match C++ PoseGraphInterface)
+    public abstract (double Score, IPoseGraph.Constraint? Constraint) ManualComputeConstraint(NodeId nodeId, SubmapId submapId);
+    public abstract double ManualComputeConstraintScore(NodeId nodeId, SubmapId submapId, Rigid3d initialPose);
+    public abstract double ManualComputeScanMatcher(NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate);
+    public abstract bool ManualRelocalization(int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback);
+}
+
+/// <summary>
+/// Pose graph trimmer interface.
+/// </summary>
+public abstract class PoseGraphTrimmer
+{
+    /// <summary>
+    /// Trims the pose graph.
+    /// </summary>
+    public abstract void Trim(ITrimmable trimmable);
+}
+
+/// <summary>
+/// Interface for trimmable pose graph operations.
+/// </summary>
+public interface ITrimmable
+{
+    int NumSubmaps(int trajectoryId);
+    List<SubmapId> GetSubmapIds(int trajectoryId);
+    MapById<SubmapId, IPoseGraph.SubmapData> GetOptimizedSubmapData();
+    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
+    List<IPoseGraph.Constraint> GetConstraints();
+    void TrimSubmap(SubmapId submapId);
+    bool IsFinished(int trajectoryId);
+    void SetTrajectoryState(int trajectoryId, IPoseGraph.TrajectoryState state);
+}
+
+/// <summary>
+/// Conversion utilities for constraints.
+/// </summary>
+public static class ConstraintOperations
+{
+    /// <summary>
+    /// Converts constraint to proto.
+    /// Match C++: ToProto in pose_graph.cc:147-169
+    /// </summary>
+    public static Models.Mapping.PoseGraph.Constraint ToProto(IPoseGraph.Constraint constraint)
+    {
+        // Match C++: convert tag (line 161 in pose_graph.cc)
+        var tag = constraint.ConstraintTag == IPoseGraph.Constraint.Tag.IntraSubmap
+            ? Models.Mapping.PoseGraph.Constraint.Tag.IntraSubmap
+            : Models.Mapping.PoseGraph.Constraint.Tag.InterSubmap;
+
+        // Match C++: convert state (line 166 in pose_graph.cc)
+        var state = constraint.ConstraintState == IPoseGraph.Constraint.State.Enabled
+            ? Models.Mapping.PoseGraph.Constraint.State.Enabled
+            : Models.Mapping.PoseGraph.Constraint.State.Disabled;
+
+        // Match C++: create constraint proto with all fields (lines 147-168 in pose_graph.cc)
+        return new Models.Mapping.PoseGraph.Constraint(
+            new Models.Mapping.PoseGraph.SubmapId(constraint.SubmapId.TrajectoryId, constraint.SubmapId.SubmapIndex),
+            new Models.Mapping.PoseGraph.NodeId(constraint.NodeId.TrajectoryId, constraint.NodeId.NodeIndex),
+            (Rigid3dProto)constraint.ConstraintPose.ZbarIj,
+            constraint.ConstraintPose.TranslationWeight,
+            constraint.ConstraintPose.RotationWeight,
+            tag,
+            constraint.Score,          // CRITICAL FIX: include score field (line 162 in pose_graph.cc)
+            state                      // CRITICAL FIX: include state field (line 166 in pose_graph.cc)
+        );
+    }
+
+    /// <summary>
+    /// Creates constraint from proto.
+    /// Match C++: FromProto in pose_graph.cc:77-97
+    /// </summary>
+    public static List<IPoseGraph.Constraint> FromProto(List<Models.Mapping.PoseGraph.Constraint> constraintProtos)
+    {
+        var constraints = new List<IPoseGraph.Constraint>();
+        foreach (var constraintProto in constraintProtos)
+        {
+            // Match C++: convert tag
+            IPoseGraph.Constraint.Tag tag;
+            if (constraintProto.ConstraintTag == Models.Mapping.PoseGraph.Constraint.Tag.IntraSubmap)
+            {
+                tag = IPoseGraph.Constraint.Tag.IntraSubmap;
+            }
+            else
+            {
+                tag = IPoseGraph.Constraint.Tag.InterSubmap;
+            }
+
+            // Match C++: convert state (lines 93 in pose_graph.cc)
+            IPoseGraph.Constraint.State state;
+            if (constraintProto.ConstraintState == Models.Mapping.PoseGraph.Constraint.State.Enabled)
+            {
+                state = IPoseGraph.Constraint.State.Enabled;
+            }
+            else
+            {
+                state = IPoseGraph.Constraint.State.Disabled;
+            }
+
+            // Match C++: extract score (line 92 in pose_graph.cc)
+            var score = constraintProto.Score;
+
+            // Match C++: create constraint with all fields (line 94 in pose_graph.cc)
+            var constraint = new IPoseGraph.Constraint(
+                new SubmapId(constraintProto.SubmapId.TrajectoryId, constraintProto.SubmapId.SubmapIndex),
+                new NodeId(constraintProto.NodeId.TrajectoryId, constraintProto.NodeId.NodeIndex),
+                new IPoseGraph.Constraint.Pose(
+                    (Rigid3d)constraintProto.RelativePose,
+                    constraintProto.TranslationWeight,
+                    constraintProto.RotationWeight),
+                tag,
+                score,   // CRITICAL FIX: include score field
+                state    // CRITICAL FIX: include state field
+            );
+            constraints.Add(constraint);
+        }
+        return constraints;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/ProbabilityValues.cs

@@ -0,0 +1,307 @@
+/*
+ * 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;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Probability value conversion utilities.
+/// </summary>
+public static class ProbabilityValues
+{
+    public const double kMinProbability = 0.1;
+    public const double kMaxProbability = 1.0 - kMinProbability;
+    public const double kMinCorrespondenceCost = 1.0 - kMaxProbability;
+    public const double kMaxCorrespondenceCost = 1.0 - kMinProbability;
+
+    private const ushort kUnknownProbabilityValue = 0;
+    private const ushort kUnknownCorrespondenceValue = kUnknownProbabilityValue;
+    private const ushort kUpdateMarker = (ushort)(1u << 15);
+
+    private const int kValueCount = 32768;
+
+    // Precomputed lookup tables
+    private static readonly double[] kValueToProbability;
+    private static readonly double[] kValueToCorrespondenceCost;
+
+    static ProbabilityValues()
+    {
+        kValueToProbability = PrecomputeValueToProbability();
+        kValueToCorrespondenceCost = PrecomputeValueToCorrespondenceCost();
+    }
+
+    /// <summary>
+    /// Converts probability to odds.
+    /// </summary>
+    public static double Odds(double probability)
+    {
+        return probability / (1.0 - probability);
+    }
+
+    /// <summary>
+    /// Converts odds to probability.
+    /// </summary>
+    public static double ProbabilityFromOdds(double odds)
+    {
+        return odds / (odds + 1.0);
+    }
+
+    /// <summary>
+    /// Converts probability to correspondence cost.
+    /// </summary>
+    public static double ProbabilityToCorrespondenceCost(double probability)
+    {
+        return 1.0 - probability;
+    }
+
+    /// <summary>
+    /// Converts correspondence cost to probability.
+    /// </summary>
+    public static double CorrespondenceCostToProbability(double correspondenceCost)
+    {
+        return 1.0 - correspondenceCost;
+    }
+
+    /// <summary>
+    /// Clamps probability to be in the range [kMinProbability, kMaxProbability].
+    /// </summary>
+    public static double ClampProbability(double probability)
+    {
+        return MathUtils.Clamp(probability, kMinProbability, kMaxProbability);
+    }
+
+    /// <summary>
+    /// Clamps correspondence cost to be in the range [kMinCorrespondenceCost, kMaxCorrespondenceCost].
+    /// </summary>
+    public static double ClampCorrespondenceCost(double correspondenceCost)
+    {
+        return MathUtils.Clamp(correspondenceCost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
+    }
+
+    /// <summary>
+    /// Converts a correspondence_cost to a uint16 in the [1, 32767] range.
+    /// </summary>
+    public static ushort CorrespondenceCostToValue(double correspondenceCost)
+    {
+        return BoundedFloatToValue(correspondenceCost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
+    }
+
+    /// <summary>
+    /// Converts a probability to a uint16 in the [1, 32767] range.
+    /// </summary>
+    public static ushort ProbabilityToValue(double probability)
+    {
+        return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
+    }
+
+    /// <summary>
+    /// Converts a uint16 (which may or may not have the update marker set) to a
+    /// probability in the range [kMinProbability, kMaxProbability].
+    /// </summary>
+    public static double ValueToProbability(ushort value)
+    {
+        if (value >= kValueToProbability.Length)
+        {
+            return kMinProbability;
+        }
+        return kValueToProbability[value];
+    }
+
+    /// <summary>
+    /// Converts a uint16 (which may or may not have the update marker set) to a
+    /// correspondence cost in the range [kMinCorrespondenceCost, kMaxCorrespondenceCost].
+    /// </summary>
+    public static double ValueToCorrespondenceCost(ushort value)
+    {
+        if (value >= kValueToCorrespondenceCost.Length)
+        {
+            return kMaxCorrespondenceCost;
+        }
+        return kValueToCorrespondenceCost[value];
+    }
+
+    /// <summary>
+    /// Converts probability value to correspondence cost value.
+    /// </summary>
+    public static ushort ProbabilityValueToCorrespondenceCostValue(ushort probabilityValue)
+    {
+        if (probabilityValue == kUnknownProbabilityValue)
+        {
+            return kUnknownCorrespondenceValue;
+        }
+
+        bool updateCarry = false;
+        if (probabilityValue > kUpdateMarker)
+        {
+            probabilityValue -= kUpdateMarker;
+            updateCarry = true;
+        }
+
+        ushort result = CorrespondenceCostToValue(
+            ProbabilityToCorrespondenceCost(ValueToProbability(probabilityValue)));
+        if (updateCarry)
+        {
+            result += kUpdateMarker;
+        }
+        return result;
+    }
+
+    /// <summary>
+    /// Converts correspondence cost value to probability value.
+    /// </summary>
+    public static ushort CorrespondenceCostValueToProbabilityValue(ushort correspondenceCostValue)
+    {
+        if (correspondenceCostValue == kUnknownCorrespondenceValue)
+        {
+            return kUnknownProbabilityValue;
+        }
+
+        bool updateCarry = false;
+        if (correspondenceCostValue > kUpdateMarker)
+        {
+            correspondenceCostValue -= kUpdateMarker;
+            updateCarry = true;
+        }
+
+        ushort result = ProbabilityToValue(CorrespondenceCostToProbability(
+            ValueToCorrespondenceCost(correspondenceCostValue)));
+        if (updateCarry)
+        {
+            result += kUpdateMarker;
+        }
+        return result;
+    }
+
+    /// <summary>
+    /// Computes lookup table to apply odds.
+    /// </summary>
+    public static List<ushort> ComputeLookupTableToApplyOdds(double odds)
+    {
+        var result = new List<ushort>(kValueCount)
+        {
+            (ushort)(ProbabilityToValue(ProbabilityFromOdds(odds)) + kUpdateMarker)
+        };
+        
+        for (int cell = 1; cell != kValueCount; cell++)
+        {
+            result.Add((ushort)(ProbabilityToValue(ProbabilityFromOdds(
+                odds * Odds(ValueToProbability((ushort)cell)))) + kUpdateMarker));
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Computes lookup table to apply correspondence cost odds.
+    /// </summary>
+    public static List<ushort> ComputeLookupTableToApplyCorrespondenceCostOdds(double odds)
+    {
+        var result = new List<ushort>(kValueCount)
+        {
+            (ushort)(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
+            ProbabilityFromOdds(odds))) + kUpdateMarker)
+        };
+        
+        for (int cell = 1; cell != kValueCount; cell++)
+        {
+            result.Add((ushort)(CorrespondenceCostToValue(
+                ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
+                    odds * Odds(CorrespondenceCostToProbability(
+                        ValueToCorrespondenceCost((ushort)cell)))))) + kUpdateMarker));
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Converts a bounded double value to a uint16 value.
+    /// </summary>
+    private static ushort BoundedFloatToValue(double floatValue, double lowerBound, double upperBound)
+    {
+        var clamped = MathUtils.Clamp(floatValue, lowerBound, upperBound);
+        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
+        // with rounding half away from zero (not banker's rounding)
+        var value = (int)Math.Round(
+            (clamped - lowerBound) * (32766.0 / (upperBound - lowerBound)),
+            MidpointRounding.AwayFromZero) + 1;
+        
+        // Clamp to valid range
+        value = Math.Max(1, Math.Min(32767, value));
+        return (ushort)value;
+    }
+
+    /// <summary>
+    /// Precomputes value to probability lookup table.
+    /// </summary>
+    private static double[] PrecomputeValueToProbability()
+    {
+        return PrecomputeValueToBoundedFloat(
+            kUnknownProbabilityValue, kMinProbability, kMinProbability, kMaxProbability);
+    }
+
+    /// <summary>
+    /// Precomputes value to correspondence cost lookup table.
+    /// </summary>
+    private static double[] PrecomputeValueToCorrespondenceCost()
+    {
+        return PrecomputeValueToBoundedFloat(
+            kUnknownCorrespondenceValue, kMaxCorrespondenceCost,
+            kMinCorrespondenceCost, kMaxCorrespondenceCost);
+    }
+
+    /// <summary>
+    /// Precomputes value to bounded double lookup table.
+    /// </summary>
+    private static double[] PrecomputeValueToBoundedFloat(
+        ushort unknownValue, double unknownResult, double lowerBound, double upperBound)
+    {
+        // Repeat two times, so that both values with and without the update marker
+        // can be converted to a probability.
+        const int kRepetitionCount = 2;
+        var result = new double[kRepetitionCount * kValueCount];
+        
+        for (int repeat = 0; repeat != kRepetitionCount; repeat++)
+        {
+            for (int value = 0; value != kValueCount; value++)
+            {
+                result[repeat * kValueCount + value] = SlowValueToBoundedFloat(
+                    (ushort)value, unknownValue, unknownResult, lowerBound, upperBound);
+            }
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Slow conversion from value to bounded double (used for precomputation).
+    /// </summary>
+    private static double SlowValueToBoundedFloat(
+        ushort value, ushort unknownValue, double unknownResult,
+        double lowerBound, double upperBound)
+    {
+        if (value == unknownValue)
+        {
+            return unknownResult;
+        }
+        
+        // Match C++: const double kScale = (upper_bound - lower_bound) / 32766.f;
+        // kValueCount = 32768, so kValueCount - 2.0 = 32766.0
+        // Use explicit 32766.0 for consistency with C++ and ValueConversionTables
+        var kScale = (upperBound - lowerBound) / 32766.0;
+        return value * kScale + (lowerBound - kScale);
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/PureLocalizationTrimmer.cs

@@ -0,0 +1,70 @@
+/*
+ * 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;
+
+/// <summary>
+/// Match C++ PureLocalizationTrimmer: keeps only the last 'num_submaps_to_keep'
+/// submaps of a localization trajectory, trimming older ones.
+/// Used when trajectory_options has pure_localization_trimmer (map_builder.cc MaybeAddPureLocalizationTrimmer).
+/// </summary>
+public class PureLocalizationTrimmer : PoseGraphTrimmer
+{
+    private readonly int _trajectoryId;
+    private int _numSubmapsToKeep;
+    private bool _finished;
+
+    /// <summary>
+    /// Match C++ PureLocalizationTrimmer(trajectory_id, num_submaps_to_keep).
+    /// CHECK_GE(num_submaps_to_keep, 2) in C++.
+    /// </summary>
+    public PureLocalizationTrimmer(int trajectoryId, int numSubmapsToKeep)
+    {
+        if (numSubmapsToKeep < 2)
+            throw new ArgumentException("Cannot trim with less than 2 submaps", nameof(numSubmapsToKeep));
+        _trajectoryId = trajectoryId;
+        _numSubmapsToKeep = numSubmapsToKeep;
+    }
+
+    /// <summary>
+    /// Match C++ Trim(Trimmable*).
+    /// </summary>
+    public override void Trim(ITrimmable trimmable)
+    {
+        if (trimmable.IsFinished(_trajectoryId))
+        {
+            _numSubmapsToKeep = 0;
+        }
+
+        var submapIds = trimmable.GetSubmapIds(_trajectoryId);
+        // C++: for (i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) TrimSubmap(submap_ids.at(i));
+        for (int i = 0; i + _numSubmapsToKeep < submapIds.Count; ++i)
+        {
+            trimmable.TrimSubmap(submapIds[i]);
+        }
+
+        if (_numSubmapsToKeep == 0)
+        {
+            _finished = true;
+            trimmable.SetTrajectoryState(_trajectoryId, IPoseGraph.TrajectoryState.Deleted);
+        }
+    }
+
+    /// <summary>
+    /// Match C++ IsFinished().
+    /// </summary>
+    public bool IsFinished() => _finished;
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Submap.cs

@@ -0,0 +1,102 @@
+/*
+ * 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.Transform;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// An individual submap, which has a 'local_pose' in the local map frame, keeps
+/// track of how many range data were inserted into it, and sets
+/// 'insertion_finished' when the map no longer changes and is ready for loop
+/// closing.
+/// </summary>
+public abstract class Submap(Rigid3d localSubmapPose)
+{
+    private int _numRangeData = 0;
+    private bool _insertionFinished = false;
+
+    /// <summary>
+    /// Pose of this submap in the local map frame.
+    /// </summary>
+    public Rigid3d LocalPose => localSubmapPose;
+
+    /// <summary>
+    /// Number of RangeData inserted.
+    /// </summary>
+    public int NumRangeData
+    {
+        get => Volatile.Read(ref _numRangeData);
+        set => Volatile.Write(ref _numRangeData, value);
+    }
+
+    /// <summary>
+    /// Whether insertion is finished.
+    /// </summary>
+    public bool InsertionFinished
+    {
+        get => _insertionFinished;
+        set => _insertionFinished = value;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public abstract Models.Mapping.Submap ToProto(bool includeGridData);
+
+    /// <summary>
+    /// Updates from proto representation.
+    /// </summary>
+    public abstract void UpdateFromProto(Models.Mapping.Submap proto);
+
+    /// <summary>
+    /// Fills data into the response proto.
+    /// Note: SubmapQueryResponse proto will be implemented later.
+    /// </summary>
+    // public abstract void ToResponseProto(Rigid3d globalSubmapPose, SubmapQueryResponse response);
+}
+
+/// <summary>
+/// Probability value conversion utilities for submaps.
+/// </summary>
+public static class SubmapProbabilityUtils
+{
+    /// <summary>
+    /// Converts the given probability to log odds.
+    /// </summary>
+    public static double Logit(double probability)
+    {
+        return Math.Log(probability / (1.0 - probability));
+    }
+
+    public static readonly double kMaxLogOdds = Logit(ProbabilityValues.kMaxProbability);
+    public static readonly double kMinLogOdds = Logit(ProbabilityValues.kMinProbability);
+
+    /// <summary>
+    /// Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
+    /// kMaxLogOdds] is mapped to [1, 255].
+    /// </summary>
+    public static byte ProbabilityToLogOddsInteger(double probability)
+    {
+        // Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
+        // with rounding half away from zero (not banker's rounding)
+        var value = (int)Math.Round((Logit(probability) - kMinLogOdds) *
+                                    254.0 / (kMaxLogOdds - kMinLogOdds),
+                                    MidpointRounding.AwayFromZero) + 1;
+        value = Math.Max(1, Math.Min(255, value));
+        return (byte)value;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/TrajectoryNode.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.Models.Mapping;
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Constant pose data (time and local pose).
+/// </summary>
+public struct ConstantPoseData
+{
+    public long Time { get; set; } // Universal Time Scale ticks
+    public Rigid3d LocalPose { get; set; }
+}
+
+/// <summary>
+/// Trajectory node pose information.
+/// </summary>
+public struct TrajectoryNodePose
+{
+    /// <summary>
+    /// The node pose in the global SLAM frame.
+    /// </summary>
+    public Rigid3d GlobalPose { get; set; }
+
+    /// <summary>
+    /// Optional constant pose data.
+    /// </summary>
+    public ConstantPoseData? ConstantPoseData { get; set; }
+}
+
+/// <summary>
+/// Trajectory node containing sensor data and pose information.
+/// </summary>
+public class TrajectoryNode
+{
+    /// <summary>
+    /// Constant data associated with a trajectory node.
+    /// </summary>
+    public class Data
+    {
+        public long Time { get; set; } // Universal Time Scale ticks
+
+        /// <summary>
+        /// Transform to approximately gravity align the tracking frame as
+        /// determined by local SLAM.
+        /// </summary>
+        public Quaternion GravityAlignment { get; set; }
+
+        /// <summary>
+        /// Used for loop closure in 2D: voxel filtered returns in the
+        /// 'gravity_alignment' frame.
+        /// </summary>
+        public PointCloud? FilteredGravityAlignedPointCloud { get; set; }
+
+        /// <summary>
+        /// Used for loop closure in 3D.
+        /// </summary>
+        public PointCloud? HighResolutionPointCloud { get; set; }
+        public PointCloud? LowResolutionPointCloud { get; set; }
+        public double[]? RotationalScanMatcherHistogram { get; set; }
+
+        /// <summary>
+        /// The node pose in the local SLAM frame.
+        /// </summary>
+        public Rigid3d LocalPose { get; set; }
+    }
+
+    /// <summary>
+    /// Gets the time of this node.
+    /// </summary>
+    public long Time => ConstantData?.Time ?? 0;
+
+    /// <summary>
+    /// This must be a shared reference. If the data is used for visualization while the
+    /// node is being trimmed, it must survive until all use finishes.
+    /// </summary>
+    public Data? ConstantData { get; set; }
+
+    /// <summary>
+    /// The node pose in the global SLAM frame.
+    /// </summary>
+    public Rigid3d GlobalPose { get; set; }
+
+    /// <summary>
+    /// Creates a shallow clone of this TrajectoryNode.
+    /// The ConstantData reference is shared (not deep cloned) since it's immutable after creation.
+    /// Only GlobalPose is independent in the clone.
+    /// Used for Copy-on-Write optimization to avoid data races.
+    /// </summary>
+    public TrajectoryNode ShallowClone()
+    {
+        return new TrajectoryNode
+        {
+            ConstantData = this.ConstantData,  // Shared reference (immutable)
+            GlobalPose = this.GlobalPose       // Value type copy
+        };
+    }
+}
+
+/// <summary>
+/// Conversion utilities for TrajectoryNode.
+/// </summary>
+public static class TrajectoryNodeOperations
+{
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public static TrajectoryNodeData ToProto(TrajectoryNode.Data constantData)
+    {
+        var proto = new TrajectoryNodeData(
+            constantData.Time,
+            (Quaterniond)constantData.GravityAlignment,
+            (Rigid3dProto)constantData.LocalPose,
+            constantData.FilteredGravityAlignedPointCloud != null
+                ? new Sensor.CompressedPointCloud(constantData.FilteredGravityAlignedPointCloud).ToProto()
+                : null,
+            constantData.HighResolutionPointCloud != null
+                ? new Sensor.CompressedPointCloud(constantData.HighResolutionPointCloud).ToProto()
+                : null,
+            constantData.LowResolutionPointCloud != null
+                ? new Sensor.CompressedPointCloud(constantData.LowResolutionPointCloud).ToProto()
+                : null,
+            constantData.RotationalScanMatcherHistogram?.ToList()
+        );
+
+        return proto;
+    }
+
+    /// <summary>
+    /// Creates from proto representation.
+    /// </summary>
+    public static TrajectoryNode.Data FromProto(TrajectoryNodeData proto)
+    {
+        return new TrajectoryNode.Data
+        {
+            Time = proto.Timestamp,
+            GravityAlignment = (Quaternion)proto.GravityAlignment,
+            LocalPose = (Rigid3d)proto.LocalPose,
+            FilteredGravityAlignedPointCloud = proto.FilteredGravityAlignedPointCloud.HasValue
+                ? Sensor.CompressedPointCloud.FromProto(proto.FilteredGravityAlignedPointCloud.Value).Decompress()
+                : null,
+            HighResolutionPointCloud = proto.HighResolutionPointCloud.HasValue
+                ? Sensor.CompressedPointCloud.FromProto(proto.HighResolutionPointCloud.Value).Decompress()
+                : null,
+            LowResolutionPointCloud = proto.LowResolutionPointCloud.HasValue
+                ? Sensor.CompressedPointCloud.FromProto(proto.LowResolutionPointCloud.Value).Decompress()
+                : null,
+            RotationalScanMatcherHistogram = proto.RotationalScanMatcherHistogram?.ToArray()
+        };
+    }
+}

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/ValueConversionTables.cs

@@ -0,0 +1,95 @@
+/*
+ * 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.
+ */
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Performs lazy computations of lookup tables for mapping from a uint16 value
+/// to a double in ['lower_bound', 'upper_bound']. The first element of the table
+/// is set to 'unknown_result'.
+/// </summary>
+public class ValueConversionTables
+{
+    private readonly Dictionary<(double unknownResult, double lowerBound, double upperBound), double[]> _boundsToLookupTable;
+
+    public ValueConversionTables()
+    {
+        _boundsToLookupTable = [];
+    }
+
+    /// <summary>
+    /// Gets or creates a conversion table for the given bounds.
+    /// </summary>
+    public double[] GetConversionTable(double unknownResult, double lowerBound, double upperBound)
+    {
+        var key = (unknownResult, lowerBound, upperBound);
+        
+        if (!_boundsToLookupTable.TryGetValue(key, out var table))
+        {
+            table = PrecomputeValueToBoundedFloat(unknownResult, lowerBound, upperBound);
+            _boundsToLookupTable[key] = table;
+        }
+        
+        return table;
+    }
+
+    /// <summary>
+    /// Precomputes value to bounded double lookup table.
+    /// Match C++: size_t num_values = std::numeric_limits<uint16>::max() + 1; (65536)
+    /// C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
+    /// </summary>
+    private static double[] PrecomputeValueToBoundedFloat(
+        double unknownResult, double lowerBound, double upperBound)
+    {
+        // Match C++: num_values = std::numeric_limits<uint16>::max() + 1 = 65536
+        const int kValueCount = 65536; // All possible ushort values (0-65535)
+        const ushort kUpdateMarker = (ushort)(1u << 15); // Highest bit
+        var result = new double[kValueCount];
+        
+        // Match C++: for (size_t value = 0; value != num_values; ++value)
+        // C++: SlowValueToBoundedFloat(static_cast<uint16>(value) & ~kUpdateMarker, ...)
+        // Mask out update marker before computing conversion
+        for (int value = 0; value != kValueCount; value++)
+        {
+            // Match C++: Mask out update marker: value & ~kUpdateMarker
+            var valueWithoutMarker = (ushort)(value & ~kUpdateMarker);
+            result[value] = SlowValueToBoundedFloat(
+                valueWithoutMarker, unknownResult, lowerBound, upperBound);
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Slow conversion from value to bounded double (used for precomputation).
+    /// Match C++: CHECK_LE(value, 32767) and kScale calculation.
+    /// </summary>
+    private static double SlowValueToBoundedFloat(
+        ushort value, double unknownResult, double lowerBound, double upperBound)
+    {
+        // Match C++: CHECK_LE(value, 32767) - value should be <= 32767 after masking
+        // (value is already masked in PrecomputeValueToBoundedFloat)
+        const ushort kUnknownValue = 0;
+        if (value == kUnknownValue)
+        {
+            return unknownResult;
+        }
+        
+        // Match C++: const double kScale = (upper_bound - lower_bound) / 32766.f;
+        var kScale = (upperBound - lowerBound) / 32766.0;
+        return value * kScale + (lowerBound - kScale);
+    }
+}