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

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

@@ -0,0 +1,162 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Mapping.Internal.D3D;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Wires up local SLAM (LocalTrajectoryBuilder3D) with the PoseGraph for 3D mapping.
+/// Handles sensor data, triggers local SLAM, and adds results to the pose graph.
+/// </summary>
+public class GlobalTrajectoryBuilder3D(
+    LocalTrajectoryBuilder3D? localTrajectoryBuilder,
+    int trajectoryId,
+    PoseGraph3D poseGraph,
+    MotionFilter? poseGraphOdometryMotionFilter = null) : ITrajectoryBuilder
+{
+    public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
+    {
+        if (localTrajectoryBuilder == null)
+        {
+            throw new InvalidOperationException("Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.");
+        }
+
+        var matchingResult = localTrajectoryBuilder.AddRangeData(sensorId, timedPointCloudData);
+        if (matchingResult == null)
+        {
+            // The range data has not been fully accumulated yet.
+            return null;
+        }
+
+        var result = matchingResult.Value;
+        ITrajectoryBuilder.InsertionResult? insertionResult = null;
+        
+        // If we have an insertion result, add node to pose graph
+        if (result.InsertionResult.HasValue)
+        {
+            var insertionResultValue = result.InsertionResult.Value;
+            if (insertionResultValue.ConstantData is null)
+                throw new InvalidOperationException($"insertionResult.ConstantData of sensorId {sensorId} is null");
+
+            // Cast submaps to Submap3D for PoseGraph3D.AddNode
+            var submaps3D = insertionResultValue.InsertionSubmaps.Cast<Mapping.D3D.Submap3D>().ToList();
+            
+            var nodeId = poseGraph.AddNode(
+                insertionResultValue.ConstantData,
+                trajectoryId,
+                submaps3D);
+
+            if (nodeId.TrajectoryId != trajectoryId)
+            {
+                throw new InvalidOperationException($"Node trajectory ID {nodeId.TrajectoryId} does not match expected {trajectoryId}");
+            }
+
+            // Update insertionResult with NodeId
+            insertionResult = new ITrajectoryBuilder.InsertionResult(
+                nodeId,
+                insertionResultValue.ConstantData,
+                insertionResultValue.InsertionSubmaps);
+            
+            // Update result with new insertionResult (including NodeId)
+            result = new ITrajectoryBuilder.MatchingResult(
+                trajectoryId,
+                result.Time,
+                result.LocalPose,
+                result.RangeDataInLocal,
+                insertionResult,
+                result.PoseConfidence,
+                result.CeresScore,
+                result.SamplePointCloudGlobal
+            );
+        }
+        
+        return result;
+    }
+
+    public void AddSensorData(string sensorId, ImuData imuData)
+    {
+        // Add to local trajectory builder if available
+        localTrajectoryBuilder?.AddImuData(imuData);
+        
+        // Always add to pose graph for global optimization
+        poseGraph.AddImuData(trajectoryId, imuData);
+    }
+
+    public void AddSensorData(string sensorId, OdometryData odometryData)
+    {
+        if (!odometryData.Pose.IsValid())
+        {
+            throw new ArgumentException($"Invalid odometry pose: {odometryData.Pose}", nameof(odometryData));
+        }
+
+        // Add to local trajectory builder if available
+        localTrajectoryBuilder?.AddOdometryData(odometryData);
+
+        // Apply motion filter if configured
+        if (poseGraphOdometryMotionFilter != null &&
+            poseGraphOdometryMotionFilter.IsSimilar(odometryData.Time, odometryData.Pose))
+        {
+            return; // Filtered out due to similar motion
+        }
+
+        // Add to pose graph
+        poseGraph.AddOdometryData(trajectoryId, odometryData);
+    }
+
+    public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
+    {
+        if (fixedFramePoseData.Pose.HasValue && !fixedFramePoseData.Pose.Value.IsValid())
+        {
+            throw new ArgumentException(
+                $"Invalid fixed frame pose: {fixedFramePoseData.Pose.Value}",
+                nameof(fixedFramePoseData));
+        }
+
+        poseGraph.AddFixedFramePoseData(trajectoryId, fixedFramePoseData);
+    }
+
+    public void AddSensorData(string sensorId, LandmarkData landmarkData)
+    {
+        poseGraph.AddLandmarkData(trajectoryId, landmarkData);
+    }
+
+    public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
+    {
+        if (localTrajectoryBuilder != null)
+        {
+            throw new InvalidOperationException(
+                "Can't add LocalSlamResultData with local_trajectory_builder_ present.");
+        }
+
+        // Add the local SLAM result directly to the pose graph
+        localSlamResultData.AddToPoseGraph(trajectoryId, poseGraph);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        return localTrajectoryBuilder?.TryGetExtrapolatedPose(time);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        return localTrajectoryBuilder?.TryGetExtrapolatedPoseFilter(time);
+    }
+}

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

@@ -0,0 +1,126 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Result of IMU integration.
+/// </summary>
+public struct IntegrateImuResult
+{
+    public Vector3 DeltaVelocity { get; set; }
+    public Vector3 DeltaTranslation { get; set; }
+    public Quaternion DeltaRotation { get; set; }
+
+    public IntegrateImuResult(Vector3 deltaVelocity, Vector3 deltaTranslation, Quaternion deltaRotation)
+    {
+        DeltaVelocity = deltaVelocity;
+        DeltaTranslation = deltaTranslation;
+        DeltaRotation = deltaRotation;
+    }
+}
+
+/// <summary>
+/// IMU integration utilities.
+/// </summary>
+public static class ImuIntegration
+{
+    /// <summary>
+    /// Integrates IMU data between start_time and end_time.
+    /// Returns delta_velocity, delta_translation, and delta_rotation.
+    /// </summary>
+    public static IntegrateImuResult IntegrateImu(
+        List<ImuData> imuData,
+        long startTime,
+        long endTime,
+        ref int imuIndex)
+    {
+        if (startTime > endTime)
+            throw new ArgumentException("startTime must be <= endTime");
+        
+        if (imuIndex < 0 || imuIndex >= imuData.Count)
+            throw new ArgumentOutOfRangeException(nameof(imuIndex));
+        
+        if (imuData[imuIndex].Time > startTime)
+            throw new ArgumentException("imuData[imuIndex].Time must be <= startTime");
+        
+        if (imuIndex + 1 < imuData.Count && imuData[imuIndex + 1].Time <= startTime)
+            throw new ArgumentException("imuData[imuIndex+1].Time must be > startTime");
+
+        var result = new IntegrateImuResult(
+            Vector3.Zero,
+            Vector3.Zero,
+            Quaternion.Identity);
+
+        long currentTime = startTime;
+
+        while (currentTime < endTime)
+        {
+            long nextImuTime = long.MaxValue;
+            if (imuIndex + 1 < imuData.Count)
+            {
+                nextImuTime = imuData[imuIndex + 1].Time;
+            }
+            
+            long nextTime = Math.Min(nextImuTime, endTime);
+            double deltaT = (nextTime - currentTime) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)
+
+            var currentImu = imuData[imuIndex];
+            
+            // Compute delta angle from angular velocity
+            var deltaAngle = currentImu.AngularVelocity * deltaT;
+            
+            // Convert angle-axis to quaternion (simplified - assumes small angles)
+            // For small angles: q ≈ [1, 0.5*angle.x, 0.5*angle.y, 0.5*angle.z]
+            var angleLength = deltaAngle.Length();
+            Quaternion deltaRotation;
+            if (angleLength < 1e-6)
+            {
+                deltaRotation = Quaternion.Identity;
+            }
+            else
+            {
+                var axis = Vector3.Normalize(deltaAngle);
+                deltaRotation = Quaternion.CreateFromAxisAngle(axis, angleLength);
+            }
+            
+            // Update cumulative rotation
+            result.DeltaRotation = Quaternion.Multiply(result.DeltaRotation, deltaRotation);
+            
+            // Integrate linear acceleration
+            // Rotate acceleration to current orientation frame
+            var rotatedAcceleration = Vector3.Transform(currentImu.LinearAcceleration, result.DeltaRotation);
+            var deltaVelocity = rotatedAcceleration * deltaT;
+            result.DeltaVelocity += deltaVelocity;
+            
+            // Integrate velocity to get translation
+            result.DeltaTranslation += result.DeltaVelocity * deltaT;
+
+            currentTime = nextTime;
+            if (currentTime == nextImuTime)
+            {
+                imuIndex++;
+            }
+        }
+
+        return result;
+    }
+}
+

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

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

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

