Initial commit

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