@@ -0,0 +1,146 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
+
+/// <summary>
+/// Penalizes differences between IMU data and optimized accelerations.
+/// Based on acceleration_cost_function_3d.h
+/// </summary>
+public class AccelerationCostFunction3D
+{
+    private readonly double _scalingFactor;
+    private readonly Vector3 _deltaVelocityImuFrame;
+    private readonly double _firstDeltaTimeSeconds;
+    private readonly double _secondDeltaTimeSeconds;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for acceleration constraint.
+    /// </summary>
+    /// <param name="scalingFactor">Scaling factor for the cost.</param>
+    /// <param name="deltaVelocityImuFrame">Delta velocity from IMU integration in IMU frame.</param>
+    /// <param name="firstDeltaTimeSeconds">Time duration of first interval in seconds.</param>
+    /// <param name="secondDeltaTimeSeconds">Time duration of second interval in seconds.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Vector3 deltaVelocityImuFrame,
+        double firstDeltaTimeSeconds,
+        double secondDeltaTimeSeconds)
+    {
+        var costFunction = new AccelerationCostFunction3D(
+            scalingFactor, 
+            deltaVelocityImuFrame, 
+            firstDeltaTimeSeconds, 
+            secondDeltaTimeSeconds);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 3, // [dx, dy, dz] - velocity difference error
+            parameterBlockSizes: [4, 3, 3, 3, 1, 4] // [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]]
+        );
+    }
+
+    private AccelerationCostFunction3D(
+        double scalingFactor,
+        Vector3 deltaVelocityImuFrame,
+        double firstDeltaTimeSeconds,
+        double secondDeltaTimeSeconds)
+    {
+        _scalingFactor = scalingFactor;
+        _deltaVelocityImuFrame = deltaVelocityImuFrame;
+        _firstDeltaTimeSeconds = firstDeltaTimeSeconds;
+        _secondDeltaTimeSeconds = secondDeltaTimeSeconds;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dz] (velocity difference error).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 6)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 3 || parameters[2].Length < 3 ||
+            parameters[3].Length < 3 || parameters[4].Length < 1 || parameters[5].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var middleRotation = parameters[0];    // [w, x, y, z]
+        var startPosition = parameters[1];     // [x, y, z]
+        var middlePosition = parameters[2];    // [x, y, z]
+        var endPosition = parameters[3];       // [x, y, z]
+        var gravityConstant = parameters[4][0]; // [g]
+        var imuCalibration = parameters[5];    // [w, x, y, z]
+
+        // Convert to quaternions
+        var middleRot = new Quaternion(
+            middleRotation[1], middleRotation[2], middleRotation[3], middleRotation[0]);
+        var imuCal = new Quaternion(
+            imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
+
+        // Convert positions to Vector3
+        var startPos = new Vector3(startPosition[0], startPosition[1], startPosition[2]);
+        var middlePos = new Vector3(middlePosition[0], middlePosition[1], middlePosition[2]);
+        var endPos = new Vector3(endPosition[0], endPosition[1], endPosition[2]);
+
+        // Compute IMU delta velocity in map frame
+        // Formula from C++: 
+        // imu_delta_velocity = middle_rotation * imu_calibration * delta_velocity_imu_frame - gravity_term
+        // where gravity_term = gravity_constant * 0.5 * (first_delta_time + second_delta_time) * UnitZ
+        
+        // Transform delta_velocity_imu_frame from IMU frame to map frame
+        // In Eigen: quaternion * vector rotates the vector
+        // In System.Numerics: Vector3.Transform(vector, quaternion) rotates the vector
+        // C++: middle_rotation * imu_calibration * delta_velocity
+        //     = middle_rotation * (imu_calibration * delta_velocity)
+        // Apply IMU calibration first, then middle rotation
+        var imuDeltaVelocityCalibrated = Vector3.Transform(_deltaVelocityImuFrame, imuCal);
+        var imuDeltaVelocityInMapFrame = Vector3.Transform(imuDeltaVelocityCalibrated, middleRot);
+        
+        // Subtract gravity contribution
+        // Gravity acts in positive Z direction in map frame (upward)
+        var gravityTerm = gravityConstant * 0.5 * (_firstDeltaTimeSeconds + _secondDeltaTimeSeconds) * Vector3.UnitZ;
+        var imuDeltaVelocity = imuDeltaVelocityInMapFrame - gravityTerm;
+
+        // Compute velocities from positions
+        // start_velocity = (middle_position - start_position) / first_delta_time
+        var startVelocity = (middlePos - startPos) / _firstDeltaTimeSeconds;
+
+        // end_velocity = (end_position - middle_position) / second_delta_time
+        var endVelocity = (endPos - middlePos) / _secondDeltaTimeSeconds;
+
+        // delta_velocity = end_velocity - start_velocity
+        var deltaVelocity = endVelocity - startVelocity;
+
+        // Error = IMU delta velocity - computed delta velocity
+        var error = imuDeltaVelocity - deltaVelocity;
+
+        // Scale error
+        residuals[0] = _scalingFactor * error.X;
+        residuals[1] = _scalingFactor * error.Y;
+        residuals[2] = _scalingFactor * error.Z;
+
+        return true;
+    }
+}
+

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

@@ -0,0 +1,109 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
+
+/// <summary>
+/// Penalizes differences between IMU data and optimized orientations.
+/// Based on rotation_cost_function_3d.h
+/// </summary>
+public class RotationCostFunction3D
+{
+    private readonly double _scalingFactor;
+    private readonly Quaternion _deltaRotationImuFrame;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for rotation constraint.
+    /// </summary>
+    /// <param name="scalingFactor">Scaling factor for the cost.</param>
+    /// <param name="deltaRotationImuFrame">Delta rotation from IMU integration in IMU frame.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Quaternion deltaRotationImuFrame)
+    {
+        var costFunction = new RotationCostFunction3D(scalingFactor, deltaRotationImuFrame);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 3, // [dx, dy, dz] - rotation error as angle-axis vector
+            parameterBlockSizes: [4, 4, 4] // [start_rotation[4], end_rotation[4], imu_calibration[4]]
+        );
+    }
+
+    private RotationCostFunction3D(double scalingFactor, Quaternion deltaRotationImuFrame)
+    {
+        _scalingFactor = scalingFactor;
+        _deltaRotationImuFrame = deltaRotationImuFrame;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [start_rotation[4], end_rotation[4], imu_calibration[4]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dz] (angle-axis error).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 3)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 4 || parameters[2].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var startRotation = parameters[0]; // [w, x, y, z] from Ceres
+        var endRotation = parameters[1];   // [w, x, y, z] from Ceres
+        var imuCalibration = parameters[2]; // [w, x, y, z] from Ceres
+
+        // Convert to quaternions
+        // C++ line 42-48: Eigen::Quaternion<T>(w, x, y, z)
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        // So we need to convert [w, x, y, z] to (x, y, z, w)
+        var start = new Quaternion(
+            startRotation[1], startRotation[2], startRotation[3], startRotation[0]);
+        var end = new Quaternion(
+            endRotation[1], endRotation[2], endRotation[3], endRotation[0]);
+        var imuCal = new Quaternion(
+            imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
+
+        // Compute error: end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
+        // C++ line 49-51: error = end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
+        // C++ line 52-54: residual = scaling_factor * error.vector() (x, y, z components of quaternion, not angle-axis)
+        var endConj = Quaternion.Conjugate(end);
+        var imuCalConj = Quaternion.Conjugate(imuCal);
+        var error = Quaternion.Multiply(
+            Quaternion.Multiply(
+                Quaternion.Multiply(
+                    Quaternion.Multiply(endConj, start),
+                    imuCal),
+                _deltaRotationImuFrame),
+            imuCalConj);
+
+        // C++ uses error.x(), error.y(), error.z() which are the vector (imaginary) parts of the quaternion
+        // NOT angle-axis representation. For small rotations, these are approximately the same, but we should match C++ exactly.
+        // Scale error using vector part of quaternion (x, y, z components)
+        residuals[0] = _scalingFactor * error.X;
+        residuals[1] = _scalingFactor * error.Y;
+        residuals[2] = _scalingFactor * error.Z;
+
+        return true;
+    }
+}
+

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

@@ -0,0 +1,190 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
+
+/// <summary>
+/// Sparse Pose Adjustment (SPA) cost function for 3D pose graph optimization.
+/// Computes the error between observed relative pose and computed relative pose.
+/// </summary>
+public class SpaCostFunction3D
+{
+    private readonly IPoseGraph.Constraint.Pose _observedRelativePose;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for SPA 3D.
+    /// </summary>
+    /// <param name="observedRelativePose">The observed relative pose constraint.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        var costFunction = new SpaCostFunction3D(observedRelativePose);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz] (quaternion rotation error as 3D vector)
+            parameterBlockSizes: [4, 3, 4, 3] // [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]]
+        );
+    }
+
+    private SpaCostFunction3D(IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        _observedRelativePose = observedRelativePose;
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dz, dqx, dqy, dqz].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 4)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 3 || 
+            parameters[2].Length < 4 || parameters[3].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 6)
+            return false;
+
+        var submapRotation = parameters[0];
+        var submapTranslation = parameters[1];
+        var nodeRotation = parameters[2];
+        var nodeTranslation = parameters[3];
+
+        // Compute unscaled error
+        var unscaledError = ComputeUnscaledError(
+            _observedRelativePose.ZbarIj,
+            submapRotation,
+            submapTranslation,
+            nodeRotation,
+            nodeTranslation
+        );
+
+        // Scale error with weights
+        var scaledError = ScaleError(
+            unscaledError,
+            _observedRelativePose.TranslationWeight,
+            _observedRelativePose.RotationWeight
+        );
+
+        residuals[0] = scaledError[0];
+        residuals[1] = scaledError[1];
+        residuals[2] = scaledError[2];
+        residuals[3] = scaledError[3];
+        residuals[4] = scaledError[4];
+        residuals[5] = scaledError[5];
+
+        return true;
+    }
+
+    /// <summary>
+    /// Computes unscaled error between observed and computed relative pose.
+    /// Based on cost_helpers_impl.h ComputeUnscaledError for 3D.
+    /// </summary>
+    private static double[] ComputeUnscaledError(
+        Rigid3d observedRelativePose,
+        double[] submapRotation,
+        double[] submapTranslation,
+        double[] nodeRotation,
+        double[] nodeTranslation)
+    {
+        // submapRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
+        // submapTranslation = [x, y, z]
+        // nodeRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
+        // nodeTranslation = [x, y, z]
+
+        // IMPORTANT: System.Numerics.Quaternion constructor is (x, y, z, w), NOT (w, x, y, z)!
+        // Eigen::Quaternion uses (w, x, y, z), so we must reorder when creating System.Numerics.Quaternion.
+
+        // Compute R_i_inverse (inverse of submap rotation)
+        // C++: Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1], -start_rotation[2], -start_rotation[3])
+        var submapQuatInv = new Quaternion(
+            -submapRotation[1],  // -x
+            -submapRotation[2],  // -y
+            -submapRotation[3],  // -z
+            submapRotation[0]    // w
+        );
+
+        // Compute delta = node_translation - submap_translation
+        var delta = new Vector3(
+            (nodeTranslation[0] - submapTranslation[0]),
+            (nodeTranslation[1] - submapTranslation[1]),
+            (nodeTranslation[2] - submapTranslation[2])
+        );
+
+        // h_translation = R_i_inverse * delta
+        var hTranslation = Vector3.Transform(delta, submapQuatInv);
+
+        // Compute h_rotation_inverse = node_rotation_inverse * submap_rotation
+        // C++: Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2], -end_rotation[3]) *
+        //      Eigen::Quaternion<T>(start_rotation[0], start_rotation[1], start_rotation[2], start_rotation[3])
+        var nodeQuatInv = new Quaternion(
+            -nodeRotation[1],  // -x
+            -nodeRotation[2],  // -y
+            -nodeRotation[3],  // -z
+            nodeRotation[0]    // w
+        );
+        var submapQuat = new Quaternion(
+            submapRotation[1],  // x
+            submapRotation[2],  // y
+            submapRotation[3],  // z
+            submapRotation[0]   // w
+        );
+        var hRotationInverse = nodeQuatInv * submapQuat;
+
+        // Compute angle-axis difference: RotationQuaternionToAngleAxisVector(h_rotation_inverse * observed_rotation)
+        var observedQuat = observedRelativePose.Rotation;
+        var angleAxisDifference = TransformOperations.RotationQuaternionToAngleAxisVector(
+            hRotationInverse * observedQuat
+        );
+
+        // Error = observed - computed
+        return
+        [
+            observedRelativePose.Translation.X - hTranslation.X,
+            observedRelativePose.Translation.Y - hTranslation.Y,
+            observedRelativePose.Translation.Z - hTranslation.Z,
+            angleAxisDifference.X,
+            angleAxisDifference.Y,
+            angleAxisDifference.Z
+        ];
+    }
+
+    /// <summary>
+    /// Scales error with translation and rotation weights.
+    /// </summary>
+    private static double[] ScaleError(
+        double[] unscaledError,
+        double translationWeight,
+        double rotationWeight)
+    {
+        return
+        [
+            translationWeight * unscaledError[0],
+            translationWeight * unscaledError[1],
+            translationWeight * unscaledError[2],
+            rotationWeight * unscaledError[3],
+            rotationWeight * unscaledError[4],
+            rotationWeight * unscaledError[5]
+        ];
+    }
+}

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

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

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

@@ -0,0 +1,141 @@
+/*
+ * Copyright 2019 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Mapping.D3D;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
+/// 'translation' and 'rotation'. The cost increases when points fall into space
+/// for which different intensity has been observed, i.e. at voxels with different
+/// values. Only points up to a certain threshold are evaluated which is intended
+/// to ignore data from retroreflections.
+/// </summary>
+/// <remarks>
+/// Creates an intensity cost function for 3D scan matching.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor (typically intensity_weight / sqrt(point_cloud.size())).</param>
+/// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
+/// <param name="pointCloud">Point cloud to match (must have intensities).</param>
+/// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
+public class IntensityCostFunction3D(
+    double scalingFactor,
+    double intensityThreshold,
+    PointCloud pointCloud,
+    IntensityHybridGrid hybridGrid) : IDisposable
+{
+    private readonly PointCloud _pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
+    private readonly InterpolatedIntensityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
+    private static readonly int[] parameterBlockSizes = [3, 4];
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for intensity matching.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        double intensityThreshold,
+        PointCloud pointCloud,
+        IntensityHybridGrid hybridGrid)
+    {
+        var costFunction = new IntensityCostFunction3D(scalingFactor, intensityThreshold, pointCloud, hybridGrid);
+        return new DynamicAutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: pointCloud.Count,
+            parameterBlockSizes: parameterBlockSizes);
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
+    /// <param name="residuals">Output residuals (one per point).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 2)
+            return false;
+        if (parameters[0].Length < 3 || parameters[1].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < _pointCloud.Count)
+            return false;
+
+        var translation = parameters[0];
+        var rotation = parameters[1]; // [w, x, y, z] from Ceres
+        
+        // Create transform from translation and rotation
+        // C++ line 48-50: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
+        // where rotation = [w, x, y, z]
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        var transform = new Rigid3d(
+            new Vector3(translation[0], translation[1], translation[2]),
+            new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
+        );
+
+        // Transform each point and compute residual
+        for (int i = 0; i < _pointCloud.Count; i++)
+        {
+            var point = _pointCloud[i];
+            
+            // Get intensity from point cloud if available, otherwise use 0
+            double intensity = 0.0;
+            if (_pointCloud.Intensities.Count > 0 && i < _pointCloud.Intensities.Count)
+            {
+                intensity = _pointCloud.Intensities[i];
+            }
+            
+            // Ignore points with intensity above threshold (retroreflections)
+            if (intensity > intensityThreshold)
+            {
+                residuals[i] = 0.0;
+                continue;
+            }
+            
+            // Transform point from local frame to world frame
+            var worldPoint = transform * point.Position;
+            
+            // Get interpolated intensity value at world point
+            var interpolatedIntensity = _interpolatedGrid.GetInterpolatedValue(
+                worldPoint.X,
+                worldPoint.Y,
+                worldPoint.Z
+            );
+            
+            // Residual = scaling_factor * (interpolated_intensity - intensity)
+            residuals[i] = scalingFactor * (interpolatedIntensity - intensity);
+        }
+
+        return true;
+    }
+
+    /// <summary>
+    /// Disposes managed resources.
+    /// </summary>
+    public void Dispose()
+    {
+        // InterpolatedIntensityGrid doesn't need disposal, but we implement IDisposable for consistency
+        GC.SuppressFinalize(this);
+    }
+}

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

@@ -0,0 +1,238 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Common.Math;
+using CartographerSharp.Mapping.D3D;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Interpolates between HybridGrid voxels using tricubic interpolation.
+/// This class is designed to work with Ceres autodiff, so the interpolation
+/// scheme must be continuously differentiable.
+/// </summary>
+public class InterpolatedProbabilityGrid(HybridGrid _hybridGrid)
+{
+    /// <summary>
+    /// Returns the interpolated value at (x, y, z) of the HybridGrid.
+    /// Uses tricubic interpolation (piecewise cubic polynomials).
+    /// </summary>
+    public double GetInterpolatedValue(double x, double y, double z)
+    {
+        ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
+
+        var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
+        var q111 = GetValue(index1);
+        var q112 = GetValue(index1 + new Array3i(0, 0, 1));
+        var q121 = GetValue(index1 + new Array3i(0, 1, 0));
+        var q122 = GetValue(index1 + new Array3i(0, 1, 1));
+        var q211 = GetValue(index1 + new Array3i(1, 0, 0));
+        var q212 = GetValue(index1 + new Array3i(1, 0, 1));
+        var q221 = GetValue(index1 + new Array3i(1, 1, 0));
+        var q222 = GetValue(index1 + new Array3i(1, 1, 1));
+
+        var normalizedX = (x - x1) / (x2 - x1);
+        var normalizedY = (y - y1) / (y2 - y1);
+        var normalizedZ = (z - z1) / (z2 - z1);
+
+        // Compute powers: t^2 and t^3
+        var normalizedXx = normalizedX * normalizedX;
+        var normalizedXxx = normalizedX * normalizedXx;
+        var normalizedYy = normalizedY * normalizedY;
+        var normalizedYyy = normalizedY * normalizedYy;
+        var normalizedZz = normalizedZ * normalizedZ;
+        var normalizedZzz = normalizedZ * normalizedZz;
+
+        // Interpolate in z, then y, then x
+        // Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
+        var q11 = (q111 - q112) * normalizedZzz * 2.0 +
+                  (q112 - q111) * normalizedZz * 3.0 + q111;
+        var q12 = (q121 - q122) * normalizedZzz * 2.0 +
+                  (q122 - q121) * normalizedZz * 3.0 + q121;
+        var q21 = (q211 - q212) * normalizedZzz * 2.0 +
+                  (q212 - q211) * normalizedZz * 3.0 + q211;
+        var q22 = (q221 - q222) * normalizedZzz * 2.0 +
+                  (q222 - q221) * normalizedZz * 3.0 + q221;
+        var q1 = (q11 - q12) * normalizedYyy * 2.0 +
+                 (q12 - q11) * normalizedYy * 3.0 + q11;
+        var q2 = (q21 - q22) * normalizedYyy * 2.0 +
+                 (q22 - q21) * normalizedYy * 3.0 + q21;
+        return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
+    }
+
+    /// <summary>
+    /// Computes interpolation data points (corners of the voxel containing the point).
+    /// </summary>
+    private void ComputeInterpolationDataPoints(
+        double x, double y, double z,
+        out double x1, out double y1, out double z1,
+        out double x2, out double y2, out double z2)
+    {
+        var lower = CenterOfLowerVoxel(x, y, z);
+        x1 = lower.X;
+        y1 = lower.Y;
+        z1 = lower.Z;
+        x2 = lower.X + _hybridGrid.Resolution;
+        y2 = lower.Y + _hybridGrid.Resolution;
+        z2 = lower.Z + _hybridGrid.Resolution;
+    }
+
+    /// <summary>
+    /// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
+    /// For each dimension, the largest voxel index so that the corresponding center
+    /// is at most the given coordinate.
+    /// </summary>
+    private Vector3 CenterOfLowerVoxel(double x, double y, double z)
+    {
+        // Center of the cell containing (x, y, z)
+        var center = _hybridGrid.GetCenterOfCell(
+            _hybridGrid.GetCellIndex(new Vector3(x, y, z))
+        );
+        
+        // Move to the next lower voxel center
+        var resolution = _hybridGrid.Resolution;
+        if (center.X > x)
+        {
+            center.X -= resolution;
+        }
+        if (center.Y > y)
+        {
+            center.Y -= resolution;
+        }
+        if (center.Z > z)
+        {
+            center.Z -= resolution;
+        }
+        return center;
+    }
+
+    /// <summary>
+    /// Gets the probability value at the given cell index.
+    /// </summary>
+    private double GetValue(Array3i index)
+    {
+        // HybridGrid.GetProbability already returns probability in range [0, 1]
+        // It internally calls ProbabilityValues.ValueToProbability which does the conversion
+        // DO NOT divide by ushort.MaxValue - that was a bug!
+        return _hybridGrid.GetProbability(index);
+    }
+}
+
+/// <summary>
+/// Interpolates between IntensityHybridGrid voxels using tricubic interpolation.
+/// </summary>
+public class InterpolatedIntensityGrid(IntensityHybridGrid _hybridGrid)
+{
+    /// <summary>
+    /// Returns the interpolated value at (x, y, z) of the IntensityHybridGrid.
+    /// Uses tricubic interpolation (piecewise cubic polynomials).
+    /// </summary>
+    public double GetInterpolatedValue(double x, double y, double z)
+    {
+        ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
+
+        var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
+        var q111 = GetValue(index1);
+        var q112 = GetValue(index1 + new Array3i(0, 0, 1));
+        var q121 = GetValue(index1 + new Array3i(0, 1, 0));
+        var q122 = GetValue(index1 + new Array3i(0, 1, 1));
+        var q211 = GetValue(index1 + new Array3i(1, 0, 0));
+        var q212 = GetValue(index1 + new Array3i(1, 0, 1));
+        var q221 = GetValue(index1 + new Array3i(1, 1, 0));
+        var q222 = GetValue(index1 + new Array3i(1, 1, 1));
+
+        var normalizedX = (x - x1) / (x2 - x1);
+        var normalizedY = (y - y1) / (y2 - y1);
+        var normalizedZ = (z - z1) / (z2 - z1);
+
+        // Compute powers: t^2 and t^3
+        var normalizedXx = normalizedX * normalizedX;
+        var normalizedXxx = normalizedX * normalizedXx;
+        var normalizedYy = normalizedY * normalizedY;
+        var normalizedYyy = normalizedY * normalizedYy;
+        var normalizedZz = normalizedZ * normalizedZ;
+        var normalizedZzz = normalizedZ * normalizedZz;
+
+        // Interpolate in z, then y, then x
+        // Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
+        var q11 = (q111 - q112) * normalizedZzz * 2.0 +
+                  (q112 - q111) * normalizedZz * 3.0 + q111;
+        var q12 = (q121 - q122) * normalizedZzz * 2.0 +
+                  (q122 - q121) * normalizedZz * 3.0 + q121;
+        var q21 = (q211 - q212) * normalizedZzz * 2.0 +
+                  (q212 - q211) * normalizedZz * 3.0 + q211;
+        var q22 = (q221 - q222) * normalizedZzz * 2.0 +
+                  (q222 - q221) * normalizedZz * 3.0 + q221;
+        var q1 = (q11 - q12) * normalizedYyy * 2.0 +
+                 (q12 - q11) * normalizedYy * 3.0 + q11;
+        var q2 = (q21 - q22) * normalizedYyy * 2.0 +
+                 (q22 - q21) * normalizedYy * 3.0 + q21;
+        return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
+    }
+
+    /// <summary>
+    /// Computes interpolation data points (corners of the voxel containing the point).
+    /// </summary>
+    private void ComputeInterpolationDataPoints(
+        double x, double y, double z,
+        out double x1, out double y1, out double z1,
+        out double x2, out double y2, out double z2)
+    {
+        var lower = CenterOfLowerVoxel(x, y, z);
+        x1 = lower.X;
+        y1 = lower.Y;
+        z1 = lower.Z;
+        x2 = lower.X + _hybridGrid.Resolution;
+        y2 = lower.Y + _hybridGrid.Resolution;
+        z2 = lower.Z + _hybridGrid.Resolution;
+    }
+
+    /// <summary>
+    /// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
+    /// </summary>
+    private Vector3 CenterOfLowerVoxel(double x, double y, double z)
+    {
+        // Center of the cell containing (x, y, z)
+        var center = _hybridGrid.GetCenterOfCell(
+            _hybridGrid.GetCellIndex(new Vector3(x, y, z))
+        );
+        
+        // Move to the next lower voxel center
+        var resolution = _hybridGrid.Resolution;
+        if (center.X > x)
+        {
+            center.X -= resolution;
+        }
+        if (center.Y > y)
+        {
+            center.Y -= resolution;
+        }
+        if (center.Z > z)
+        {
+            center.Z -= resolution;
+        }
+        return center;
+    }
+
+    /// <summary>
+    /// Gets the intensity value at the given cell index.
+    /// </summary>
+    private double GetValue(Array3i index)
+    {
+        return _hybridGrid.GetIntensity(index);
+    }
+}

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

@@ -0,0 +1,121 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Mapping.D3D;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
+/// 'translation' and 'rotation'. The cost increases when points fall into less
+/// occupied space, i.e. at voxels with lower values.
+/// </summary>
+/// <remarks>
+/// Creates an occupied space cost function for 3D scan matching.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
+/// <param name="pointCloud">Point cloud to match.</param>
+/// <param name="hybridGrid">Hybrid grid to match against.</param>
+public class OccupiedSpaceCostFunction3D(
+    double scalingFactor,
+    PointCloud _pointCloud,
+    HybridGrid hybridGrid) : IDisposable
+{
+    private readonly InterpolatedProbabilityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for occupied space matching.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="pointCloud">Point cloud to match.</param>
+    /// <param name="hybridGrid">Hybrid grid to match against.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        PointCloud pointCloud,
+        HybridGrid hybridGrid)
+    {
+        var costFunction = new OccupiedSpaceCostFunction3D(scalingFactor, pointCloud, hybridGrid);
+        return new DynamicAutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: pointCloud.Count,
+            parameterBlockSizes: [3, 4] // [translation[3], rotation[4]]
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
+    /// <param name="residuals">Output residuals (one per point).</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 2)
+            return false;
+        if (parameters[0].Length < 3 || parameters[1].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < _pointCloud.Count)
+            return false;
+
+        var translation = parameters[0];
+        var rotation = parameters[1]; // [w, x, y, z] from Ceres
+        
+        // Create transform from translation and rotation
+        // C++ line 52-53: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
+        // where rotation = [w, x, y, z]
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        var transform = new Rigid3d(
+            new Vector3(translation[0], translation[1], translation[2]),
+            new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
+        );
+
+        // Transform each point and compute residual
+        for (int i = 0; i < _pointCloud.Count; i++)
+        {
+            var point = _pointCloud[i];
+            
+            // Transform point from local frame to world frame
+            var worldPoint = transform * point.Position;
+            
+            // Get interpolated probability value at world point
+            var probability = _interpolatedGrid.GetInterpolatedValue(
+                worldPoint.X,
+                worldPoint.Y,
+                worldPoint.Z
+            );
+            
+            // Residual = scaling_factor * (1 - probability)
+            // Higher probability (occupied space) = lower residual = better match
+            residuals[i] = scalingFactor * (1.0 - probability);
+        }
+
+        return true;
+    }
+
+    /// <summary>
+    /// Disposes managed resources.
+    /// </summary>
+    public void Dispose()
+    {
+        // InterpolatedProbabilityGrid doesn't need disposal, but we implement IDisposable for consistency
+        GC.SuppressFinalize(this);
+    }
+}

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

@@ -0,0 +1,152 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Common.Math;
+using CartographerSharp.Mapping.D3D;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Precomputation grid for 3D scan matching using 8-bit values instead of 16-bit.
+/// This is used for branch-and-bound algorithm in Fast Correlative Scan Matcher.
+/// </summary>
+/// <remarks>
+/// Creates a new PrecomputationGrid3D with the specified resolution.
+/// </remarks>
+public class PrecomputationGrid3D(double resolution) : HybridGridBase<byte>(resolution)
+{
+    /// <summary>
+    /// Minimum probability value.
+    /// </summary>
+    public const double kMinProbability = 0.1;
+
+    /// <summary>
+    /// Maximum probability value.
+    /// </summary>
+    public const double kMaxProbability = 0.9;
+
+    /// <summary>
+    /// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
+    /// </summary>
+    public static double ToProbability(double value)
+    {
+        return kMinProbability +
+               value * ((kMaxProbability - kMinProbability) / 255.0);
+    }
+
+    /// <summary>
+    /// Gets the value at the given cell index.
+    /// </summary>
+    public new byte GetValue(Array3i index)
+    {
+        return base.GetValue(index);
+    }
+
+    /// <summary>
+    /// Sets the value at the given cell index.
+    /// </summary>
+    public void SetValue(Array3i index, byte value)
+    {
+        ref var cell = ref GetMutableValue(index);
+        cell = value;
+    }
+}
+
+/// <summary>
+/// Converts a HybridGrid to a PrecomputationGrid3D representing the same data,
+/// but only using 8 bit instead of 2 x 16 bit.
+/// </summary>
+public static class PrecomputationGrid3DOperations
+{
+    /// <summary>
+    /// Converts a HybridGrid to a PrecomputationGrid3D.
+    /// </summary>
+    public static PrecomputationGrid3D ConvertToPrecomputationGrid(Mapping.D3D.HybridGrid hybridGrid)
+    {
+        var result = new PrecomputationGrid3D(hybridGrid.Resolution);
+        
+        // Iterate through all cells in the hybrid grid
+        foreach (var (index, value) in hybridGrid)
+        {
+            // Convert probability (ushort) to byte [0, 255]
+            var probability = ProbabilityValues.ValueToProbability(value);
+            var cellValue = (int)Math.Round(
+                (probability - PrecomputationGrid3D.kMinProbability) *
+                (255.0 / (PrecomputationGrid3D.kMaxProbability - PrecomputationGrid3D.kMinProbability))
+            );
+            cellValue = Math.Max(0, Math.Min(255, cellValue));
+            result.SetValue(index, (byte)cellValue);
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Returns a grid of the same resolution containing the maximum value of
+    /// original voxels in 'grid'. This maximum is over the 8 voxels that have
+    /// any combination of index components optionally increased by 'shift'.
+    /// </summary>
+    public static PrecomputationGrid3D PrecomputeGrid(
+        PrecomputationGrid3D grid,
+        bool halfResolution,
+        Array3i shift)
+    {
+        var result = new PrecomputationGrid3D(grid.Resolution);
+        
+        // Iterate through all cells in the input grid
+        foreach (var (index, value) in grid)
+        {
+            // Update 8 values in the resulting grid
+            for (int i = 0; i < 8; i++)
+            {
+                var octant = HybridGridBase<byte>.GetOctant(i);
+                // Element-wise multiplication: shift * octant
+                var shiftOctant = new Array3i(
+                    shift.X * octant.X,
+                    shift.Y * octant.Y,
+                    shift.Z * octant.Z
+                );
+                var cellIndex = index - shiftOctant;
+                
+                if (halfResolution)
+                {
+                    // Convert to half resolution index
+                    cellIndex = CellIndexAtHalfResolution(cellIndex);
+                }
+                
+                // Take maximum value
+                var currentValue = result.GetValue(cellIndex);
+                var newValue = (byte)Math.Max(value, currentValue);
+                result.SetValue(cellIndex, newValue);
+            }
+        }
+        
+        return result;
+    }
+
+    /// <summary>
+    /// Computes the half resolution index corresponding to the full resolution
+    /// 'cell_index'. Uses bit shift to round towards negative infinity.
+    /// </summary>
+    private static Array3i CellIndexAtHalfResolution(Array3i cellIndex)
+    {
+        return new Array3i(
+            cellIndex.X >> 1, // Divide by 2, rounding towards negative infinity
+            cellIndex.Y >> 1,
+            cellIndex.Z >> 1
+        );
+    }
+}

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

@@ -0,0 +1,90 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Common.Math;
+using CartographerSharp.Models.Mapping;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Stack of precomputation grids for branch-and-bound algorithm.
+/// </summary>
+public class PrecomputationGridStack3D
+{
+    private readonly List<PrecomputationGrid3D> _precomputationGrids;
+
+    /// <summary>
+    /// Creates a precomputation grid stack from a hybrid grid.
+    /// </summary>
+    public PrecomputationGridStack3D(
+        Mapping.D3D.HybridGrid hybridGrid,
+        FastCorrelativeScanMatcherOptions3D options)
+    {
+        if (options.BranchAndBoundDepth < 1)
+            throw new ArgumentException("branch_and_bound_depth must be >= 1", nameof(options));
+        if (options.FullResolutionDepth < 1)
+            throw new ArgumentException("full_resolution_depth must be >= 1", nameof(options));
+
+        _precomputationGrids = new List<PrecomputationGrid3D>(options.BranchAndBoundDepth)
+        {
+            // First grid: convert from hybrid grid
+            PrecomputationGrid3DOperations.ConvertToPrecomputationGrid(hybridGrid)
+        };
+        
+        var lastWidth = new Array3i(1, 1, 1);
+        
+        // Create grids for each depth
+        for (int depth = 1; depth < options.BranchAndBoundDepth; depth++)
+        {
+            var halfResolution = depth >= options.FullResolutionDepth;
+            var nextWidth = new Array3i(1 << depth, 1 << depth, 1 << depth);
+            
+            var fullVoxelsPerHighResolutionVoxel = 1 << Math.Max(0, depth - options.FullResolutionDepth);
+            // Element-wise division: (nextWidth - lastWidth + (fullVoxelsPerHighResolutionVoxel - 1)) / fullVoxelsPerHighResolutionVoxel
+            var numerator = nextWidth - lastWidth + new Array3i(fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1);
+            var shift = new Array3i(
+                numerator.X / fullVoxelsPerHighResolutionVoxel,
+                numerator.Y / fullVoxelsPerHighResolutionVoxel,
+                numerator.Z / fullVoxelsPerHighResolutionVoxel
+            );
+            
+            _precomputationGrids.Add(
+                PrecomputationGrid3DOperations.PrecomputeGrid(
+                    _precomputationGrids[^1],
+                    halfResolution,
+                    shift
+                )
+            );
+            
+            lastWidth = nextWidth;
+        }
+    }
+
+    /// <summary>
+    /// Gets the precomputation grid at the specified depth.
+    /// </summary>
+    public PrecomputationGrid3D Get(int depth)
+    {
+        if (depth < 0 || depth >= _precomputationGrids.Count)
+            throw new ArgumentOutOfRangeException(nameof(depth));
+        return _precomputationGrids[depth];
+    }
+
+    /// <summary>
+    /// Gets the maximum depth (0-based).
+    /// </summary>
+    public int MaxDepth => _precomputationGrids.Count - 1;
+}

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

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

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

@@ -0,0 +1,100 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
+/// Cost increases with the solution's distance from 'target_rotation'.
+/// </summary>
+/// <remarks>
+/// Creates a rotation delta cost functor for 3D.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor.</param>
+/// <param name="targetRotation">Target rotation to match.</param>
+public class RotationDeltaCostFunctor3D(double scalingFactor, Quaternion targetRotation)
+{
+    private readonly double[] _targetRotationInverse =
+        [
+            targetRotation.W,
+            -targetRotation.X,
+            -targetRotation.Y,
+            -targetRotation.Z
+        ]; // [w, x, y, z]
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for rotation delta.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="targetRotation">Target rotation to match.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Quaternion targetRotation)
+    {
+        var functor = new RotationDeltaCostFunctor3D(scalingFactor, targetRotation);
+        return new DynamicAutoDiffCostFunction(
+            functor.Evaluate,
+            numResiduals: 3, // [x, y, z] - imaginary part of delta quaternion
+            parameterBlockSizes: [4] // [w, x, y, z] - quaternion
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// Computes delta = target_rotation_inverse * rotation_quaternion
+    /// Returns the imaginary part (x, y, z) of the delta quaternion.
+    /// </summary>
+    /// <param name="parameters">Rotation quaternion [w, x, y, z].</param>
+    /// <param name="residuals">Output residuals [x, y, z] - imaginary part of delta.</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 1 || parameters[0].Length < 4)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var rotation = parameters[0];
+        
+        // Compute quaternion product: target_rotation_inverse * rotation
+        // delta = q_inv * q = [w1, x1, y1, z1] * [w2, x2, y2, z2]
+        // delta.w = w1*w2 - x1*x2 - y1*y2 - z1*z2
+        // delta.x = w1*x2 + x1*w2 + y1*z2 - z1*y2
+        // delta.y = w1*y2 - x1*z2 + y1*w2 + z1*x2
+        // delta.z = w1*z2 + x1*y2 - y1*x2 + z1*w2
+        var w1 = _targetRotationInverse[0];
+        var x1 = _targetRotationInverse[1];
+        var y1 = _targetRotationInverse[2];
+        var z1 = _targetRotationInverse[3];
+        
+        var w2 = rotation[0];
+        var x2 = rotation[1];
+        var y2 = rotation[2];
+        var z2 = rotation[3];
+        
+        // Compute delta quaternion (only need imaginary part for residual)
+        // The squared norm of the imaginary component is sin(phi/2)^2
+        residuals[0] = scalingFactor * (w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2);
+        residuals[1] = scalingFactor * (w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2);
+        residuals[2] = scalingFactor * (w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2);
+        
+        return true;
+    }
+}

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

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

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

@@ -0,0 +1,74 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
+
+/// <summary>
+/// Computes the cost of translating 'translation' to 'target_translation'.
+/// Cost increases with the solution's distance from 'target_translation'.
+/// </summary>
+/// <remarks>
+/// Creates a translation delta cost functor for 3D.
+/// </remarks>
+/// <param name="scalingFactor">Weight factor.</param>
+/// <param name="targetTranslation">Target translation to match.</param>
+public class TranslationDeltaCostFunctor3D(double scalingFactor, Vector3 targetTranslation)
+{
+    private readonly double _targetX = targetTranslation.X;
+    private readonly double _targetY = targetTranslation.Y;
+    private readonly double _targetZ = targetTranslation.Z;
+
+    /// <summary>
+    /// Creates a DynamicAutoDiff cost function for translation delta.
+    /// </summary>
+    /// <param name="scalingFactor">Weight factor.</param>
+    /// <param name="targetTranslation">Target translation to match.</param>
+    /// <returns>DynamicAutoDiff cost function.</returns>
+    public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
+        double scalingFactor,
+        Vector3 targetTranslation)
+    {
+        var functor = new TranslationDeltaCostFunctor3D(scalingFactor, targetTranslation);
+        return new DynamicAutoDiffCostFunction(
+            functor.Evaluate,
+            numResiduals: 3, // [x, y, z]
+            parameterBlockSizes: [3] // [x, y, z]
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    /// <param name="parameters">Translation parameters [x, y, z].</param>
+    /// <param name="residuals">Output residuals [x, y, z].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 3)
+            return false;
+
+        var translation = parameters[0];
+        residuals[0] = scalingFactor * (translation[0] - _targetX);
+        residuals[1] = scalingFactor * (translation[1] - _targetY);
+        residuals[2] = scalingFactor * (translation[2] - _targetZ);
+        return true;
+    }
+}

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

@@ -0,0 +1,80 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Adapter to make LocalTrajectoryBuilder3D implement TrajectoryBuilderInterface.
+/// </summary>
+internal class TrajectoryBuilder3DAdapter(LocalTrajectoryBuilder3D localBuilder) : ITrajectoryBuilder
+{
+    private readonly LocalTrajectoryBuilder3D _localBuilder = localBuilder ?? throw new ArgumentNullException(nameof(localBuilder));
+
+    public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
+    {
+        // LocalTrajectoryBuilder3D now returns ITrajectoryBuilder.MatchingResult directly
+        return _localBuilder.AddRangeData(sensorId, timedPointCloudData);
+    }
+
+    public void AddSensorData(string sensorId, ImuData imuData)
+    {
+        _localBuilder.AddImuData(imuData);
+    }
+
+    public void AddSensorData(string sensorId, OdometryData odometryData)
+    {
+        _localBuilder.AddOdometryData(odometryData);
+    }
+
+    public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
+    {
+        // Fixed frame pose data is typically used for external localization sources
+        // Forward to the wrapped trajectory builder if it supports it
+        // In this adapter, we assume LocalTrajectoryBuilder3D does not directly handle this,
+        // so we do nothing or could potentially pass it to a different component if available.
+        // For now, it remains unimplemented for _localBuilder.
+    }
+
+    public void AddSensorData(string sensorId, LandmarkData landmarkData)
+    {
+        // Landmark data is used for landmark-based SLAM
+        // Forward to the wrapped trajectory builder if it supports it
+        // In this adapter, we assume LocalTrajectoryBuilder3D does not directly handle this,
+        // so we do nothing or could potentially pass it to a different component if available.
+        // For now, it remains unimplemented for _localBuilder.
+    }
+
+    public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
+    {
+        // LocalTrajectoryBuilder3D doesn't use this method
+        // Results are returned directly from AddRangeData
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPose(long time)
+    {
+        return _localBuilder.TryGetExtrapolatedPose(time);
+    }
+
+    /// <inheritdoc />
+    public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
+    {
+        return _localBuilder.TryGetExtrapolatedPoseFilter(time);
+    }
+}

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

@@ -0,0 +1,192 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// A class that tracks the connectivity structure between trajectories.
+///
+/// Connectivity includes both the count ("How many times have I _directly_
+/// connected trajectories i and j?") and the transitive connectivity.
+///
+/// Uses Union-Find (disjoint set forest) algorithm for efficient transitive
+/// connectivity tracking.
+///
+/// Match C++ ConnectedComponents (connected_components.cc)
+/// </summary>
+public class ConnectedComponents
+{
+    private readonly object _lock = new();
+
+    // Tracks transitive connectivity using a disjoint set forest, i.e. each
+    // entry points towards the representative for the given trajectory.
+    private readonly Dictionary<int, int> _forest = new();
+
+    // Tracks the number of direct connections between a pair of trajectories.
+    private readonly Dictionary<(int, int), int> _connectionMap = new();
+
+    /// <summary>
+    /// Add a trajectory which is initially connected to only itself.
+    /// </summary>
+    public void Add(int trajectoryId)
+    {
+        lock (_lock)
+        {
+            // Use TryAdd to avoid overwriting existing entries
+            _forest.TryAdd(trajectoryId, trajectoryId);
+        }
+    }
+
+    /// <summary>
+    /// Connect two trajectories. If either trajectory is untracked, it will be
+    /// tracked. This function is invariant to the order of its arguments. Repeated
+    /// calls to Connect increment the connectivity count.
+    /// </summary>
+    public void Connect(int trajectoryIdA, int trajectoryIdB)
+    {
+        lock (_lock)
+        {
+            Union(trajectoryIdA, trajectoryIdB);
+            var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+            if (!_connectionMap.TryGetValue(sortedPair, out var count))
+            {
+                count = 0;
+            }
+            _connectionMap[sortedPair] = count + 1;
+        }
+    }
+
+    /// <summary>
+    /// Determines if two trajectories have been (transitively) connected. If
+    /// either trajectory is not being tracked, returns false, except when it is
+    /// the same trajectory, where it returns true. This function is invariant to
+    /// the order of its arguments.
+    /// </summary>
+    public bool TransitivelyConnected(int trajectoryIdA, int trajectoryIdB)
+    {
+        if (trajectoryIdA == trajectoryIdB)
+        {
+            return true;
+        }
+
+        lock (_lock)
+        {
+            if (!_forest.ContainsKey(trajectoryIdA) || !_forest.ContainsKey(trajectoryIdB))
+            {
+                return false;
+            }
+            return FindSet(trajectoryIdA) == FindSet(trajectoryIdB);
+        }
+    }
+
+    /// <summary>
+    /// Return the number of _direct_ connections between 'trajectoryIdA' and
+    /// 'trajectoryIdB'. If either trajectory is not being tracked, returns 0.
+    /// This function is invariant to the order of its arguments.
+    /// </summary>
+    public int ConnectionCount(int trajectoryIdA, int trajectoryIdB)
+    {
+        lock (_lock)
+        {
+            var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+            return _connectionMap.TryGetValue(sortedPair, out var count) ? count : 0;
+        }
+    }
+
+    /// <summary>
+    /// The trajectory IDs, grouped by connectivity.
+    /// </summary>
+    public List<List<int>> Components()
+    {
+        lock (_lock)
+        {
+            // Map from cluster exemplar -> growing cluster
+            var map = new Dictionary<int, List<int>>();
+            foreach (var entry in _forest)
+            {
+                var representative = FindSet(entry.Key);
+                if (!map.TryGetValue(representative, out var component))
+                {
+                    component = [];
+                    map[representative] = component;
+                }
+                component.Add(entry.Key);
+            }
+
+            return [.. map.Values];
+        }
+    }
+
+    /// <summary>
+    /// The list of trajectory IDs that belong to the same connected component as
+    /// 'trajectoryId'.
+    /// </summary>
+    public List<int> GetComponent(int trajectoryId)
+    {
+        lock (_lock)
+        {
+            if (!_forest.ContainsKey(trajectoryId))
+            {
+                return [trajectoryId];
+            }
+
+            var setId = FindSet(trajectoryId);
+            var trajectoryIds = new List<int>();
+            foreach (var entry in _forest)
+            {
+                if (FindSet(entry.Key) == setId)
+                {
+                    trajectoryIds.Add(entry.Key);
+                }
+            }
+            return trajectoryIds;
+        }
+    }
+
+    /// <summary>
+    /// Find the representative and compresses the path to it.
+    /// Must be called with lock held.
+    /// </summary>
+    private int FindSet(int trajectoryId)
+    {
+        if (!_forest.TryGetValue(trajectoryId, out var parent))
+        {
+            return trajectoryId;
+        }
+
+        if (trajectoryId != parent)
+        {
+            // Path compression for efficiency
+            _forest[trajectoryId] = FindSet(parent);
+        }
+        return _forest[trajectoryId];
+    }
+
+    /// <summary>
+    /// Union two sets.
+    /// Must be called with lock held.
+    /// </summary>
+    private void Union(int trajectoryIdA, int trajectoryIdB)
+    {
+        // Add trajectories if not already tracked
+        _forest.TryAdd(trajectoryIdA, trajectoryIdA);
+        _forest.TryAdd(trajectoryIdB, trajectoryIdB);
+
+        var representativeA = FindSet(trajectoryIdA);
+        var representativeB = FindSet(trajectoryIdB);
+        _forest[representativeA] = representativeB;
+    }
+}

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

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

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

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

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

@@ -0,0 +1,36 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Corresponds to C++ <c>internal/global_trajectory_builder.cc</c> (single file in <c>internal/</c>).
+/// <para>
+/// C++ has one template class <c>GlobalTrajectoryBuilder&lt;LocalTrajectoryBuilder, PoseGraph&gt;</c>
+/// and two factory functions: <c>CreateGlobalTrajectoryBuilder2D</c>, <c>CreateGlobalTrajectoryBuilder3D</c>.
+/// </para>
+/// <para>
+/// C# has no templates, so the logic is split into two classes that mirror the template instantiations:
+/// <list type="bullet">
+///   <item><c>GlobalTrajectoryBuilder2D</c> in <c>Internal/2D/GlobalTrajectoryBuilder2D.cs</c> — corresponds to <c>GlobalTrajectoryBuilder&lt;LocalTrajectoryBuilder2D, PoseGraph2D&gt;</c></item>
+///   <item><c>GlobalTrajectoryBuilder3D</c> in <c>Internal/3D/GlobalTrajectoryBuilder3D.cs</c> — corresponds to <c>GlobalTrajectoryBuilder&lt;LocalTrajectoryBuilder3D, PoseGraph3D&gt;</c></item>
+/// </list>
+/// Creation is performed in <see cref="MapBuilder.AddTrajectoryBuilder"/>, equivalent to C++ MapBuilder::AddTrajectory calling CreateGlobalTrajectoryBuilder2D/3D.
+/// </para>
+/// </summary>
+public static class GlobalTrajectoryBuilder
+{
+}

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

@@ -0,0 +1,67 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Transform;
+using static CartographerSharp.Transform.TransformOperations;
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Takes poses as input and filters them to get fewer poses.
+/// </summary>
+public class MotionFilter(MotionFilterOptions options)
+{
+    private int _numTotal;
+    private int _numDifferent;
+    private long _lastTime; // Universal Time Scale ticks
+    private Rigid3d _lastPose = Rigid3d.Identity;
+
+    /// <summary>
+    /// If the accumulated motion (linear, rotational, or time) is above the
+    /// threshold, returns false. Otherwise the relative motion is accumulated and
+    /// true is returned.
+    /// </summary>
+    public bool IsSimilar(long time, Rigid3d pose)
+    {
+        _numTotal++;
+        
+        // Match C++ logic: Check all conditions in one if statement for short-circuit evaluation
+        // C++: if (num_total_ > 1 && time - last_time_ <= ... && translation <= ... && rotation <= ...)
+        if (_numTotal > 1)
+        {
+            var timeDelta = (time - _lastTime) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)
+            var translationDelta = (pose.Translation - _lastPose.Translation).Length();
+            var rotationDelta = GetAngle(pose.Inverse() * _lastPose);
+            
+            // Match C++: All conditions must be true (using && for short-circuit evaluation)
+            if (timeDelta <= options.MaxTimeSeconds &&
+                translationDelta <= options.MaxDistanceMeters &&
+                rotationDelta <= options.MaxAngleRadians)
+            {
+                // Motion is similar - return true without updating last_time_ and last_pose_
+                // (matches C++ behavior where last_time_ and last_pose_ are only updated when returning false)
+                return true;
+            }
+        }
+        
+        // Motion is NOT similar - update last_time_ and last_pose_ (match C++ lines 57-59)
+        _lastTime = time;
+        _lastPose = pose;
+        _numDifferent++;
+        return false;
+    }
+}

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

@@ -0,0 +1,199 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Helper functions for cost function computation.
+/// </summary>
+internal static class CostHelpers
+{
+    /// <summary>
+    /// Computes spherical linear interpolation of unit quaternions.
+    /// </summary>
+    public static Quaternion SlerpQuaternions(Quaternion start, Quaternion end, double factor)
+    {
+        // Normalize quaternions
+        start = Quaternion.Normalize(start);
+        end = Quaternion.Normalize(end);
+
+        // Compute dot product
+        var cosTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
+        // Clamp to [-1, 1] to handle floating-point errors that could cause Math.Acos to return NaN
+        var absCosTheta = Math.Min(1.0, Math.Abs(cosTheta));
+
+        // If quaternions are nearly collinear, use linear interpolation
+        const double kEpsilon = 1e-6;
+        double prevScale = 1.0 - factor;
+        double nextScale = factor;
+
+        if (absCosTheta < 1.0 - kEpsilon)
+        {
+            var theta = Math.Acos(absCosTheta);
+            var sinTheta = Math.Sin(theta);
+            if (sinTheta > kEpsilon)
+            {
+                prevScale = Math.Sin((1.0 - factor) * theta) / sinTheta;
+                nextScale = Math.Sin(factor * theta) / sinTheta;
+            }
+        }
+
+        if (cosTheta < 0.0)
+        {
+            nextScale = -nextScale;
+        }
+
+        // Quaternion constructor is (x, y, z, w), matching C++ output format [w, x, y, z]
+        // but converting to C# Quaternion format (x, y, z, w)
+        var result = new Quaternion(
+            prevScale * start.X + nextScale * end.X,
+            prevScale * start.Y + nextScale * end.Y,
+            prevScale * start.Z + nextScale * end.Z,
+            prevScale * start.W + nextScale * end.W
+        );
+        // Normalize to ensure unit quaternion (Eigen SLERP automatically normalizes)
+        return Quaternion.Normalize(result);
+    }
+
+    /// <summary>
+    /// Interpolates 3D nodes.
+    /// </summary>
+    public static (Quaternion rotation, Vector3 translation) InterpolateNodes3D(
+        double[] prevNodeRotation, // [w, x, y, z]
+        double[] prevNodeTranslation, // [x, y, z]
+        double[] nextNodeRotation, // [w, x, y, z]
+        double[] nextNodeTranslation, // [x, y, z]
+        double interpolationParameter)
+    {
+        // Match C++: prev_node_rotation is [w, x, y, z]
+        // System.Numerics.Quaternion constructor is (x, y, z, w)
+        var prevQuaternion = new Quaternion(
+            prevNodeRotation[1], // x
+            prevNodeRotation[2], // y
+            prevNodeRotation[3], // z
+            prevNodeRotation[0]  // w
+        );
+        var nextQuaternion = new Quaternion(
+            nextNodeRotation[1], // x
+            nextNodeRotation[2], // y
+            nextNodeRotation[3], // z
+            nextNodeRotation[0]  // w
+        );
+
+        // Interpolate rotation using SLERP
+        var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);
+
+        // Interpolate translation linearly
+        var interpolatedTranslation = new Vector3(
+            (prevNodeTranslation[0] + interpolationParameter * (nextNodeTranslation[0] - prevNodeTranslation[0])),
+            (prevNodeTranslation[1] + interpolationParameter * (nextNodeTranslation[1] - prevNodeTranslation[1])),
+            (prevNodeTranslation[2] + interpolationParameter * (nextNodeTranslation[2] - prevNodeTranslation[2]))
+        );
+
+        return (interpolatedRotation, interpolatedTranslation);
+    }
+
+    /// <summary>
+    /// Interpolates 2D nodes embedded in 3D space.
+    /// </summary>
+    public static (Quaternion rotation, Vector3 translation) InterpolateNodes2D(
+        double[] prevNodePose, // [x, y, theta]
+        Quaternion prevNodeGravityAlignment,
+        double[] nextNodePose, // [x, y, theta]
+        Quaternion nextNodeGravityAlignment,
+        double interpolationParameter)
+    {
+        // Embed 2D pose into 3D with gravity alignment
+        // Equivalent to: Embed3D(prev_node_pose) * Rigid3d::Rotation(prev_node_gravity_alignment)
+        var prevRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, prevNodePose[2]);
+        var prevQuaternion = Quaternion.Normalize(prevRotation2D * prevNodeGravityAlignment);
+
+        var nextRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, nextNodePose[2]);
+        var nextQuaternion = Quaternion.Normalize(nextRotation2D * nextNodeGravityAlignment);
+
+        // Interpolate rotation using SLERP
+        var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);
+
+        // Interpolate translation linearly (2D, z=0)
+        var interpolatedTranslation = new Vector3(
+            (prevNodePose[0] + interpolationParameter * (nextNodePose[0] - prevNodePose[0])),
+            (prevNodePose[1] + interpolationParameter * (nextNodePose[1] - prevNodePose[1])),
+            0.0
+        );
+
+        return (interpolatedRotation, interpolatedTranslation);
+    }
+
+    /// <summary>
+    /// Computes unscaled error for 3D poses.
+    /// Error = observed_relative_pose - computed_relative_pose
+    /// </summary>
+    public static double[] ComputeUnscaledError3D(
+        Rigid3d observedRelativePose,
+        Quaternion startRotation,
+        Vector3 startTranslation,
+        Quaternion endRotation,
+        Vector3 endTranslation)
+    {
+        // Compute relative transform: start^-1 * end
+        var startInverse = Quaternion.Inverse(startRotation);
+        var deltaTranslation = endTranslation - startTranslation;
+        var rotatedDelta = Vector3.Transform(deltaTranslation, startInverse);
+
+        // Compute h_rotation_inverse = (end^-1) * start (matching C++ implementation)
+        // This is equivalent to: endRotation.Inverse() * startRotation
+        var endInverse = Quaternion.Inverse(endRotation);
+        var hRotationInverse = endInverse * startRotation;
+
+        // Error rotation: h_rotation_inverse * observed_relative_rotation
+        var errorRotation = hRotationInverse * observedRelativePose.Rotation;
+
+        // Convert rotation error to angle-axis
+        var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(errorRotation);
+
+        return
+        [
+            observedRelativePose.Translation.X - rotatedDelta.X,
+            observedRelativePose.Translation.Y - rotatedDelta.Y,
+            observedRelativePose.Translation.Z - rotatedDelta.Z,
+            angleAxis.X,
+            angleAxis.Y,
+            angleAxis.Z
+        ];
+    }
+
+    /// <summary>
+    /// Scales error with translation and rotation weights.
+    /// </summary>
+    public static double[] ScaleError3D(
+        double[] unscaledError,
+        double translationWeight,
+        double rotationWeight)
+    {
+        return
+        [
+            translationWeight * unscaledError[0],
+            translationWeight * unscaledError[1],
+            translationWeight * unscaledError[2],
+            rotationWeight * unscaledError[3],
+            rotationWeight * unscaledError[4],
+            rotationWeight * unscaledError[5]
+        ];
+    }
+}

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

@@ -0,0 +1,124 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CeresSharp;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Cost function measuring the weighted error between the observed pose given by
+/// the landmark measurement and the linearly interpolated pose of embedded in 3D
+/// space node poses.
+/// </summary>
+public class LandmarkCostFunction2D
+{
+    private readonly IPoseGraph.LandmarkNode.LandmarkObservation _observation;
+    private readonly NodeSpec2D _prevNode;
+    private readonly NodeSpec2D _nextNode;
+    private readonly double _interpolationParameter;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for landmark constraints.
+    /// </summary>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec2D prevNode,
+        NodeSpec2D nextNode)
+    {
+        var costFunction = new LandmarkCostFunction2D(observation, prevNode, nextNode);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz]
+            parameterBlockSizes: [3, 3, 4, 3] // [prev_node[3], next_node[3], landmark_rotation[4], landmark_translation[3]]
+        );
+    }
+
+    private LandmarkCostFunction2D(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec2D prevNode,
+        NodeSpec2D nextNode)
+    {
+        _observation = observation;
+        _prevNode = prevNode;
+        _nextNode = nextNode;
+
+        // Compute interpolation parameter
+        _interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
+            _observation.Time,
+            _prevNode.Time,
+            _nextNode.Time
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 4)
+            return false;
+        if (parameters[0].Length < 3 || parameters[1].Length < 3 ||
+            parameters[2].Length < 4 || parameters[3].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 6)
+            return false;
+
+        var prevNodePose = parameters[0]; // [x, y, theta]
+        var nextNodePose = parameters[1]; // [x, y, theta]
+        var landmarkRotation = parameters[2]; // [w, x, y, z]
+        var landmarkTranslation = parameters[3]; // [x, y, z]
+
+        // Interpolate node poses
+        var (interpolatedRotation, interpolatedTranslation) = CostHelpers.InterpolateNodes2D(
+            prevNodePose,
+            _prevNode.GravityAlignment,
+            nextNodePose,
+            _nextNode.GravityAlignment,
+            _interpolationParameter
+        );
+
+        // Landmark pose parameters
+        var landmarkRotationQuat = OptimizationHelpers.ParametersToQuaternion(landmarkRotation);
+        var landmarkTranslationVec = OptimizationHelpers.ParametersToVector3(landmarkTranslation);
+
+        // The landmark cost function computes error between:
+        // - observed: landmark_to_tracking_transform (from observation)
+        // - computed: (interpolated_tracking_pose^-1 * landmark_pose)
+        // Error = observed - computed
+        // This is equivalent to: landmark_to_tracking_transform - (interpolated_pose^-1 * landmark_pose)
+        var unscaledError = CostHelpers.ComputeUnscaledError3D(
+            _observation.LandmarkToTrackingTransform,
+            interpolatedRotation,
+            interpolatedTranslation,
+            landmarkRotationQuat,
+            landmarkTranslationVec
+        );
+
+        // Scale error
+        var scaledError = CostHelpers.ScaleError3D(
+            unscaledError,
+            _observation.TranslationWeight,
+            _observation.RotationWeight
+        );
+
+        for (int i = 0; i < 6; i++)
+        {
+            residuals[i] = scaledError[i];
+        }
+
+        return true;
+    }
+}

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

@@ -0,0 +1,122 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Mapping.Internal.D3D.Optimization;
+using CeresSharp;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Cost function measuring the weighted error between the observed pose given by
+/// the landmark measurement and the linearly interpolated pose.
+/// </summary>
+public class LandmarkCostFunction3D
+{
+    private readonly IPoseGraph.LandmarkNode.LandmarkObservation _observation;
+    private readonly NodeSpec3D _prevNode;
+    private readonly NodeSpec3D _nextNode;
+    private readonly double _interpolationParameter;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for landmark constraints in 3D.
+    /// </summary>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec3D prevNode,
+        NodeSpec3D nextNode)
+    {
+        var costFunction = new LandmarkCostFunction3D(observation, prevNode, nextNode);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz]
+            parameterBlockSizes: [4, 3, 4, 3, 4, 3] // [prev_rotation[4], prev_translation[3], next_rotation[4], next_translation[3], landmark_rotation[4], landmark_translation[3]]
+        );
+    }
+
+    private LandmarkCostFunction3D(
+        IPoseGraph.LandmarkNode.LandmarkObservation observation,
+        NodeSpec3D prevNode,
+        NodeSpec3D nextNode)
+    {
+        _observation = observation;
+        _prevNode = prevNode;
+        _nextNode = nextNode;
+
+        // Compute interpolation parameter
+        _interpolationParameter = OptimizationHelpers.ComputeInterpolationParameter(
+            _observation.Time,
+            _prevNode.Time,
+            _nextNode.Time
+        );
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// </summary>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 6)
+            return false;
+        if (parameters[0].Length < 4 || parameters[1].Length < 3 ||
+            parameters[2].Length < 4 || parameters[3].Length < 3 ||
+            parameters[4].Length < 4 || parameters[5].Length < 3)
+            return false;
+        if (residuals == null || residuals.Length < 6)
+            return false;
+
+        var prevNodeRotation = parameters[0]; // [w, x, y, z]
+        var prevNodeTranslation = parameters[1]; // [x, y, z]
+        var nextNodeRotation = parameters[2]; // [w, x, y, z]
+        var nextNodeTranslation = parameters[3]; // [x, y, z]
+        var landmarkRotation = parameters[4]; // [w, x, y, z]
+        var landmarkTranslation = parameters[5]; // [x, y, z]
+
+        // Interpolate node poses
+        var (interpolatedRotationQuat, interpolatedTranslationVec) = CostHelpers.InterpolateNodes3D(
+            prevNodeRotation,
+            prevNodeTranslation,
+            nextNodeRotation,
+            nextNodeTranslation,
+            _interpolationParameter
+        );
+
+        var landmarkRotationQuat = OptimizationHelpers.ParametersToQuaternion(landmarkRotation);
+        var landmarkTranslationVec = OptimizationHelpers.ParametersToVector3(landmarkTranslation);
+
+        // Compute error
+        var unscaledError = CostHelpers.ComputeUnscaledError3D(
+            _observation.LandmarkToTrackingTransform,
+            interpolatedRotationQuat,
+            interpolatedTranslationVec,
+            landmarkRotationQuat,
+            landmarkTranslationVec
+        );
+
+        // Scale error
+        var scaledError = CostHelpers.ScaleError3D(
+            unscaledError,
+            _observation.TranslationWeight,
+            _observation.RotationWeight
+        );
+
+        for (int i = 0; i < 6; i++)
+        {
+            residuals[i] = scaledError[i];
+        }
+
+        return true;
+    }
+}

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

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

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

@@ -0,0 +1,187 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Transform;
+using CeresSharp;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.Optimization;
+
+/// <summary>
+/// Sparse Pose Adjustment (SPA) cost function for 2D pose graph optimization.
+/// Computes the error between observed relative pose and computed relative pose.
+/// </summary>
+public class SpaCostFunction2D
+{
+    private readonly IPoseGraph.Constraint.Pose _observedRelativePose;
+    private readonly Rigid2d _observedRelativePose2D;
+
+    /// <summary>
+    /// Creates an AutoDiff cost function for SPA.
+    /// </summary>
+    /// <param name="observedRelativePose">The observed relative pose constraint.</param>
+    /// <returns>AutoDiff cost function.</returns>
+    public static AutoDiffCostFunction CreateAutoDiffCostFunction(
+        IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        var costFunction = new SpaCostFunction2D(observedRelativePose);
+        return new AutoDiffCostFunction(
+            costFunction.Evaluate,
+            numResiduals: 3, // [dx, dy, dtheta]
+            parameterBlockSizes: [3, 3] // [start_pose[3], end_pose[3]]
+        );
+    }
+
+    private SpaCostFunction2D(IPoseGraph.Constraint.Pose observedRelativePose)
+    {
+        _observedRelativePose = observedRelativePose;
+        // Project 3D pose to 2D
+        _observedRelativePose2D = TransformOperations.Project2D(observedRelativePose.ZbarIj);
+    }
+
+    /// <summary>
+    /// Evaluates the cost function.
+    /// Match C++ spa_cost_function_2d.h operator() implementation.
+    /// </summary>
+    /// <param name="parameters">Parameter blocks [start_pose[3], end_pose[3]].</param>
+    /// <param name="residuals">Output residuals [dx, dy, dtheta].</param>
+    /// <returns>True on success.</returns>
+    private bool Evaluate(double[][] parameters, double[] residuals)
+    {
+        if (parameters == null || parameters.Length < 2)
+        {
+            return false;
+        }
+        if (parameters[0].Length < 3 || parameters[1].Length < 3)
+        {
+            return false;
+        }
+        if (residuals == null || residuals.Length < 3)
+        {
+            return false;
+        }
+
+        var startPose = parameters[0];
+        var endPose = parameters[1];
+        
+        // Validate parameters for NaN/Infinity
+        for (int i = 0; i < 3; i++)
+        {
+            if (double.IsNaN(startPose[i]) || double.IsInfinity(startPose[i]))
+            {
+                return false;
+            }
+            if (double.IsNaN(endPose[i]) || double.IsInfinity(endPose[i]))
+            {
+                return false;
+            }
+        }
+        
+        // NOTE: Weight validation removed to match C++ behavior.
+        // C++ does not validate weights - Ceres handles invalid weights internally.
+        // Validation was causing constraints to be incorrectly rejected.
+
+        // NOTE: Pose explosion handling REMOVED to match C++ behavior.
+        // The original C++ spa_cost_function_2d.h does NOT have any pose distance checks.
+        // Returning zero residuals was causing optimization to skip constraints incorrectly,
+        // leading to optimization failures and incorrect pose graph results.
+        // If poses diverge, Ceres will handle it through its own convergence criteria.
+
+        // Compute unscaled error (match C++ cost_helpers_impl.h ComputeUnscaledError)
+        var unscaledError = ComputeUnscaledError(
+            _observedRelativePose2D,
+            startPose,
+            endPose
+        );
+
+        // Scale error with weights (match C++ ScaleError)
+        var translationWeight = _observedRelativePose.TranslationWeight;
+        var rotationWeight = _observedRelativePose.RotationWeight;
+
+        var scaledError = ScaleError(
+            unscaledError,
+            translationWeight,
+            rotationWeight
+        );
+
+        residuals[0] = scaledError[0];
+        residuals[1] = scaledError[1];
+        residuals[2] = scaledError[2];
+
+        return true;
+    }
+
+    /// <summary>
+    /// Computes unscaled error between observed and computed relative pose.
+    /// Match C++: Uses direct formula for numerical stability with Ceres autodiff.
+    /// </summary>
+    private static double[] ComputeUnscaledError(
+        Rigid2d observedRelativePose,
+        double[] startPose,
+        double[] endPose)
+    {
+        // Match C++ implementation in cost_helpers_impl.h
+        // startPose = [x1, y1, theta1]
+        // endPose = [x2, y2, theta2]
+        // observedRelativePose = relative pose from start to end (in start frame)
+
+        var cosThetaI = Math.Cos(startPose[2]);
+        var sinThetaI = Math.Sin(startPose[2]);
+        var deltaX = endPose[0] - startPose[0];
+        var deltaY = endPose[1] - startPose[1];
+        
+        // Compute h = relative pose from start to end (in start frame)
+        // h[0] = cos_theta_i * delta_x + sin_theta_i * delta_y
+        // h[1] = -sin_theta_i * delta_x + cos_theta_i * delta_y
+        // h[2] = end[2] - start[2]
+        var h0 = cosThetaI * deltaX + sinThetaI * deltaY;
+        var h1 = -sinThetaI * deltaX + cosThetaI * deltaY;
+        var h2 = endPose[2] - startPose[2];
+
+        // Error = observed - computed
+        var translationErrorX = observedRelativePose.Translation.X - h0;
+        var translationErrorY = observedRelativePose.Translation.Y - h1;
+        
+        // Rotation error (normalize angle difference)
+        var rotationError = OptimizationHelpers.NormalizeAngleDifference(
+            observedRelativePose.Rotation - h2
+        );
+
+        return
+        [
+            translationErrorX,
+            translationErrorY,
+            rotationError
+        ];
+    }
+
+    /// <summary>
+    /// Scales error with translation and rotation weights.
+    /// </summary>
+    private static double[] ScaleError(
+        double[] unscaledError,
+        double translationWeight,
+        double rotationWeight)
+    {
+        return
+        [
+            translationWeight * unscaledError[0],
+            translationWeight * unscaledError[1],
+            rotationWeight * unscaledError[2]
+        ];
+    }
+
+}

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

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

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

@@ -0,0 +1,117 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Tracks the connectivity state between trajectories and the last time a global
+/// constraint connected two trajectories.
+///
+/// Compared to ConnectedComponents it tracks additionally the last time that a global
+/// constraint connected two trajectories.
+///
+/// Match C++ TrajectoryConnectivityState (trajectory_connectivity_state.cc)
+/// </summary>
+public class TrajectoryConnectivityState
+{
+    // ConnectedComponents is thread safe
+    private readonly ConnectedComponents _connectedComponents = new();
+
+    // Tracks the last time a direct connection between two trajectories has
+    // been added. The exception is when a connection between two trajectories
+    // connects two formerly unconnected connected components. In this case all
+    // bipartite trajectories entries for these components are updated with the
+    // new connection time.
+    private readonly Dictionary<(int, int), long> _lastConnectionTimeMap = new();
+
+    /// <summary>
+    /// Add a trajectory which is initially connected to only itself.
+    /// </summary>
+    public void Add(int trajectoryId)
+    {
+        _connectedComponents.Add(trajectoryId);
+    }
+
+    /// <summary>
+    /// Connect two trajectories. If either trajectory is untracked, it will be
+    /// tracked. This function is invariant to the order of its arguments. Repeated
+    /// calls to Connect increment the connectivity count and update the last
+    /// connected time.
+    /// </summary>
+    public void Connect(int trajectoryIdA, int trajectoryIdB, long time)
+    {
+        if (TransitivelyConnected(trajectoryIdA, trajectoryIdB))
+        {
+            // The trajectories are transitively connected, i.e. they belong to the same
+            // connected component. In this case we only update the last connection time
+            // of those two trajectories.
+            var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+            if (!_lastConnectionTimeMap.TryGetValue(sortedPair, out var existing) || existing < time)
+            {
+                _lastConnectionTimeMap[sortedPair] = time;
+            }
+        }
+        else
+        {
+            // The connection between these two trajectories is about to join two
+            // connected components. Here we update all bipartite trajectory pairs for
+            // the two connected components with the connection time. This is to quickly
+            // change to a more efficient loop closure search (by constraining the
+            // search window) when connected components are joined.
+            var componentA = _connectedComponents.GetComponent(trajectoryIdA);
+            var componentB = _connectedComponents.GetComponent(trajectoryIdB);
+            foreach (var idA in componentA)
+            {
+                foreach (var idB in componentB)
+                {
+                    var idPair = (Math.Min(idA, idB), Math.Max(idA, idB));
+                    _lastConnectionTimeMap[idPair] = time;
+                }
+            }
+        }
+        _connectedComponents.Connect(trajectoryIdA, trajectoryIdB);
+    }
+
+    /// <summary>
+    /// Determines if two trajectories have been (transitively) connected. If
+    /// either trajectory is not being tracked, returns false, except when it is
+    /// the same trajectory, where it returns true. This function is invariant to
+    /// the order of its arguments.
+    /// </summary>
+    public bool TransitivelyConnected(int trajectoryIdA, int trajectoryIdB)
+    {
+        return _connectedComponents.TransitivelyConnected(trajectoryIdA, trajectoryIdB);
+    }
+
+    /// <summary>
+    /// The trajectory IDs, grouped by connectivity.
+    /// </summary>
+    public List<List<int>> Components()
+    {
+        return _connectedComponents.Components();
+    }
+
+    /// <summary>
+    /// Returns the last connection time between the two trajectories.
+    /// If either of the trajectories is untracked or they have never been
+    /// connected returns 0 (beginning of time).
+    /// </summary>
+    public long LastConnectionTime(int trajectoryIdA, int trajectoryIdB)
+    {
+        var sortedPair = (Math.Min(trajectoryIdA, trajectoryIdB), Math.Max(trajectoryIdA, trajectoryIdB));
+        return _lastConnectionTimeMap.TryGetValue(sortedPair, out var t) ? t : 0L;
+    }
+}

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

@@ -0,0 +1,40 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+namespace CartographerSharp.Mapping.Internal;
+
+/// <summary>
+/// Result of processing a work item, indicating what should happen next.
+/// Similar to Cartographer C++ WorkItem::Result.
+/// </summary>
+public enum WorkItemResult
+{
+    /// <summary>
+    /// Do not run optimization after this work item.
+    /// </summary>
+    DoNotRunOptimization,
+    
+    /// <summary>
+    /// Run optimization after this work item.
+    /// </summary>
+    RunOptimization
+}
+
+/// <summary>
+/// Represents a work item to be processed by the pose graph.
+/// Similar to Cartographer C++ WorkItem.
+/// </summary>
+public record WorkItem(Func<WorkItemResult> Action);

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

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