Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/CovarianceCalculator.cs

@@ -0,0 +1,81 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for calculating covariance from Cartographer constraints
+/// </summary>
+public static class CovarianceCalculator
+{
+    /// <summary>
+    /// Result of covariance calculation
+    /// </summary>
+    public record Result(
+        Matrix3x3? Covariance,
+        int ConstraintCount,
+        double AverageConstraintQuality);
+
+    /// <summary>
+    /// Calculate covariance and quality metrics from constraints
+    /// </summary>
+    /// <param name="constraints">List of pose graph constraints</param>
+    /// <returns>Covariance calculation result</returns>
+    public static Result Calculate(IList<IPoseGraph.Constraint> constraints)
+    {
+        if (constraints == null || constraints.Count == 0)
+        {
+            return new Result(null, 0, 0.0);
+        }
+
+        int constraintCount = constraints.Count;
+        
+        // Calculate average constraint weights
+        var avgTranslationWeight = constraints.Average(c => c.ConstraintPose.TranslationWeight);
+        var avgRotationWeight = constraints.Average(c => c.ConstraintPose.RotationWeight);
+
+        // Calculate constraint quality score (0.0 - 1.0)
+        var normalizedTranslationQuality = Math.Min(1.0, avgTranslationWeight / 100.0);
+        var normalizedRotationQuality = Math.Min(1.0, avgRotationWeight / 100.0);
+        var averageConstraintQuality = (normalizedTranslationQuality + normalizedRotationQuality) / 2.0;
+
+        // Calculate covariance from constraint weights
+        var translationWeightVariance = constraints
+            .Select(c => c.ConstraintPose.TranslationWeight)
+            .Select(w => Math.Pow(w - avgTranslationWeight, 2))
+            .Average();
+            
+        var rotationWeightVariance = constraints
+            .Select(c => c.ConstraintPose.RotationWeight)
+            .Select(w => Math.Pow(w - avgRotationWeight, 2))
+            .Average();
+
+        // Convert weights to covariance (inverse relationship)
+        var translationCovBase = 1.0 / (avgTranslationWeight + 1e-6);
+        var rotationCovBase = 1.0 / (avgRotationWeight + 1e-6);
+
+        // Adjust covariance based on variance
+        var translationVarianceFactor = 1.0 + (translationWeightVariance / (avgTranslationWeight * avgTranslationWeight + 1e-6));
+        var rotationVarianceFactor = 1.0 + (rotationWeightVariance / (avgRotationWeight * avgRotationWeight + 1e-6));
+
+        var translationCov = translationCovBase * translationVarianceFactor;
+        var rotationCov = rotationCovBase * rotationVarianceFactor;
+
+        // Adjust covariance based on number of constraints
+        var constraintCountFactor = 1.0 / (1.0 + Math.Log10(Math.Max(1, constraintCount)));
+        translationCov *= constraintCountFactor;
+        rotationCov *= constraintCountFactor;
+
+        // Create covariance matrix for 2D pose (x, y, theta)
+        var covariance = Matrix3x3.Covariance(
+            xx: translationCov,
+            yy: translationCov,
+            tt: rotationCov,
+            xy: 0.0,
+            xt: 0.0,
+            yt: 0.0
+        );
+
+        return new Result(covariance, constraintCount, averageConstraintQuality);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/DistanceTransformHelper.cs

@@ -0,0 +1,124 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Euclidean Distance Transform using Felzenszwalb-Huttenlocher algorithm O(n).
+/// Shared implementation used by both MclService and ScanMatchingQualityEvaluator.
+/// Reference: "Distance Transforms of Sampled Functions", Felzenszwalb & Huttenlocher, 2012.
+/// </summary>
+public static class DistanceTransformHelper
+{
+    #region Public API
+
+    /// <summary>
+    /// Compute Euclidean distance (in meters) from each cell to the nearest occupied cell.
+    /// binaryMap[v,u] == 0 → occupied, != 0 → free.
+    /// </summary>
+    public static double[,] ComputeEuclidean(byte[,] binaryMap, int width, int height, double resolution)
+    {
+        // Step 1: Initialize squared distances (0 for occupied, inf for free)
+        const int inf = int.MaxValue / 2;
+        var distSq = new int[height, width];
+        for (int v = 0; v < height; v++)
+            for (int u = 0; u < width; u++)
+                distSq[v, u] = binaryMap[v, u] == 0 ? 0 : inf;
+
+        // Step 2: 1D distance transform along rows (horizontal pass)
+        var tempDist = new int[Math.Max(width, height)];
+        for (int y = 0; y < height; y++)
+        {
+            for (int x = 0; x < width; x++)
+                tempDist[x] = distSq[y, x];
+
+            DistanceTransform1D(tempDist, width);
+
+            for (int x = 0; x < width; x++)
+                distSq[y, x] = tempDist[x];
+        }
+
+        // Step 3: 1D distance transform along columns (vertical pass)
+        for (int x = 0; x < width; x++)
+        {
+            for (int y = 0; y < height; y++)
+                tempDist[y] = distSq[y, x];
+
+            DistanceTransform1D(tempDist, height);
+
+            for (int y = 0; y < height; y++)
+                distSq[y, x] = tempDist[y];
+        }
+
+        // Step 4: Convert squared distance (in pixels) to Euclidean distance (in meters)
+        var result = new double[height, width];
+        for (int v = 0; v < height; v++)
+            for (int u = 0; u < width; u++)
+                result[v, u] = Math.Sqrt(distSq[v, u]) * resolution;
+
+        return result;
+    }
+
+    #endregion
+
+    #region 1D Transform
+
+    /// <summary>
+    /// 1D squared Euclidean distance transform using parabola lower envelope algorithm.
+    /// Operates in-place on the input array.
+    /// </summary>
+    private static void DistanceTransform1D(int[] f, int n)
+    {
+        if (n == 0) return;
+
+        // v stores parabola indices, z stores intersection points
+        var v = new int[n];
+        var z = new double[n + 1];
+        int k = 0; // index of rightmost parabola
+        v[0] = 0;
+        z[0] = double.NegativeInfinity;
+        z[1] = double.PositiveInfinity;
+
+        // Build lower envelope of parabolas
+        for (int q = 1; q < n; q++)
+        {
+            double s;
+            while (true)
+            {
+                int vk = v[k];
+                double fq = f[q];
+                double fvk = f[vk];
+                s = ((fq + q * q) - (fvk + vk * vk)) / (2.0 * (q - vk));
+
+                if (s > z[k])
+                    break;
+
+                k--;
+                if (k < 0)
+                {
+                    k = 0;
+                    break;
+                }
+            }
+
+            k++;
+            v[k] = q;
+            z[k] = s;
+            z[k + 1] = double.PositiveInfinity;
+        }
+
+        // Fill in values of distance transform
+        k = 0;
+        var result = new int[n];
+        for (int q = 0; q < n; q++)
+        {
+            while (z[k + 1] < q)
+                k++;
+            int vk = v[k];
+            int dx = q - vk;
+            result[q] = dx * dx + f[vk];
+        }
+
+        // Copy result back
+        Array.Copy(result, f, n);
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/DriftDetector.cs

@@ -0,0 +1,808 @@
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Detects map drift during localization by monitoring multiple metrics.
+/// Combines scan matching quality, odometry residuals, and optional MCL cross-validation.
+/// </summary>
+public class DriftDetector
+{
+    #region Configuration
+
+    /// <summary>Configuration for drift detection thresholds and weights</summary>
+    public record DriftDetectorConfig
+    {
+        /// <summary>Window size for moving average calculations</summary>
+        public int WindowSize { get; init; } = 20;
+
+        /// <summary>Minimum scan match score to consider "good" (0.0-1.0)</summary>
+        public double MinScanMatchScore { get; init; } = 0.4;
+
+        /// <summary>Maximum allowed odometry residual in meters before flagging drift</summary>
+        public double MaxOdometryResidual { get; init; } = 0.5;
+
+        /// <summary>Maximum allowed MCL-Cartographer divergence in meters</summary>
+        public double MaxMclDivergence { get; init; } = 0.3;
+
+        /// <summary>Maximum allowed MCL-Cartographer yaw divergence in radians</summary>
+        public double MaxMclYawDivergence { get; init; } = 0.2;
+
+        /// <summary>Threshold below which to flag potential drift (0.0-1.0)</summary>
+        public double DriftWarningThreshold { get; init; } = 0.5;
+
+        /// <summary>Threshold below which to flag critical drift (0.0-1.0)</summary>
+        public double DriftCriticalThreshold { get; init; } = 0.3;
+
+        /// <summary>
+        /// Critical scan match score threshold for "veto" logic.
+        /// When ScanMatchScore falls below this, CombinedScore is capped regardless of other metrics.
+        /// This prevents other "good" metrics from masking a fundamental scan matching failure.
+        /// </summary>
+        public double ScanMatchVetoThreshold { get; init; } = 0.25;
+
+        /// <summary>
+        /// Maximum CombinedScore allowed when ScanMatchScore is below veto threshold.
+        /// Even if other metrics are perfect, the score cannot exceed this cap.
+        /// </summary>
+        public double ScanMatchVetoCap { get; init; } = 0.35;
+
+        /// <summary>
+        /// Maximum allowed pose jump distance (meters) per update cycle.
+        /// Jumps larger than this indicate teleportation or severe error.
+        /// </summary>
+        public double MaxPoseJumpDistance { get; init; } = 0.5;
+
+        /// <summary>
+        /// Maximum allowed pose jump rotation (radians) per update cycle.
+        /// </summary>
+        public double MaxPoseJumpRotation { get; init; } = 0.5;
+
+        /// <summary>
+        /// Scan match variance threshold to distinguish drift vs dynamic obstacles.
+        /// High variance (> threshold) suggests dynamic obstacles.
+        /// Low variance with low score suggests drift.
+        /// </summary>
+        public double ScanMatchVarianceThreshold { get; init; } = 0.04; // std dev ~0.2
+
+        // Weights for combining different metrics
+        public double ScanMatchWeight { get; init; } = 0.30;
+        public double OdometryResidualWeight { get; init; } = 0.20;
+        public double MclDivergenceWeight { get; init; } = 0.25;
+        public double ConstraintQualityWeight { get; init; } = 0.15;
+        public double CovarianceWeight { get; init; } = 0.10;
+    }
+
+    #endregion
+
+    #region Metrics Result
+
+    /// <summary>Result containing all drift detection metrics</summary>
+    public record DriftMetrics
+    {
+        /// <summary>Raw scan match score from Cartographer (0.0-1.0)</summary>
+        public double ScanMatchScore { get; init; }
+
+        /// <summary>Normalized scan match score (0.0-1.0)</summary>
+        public double ScanMatchScoreNormalized { get; init; }
+
+        /// <summary>Odometry residual in meters (accumulated drift from odometry)</summary>
+        public double OdometryResidual { get; init; }
+
+        /// <summary>Normalized odometry residual score (0.0-1.0, higher is better)</summary>
+        public double OdometryResidualScore { get; init; }
+
+        /// <summary>Distance between MCL pose and Cartographer pose in meters</summary>
+        public double? MclDivergence { get; init; }
+
+        /// <summary>Yaw difference between MCL and Cartographer in radians</summary>
+        public double? MclYawDivergence { get; init; }
+
+        /// <summary>Normalized MCL divergence score (0.0-1.0, higher is better)</summary>
+        public double MclDivergenceScore { get; init; }
+
+        /// <summary>Constraint quality from pose graph (0.0-1.0)</summary>
+        public double ConstraintQuality { get; init; }
+
+        /// <summary>Covariance-based score (0.0-1.0)</summary>
+        public double CovarianceScore { get; init; }
+
+        /// <summary>Combined drift score (0.0-1.0, higher means more confident/less drift)</summary>
+        public double CombinedScore { get; init; }
+
+        /// <summary>Drift status based on combined score</summary>
+        public DriftStatus Status { get; init; }
+
+        /// <summary>Moving average of combined score over window</summary>
+        public double MovingAverageScore { get; init; }
+
+        /// <summary>Trend of score: positive = improving, negative = degrading</summary>
+        public double ScoreTrend { get; init; }
+
+        /// <summary>True if a sudden pose jump was detected (possible kidnapping or severe error)</summary>
+        public bool PoseJumpDetected { get; init; }
+
+        /// <summary>Distance of pose jump in meters (0 if no jump)</summary>
+        public double PoseJumpDistance { get; init; }
+
+        /// <summary>Variance of scan match scores over the window (high variance = dynamic obstacles)</summary>
+        public double ScanMatchVariance { get; init; }
+
+        /// <summary>
+        /// Type of localization degradation detected.
+        /// Helps distinguish between drift and dynamic obstacles.
+        /// </summary>
+        public DegradationType Degradation { get; init; }
+    }
+
+    /// <summary>Drift status levels</summary>
+    public enum DriftStatus
+    {
+        /// <summary>Localization is stable and confident</summary>
+        Stable,
+
+        /// <summary>Minor degradation detected, monitoring</summary>
+        Warning,
+
+        /// <summary>Significant drift detected, may need relocalization</summary>
+        Critical,
+
+        /// <summary>Severe drift, relocalization recommended</summary>
+        Lost
+    }
+
+    /// <summary>
+    /// Type of localization degradation, helps distinguish root cause.
+    /// </summary>
+    public enum DegradationType
+    {
+        /// <summary>No degradation, localization is healthy</summary>
+        None,
+
+        /// <summary>
+        /// Gradual drift detected: low scan match scores with low variance,
+        /// increasing odometry residual over time. Robot position is slowly
+        /// diverging from true position.
+        /// </summary>
+        Drift,
+
+        /// <summary>
+        /// Dynamic obstacles detected: high scan match variance (fluctuating scores),
+        /// but odometry residual remains low. Temporary occlusion from moving objects.
+        /// </summary>
+        DynamicObstacles,
+
+        /// <summary>
+        /// Sudden pose jump detected: large position change in short time.
+        /// Possible causes: robot kidnapping, scan matcher jumped to wrong location,
+        /// or map ambiguity (similar-looking areas).
+        /// </summary>
+        PoseJump,
+
+        /// <summary>
+        /// Featureless area: low scan match scores due to lack of distinctive features.
+        /// Common in long corridors or open spaces.
+        /// </summary>
+        FeaturelessArea,
+
+        /// <summary>
+        /// Unknown degradation: cannot determine specific cause.
+        /// </summary>
+        Unknown
+    }
+
+    #endregion
+
+    #region Fields
+
+    private readonly DriftDetectorConfig _config;
+    private readonly Lock _lock = new();
+
+    // Moving window for score history
+    private readonly Queue<double> _scoreHistory;
+    private readonly Queue<double> _scanMatchHistory;
+    private readonly Queue<double> _odometryResidualHistory;
+
+    // Odometry tracking for residual calculation
+    private Pose _lastOdometryPose;
+    private Pose _lastCartographerPose;
+    private double _accumulatedOdometryDistance;
+    private double _accumulatedCartographerDistance;
+    private bool _initialized;
+
+    // Pose jump detection
+    private Pose _previousPoseForJumpDetection;
+    private bool _poseJumpInitialized;
+
+    // Latest metrics
+    private DriftMetrics _latestMetrics = new()
+    {
+        ScanMatchScore = 1.0,
+        ScanMatchScoreNormalized = 1.0,
+        OdometryResidual = 0.0,
+        OdometryResidualScore = 1.0,
+        MclDivergenceScore = 1.0,
+        ConstraintQuality = 1.0,
+        CovarianceScore = 1.0,
+        CombinedScore = 1.0,
+        Status = DriftStatus.Stable,
+        MovingAverageScore = 1.0,
+        ScoreTrend = 0.0,
+        PoseJumpDetected = false,
+        PoseJumpDistance = 0.0,
+        ScanMatchVariance = 0.0,
+        Degradation = DegradationType.None
+    };
+
+    #endregion
+
+    #region Constructor
+
+    public DriftDetector(DriftDetectorConfig? config = null)
+    {
+        _config = config ?? new DriftDetectorConfig();
+        _scoreHistory = new Queue<double>(_config.WindowSize);
+        _scanMatchHistory = new Queue<double>(_config.WindowSize);
+        _odometryResidualHistory = new Queue<double>(_config.WindowSize);
+    }
+
+    #endregion
+
+    #region Public Methods
+
+    /// <summary>
+    /// Update drift detection with new sensor data.
+    /// Call this method each time new localization data is available.
+    /// </summary>
+    /// <param name="cartographerPose">Current pose from Cartographer</param>
+    /// <param name="odometryPose">Current pose from odometry (optional)</param>
+    /// <param name="scanMatchScore">Scan match confidence from Cartographer (PoseConfidence)</param>
+    /// <param name="covariance">Pose covariance matrix (optional)</param>
+    /// <param name="constraintCount">Number of constraints in pose graph</param>
+    /// <param name="constraintQuality">Average constraint quality (0.0-1.0)</param>
+    /// <param name="mclPose">Pose from MCL if running in parallel (optional)</param>
+    /// <param name="mclReliability">MCL reliability score (optional)</param>
+    /// <returns>Updated drift metrics</returns>
+    public DriftMetrics Update(
+        Pose cartographerPose,
+        Pose? odometryPose,
+        double scanMatchScore,
+        Matrix3x3? covariance,
+        int constraintCount,
+        double constraintQuality,
+        Pose? mclPose = null,
+        double? mclReliability = null)
+    {
+        lock (_lock)
+        {
+            // 1. Calculate scan match score (normalized)
+            var scanMatchScoreNormalized = NormalizeScanMatchScore(scanMatchScore);
+            AddToHistory(_scanMatchHistory, scanMatchScoreNormalized);
+
+            // 2. Calculate odometry residual
+            double odometryResidual = 0.0;
+            double odometryResidualScore = 1.0;
+
+            if (odometryPose.HasValue)
+            {
+                odometryResidual = CalculateOdometryResidual(cartographerPose, odometryPose.Value);
+                odometryResidualScore = CalculateOdometryResidualScore(odometryResidual);
+                AddToHistory(_odometryResidualHistory, odometryResidual);
+            }
+
+            // 3. Detect pose jump (sudden large position change)
+            var (poseJumpDetected, poseJumpDistance) = DetectPoseJump(cartographerPose);
+
+            // 4. Calculate MCL divergence (if MCL pose available)
+            double? mclDivergence = null;
+            double? mclYawDivergence = null;
+            double mclDivergenceScore = 0.5; // Neutral if no MCL
+
+            if (mclPose.HasValue)
+            {
+                (mclDivergence, mclYawDivergence) = CalculateMclDivergence(cartographerPose, mclPose.Value);
+                mclDivergenceScore = CalculateMclDivergenceScore(mclDivergence.Value, mclYawDivergence.Value, mclReliability);
+            }
+
+            // 5. Calculate covariance score
+            var covarianceScore = CalculateCovarianceScore(covariance);
+
+            // 6. Calculate combined score with adaptive weights
+            var weights = CalculateAdaptiveWeights(
+                hasMcl: mclPose.HasValue,
+                hasOdometry: odometryPose.HasValue,
+                constraintCount: constraintCount);
+
+            var combinedScore =
+                (scanMatchScoreNormalized * weights.ScanMatch) +
+                (odometryResidualScore * weights.OdometryResidual) +
+                (mclDivergenceScore * weights.MclDivergence) +
+                (constraintQuality * weights.ConstraintQuality) +
+                (covarianceScore * weights.Covariance);
+
+            // 6b. Apply "veto" logic: if ScanMatchScore is critically low, cap CombinedScore
+            // This prevents other "good" metrics from masking a fundamental scan matching failure.
+            // Rationale: When scan matching fails, covariance/constraints computed from bad matches
+            // are unreliable, so their high values shouldn't override the scan match warning.
+            if (scanMatchScoreNormalized < _config.ScanMatchVetoThreshold)
+            {
+                combinedScore = Math.Min(combinedScore, _config.ScanMatchVetoCap);
+            }
+
+            // 6c. If pose jump detected, cap score severely (possible kidnapping)
+            if (poseJumpDetected)
+            {
+                combinedScore = Math.Min(combinedScore, 0.2);
+            }
+
+            combinedScore = Math.Clamp(combinedScore, 0.0, 1.0);
+
+            // 7. Update score history and calculate moving average
+            AddToHistory(_scoreHistory, combinedScore);
+            var movingAverage = _scoreHistory.Count > 0 ? _scoreHistory.Average() : combinedScore;
+
+            // 8. Calculate trend (positive = improving, negative = degrading)
+            var trend = CalculateTrend();
+
+            // 9. Calculate scan match variance (helps distinguish drift vs dynamic obstacles)
+            var scanMatchVariance = CalculateScanMatchVariance();
+
+            // 10. Determine drift status (pass scanMatchScoreNormalized for veto logic)
+            var status = DetermineDriftStatus(movingAverage, trend, scanMatchScoreNormalized);
+
+            // 10b. If pose jump detected, force Critical status
+            if (poseJumpDetected && status < DriftStatus.Critical)
+            {
+                status = DriftStatus.Critical;
+            }
+
+            // 11. Determine degradation type (drift vs dynamic obstacles vs pose jump)
+            var degradationType = DetermineDegradationType(
+                status,
+                scanMatchScoreNormalized,
+                scanMatchVariance,
+                odometryResidual,
+                poseJumpDetected,
+                constraintCount);
+
+            // 12. Build result
+            _latestMetrics = new DriftMetrics
+            {
+                ScanMatchScore = scanMatchScore,
+                ScanMatchScoreNormalized = scanMatchScoreNormalized,
+                OdometryResidual = odometryResidual,
+                OdometryResidualScore = odometryResidualScore,
+                MclDivergence = mclDivergence,
+                MclYawDivergence = mclYawDivergence,
+                MclDivergenceScore = mclDivergenceScore,
+                ConstraintQuality = constraintQuality,
+                CovarianceScore = covarianceScore,
+                CombinedScore = combinedScore,
+                Status = status,
+                MovingAverageScore = movingAverage,
+                ScoreTrend = trend,
+                PoseJumpDetected = poseJumpDetected,
+                PoseJumpDistance = poseJumpDistance,
+                ScanMatchVariance = scanMatchVariance,
+                Degradation = degradationType
+            };
+
+            return _latestMetrics;
+        }
+    }
+
+    /// <summary>Gets the latest drift metrics without updating</summary>
+    public DriftMetrics GetLatestMetrics()
+    {
+        lock (_lock)
+        {
+            return _latestMetrics;
+        }
+    }
+
+    /// <summary>Resets the drift detector state</summary>
+    public void Reset()
+    {
+        lock (_lock)
+        {
+            _scoreHistory.Clear();
+            _scanMatchHistory.Clear();
+            _odometryResidualHistory.Clear();
+            _initialized = false;
+            _poseJumpInitialized = false;
+            _accumulatedOdometryDistance = 0;
+            _accumulatedCartographerDistance = 0;
+            _latestMetrics = new DriftMetrics
+            {
+                ScanMatchScore = 1.0,
+                ScanMatchScoreNormalized = 1.0,
+                OdometryResidual = 0.0,
+                OdometryResidualScore = 1.0,
+                MclDivergenceScore = 1.0,
+                ConstraintQuality = 1.0,
+                CovarianceScore = 1.0,
+                CombinedScore = 1.0,
+                Status = DriftStatus.Stable,
+                MovingAverageScore = 1.0,
+                ScoreTrend = 0.0,
+                PoseJumpDetected = false,
+                PoseJumpDistance = 0.0,
+                ScanMatchVariance = 0.0,
+                Degradation = DegradationType.None
+            };
+        }
+    }
+
+    #endregion
+
+    #region Private Methods
+
+    private static double NormalizeScanMatchScore(double rawScore)
+    {
+        // PoseConfidence from Cartographer is returned as percentage (0-100),
+        // not as a normalized value (0-1). Handle both ranges.
+        if (rawScore < 0) return 0.0;
+
+        // Normalize to [0, 1] range if input is in [0, 100] range
+        var score = rawScore;
+        if (score > 1.0)
+        {
+            score = score / 100.0;
+        }
+
+        // Clamp to valid range
+        return Math.Clamp(score, 0.0, 1.0);
+    }
+
+    /// <summary>
+    /// Detects sudden large pose changes (teleportation or severe localization error).
+    /// </summary>
+    /// <returns>Tuple of (jumpDetected, jumpDistance)</returns>
+    private (bool Detected, double Distance) DetectPoseJump(Pose currentPose)
+    {
+        if (!_poseJumpInitialized)
+        {
+            _previousPoseForJumpDetection = currentPose;
+            _poseJumpInitialized = true;
+            return (false, 0.0);
+        }
+
+        // Calculate position change
+        var dx = currentPose.Position.X - _previousPoseForJumpDetection.Position.X;
+        var dy = currentPose.Position.Y - _previousPoseForJumpDetection.Position.Y;
+        var distance = Math.Sqrt(dx * dx + dy * dy);
+
+        // Calculate rotation change (normalize to [-π, π])
+        // Extract yaw from quaternion orientation
+        var currentYaw = currentPose.Orientation.ToYawRadian();
+        var previousYaw = _previousPoseForJumpDetection.Orientation.ToYawRadian();
+        var dyaw = currentYaw - previousYaw;
+        while (dyaw > Math.PI) dyaw -= 2 * Math.PI;
+        while (dyaw < -Math.PI) dyaw += 2 * Math.PI;
+        var rotationChange = Math.Abs(dyaw);
+
+        // Update previous pose
+        _previousPoseForJumpDetection = currentPose;
+
+        // Check for jump
+        bool isJump = distance > _config.MaxPoseJumpDistance ||
+                      rotationChange > _config.MaxPoseJumpRotation;
+
+        return (isJump, distance);
+    }
+
+    /// <summary>
+    /// Calculates variance of scan match scores over the history window.
+    /// High variance indicates fluctuating scores (likely dynamic obstacles).
+    /// Low variance with low mean indicates consistent poor matching (likely drift).
+    /// </summary>
+    private double CalculateScanMatchVariance()
+    {
+        if (_scanMatchHistory.Count < 2)
+            return 0.0;
+
+        var mean = _scanMatchHistory.Average();
+        var sumSquaredDiff = _scanMatchHistory.Sum(x => Math.Pow(x - mean, 2));
+        return sumSquaredDiff / _scanMatchHistory.Count;
+    }
+
+    /// <summary>
+    /// Determines the type of localization degradation based on multiple metrics.
+    /// This helps users understand the root cause of localization issues.
+    /// </summary>
+    private DegradationType DetermineDegradationType(
+        DriftStatus status,
+        double scanMatchScore,
+        double scanMatchVariance,
+        double odometryResidual,
+        bool poseJumpDetected,
+        int constraintCount)
+    {
+        // If localization is stable, no degradation
+        if (status == DriftStatus.Stable)
+            return DegradationType.None;
+
+        // Pose jump takes priority - it's a clear signal
+        if (poseJumpDetected)
+            return DegradationType.PoseJump;
+
+        // High variance in scan match scores suggests dynamic obstacles
+        // (scores fluctuate as obstacles move in and out of view)
+        bool highVariance = scanMatchVariance > _config.ScanMatchVarianceThreshold;
+
+        // Low odometry residual means odometry and Cartographer agree on movement
+        // High residual means they disagree (accumulated drift)
+        bool lowOdometryResidual = odometryResidual < _config.MaxOdometryResidual * 0.5;
+
+        // Low scan match with high variance + low odometry residual = dynamic obstacles
+        // Robot is in the right place, but moving objects are confusing the scan matcher
+        if (highVariance && lowOdometryResidual)
+            return DegradationType.DynamicObstacles;
+
+        // Low scan match with low variance + increasing odometry residual = drift
+        // Scan matcher consistently can't match well, and position is drifting
+        if (!highVariance && !lowOdometryResidual)
+            return DegradationType.Drift;
+
+        // Low scan match with low variance + low odometry residual = featureless area
+        // Not enough distinctive features for reliable matching, but robot hasn't moved much
+        if (!highVariance && lowOdometryResidual && constraintCount < 5)
+            return DegradationType.FeaturelessArea;
+
+        // Low scan match but with some variance, could be drift beginning
+        if (!highVariance && scanMatchScore < _config.MinScanMatchScore)
+            return DegradationType.Drift;
+
+        return DegradationType.Unknown;
+    }
+
+    private double CalculateOdometryResidual(Pose cartographerPose, Pose odometryPose)
+    {
+        if (!_initialized)
+        {
+            _lastOdometryPose = odometryPose;
+            _lastCartographerPose = cartographerPose;
+            _accumulatedOdometryDistance = 0;
+            _accumulatedCartographerDistance = 0;
+            _initialized = true;
+            return 0.0;
+        }
+
+        // Calculate distance traveled according to odometry
+        var odomDelta = Math.Sqrt(
+            Math.Pow(odometryPose.Position.X - _lastOdometryPose.Position.X, 2) +
+            Math.Pow(odometryPose.Position.Y - _lastOdometryPose.Position.Y, 2));
+
+        // Calculate distance traveled according to Cartographer
+        var cartoDelta = Math.Sqrt(
+            Math.Pow(cartographerPose.Position.X - _lastCartographerPose.Position.X, 2) +
+            Math.Pow(cartographerPose.Position.Y - _lastCartographerPose.Position.Y, 2));
+
+        _accumulatedOdometryDistance += odomDelta;
+        _accumulatedCartographerDistance += cartoDelta;
+
+        // Update last poses
+        _lastOdometryPose = odometryPose;
+        _lastCartographerPose = cartographerPose;
+
+        // Calculate residual as absolute difference in accumulated distances
+        // This indicates drift between odometry and SLAM
+        var residual = Math.Abs(_accumulatedOdometryDistance - _accumulatedCartographerDistance);
+
+        // Reset accumulated distances periodically to avoid unbounded growth
+        if (_accumulatedOdometryDistance > 10.0 || _accumulatedCartographerDistance > 10.0)
+        {
+            _accumulatedOdometryDistance = 0;
+            _accumulatedCartographerDistance = 0;
+        }
+
+        return residual;
+    }
+
+    private double CalculateOdometryResidualScore(double residual)
+    {
+        // Convert residual to score: lower residual = higher score
+        // Use exponential decay: score = exp(-k * residual)
+        // k chosen so that MaxOdometryResidual gives ~0.37 (1/e)
+        double k = 1.0 / _config.MaxOdometryResidual;
+        return Math.Exp(-k * residual);
+    }
+
+    private (double distance, double yawDiff) CalculateMclDivergence(Pose cartographerPose, Pose mclPose)
+    {
+        // Calculate Euclidean distance between poses
+        var distance = Math.Sqrt(
+            Math.Pow(cartographerPose.Position.X - mclPose.Position.X, 2) +
+            Math.Pow(cartographerPose.Position.Y - mclPose.Position.Y, 2));
+
+        // Calculate yaw difference
+        var cartoYaw = cartographerPose.Orientation.ToYawRadian();
+        var mclYaw = mclPose.Orientation.ToYawRadian();
+        var yawDiff = Math.Abs(NormalizeAngle(cartoYaw - mclYaw));
+
+        return (distance, yawDiff);
+    }
+
+    private double CalculateMclDivergenceScore(double distance, double yawDiff, double? mclReliability)
+    {
+        // Distance score: exponential decay
+        double distanceScore = Math.Exp(-distance / _config.MaxMclDivergence);
+
+        // Yaw score: exponential decay
+        double yawScore = Math.Exp(-yawDiff / _config.MaxMclYawDivergence);
+
+        // Combine distance and yaw scores
+        double geometricScore = (distanceScore * 0.7) + (yawScore * 0.3);
+
+        // Weight by MCL reliability if available
+        if (mclReliability.HasValue)
+        {
+            // If MCL is reliable and diverges from Cartographer, that's a strong signal
+            // If MCL is unreliable, don't trust the divergence as much
+            return geometricScore * (0.5 + 0.5 * mclReliability.Value);
+        }
+
+        return geometricScore;
+    }
+
+    private double CalculateCovarianceScore(Matrix3x3? covariance)
+    {
+        if (!covariance.HasValue)
+            return 0.5; // Neutral if no covariance
+
+        var cov = covariance.Value;
+        var trace = cov[0, 0] + cov[1, 1] + cov[2, 2];
+
+        if (trace < 1e-6)
+            return 1.0; // Very low covariance = high confidence
+
+        // Normalize: score = 1 / (1 + trace)
+        return 1.0 / (1.0 + trace);
+    }
+
+    private record struct WeightSet(
+        double ScanMatch,
+        double OdometryResidual,
+        double MclDivergence,
+        double ConstraintQuality,
+        double Covariance);
+
+    private WeightSet CalculateAdaptiveWeights(bool hasMcl, bool hasOdometry, int constraintCount)
+    {
+        // Start with configured weights
+        double scanMatch = _config.ScanMatchWeight;
+        double odometry = _config.OdometryResidualWeight;
+        double mcl = _config.MclDivergenceWeight;
+        double constraint = _config.ConstraintQualityWeight;
+        double covariance = _config.CovarianceWeight;
+
+        // Redistribute MCL weight if not available
+        if (!hasMcl)
+        {
+            // Give MCL weight to scan match (most reliable alternative)
+            scanMatch += mcl * 0.6;
+            odometry += mcl * 0.2;
+            constraint += mcl * 0.2;
+            mcl = 0;
+        }
+
+        // Redistribute odometry weight if not available
+        if (!hasOdometry)
+        {
+            scanMatch += odometry * 0.5;
+            constraint += odometry * 0.3;
+            covariance += odometry * 0.2;
+            odometry = 0;
+        }
+
+        // Reduce constraint weight if few constraints
+        if (constraintCount < 5)
+        {
+            double reduction = constraint * 0.5;
+            constraint *= 0.5;
+            scanMatch += reduction;
+        }
+
+        // Normalize to sum to 1.0
+        double total = scanMatch + odometry + mcl + constraint + covariance;
+        if (total > 0)
+        {
+            scanMatch /= total;
+            odometry /= total;
+            mcl /= total;
+            constraint /= total;
+            covariance /= total;
+        }
+
+        return new WeightSet(scanMatch, odometry, mcl, constraint, covariance);
+    }
+
+    private double CalculateTrend()
+    {
+        if (_scoreHistory.Count < 3)
+            return 0.0;
+
+        var scores = _scoreHistory.ToArray();
+        int n = scores.Length;
+
+        // Calculate simple linear trend using least squares
+        // trend = (n * sum(i*y[i]) - sum(i) * sum(y[i])) / (n * sum(i^2) - sum(i)^2)
+        double sumI = 0, sumY = 0, sumIY = 0, sumI2 = 0;
+        for (int i = 0; i < n; i++)
+        {
+            sumI += i;
+            sumY += scores[i];
+            sumIY += i * scores[i];
+            sumI2 += i * i;
+        }
+
+        double denominator = n * sumI2 - sumI * sumI;
+        if (Math.Abs(denominator) < 1e-10)
+            return 0.0;
+
+        double trend = (n * sumIY - sumI * sumY) / denominator;
+
+        // Normalize trend to roughly [-1, 1] range
+        // Multiply by window size to make it scale-independent
+        return trend * n;
+    }
+
+    private DriftStatus DetermineDriftStatus(double movingAverage, double trend, double scanMatchScoreNormalized)
+    {
+        // Veto logic: if ScanMatchScore is critically low, force at least Warning status
+        // regardless of combined score. This ensures scan matching failures are never masked.
+        DriftStatus minStatus = DriftStatus.Stable;
+        if (scanMatchScoreNormalized < _config.ScanMatchVetoThreshold)
+        {
+            // Scan matching is failing - at minimum this is Critical
+            minStatus = DriftStatus.Critical;
+        }
+        else if (scanMatchScoreNormalized < _config.MinScanMatchScore)
+        {
+            // Scan matching is poor - at minimum this is Warning
+            minStatus = DriftStatus.Warning;
+        }
+
+        // Consider both current score and trend
+        double effectiveScore = movingAverage;
+
+        // If score is declining rapidly, be more aggressive
+        if (trend < -0.1)
+        {
+            effectiveScore -= 0.1;
+        }
+
+        DriftStatus computedStatus;
+        if (effectiveScore < 0.15)
+            computedStatus = DriftStatus.Lost;
+        else if (effectiveScore < _config.DriftCriticalThreshold)
+            computedStatus = DriftStatus.Critical;
+        else if (effectiveScore < _config.DriftWarningThreshold)
+            computedStatus = DriftStatus.Warning;
+        else
+            computedStatus = DriftStatus.Stable;
+
+        // Return the worse of computed status and veto-enforced minimum status
+        // DriftStatus enum: Stable=0, Warning=1, Critical=2, Lost=3
+        return (DriftStatus)Math.Max((int)computedStatus, (int)minStatus);
+    }
+
+    private void AddToHistory(Queue<double> history, double value)
+    {
+        if (history.Count >= _config.WindowSize)
+        {
+            history.Dequeue();
+        }
+        history.Enqueue(value);
+    }
+
+    private static double NormalizeAngle(double angle)
+    {
+        while (angle > Math.PI) angle -= 2 * Math.PI;
+        while (angle < -Math.PI) angle += 2 * Math.PI;
+        return angle;
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/LocalizationScoreCalculator.cs

@@ -0,0 +1,364 @@
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for calculating localization confidence scores with MCL reliability metrics
+/// and scan matching quality for drift detection.
+/// </summary>
+public static class LocalizationScoreCalculator
+{
+    private const int MinConstraintsForGoodScore = 5;
+    private const int MaxConstraintsForScaling = 50;
+
+    // Base weights when all data is available (v2 - includes scan match score)
+    private const double ScanMatchWeight = 0.30;         // NEW - most important for drift detection
+    private const double CovarianceWeight = 0.15;        // Reduced from 0.25
+    private const double ConstraintCountWeight = 0.10;   // Reduced from 0.20
+    private const double ConstraintQualityWeight = 0.10; // Reduced from 0.15
+    private const double MclReliabilityWeight = 0.20;    // Reduced from 0.25
+    private const double MclMaeWeight = 0.15;            // Same as before
+
+    /// <summary>
+    /// Calculate localization confidence score (0.0 - 1.0) from multiple factors including MCL metrics
+    /// and scan matching quality. This is the primary method for Localizing state.
+    /// </summary>
+    /// <param name="covariance">Pose covariance matrix (nullable)</param>
+    /// <param name="constraintCount">Number of constraints</param>
+    /// <param name="constraintQuality">Average constraint quality (0.0 - 1.0)</param>
+    /// <param name="mclReliability">MCL reliability [0,1] from decision model (nullable)</param>
+    /// <param name="mclMae">MCL mean absolute error in meters (nullable)</param>
+    /// <param name="scanMatchScore">Scan matching score (PoseConfidence) from Cartographer (nullable)</param>
+    /// <returns>Combined localization score (0.0 - 1.0)</returns>
+    public static double Calculate(
+        Matrix3x3? covariance,
+        int constraintCount,
+        double constraintQuality,
+        double? mclReliability = null,
+        double? mclMae = null,
+        double? scanMatchScore = null)
+    {
+        var covarianceScore = CalculateCovarianceScoreImproved(covariance);
+        var constraintCountScore = CalculateConstraintCountScore(constraintCount);
+        var constraintQualityScore = constraintQuality;
+
+        // MCL metrics (default to neutral values if not available)
+        var mclReliabilityScore = mclReliability ?? 0.5;
+        var mclMaeScore = CalculateMaeScore(mclMae);
+
+        // Scan match score (critical for drift detection)
+        var normalizedScanMatchScore = CalculateScanMatchScore(scanMatchScore);
+
+        // Adaptive weight adjustment based on data availability
+        var weights = CalculateAdaptiveWeightsV2(
+            hasMcl: mclReliability.HasValue,
+            hasCovariance: covariance.HasValue,
+            hasScanMatch: scanMatchScore.HasValue,
+            constraintCount: constraintCount);
+
+        var combinedScore =
+            (normalizedScanMatchScore * weights.ScanMatch) +
+            (covarianceScore * weights.Covariance) +
+            (constraintCountScore * weights.ConstraintCount) +
+            (constraintQualityScore * weights.ConstraintQuality) +
+            (mclReliabilityScore * weights.MclReliability) +
+            (mclMaeScore * weights.MclMae);
+
+        return Math.Clamp(combinedScore, 0.0, 1.0);
+    }
+
+    /// <summary>
+    /// Calculate score from scan matching confidence (PoseConfidence from Cartographer).
+    /// This is the most direct indicator of how well the current scan matches the map.
+    /// Low scan match score often indicates drift or being in a featureless area.
+    ///
+    /// IMPORTANT: PoseConfidence from Cartographer is returned as percentage (0-100),
+    /// not as a normalized value (0-1). This method handles both ranges.
+    /// </summary>
+    private static double CalculateScanMatchScore(double? scanMatchScore)
+    {
+        if (!scanMatchScore.HasValue)
+        {
+            return 0.5; // Neutral if not available
+        }
+
+        var score = scanMatchScore.Value;
+
+        // Normalize to [0, 1] range if input is in [0, 100] range (PoseConfidence is percentage)
+        // Cartographer's LocalPose_Confidence returns 0-100
+        if (score > 1.0)
+        {
+            score = score / 100.0;
+        }
+
+        // Clamp to valid range
+        score = Math.Clamp(score, 0.0, 1.0);
+
+        // Apply sigmoid transformation to make it more sensitive in mid-range
+        // This helps detect gradual degradation before it becomes critical
+        // Sigmoid: score' = 1 / (1 + exp(-10 * (score - 0.5)))
+        // This maps: 0.3 -> ~0.12, 0.5 -> 0.5, 0.7 -> ~0.88
+        return 1.0 / (1.0 + Math.Exp(-10.0 * (score - 0.5)));
+    }
+
+    /// <summary>
+    /// Calculate score from covariance matrix using trace instead of determinant
+    /// Trace = sum of diagonal elements (sum of variances) - better captures overall uncertainty
+    /// </summary>
+    private static double CalculateCovarianceScoreImproved(Matrix3x3? covariance)
+    {
+        if (covariance == null)
+        {
+            return 0.5; // Default to moderate confidence
+        }
+
+        // Use trace (sum of diagonal elements) instead of determinant
+        // Trace = σ_x² + σ_y² + σ_θ² (sum of variances)
+        // Better captures overall uncertainty and less sensitive to directional bias
+        var cov = covariance.Value;
+        var trace = cov[0, 0] + cov[1, 1] + cov[2, 2];
+
+        if (trace < 1e-6)
+        {
+            return 1.0; // Very low covariance = high confidence
+        }
+
+        // Normalize to 0.0-1.0 range using inverse relationship
+        // Typical good localization: trace < 0.1
+        // Typical poor localization: trace > 1.0
+        // Formula: score = 1 / (1 + trace)
+        return 1.0 / (1.0 + trace);
+    }
+
+    /// <summary>
+    /// Calculate score from constraint count
+    /// More constraints = more reliable localization
+    /// </summary>
+    private static double CalculateConstraintCountScore(int constraintCount)
+    {
+        if (constraintCount == 0)
+        {
+            return 0.0; // No constraints = no confidence
+        }
+
+        if (constraintCount < MinConstraintsForGoodScore)
+        {
+            // Few constraints: linear scaling from 0 to 0.5
+            return constraintCount / (double)MinConstraintsForGoodScore * 0.5;
+        }
+
+        // Many constraints: logarithmic scaling from 0.5 to 1.0
+        var normalizedCount = Math.Min(constraintCount, MaxConstraintsForScaling);
+        var logFactor = Math.Log10(normalizedCount + 1) / Math.Log10(MaxConstraintsForScaling + 1);
+        return 0.5 + (logFactor * 0.5);
+    }
+
+    /// <summary>
+    /// Calculate score from MCL MAE (mean absolute error)
+    /// Lower MAE = better scan-map fit = higher score
+    /// </summary>
+    private static double CalculateMaeScore(double? mae)
+    {
+        if (!mae.HasValue)
+        {
+            return 0.5; // Default to moderate confidence if MAE not available
+        }
+
+        var maeValue = mae.Value;
+
+        if (maeValue < 0.01)
+        {
+            return 1.0; // Excellent fit (< 1cm error)
+        }
+
+        // Normalize MAE using exponential decay
+        // 0m = 1.0, 0.1m = 0.61, 0.2m = 0.37, 0.5m = 0.08
+        // Formula: score = exp(-5 * mae)
+        return Math.Exp(-5.0 * maeValue);
+    }
+
+    /// <summary>
+    /// Calculate adaptive weights based on available data
+    /// Redistributes weights when some data is not available
+    /// </summary>
+    private static WeightSet CalculateAdaptiveWeights(
+        bool hasMcl,
+        bool hasCovariance,
+        int constraintCount)
+    {
+        // Start with base weights
+        var weights = new WeightSet
+        {
+            Covariance = CovarianceWeight,
+            ConstraintCount = ConstraintCountWeight,
+            ConstraintQuality = ConstraintQualityWeight,
+            MclReliability = MclReliabilityWeight,
+            MclMae = MclMaeWeight
+        };
+
+        // If MCL not available, redistribute its weight
+        if (!hasMcl)
+        {
+            double mclTotalWeight = MclReliabilityWeight + MclMaeWeight;
+            weights.MclReliability = 0.0;
+            weights.MclMae = 0.0;
+
+            // Give more weight to covariance and constraint quality
+            weights.Covariance += mclTotalWeight * 0.5;
+            weights.ConstraintQuality += mclTotalWeight * 0.5;
+        }
+
+        // If few constraints, reduce constraint score weight
+        if (constraintCount < MinConstraintsForGoodScore)
+        {
+            double reduction = weights.ConstraintCount * 0.5;
+            weights.ConstraintCount *= 0.5;
+
+            // Redistribute to covariance and quality
+            weights.Covariance += reduction * 0.5;
+            weights.ConstraintQuality += reduction * 0.5;
+        }
+
+        // Normalize weights to sum to 1.0
+        double total = weights.Covariance + weights.ConstraintCount +
+                      weights.ConstraintQuality + weights.MclReliability + weights.MclMae;
+
+        if (total > 0)
+        {
+            weights.Covariance /= total;
+            weights.ConstraintCount /= total;
+            weights.ConstraintQuality /= total;
+            weights.MclReliability /= total;
+            weights.MclMae /= total;
+        }
+
+        return weights;
+    }
+
+    /// <summary>
+    /// Weight set for adaptive weighting (legacy - without scan match)
+    /// </summary>
+    private record struct WeightSet
+    {
+        public double Covariance { get; set; }
+        public double ConstraintCount { get; set; }
+        public double ConstraintQuality { get; set; }
+        public double MclReliability { get; set; }
+        public double MclMae { get; set; }
+    }
+
+    /// <summary>
+    /// Calculate adaptive weights V2 including scan match score.
+    /// Redistributes weights when some data is not available.
+    /// Prioritizes scan match score as it's the most direct drift indicator.
+    /// </summary>
+    private static WeightSetV2 CalculateAdaptiveWeightsV2(
+        bool hasMcl,
+        bool hasCovariance,
+        bool hasScanMatch,
+        int constraintCount)
+    {
+        // Start with base weights
+        var weights = new WeightSetV2
+        {
+            ScanMatch = ScanMatchWeight,
+            Covariance = CovarianceWeight,
+            ConstraintCount = ConstraintCountWeight,
+            ConstraintQuality = ConstraintQualityWeight,
+            MclReliability = MclReliabilityWeight,
+            MclMae = MclMaeWeight
+        };
+
+        // If scan match not available, redistribute to MCL and covariance
+        if (!hasScanMatch)
+        {
+            double scanMatchTotal = weights.ScanMatch;
+            weights.ScanMatch = 0.0;
+
+            if (hasMcl)
+            {
+                // Give more weight to MCL (it provides similar information)
+                weights.MclReliability += scanMatchTotal * 0.5;
+                weights.MclMae += scanMatchTotal * 0.3;
+                weights.Covariance += scanMatchTotal * 0.2;
+            }
+            else
+            {
+                // Redistribute to covariance and constraints
+                weights.Covariance += scanMatchTotal * 0.4;
+                weights.ConstraintQuality += scanMatchTotal * 0.4;
+                weights.ConstraintCount += scanMatchTotal * 0.2;
+            }
+        }
+
+        // If MCL not available, redistribute its weight
+        if (!hasMcl)
+        {
+            double mclTotalWeight = weights.MclReliability + weights.MclMae;
+            weights.MclReliability = 0.0;
+            weights.MclMae = 0.0;
+
+            if (hasScanMatch)
+            {
+                // Scan match is available, give it more weight
+                weights.ScanMatch += mclTotalWeight * 0.5;
+                weights.Covariance += mclTotalWeight * 0.3;
+                weights.ConstraintQuality += mclTotalWeight * 0.2;
+            }
+            else
+            {
+                // No direct quality indicators, rely on constraints
+                weights.Covariance += mclTotalWeight * 0.5;
+                weights.ConstraintQuality += mclTotalWeight * 0.5;
+            }
+        }
+
+        // If few constraints, reduce constraint score weight
+        if (constraintCount < MinConstraintsForGoodScore)
+        {
+            double reduction = weights.ConstraintCount * 0.5;
+            weights.ConstraintCount *= 0.5;
+
+            // Redistribute to scan match (if available) or covariance
+            if (hasScanMatch)
+            {
+                weights.ScanMatch += reduction * 0.6;
+                weights.Covariance += reduction * 0.4;
+            }
+            else
+            {
+                weights.Covariance += reduction * 0.5;
+                weights.ConstraintQuality += reduction * 0.5;
+            }
+        }
+
+        // Normalize weights to sum to 1.0
+        double total = weights.ScanMatch + weights.Covariance + weights.ConstraintCount +
+                      weights.ConstraintQuality + weights.MclReliability + weights.MclMae;
+
+        if (total > 0)
+        {
+            weights.ScanMatch /= total;
+            weights.Covariance /= total;
+            weights.ConstraintCount /= total;
+            weights.ConstraintQuality /= total;
+            weights.MclReliability /= total;
+            weights.MclMae /= total;
+        }
+
+        return weights;
+    }
+
+    /// <summary>
+    /// Weight set V2 for adaptive weighting (includes scan match)
+    /// </summary>
+    private record struct WeightSetV2
+    {
+        public double ScanMatch { get; set; }
+        public double Covariance { get; set; }
+        public double ConstraintCount { get; set; }
+        public double ConstraintQuality { get; set; }
+        public double MclReliability { get; set; }
+        public double MclMae { get; set; }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/MapBuilderExtensions.cs

@@ -0,0 +1,232 @@
+using CartographerSharp.Mapping;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Transform;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+using RobotNet10.Shared.Geometry;
+using System.Reflection;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Extension methods for IMapBuilder
+/// Provides trajectory management, loading, and serialization operations
+/// </summary>
+public static class MapBuilderExtensions
+{
+    /// <summary>
+    /// Add trajectory builder to MapBuilder with sensor IDs and optional callback.
+    /// </summary>
+    /// <param name="gridTypeOverride">Override grid type for submap creation. Use ProbabilityGrid for ScanMapping, Tsdf for Localizing.</param>
+    public static (int trajectoryId, ITrajectoryBuilder? trajectoryBuilder) AddTrajectoryBuilder(
+        this IMapBuilder mapBuilder,
+        CartographerConfiguration config,
+        TrajectoryBuilderOptions? trajectoryOptionsOverride = null,
+        GridOptions2D.GridType? gridTypeOverride = null)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+        ArgumentNullException.ThrowIfNull(config);
+
+        var sensorIds = TrajectoryHelper.BuildSensorIds(config);
+        var trajectoryOptions = trajectoryOptionsOverride ?? CreateTrajectoryBuilderOptions(config, gridTypeOverride);
+
+        var trajectoryId = mapBuilder.AddTrajectoryBuilder(sensorIds, trajectoryOptions);
+        var trajectoryBuilder = mapBuilder.GetTrajectoryBuilder(trajectoryId);
+
+        return (trajectoryId, trajectoryBuilder);
+    }
+
+    /// <summary>
+    /// Wait for trajectory to finish with async support
+    /// </summary>
+    public static async Task WaitForTrajectoryFinishedAsync(
+        this IMapBuilder mapBuilder,
+        int trajectoryId,
+        CancellationToken cancellationToken)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+
+        var checkInterval = TimeSpan.FromMilliseconds(200);
+
+        while (true)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+
+            if (mapBuilder.PoseGraph.IsTrajectoryFinished(trajectoryId))
+                return;
+
+            await Task.Delay(checkInterval, cancellationToken);
+        }
+    }
+
+    /// <summary>
+    /// Finish all active trajectories before final optimization
+    /// </summary>
+    public static async Task FinishAllActiveTrajectoriesAsync(
+        this IMapBuilder mapBuilder,
+        CancellationToken cancellationToken)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+
+        var trajectoryStates = mapBuilder.PoseGraph.GetTrajectoryStates();
+        var activeTrajectories = trajectoryStates
+            .Where(kvp => kvp.Value == IPoseGraph.TrajectoryState.Active)
+            .Select(kvp => kvp.Key)
+            .ToList();
+
+        if (activeTrajectories.Count == 0)
+            return;
+
+        foreach (var trajectoryId in activeTrajectories)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            mapBuilder.FinishTrajectory(trajectoryId);
+            await mapBuilder.WaitForTrajectoryFinishedAsync(trajectoryId, cancellationToken);
+        }
+    }
+
+    /// <summary>
+    /// Find first frozen trajectory in a loaded map
+    /// </summary>
+    public static int? FindFrozenTrajectory(this IMapBuilder mapBuilder)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+
+        var trajectoryStates = mapBuilder.PoseGraph.GetTrajectoryStates();
+        if (trajectoryStates == null || trajectoryStates.Count == 0)
+            return null;
+
+        foreach (var kvp in trajectoryStates)
+        {
+            if (kvp.Value == IPoseGraph.TrajectoryState.Frozen)
+            {
+                var trajectoryId = kvp.Key;
+
+                // Validate frozen trajectory has nodes
+                var trajectoryNodePoses = mapBuilder.PoseGraph.GetTrajectoryNodePoses();
+                if (trajectoryNodePoses != null && !trajectoryNodePoses.IsEmpty)
+                {
+                    var trajectoryNodes = trajectoryNodePoses.BeginOfTrajectory(trajectoryId).ToList();
+                    if (trajectoryNodes.Count > 0)
+                        return trajectoryId;
+                }
+            }
+        }
+
+        return null;
+    }
+
+    /// <summary>
+    /// Check if MapBuilder has trajectories and dispose if so
+    /// </summary>
+    public static bool DisposeIfHasTrajectories(this IMapBuilder mapBuilder)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+
+        var trajectoryStates = mapBuilder.PoseGraph.GetTrajectoryStates();
+        if (trajectoryStates != null && trajectoryStates.Count > 0)
+        {
+            (mapBuilder as IDisposable)?.Dispose();
+            return true;
+        }
+
+        return false;
+    }
+
+    /// <summary>
+    /// Finish trajectory if active, then dispose MapBuilder
+    /// </summary>
+    public static void FinishTrajectoryAndDispose(this IMapBuilder mapBuilder, int trajectoryId)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+
+        if (trajectoryId >= 0)
+        {
+            var poseGraph = mapBuilder.PoseGraph;
+            if (!poseGraph.IsTrajectoryFinished(trajectoryId))
+            {
+                mapBuilder.FinishTrajectory(trajectoryId);
+            }
+        }
+
+        (mapBuilder as IDisposable)?.Dispose();
+    }
+
+    /// <summary>
+    /// Add localization trajectory builder with optional initial pose.
+    /// </summary>
+    /// <param name="gridTypeOverride">Override grid type for submap creation. Use Tsdf for Localizing.</param>
+    public static (int trajectoryId, ITrajectoryBuilder? trajectoryBuilder, Rigid3d? initialPoseInMapFrame) AddLocalizationTrajectoryBuilder(
+            this IMapBuilder mapBuilder,
+            CartographerConfiguration config,
+            Pose? initialPose,
+            GridOptions2D.GridType? gridTypeOverride = null)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+        ArgumentNullException.ThrowIfNull(config);
+
+        var frozenTrajectoryId = mapBuilder.FindFrozenTrajectory();
+        var trajectoryOptions = CreateTrajectoryBuilderOptions(config, gridTypeOverride);
+        CartographerSharp.Transform.Rigid3d? initialPoseInMapFrame = null;
+
+        if (frozenTrajectoryId.HasValue)
+        {
+            CartographerSharp.Models.Transform.Rigid3dProto relativePoseProto;
+
+            if (initialPose.HasValue)
+            {
+                var poseInMapFrame = mapBuilder.PoseGraph.GetTransformToMap() * PoseConverter.ToRigid3d(initialPose.Value);
+                initialPoseInMapFrame = poseInMapFrame;
+                relativePoseProto = (CartographerSharp.Models.Transform.Rigid3dProto)poseInMapFrame;
+            }
+            else
+            {
+                relativePoseProto = new CartographerSharp.Models.Transform.Rigid3dProto(
+                    new CartographerSharp.Models.Transform.Vector3d(0, 0, 0),
+                    new CartographerSharp.Models.Transform.Quaterniond(0, 0, 0, 1));
+            }
+
+            var initialTrajectoryPose = new CartographerSharp.Models.Mapping.InitialTrajectoryPose(
+                relativePoseProto,
+                frozenTrajectoryId.Value,
+                DateTimeOffset.UtcNow.ToUnixTimeSeconds() * 1000000000L);
+
+            trajectoryOptions = new TrajectoryBuilderOptions(
+                trajectoryBuilder2DOptions: trajectoryOptions.TrajectoryBuilder2DOptions,
+                trajectoryBuilder3DOptions: trajectoryOptions.TrajectoryBuilder3DOptions,
+                initialTrajectoryPose: initialTrajectoryPose,
+                pureLocalizationTrimmer: trajectoryOptions.PureLocalizationTrimmer,
+                collateFixedFrame: trajectoryOptions.CollateFixedFrame,
+                collateLandmarks: trajectoryOptions.CollateLandmarks,
+                poseGraphOdometryMotionFilter: trajectoryOptions.PoseGraphOdometryMotionFilter);
+        }
+
+        var (trajectoryId, trajectoryBuilder) = mapBuilder.AddTrajectoryBuilder(config, trajectoryOptions);
+        return (trajectoryId, trajectoryBuilder, initialPoseInMapFrame);
+    }
+
+    /// <summary>
+    /// Create trajectory builder options from config.
+    /// </summary>
+    /// <param name="gridTypeOverride">Override grid type. ProbabilityGrid for ScanMapping, Tsdf for Localizing.</param>
+    public static TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
+        CartographerConfiguration config,
+        GridOptions2D.GridType? gridTypeOverride = null)
+    {
+        ArgumentNullException.ThrowIfNull(config);
+
+        if (config.TrajectoryBuilder.Use2D)
+        {
+            return TrajectoryBuilderOptionsFactory.Create2D(
+                config.TrajectoryBuilder,
+                config.Sensors.Imu.Enabled,
+                gridTypeOverride);
+        }
+        else
+        {
+            return TrajectoryBuilderOptionsFactory.Create3D(
+                config.TrajectoryBuilder,
+                config.Sensors.Imu.Enabled);
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/MapBuilderHelper.cs

@@ -0,0 +1,214 @@
+using CartographerSharp.Mapping;
+using CartographerSharp.Models.Mapping;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for creating and configuring MapBuilder instances
+/// Factory methods for MapBuilder and related configurations
+/// Extension methods are now in MapBuilderExtensions class
+/// </summary>
+public static class MapBuilderHelper
+{
+    /// <summary>
+    /// Create a new MapBuilder with configuration
+    /// </summary>
+    public static MapBuilder CreateMapBuilder(CartographerConfiguration config)
+    {
+        ArgumentNullException.ThrowIfNull(config);
+
+        var use2D = config.MapBuilder.UseTrajectoryBuilder2D ?? config.TrajectoryBuilder.Use2D;
+        var use3D = config.MapBuilder.UseTrajectoryBuilder3D ?? !config.TrajectoryBuilder.Use2D;
+
+        var poseGraphOptions = TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(config.MapBuilder);
+
+        var mapBuilderOptions = new MapBuilderOptions(
+            useTrajectoryBuilder2D: use2D,
+            useTrajectoryBuilder3D: use3D,
+            numBackgroundThreads: config.MapBuilder.NumBackgroundThreads,
+            poseGraphOptions: poseGraphOptions,
+            collateByTrajectory: config.MapBuilder.CollateByTrajectory);
+
+        return new MapBuilder(mapBuilderOptions);
+    }
+
+    /// <summary>
+    /// Create MapBuilder from MapLoadResult (pbstream path and saved config).
+    /// Uses saved config if available, otherwise uses current config.
+    /// Then loads state from pbstream file with frozen trajectories for localization.
+    /// </summary>
+    public static MapBuilder CreateMapBuilderFromLoadResult(
+        MapCartographerLoadResult loadResult,
+        CartographerConfiguration currentConfig,
+        ILogger? logger = null,
+        bool loadFrozenState = true)
+    {
+        ArgumentNullException.ThrowIfNull(loadResult);
+        ArgumentNullException.ThrowIfNull(currentConfig);
+
+        if (string.IsNullOrEmpty(loadResult.PbstreamPath))
+        {
+            throw new ArgumentException("PbstreamPath cannot be null or empty", nameof(loadResult));
+        }
+
+        if (!File.Exists(loadResult.PbstreamPath))
+        {
+            throw new FileNotFoundException("Pbstream file not found", loadResult.PbstreamPath);
+        }
+
+        // Get MapBuilder options from saved config or current config
+        bool use2D;
+        bool use3D;
+        int numBackgroundThreads;
+        PoseGraphOptions poseGraphOptions;
+
+        if (loadResult.SavedConfig != null)
+        {
+            // Use saved config
+            use2D = loadResult.SavedConfig.MapBuilder.UseTrajectoryBuilder2D ?? loadResult.SavedConfig.Use2D;
+            use3D = loadResult.SavedConfig.MapBuilder.UseTrajectoryBuilder3D ?? !loadResult.SavedConfig.Use2D;
+            numBackgroundThreads = loadResult.SavedConfig.MapBuilder.NumBackgroundThreads;
+            poseGraphOptions = CreatePoseGraphOptionsFromSnapshot(loadResult.SavedConfig);
+
+            // Warn if config mismatch
+            var currentUse2D = currentConfig.MapBuilder.UseTrajectoryBuilder2D ?? currentConfig.TrajectoryBuilder.Use2D;
+            if (currentUse2D != use2D)
+            {
+                logger?.LogWarning(
+                    "MapBuilderHelper: Map config mismatch detected. Using saved config (2D={Saved2D}) instead of current config (2D={Current2D}).",
+                    use2D, currentUse2D);
+            }
+        }
+        else
+        {
+            // Use current config
+            use2D = currentConfig.MapBuilder.UseTrajectoryBuilder2D ?? currentConfig.TrajectoryBuilder.Use2D;
+            use3D = currentConfig.MapBuilder.UseTrajectoryBuilder3D ?? !currentConfig.TrajectoryBuilder.Use2D;
+            numBackgroundThreads = currentConfig.MapBuilder.NumBackgroundThreads;
+            poseGraphOptions = TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(currentConfig.MapBuilder);
+        }
+
+        var mapBuilderOptions = new MapBuilderOptions(
+            useTrajectoryBuilder2D: use2D,
+            useTrajectoryBuilder3D: use3D,
+            numBackgroundThreads: numBackgroundThreads,
+            poseGraphOptions: poseGraphOptions
+        );
+
+        var mapBuilder = new MapBuilder(mapBuilderOptions);
+
+        // Load state from pbstream file
+        // loadFrozenState: true freezes trajectories in the loaded map for localization
+        // This prevents modifying the map during localization (as per xloc reference implementation)
+        _ = mapBuilder.LoadStateFromFile(loadResult.PbstreamPath, loadFrozenState: loadFrozenState);
+
+        logger?.LogInformation(
+            "MapBuilderHelper: Created MapBuilder from pbstream file: {PbstreamPath}, loadFrozenState={LoadFrozenState}",
+            loadResult.PbstreamPath, loadFrozenState);
+
+        return mapBuilder;
+    }
+
+    /// <summary>
+    /// Creates PoseGraphOptions from saved config snapshot
+    /// </summary>
+    private static PoseGraphOptions CreatePoseGraphOptionsFromSnapshot(MapCartographerConfigSnapshot savedConfig)
+    {
+        // Create temporary MapBuilderConfiguration from snapshot
+        var tempMapBuilderConfig = new MapBuilderConfiguration
+        {
+            UseTrajectoryBuilder2D = savedConfig.MapBuilder.UseTrajectoryBuilder2D,
+            UseTrajectoryBuilder3D = savedConfig.MapBuilder.UseTrajectoryBuilder3D,
+            NumBackgroundThreads = savedConfig.MapBuilder.NumBackgroundThreads,
+            OptimizeEveryNNodes = savedConfig.MapBuilder.OptimizeEveryNNodes,
+            MatcherTranslationWeight = savedConfig.MapBuilder.MatcherTranslationWeight,
+            MatcherRotationWeight = savedConfig.MapBuilder.MatcherRotationWeight,
+            MaxNumFinalIterations = savedConfig.MapBuilder.MaxNumFinalIterations,
+            GlobalSamplingRatio = savedConfig.MapBuilder.GlobalSamplingRatio,
+            LogResidualHistograms = savedConfig.MapBuilder.LogResidualHistograms,
+            GlobalConstraintSearchAfterNSeconds = savedConfig.MapBuilder.GlobalConstraintSearchAfterNSeconds,
+            EnableSingleTrajectoryLoopClosure = savedConfig.MapBuilder.EnableSingleTrajectoryLoopClosure,
+            SingleTrajectoryLoopClosureDistanceThreshold = savedConfig.MapBuilder.SingleTrajectoryLoopClosureDistanceThreshold,
+            PoseGraphOptimizationProblemOptions = new OptimizationProblemOptions
+            {
+                HuberScale = savedConfig.MapBuilder.OptimizationProblemOptions.HuberScale,
+                AccelerationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.AccelerationWeight,
+                RotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.RotationWeight,
+                LocalSlamPoseTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.LocalSlamPoseTranslationWeight,
+                LocalSlamPoseRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.LocalSlamPoseRotationWeight,
+                OdometryTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.OdometryTranslationWeight,
+                OdometryRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.OdometryRotationWeight,
+                FixedFramePoseTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTranslationWeight,
+                FixedFramePoseRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseRotationWeight,
+                FixedFramePoseUseTolerantLoss = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
+                FixedFramePoseTolerantLossParamA = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
+                FixedFramePoseTolerantLossParamB = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
+                LogSolverSummary = savedConfig.MapBuilder.OptimizationProblemOptions.LogSolverSummary,
+                MaxNumIterations = savedConfig.MapBuilder.OptimizationProblemOptions.MaxNumIterations
+            }
+        };
+
+        return TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(tempMapBuilderConfig);
+    }
+
+    /// <summary>
+    /// Tạo config snapshot từ configuration hiện tại (để lưu vào map.json khi save).
+    /// </summary>
+    public static MapCartographerConfigSnapshot CreateConfigSnapshot(CartographerConfiguration config)
+    {
+        ArgumentNullException.ThrowIfNull(config);
+        var use2D = config.MapBuilder.UseTrajectoryBuilder2D ?? config.TrajectoryBuilder.Use2D;
+
+        return new MapCartographerConfigSnapshot
+        {
+            Use2D = use2D,
+            MapBuilder = new MapCarographerBuilderConfigSnapshot
+            {
+                UseTrajectoryBuilder2D = config.MapBuilder.UseTrajectoryBuilder2D,
+                UseTrajectoryBuilder3D = config.MapBuilder.UseTrajectoryBuilder3D,
+                NumBackgroundThreads = config.MapBuilder.NumBackgroundThreads,
+                OptimizeEveryNNodes = config.MapBuilder.OptimizeEveryNNodes,
+                MatcherTranslationWeight = config.MapBuilder.MatcherTranslationWeight,
+                MatcherRotationWeight = config.MapBuilder.MatcherRotationWeight,
+                MaxNumFinalIterations = config.MapBuilder.MaxNumFinalIterations,
+                GlobalSamplingRatio = config.MapBuilder.GlobalSamplingRatio,
+                LogResidualHistograms = config.MapBuilder.LogResidualHistograms,
+                GlobalConstraintSearchAfterNSeconds = config.MapBuilder.GlobalConstraintSearchAfterNSeconds,
+                EnableSingleTrajectoryLoopClosure = config.MapBuilder.EnableSingleTrajectoryLoopClosure,
+                SingleTrajectoryLoopClosureDistanceThreshold = config.MapBuilder.SingleTrajectoryLoopClosureDistanceThreshold,
+                OptimizationProblemOptions = new OptimizationProblemOptionsSnapshot
+                {
+                    HuberScale = config.MapBuilder.PoseGraphOptimizationProblemOptions.HuberScale,
+                    AccelerationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.AccelerationWeight,
+                    RotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.RotationWeight,
+                    LocalSlamPoseTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.LocalSlamPoseTranslationWeight,
+                    LocalSlamPoseRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.LocalSlamPoseRotationWeight,
+                    OdometryTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.OdometryTranslationWeight,
+                    OdometryRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.OdometryRotationWeight,
+                    FixedFramePoseTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTranslationWeight,
+                    FixedFramePoseRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseRotationWeight,
+                    FixedFramePoseUseTolerantLoss = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
+                    FixedFramePoseTolerantLossParamA = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
+                    FixedFramePoseTolerantLossParamB = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
+                    LogSolverSummary = config.MapBuilder.PoseGraphOptimizationProblemOptions.LogSolverSummary,
+                    MaxNumIterations = config.MapBuilder.PoseGraphOptimizationProblemOptions.MaxNumIterations
+                }
+            },
+            TrajectoryBuilder = new TrajectoryBuilderConfigSnapshot
+            {
+                Use2D = config.TrajectoryBuilder.Use2D,
+                MinRange = config.TrajectoryBuilder.MinRange,
+                MaxRange = config.TrajectoryBuilder.MaxRange,
+                MinZ = config.TrajectoryBuilder.MinZ,
+                MaxZ = config.TrajectoryBuilder.MaxZ,
+                MissingDataRayLength = config.TrajectoryBuilder.MissingDataRayLength,
+                VoxelFilterSize = config.TrajectoryBuilder.VoxelFilterSize,
+                NumAccumulatedRangeData = config.TrajectoryBuilder.NumAccumulatedRangeData,
+                UseImuData = config.TrajectoryBuilder.UseImuData,
+                UseOnlineCorrelativeScanMatching = config.TrajectoryBuilder.UseOnlineCorrelativeScanMatching
+            }
+        };
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/MapNameHelper.cs

@@ -0,0 +1,60 @@
+using RobotNet10.RobotApp.Shared;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Shared helper for map name sanitization and legacy metadata migration.
+/// Used by CartographerService and MapSaveProcessor.
+/// </summary>
+public static class MapNameHelper
+{
+    /// <summary>
+    /// Sanitize map name for use as directory name.
+    /// Removes invalid filename characters and trims spaces/dots.
+    /// </summary>
+    public static string Sanitize(string mapName)
+    {
+        if (string.IsNullOrWhiteSpace(mapName))
+            throw new ArgumentException("Map name cannot be null or empty", nameof(mapName));
+
+        var invalidChars = Path.GetInvalidFileNameChars();
+        var sanitized = mapName;
+
+        foreach (var c in invalidChars)
+        {
+            sanitized = sanitized.Replace(c, '_');
+        }
+
+        sanitized = sanitized.Trim(' ', '.');
+
+        if (string.IsNullOrWhiteSpace(sanitized))
+            throw new ArgumentException("Map name is invalid after sanitization", nameof(mapName));
+
+        return sanitized;
+    }
+
+    /// <summary>
+    /// Ensures MapInfo has Size calculated from Bounds for legacy map.json files
+    /// that don't have Size field populated.
+    /// </summary>
+    public static MapInfo EnsureMapSize(MapInfo metadata)
+    {
+        if (metadata.Size.Width == 0 && metadata.Size.Height == 0)
+        {
+            var width = metadata.Bounds.MaxX - metadata.Bounds.MinX;
+            var height = metadata.Bounds.MaxY - metadata.Bounds.MinY;
+            return new MapInfo
+            {
+                Name = metadata.Name,
+                FolderPath = metadata.FolderPath,
+                CreatedDate = metadata.CreatedDate,
+                Resolution = metadata.Resolution,
+                Size = new MapSize(width, height),
+                Origin = metadata.Origin,
+                Bounds = metadata.Bounds,
+                TrajectoryNodeCount = metadata.TrajectoryNodeCount
+            };
+        }
+        return metadata;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/MapPathHelper.cs

@@ -0,0 +1,73 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Centralizes map directory path construction and legacy file name resolution.
+/// Eliminates duplicated Path.Combine + legacy fallback patterns across CartographerService.
+/// </summary>
+public static class MapPathHelper
+{
+    #region Path Construction
+
+    /// <summary>
+    /// Get the full path to a map directory, sanitizing the map name.
+    /// </summary>
+    public static string GetMapPath(string mapsDirectory, string mapName)
+    {
+        var sanitizedMapName = MapNameHelper.Sanitize(mapName);
+        return Path.Combine(Path.GetFullPath(mapsDirectory), sanitizedMapName);
+    }
+
+    #endregion
+
+    #region File Resolution
+
+    /// <summary>
+    /// Resolve metadata JSON path, trying map.json first then legacy metadata.json.
+    /// Returns null if neither exists.
+    /// </summary>
+    public static string? ResolveMetadataPath(string mapPath)
+    {
+        var metadataPath = Path.Combine(mapPath, "map.json");
+        if (File.Exists(metadataPath))
+            return metadataPath;
+
+        var legacyPath = Path.Combine(mapPath, "metadata.json");
+        if (File.Exists(legacyPath))
+            return legacyPath;
+
+        return null;
+    }
+
+    /// <summary>
+    /// Resolve pbstream path, trying map.pbstream first then any .pbstream file in the directory.
+    /// Returns null if none found.
+    /// </summary>
+    public static string? ResolvePbstreamPath(string mapPath)
+    {
+        var pbstreamPath = Path.Combine(mapPath, "map.pbstream");
+        if (File.Exists(pbstreamPath))
+            return pbstreamPath;
+
+        var pbstreamFiles = Directory.GetFiles(mapPath, "*.pbstream");
+        return pbstreamFiles.Length > 0 ? pbstreamFiles[0] : null;
+    }
+
+    /// <summary>
+    /// Resolve map image path (PNG or JPG).
+    /// Returns null if no image found.
+    /// </summary>
+    public static string? ResolveImagePath(string mapPath)
+    {
+        var pngPath = Path.Combine(mapPath, "map.png");
+        if (File.Exists(pngPath))
+            return pngPath;
+
+        var jpgPath = Path.Combine(mapPath, "map.jpg");
+        if (File.Exists(jpgPath))
+            return jpgPath;
+
+        return null;
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/MapSaveProcessor.cs

@@ -0,0 +1,636 @@
+using CartographerSharp.IO;
+using CartographerSharp.Mapping;
+using CartographerSharp.Transform;
+using RobotNet10.RobotApp.Shared;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using System.Text.Json;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Handles map saving workflow including scan matching, optimization, and file I/O
+/// </summary>
+public class MapSaveProcessor
+{
+    #region Fields and Constructor
+
+    private readonly CartographerConfiguration _config;
+    private readonly ILogger<MapSaveProcessor> _logger;
+    private readonly string _mapsDirectory;
+    private static readonly JsonSerializerOptions SerializerOptions = new() { WriteIndented = true };
+
+    public MapSaveProcessor(
+        CartographerConfiguration config,
+        ILogger<MapSaveProcessor> logger)
+    {
+        _config = config ?? throw new ArgumentNullException(nameof(config));
+        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
+
+        // Get maps directory from configuration
+        _mapsDirectory = Path.GetFullPath(_config.MapStorage.Directory);
+        Directory.CreateDirectory(_mapsDirectory);
+    }
+
+    #endregion
+
+    #region Save Workflow
+
+    /// <summary>
+    /// Execute full save map workflow with optimization and file I/O
+    /// </summary>
+    /// <param name="mapName">Name of the map to save</param>
+    /// <param name="mapBuilder">MapBuilder containing map data</param>
+    /// <param name="trajectoryId">Active trajectory ID to finish (-1 if none)</param>
+    /// <param name="progressCallback">Progress callback (total, current, percent)</param>
+    /// <param name="newOrigin">Optional new origin to transform the map before saving (e.g., wall-aligned pose)</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    public async Task<string> SaveMapAsync(
+        string mapName,
+        IMapBuilder mapBuilder,
+        int trajectoryId,
+        Func<int, int, int, Task> progressCallback,
+        Pose? newOrigin,
+        CancellationToken cancellationToken)
+    {
+        ArgumentNullException.ThrowIfNull(mapBuilder);
+        ArgumentNullException.ThrowIfNull(mapName);
+
+        try
+        {
+            _logger.LogInformation("MapSaveProcessor: Starting map save process for: {MapName}", mapName);
+
+            // Wait for active scan matching to complete
+            await WaitForScanMatchingAsync(cancellationToken);
+
+            // Track total progress across all phases
+            // Phase 1: Initial work queue drain (0-20%)
+            // Phase 2: Finish trajectory (20-40%)
+            // Phase 3: RunFinalOptimization - work queue + constraint builder (40-90%)
+            // Phase 4: Final scan matching + file saving (90-100%)
+
+            // Finish trajectory if active
+            if (trajectoryId >= 0)
+            {
+                // Phase 1: Drain work queue before finishing trajectory (0-20%)
+                _logger.LogDebug("MapSaveProcessor: Phase 1 - Draining work queue before finishing trajectory");
+                await DrainWorkQueueWithProgressAsync(mapBuilder, progressCallback, 0, 20, cancellationToken);
+
+                // Phase 2: Finish trajectory (20-40%)
+                _logger.LogDebug("MapSaveProcessor: Phase 2 - Finishing trajectory ID: {TrajectoryId}", trajectoryId);
+                await progressCallback(100, 20, 20);
+                await FinishTrajectoryAsync(mapBuilder, trajectoryId, cancellationToken);
+                await progressCallback(100, 40, 40);
+            }
+            else
+            {
+                // No trajectory to finish, skip to phase 3
+                await progressCallback(100, 40, 40);
+            }
+
+            _logger.LogDebug("MapSaveProcessor: Finishing all active trajectories");
+            // Finish all other active trajectories
+            await mapBuilder.FinishAllActiveTrajectoriesAsync(cancellationToken);
+
+            // Phase 3: Run final optimization (40-90%)
+            // RunFinalOptimization internally calls WaitForAllComputations which:
+            // - Drains work queue
+            // - Waits for constraint builder to finish
+            // - Runs optimization
+            cancellationToken.ThrowIfCancellationRequested();
+            _logger.LogDebug("MapSaveProcessor: Phase 3 - Running final pose graph optimization");
+
+            // Start optimization in background task and track progress
+            var optimizationTask = Task.Run(() => mapBuilder.PoseGraph.RunFinalOptimization(), cancellationToken);
+
+            // Track progress while optimization is running
+            await TrackOptimizationProgressAsync(mapBuilder, progressCallback, 40, 90, optimizationTask, cancellationToken);
+
+            // Phase 4: Final scan matching + file saving (90-100%)
+            _logger.LogDebug("MapSaveProcessor: Phase 4 - Final scan matching and file saving");
+            await progressCallback(100, 90, 90);
+
+            // Final wait for scan matching
+            await WaitForScanMatchingAsync(cancellationToken);
+
+            // Get map path
+            var mapPath = GetMapPath(mapName);
+
+            // Validate map before saving
+            var (isValid, errorMessage) = ValidateMapBeforeSave(mapBuilder, mapName, mapPath);
+            if (!isValid)
+            {
+                throw new InvalidOperationException($"Map validation failed: {errorMessage}");
+            }
+
+            _logger.LogDebug("MapSaveProcessor: Saving pbstream and metadata to: {MapPath}", mapPath);
+            // Save pbstream file
+            SavePbstream(mapBuilder, mapPath);
+
+            // Transform origin if newOrigin is provided (e.g., wall alignment)
+            IMapBuilder mapBuilderForMetadata = mapBuilder;
+            if (newOrigin.HasValue && !PbstreamTransformHelper.IsIdentityPose(newOrigin.Value))
+            {
+                _logger.LogDebug("MapSaveProcessor: Transforming map origin with wall-aligned pose");
+                var pbstreamPath = Path.Combine(mapPath, "map.pbstream");
+
+                // For wall alignment, we need to apply the INVERSE of the compensation pose.
+                // The compensation angle represents "how much the robot should rotate to align the wall",
+                // but TransformToMap uses the inverse when rendering (via TransformToMapInverse).
+                // So we invert the pose here to get the correct rotation direction in the final map.
+                var poseRigid = PbstreamTransformHelper.PoseToRigid3d(newOrigin.Value);
+                var invertedRigid = poseRigid.Inverse();
+                var invertedPose = new Pose
+                {
+                    Position = new RobotNet10.Shared.Numbers.Vector3(
+                        invertedRigid.Translation.X,
+                        invertedRigid.Translation.Y,
+                        invertedRigid.Translation.Z),
+                    Orientation = new RobotNet10.Shared.Numbers.Quaternion(
+                        invertedRigid.Rotation.X,
+                        invertedRigid.Rotation.Y,
+                        invertedRigid.Rotation.Z,
+                        invertedRigid.Rotation.W)
+                };
+
+                PbstreamTransformHelper.TransformPbstreamOrigin(pbstreamPath, invertedPose, _logger);
+
+                // Reload MapBuilder from transformed pbstream
+                var loadResult = new MapCartographerLoadResult
+                {
+                    PbstreamPath = pbstreamPath,
+                    MapPath = mapPath,
+                    SavedConfig = null // Will use current config
+                };
+                mapBuilderForMetadata = MapBuilderHelper.CreateMapBuilderFromLoadResult(
+                    loadResult, _config, _logger, loadFrozenState: false);
+            }
+
+            _logger.LogDebug("MapSaveProcessor: Generating occupancy grid and saving metadata");
+            // Generate occupancy grid and save metadata
+            var saveResult = await SaveMapMetadataAsync(mapName, mapBuilderForMetadata, mapPath, cancellationToken);
+
+            // Dispose reloaded MapBuilder if it was created
+            if (mapBuilderForMetadata != mapBuilder && mapBuilderForMetadata is IDisposable disposable)
+            {
+                disposable.Dispose();
+            }
+
+            // Final progress update (100%)
+            await progressCallback(100, 100, 100);
+
+            _logger.LogInformation("MapSaveProcessor: Map saved successfully: {MapName}", mapName);
+            return saveResult;
+        }
+        catch (OperationCanceledException)
+        {
+            _logger.LogWarning("MapSaveProcessor: Map save cancelled for: {MapName}", mapName);
+            throw;
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "MapSaveProcessor: Failed to save map: {MapName}", mapName);
+            throw;
+        }
+    }
+
+    #endregion
+
+    #region Progress Tracking
+
+    /// <summary>
+    /// Wait for active scan matching operations to complete
+    /// </summary>
+    private static async Task WaitForScanMatchingAsync(CancellationToken cancellationToken, int checkIntervalMs = 100)
+    {
+        while (true)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+
+            var activeCount = CartographerSharp.Mapping.Internal.D2D.ScanMatching.CeresScanMatcher2D.GetActiveScanMatchingCount();
+            if (activeCount == 0)
+                break;
+
+            await Task.Delay(checkIntervalMs, cancellationToken);
+        }
+    }
+
+    /// <summary>
+    /// Finish active trajectory and wait for completion
+    /// </summary>
+    private static async Task FinishTrajectoryAsync(IMapBuilder mapBuilder, int trajectoryId, CancellationToken cancellationToken)
+    {
+        cancellationToken.ThrowIfCancellationRequested();
+        
+        // Wait for scan matching to complete before finishing trajectory
+        // Wait for scan matching to complete
+        await WaitForScanMatchingAsync(cancellationToken);
+
+        // Finish trajectory
+        cancellationToken.ThrowIfCancellationRequested();
+
+        // FinishTrajectory is called synchronously
+        mapBuilder.FinishTrajectory(trajectoryId);
+
+        // Wait for trajectory to finish asynchronously
+        // Wait for trajectory to finish
+        await mapBuilder.WaitForTrajectoryFinishedAsync(trajectoryId, cancellationToken);
+
+        // Trajectory finished successfully
+    }
+
+    /// <summary>
+    /// Drain work queue with progress updates mapped to a percentage range.
+    /// </summary>
+    private static async Task DrainWorkQueueWithProgressAsync(
+        IMapBuilder mapBuilder,
+        Func<int, int, int, Task> progressCallback,
+        int progressStart,
+        int progressEnd,
+        CancellationToken cancellationToken)
+    {
+        var poseGraph = mapBuilder.PoseGraph;
+        var initialQueueCount = poseGraph.WorkQueueCount;
+
+        if (initialQueueCount == 0)
+        {
+            await progressCallback(100, progressEnd, progressEnd);
+            return;
+        }
+
+        var progressRange = progressEnd - progressStart;
+
+        while (poseGraph.WorkQueueCount > 0)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+
+            var currentQueueCount = poseGraph.WorkQueueCount;
+            var processed = initialQueueCount - currentQueueCount;
+
+            // Calculate progress within the range
+            int percentComplete;
+            if (initialQueueCount > 0)
+            {
+                var phaseProgress = (double)processed / initialQueueCount;
+                percentComplete = progressStart + (int)(phaseProgress * progressRange);
+            }
+            else
+            {
+                percentComplete = progressEnd;
+            }
+            percentComplete = Math.Clamp(percentComplete, progressStart, progressEnd);
+
+            await progressCallback(initialQueueCount, processed, percentComplete);
+
+            await Task.Delay(500, cancellationToken);
+        }
+
+        await progressCallback(initialQueueCount, initialQueueCount, progressEnd);
+    }
+
+    /// <summary>
+    /// Track optimization progress (work queue + constraint builder) while optimization task is running.
+    /// </summary>
+    private static async Task TrackOptimizationProgressAsync(
+        IMapBuilder mapBuilder,
+        Func<int, int, int, Task> progressCallback,
+        int progressStart,
+        int progressEnd,
+        Task optimizationTask,
+        CancellationToken cancellationToken)
+    {
+        var poseGraph = mapBuilder.PoseGraph;
+        var progressRange = progressEnd - progressStart;
+
+        // Capture the remaining work at the start of tracking.
+        // Progress is calculated based on how much of this remaining work has been completed,
+        // so the remaining portion at call time = 100%.
+        var initialWorkQueueCount = poseGraph.WorkQueueCount;
+        var initialConstraintTasksFinished = poseGraph.ConstraintTasksFinished;
+        var initialConstraintTasksTotal = poseGraph.ConstraintTasksTotal;
+        var remainingConstraintTasks = initialConstraintTasksTotal - initialConstraintTasksFinished;
+
+        // Tracking optimization progress
+
+        var lastLogTime = DateTime.UtcNow;
+
+        while (!optimizationTask.IsCompleted)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+
+            var workQueueCount = poseGraph.WorkQueueCount;
+            var constraintTasksTotal = poseGraph.ConstraintTasksTotal;
+            var constraintTasksFinished = poseGraph.ConstraintTasksFinished;
+
+            // Recalculate remaining tasks as total may grow during processing
+            var currentRemainingTotal = constraintTasksTotal - initialConstraintTasksFinished;
+            var effectiveRemaining = Math.Max(remainingConstraintTasks, currentRemainingTotal);
+
+            // Calculate progress based on remaining work from the start of tracking:
+            // 1. Work queue drain progress (30% of phase)
+            double workQueueProgress;
+            if (initialWorkQueueCount > 0)
+            {
+                var workQueueProcessed = initialWorkQueueCount - workQueueCount;
+                workQueueProgress = Math.Min(1.0, (double)workQueueProcessed / initialWorkQueueCount);
+            }
+            else
+            {
+                workQueueProgress = 1.0; // No work queue items at start
+            }
+
+            // 2. Constraint task progress (70% of phase)
+            double constraintProgress;
+            if (effectiveRemaining > 0)
+            {
+                var newlyFinished = constraintTasksFinished - initialConstraintTasksFinished;
+                constraintProgress = Math.Min(1.0, (double)newlyFinished / effectiveRemaining);
+            }
+            else
+            {
+                constraintProgress = 1.0; // No constraint tasks to process
+            }
+
+            // Combined progress weighted by actual work amounts
+            var totalWork = initialWorkQueueCount + effectiveRemaining;
+            var combinedProgress = totalWork > 0
+                ? (workQueueProgress * initialWorkQueueCount + constraintProgress * effectiveRemaining) / totalWork
+                : 1.0;
+            var percentComplete = progressStart + (int)(combinedProgress * progressRange);
+            percentComplete = Math.Clamp(percentComplete, progressStart, progressEnd);
+
+            await progressCallback(constraintTasksTotal, constraintTasksFinished, percentComplete);
+
+            // Update last log time periodically (logging removed to reduce noise)
+            if ((DateTime.UtcNow - lastLogTime).TotalSeconds >= 10)
+            {
+                lastLogTime = DateTime.UtcNow;
+            }
+
+            // Check more frequently for quicker updates
+            await Task.Delay(1000, cancellationToken);
+        }
+
+        // Wait for optimization task to complete (may throw if cancelled)
+        await optimizationTask;
+
+        await progressCallback(poseGraph.ConstraintTasksTotal, poseGraph.ConstraintTasksFinished, progressEnd);
+        // Optimization completed successfully
+    }
+
+    #endregion
+
+    #region File I/O
+
+    /// <summary>
+    /// Save pbstream file from MapBuilder state
+    /// </summary>
+    private void SavePbstream(IMapBuilder mapBuilder, string mapPath)
+    {
+        Directory.CreateDirectory(mapPath);
+        var pbstreamPath = Path.Combine(mapPath, "map.pbstream");
+        var includeUnfinished = _config.MapStorage.IncludeUnfinishedSubmaps;
+
+        try
+        {
+            using var writer = new ProtoStreamWriter(pbstreamPath);
+            mapBuilder.SerializeState(includeUnfinishedSubmaps: includeUnfinished, writer);
+            if (!writer.Close())
+            {
+                throw new InvalidOperationException($"ProtoStreamWriter.Close() failed for {pbstreamPath}");
+            }
+
+            _logger.LogInformation("MapSaveProcessor: Pbstream saved to: {Path}", pbstreamPath);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "MapSaveProcessor: Failed to save pbstream to: {Path}", pbstreamPath);
+            throw new InvalidOperationException($"Failed to save pbstream: {ex.Message}", ex);
+        }
+    }
+
+    /// <summary>
+    /// Generate occupancy grid and save metadata files (PGM, PNG, JSON).
+    /// Can be used for both new map creation and map update (e.g., after transform).
+    /// </summary>
+    /// <param name="mapName">Map name for sanitization</param>
+    /// <param name="mapBuilder">MapBuilder containing the map data</param>
+    /// <param name="mapPath">Directory path to save files</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>The map path</returns>
+    public async Task<string> SaveMapMetadataAsync(
+        string mapName,
+        IMapBuilder mapBuilder,
+        string mapPath,
+        CancellationToken cancellationToken = default)
+    {
+        cancellationToken.ThrowIfCancellationRequested();
+
+        // Generate occupancy grid
+        var (occupancyGrid, submapCount) = GenerateOccupancyGridForSave(mapBuilder, _config.MapStorage.OccupancyGridResolution);
+
+        var poseGraph = mapBuilder.PoseGraph;
+        var trajectoryNodePoses = poseGraph.GetTrajectoryNodePoses();
+        var trajectoryNodeCount = trajectoryNodePoses?.Count ?? 0;
+
+        var finalSubmapCount = submapCount;
+        if (finalSubmapCount == 0)
+        {
+            var allSubmapData = poseGraph.GetAllSubmapData();
+            finalSubmapCount = allSubmapData?.Count ?? 0;
+        }
+
+        // Save occupancy grid files if available
+        if (occupancyGrid != null)
+        {
+            await OccupancyGridFileHelper.SaveAllFormatsAsync(occupancyGrid, mapPath, cancellationToken, _logger);
+        }
+        else
+        {
+            _logger.LogWarning("MapSaveProcessor: No occupancy grid generated, skipping PGM/PNG/JPG files");
+        }
+
+        // Calculate bounds and size (in meters)
+        var bounds = new BoundingBox();
+        var size = new MapSize();
+        if (occupancyGrid != null)
+        {
+            var originX = occupancyGrid.Origin.Position.X;
+            var originY = occupancyGrid.Origin.Position.Y;
+            var resolution = occupancyGrid.Resolution;
+            // Size in meters = pixels * resolution
+            var widthMeters = occupancyGrid.Width * resolution;
+            var heightMeters = occupancyGrid.Height * resolution;
+            size = new MapSize(widthMeters, heightMeters);
+            bounds = new BoundingBox
+            {
+                MinX = originX,
+                MinY = originY,
+                MaxX = originX + widthMeters,
+                MaxY = originY + heightMeters
+            };
+        }
+
+        // Create metadata
+        var sanitizedMapName = MapNameHelper.Sanitize(mapName);
+        var metadata = new MapCartographerInfo
+        {
+            Name = sanitizedMapName,
+            FolderPath = mapPath,
+            CreatedDate = DateTime.UtcNow,
+            Resolution = occupancyGrid?.Resolution ?? _config.MapStorage.OccupancyGridResolution,
+            Size = size,
+            Origin = occupancyGrid?.Origin ?? new Pose
+            {
+                Position = new RobotNet10.Shared.Numbers.Vector3(0, 0, 0),
+                Orientation = new RobotNet10.Shared.Numbers.Quaternion(0, 0, 0, 1)
+            },
+            Bounds = bounds,
+            TrajectoryNodeCount = trajectoryNodeCount,
+            MapConfig = MapBuilderHelper.CreateConfigSnapshot(_config)
+        };
+
+        // Save metadata JSON
+        var metadataPath = Path.Combine(mapPath, "map.json");
+        await File.WriteAllTextAsync(metadataPath, JsonSerializer.Serialize(metadata, SerializerOptions), cancellationToken);
+
+        return mapPath;
+    }
+
+    #endregion
+
+    #region Validation
+
+    /// <summary>
+    /// Get map path from map name
+    /// </summary>
+    private string GetMapPath(string mapName)
+    {
+        var sanitized = MapNameHelper.Sanitize(mapName);
+        return Path.Combine(_mapsDirectory, sanitized);
+    }
+
+    /// <summary>
+    /// Validate map before saving
+    /// </summary>
+    private (bool IsValid, string? ErrorMessage) ValidateMapBeforeSave(IMapBuilder mapBuilder, string mapName, string mapPath)
+    {
+        try
+        {
+            var poseGraph = mapBuilder.PoseGraph;
+            var trajectoryStates = poseGraph.GetTrajectoryStates();
+            var trajectoryNodePoses = poseGraph.GetTrajectoryNodePoses();
+            var allSubmapData = poseGraph.GetAllSubmapData();
+
+            // Check at least 1 trajectory
+            if (trajectoryStates.Count == 0)
+            {
+                return (false, "No trajectories found. Cannot save empty map.");
+            }
+
+            // Check at least 1 node
+            if (trajectoryNodePoses.Count == 0)
+            {
+                return (false, "No trajectory nodes found. Cannot save map without nodes.");
+            }
+
+            // Check at least 1 submap
+            if (allSubmapData.Count == 0)
+            {
+                return (false, "No submaps found. Cannot save map without submaps.");
+            }
+
+            // Validate map name
+            var sanitizedMapName = MapNameHelper.Sanitize(mapName);
+            if (string.IsNullOrWhiteSpace(sanitizedMapName))
+            {
+                return (false, $"Invalid map name: '{mapName}' (sanitized to empty string)");
+            }
+
+            // Check poses for NaN/Infinity
+            int invalidPoseCount = 0;
+            foreach (var nodePose in trajectoryNodePoses)
+            {
+                var globalPose = nodePose.Data.GlobalPose;
+                if (!globalPose.IsValid())
+                {
+                    invalidPoseCount++;
+                }
+            }
+
+            if (invalidPoseCount > 0)
+            {
+                return (false, $"Found {invalidPoseCount} trajectory node(s) with invalid poses (NaN/Infinity or invalid quaternion).");
+            }
+
+            // Check submap poses for NaN/Infinity
+            int invalidSubmapPoseCount = 0;
+            foreach (var submapPose in poseGraph.GetAllSubmapPoses())
+            {
+                var pose = submapPose.Data.Pose;
+                if (!pose.IsValid())
+                {
+                    invalidSubmapPoseCount++;
+                }
+            }
+
+            if (invalidSubmapPoseCount > 0)
+            {
+                return (false, $"Found {invalidSubmapPoseCount} submap(s) with invalid poses (NaN/Infinity or invalid quaternion).");
+            }
+
+            // Validate occupancy grid resolution
+            if (_config.MapStorage.OccupancyGridResolution <= 0)
+            {
+                return (false, $"Invalid occupancy grid resolution: {_config.MapStorage.OccupancyGridResolution}. Must be > 0.");
+            }
+
+            // All validations passed
+            return (true, string.Empty);
+        }
+        catch (Exception ex)
+        {
+            return (false, $"Validation error: {ex.Message}");
+        }
+    }
+
+    #endregion
+
+    #region Grid Generation
+
+    /// <summary>
+    /// Generate occupancy grid from MapBuilder for save
+    /// </summary>
+    private (OccupancyGrid? Grid, int SubmapCount) GenerateOccupancyGridForSave(IMapBuilder mapBuilder, double targetResolution)
+    {
+        try
+        {
+            var padding = _config.MapStorage.MapPadding;
+
+            // Use the same Generate() dispatch as OccupancyGridManager (UI display)
+            // to ensure saved files match what the user sees on the UI.
+            // Generate() respects OccupancyGridConfiguration.MergeStrategy (default: LogOddsSum).
+            var occupancyGrid = OccupancyGridGenerator.Generate(
+                mapBuilder, targetResolution, padding, _logger, _config.OccupancyGrid);
+
+            if (occupancyGrid == null)
+            {
+                _logger.LogWarning("MapSaveProcessor: Occupancy grid generation returned null");
+                return (null, 0);
+            }
+
+            // Get submap count for result
+            var submapCount = mapBuilder.PoseGraph.GetAllSubmapData()?.Count ?? 0;
+
+            return (occupancyGrid, submapCount);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "MapSaveProcessor: Failed to generate occupancy grid from MapBuilder");
+            return (null, 0);
+        }
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/MclProcessor.cs

@@ -0,0 +1,269 @@
+using CartographerSharp.Sensor;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Reason for MCL convergence
+/// </summary>
+public enum MclConvergenceReason
+{
+    /// <summary>MCL has not converged yet</summary>
+    NotConverged,
+    /// <summary>Pose is stable and quality criteria met</summary>
+    PoseStable,
+    /// <summary>Timeout reached without convergence</summary>
+    Timeout,
+    /// <summary>Max iterations reached without convergence</summary>
+    MaxIterations
+}
+
+/// <summary>
+/// Processes MCL (Monte Carlo Localization) convergence and pose tracking
+/// </summary>
+public class MclProcessor
+{
+    private readonly ILogger<MclProcessor> _logger;
+    private readonly CartographerConfiguration _config;
+    private readonly Lock _lock = new();
+
+    private bool _running;
+    private string? _primaryLidarId;
+    private DateTime? _stableSinceUtc;
+    private DateTime _runningSinceUtc = DateTime.MinValue;
+    private int _iterationCount;
+    private Pose? _lastPose;
+    private Pose _initialPose; // Initial pose for fallback on timeout
+    private OdometryData? _lastOdometryData;
+    private double _lastOdometryTimestampSec;
+
+    public MclProcessor(CartographerConfiguration config, ILogger<MclProcessor> logger)
+    {
+        _config = config ?? throw new ArgumentNullException(nameof(config));
+        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
+    }
+
+    public bool IsRunning
+    {
+        get { lock (_lock) { return _running; } }
+    }
+
+    public string? PrimaryLidarId
+    {
+        get { lock (_lock) { return _primaryLidarId; } }
+    }
+
+    /// <summary>
+    /// Gets the initial pose used when MCL started (for fallback on timeout)
+    /// </summary>
+    public Pose InitialPose
+    {
+        get { lock (_lock) { return _initialPose; } }
+    }
+
+    /// <summary>
+    /// Start MCL with initial seed pose
+    /// </summary>
+    public void Start(Pose seedPose, string effectivePrimaryLiderId, OccupancyGrid? grid)
+    {
+        lock (_lock)
+        {
+            _running = true;
+            _primaryLidarId = effectivePrimaryLiderId;
+            _iterationCount = 0;
+            _lastPose = seedPose;
+            _initialPose = seedPose; // Store initial pose for timeout fallback
+            _stableSinceUtc = null;
+            _runningSinceUtc = DateTime.UtcNow;
+            _lastOdometryData = null;
+            _lastOdometryTimestampSec = 0;
+        }
+
+        _logger.LogInformation("MclProcessor: MCL started with primary lidar: {LidarId}", effectivePrimaryLiderId);
+    }
+
+    /// <summary>
+    /// Stop MCL
+    /// </summary>
+    public void Stop()
+    {
+        lock (_lock)
+        {
+            _running = false;
+            _primaryLidarId = null;
+            _stableSinceUtc = null;
+            _iterationCount = 0;
+            _lastPose = null;
+            _lastOdometryData = null;
+            _lastOdometryTimestampSec = 0;
+        }
+    }
+
+    /// <summary>
+    /// Update MCL seed pose during running state
+    /// </summary>
+    public void UpdateSeedPose(Pose newPose)
+    {
+        lock (_lock)
+        {
+            if (!_running)
+                return;
+
+            _iterationCount = 0;
+            _lastPose = null;
+            _stableSinceUtc = null;
+            _lastOdometryData = null;
+            _lastOdometryTimestampSec = 0;
+        }
+
+        _logger.LogInformation("MclProcessor: Seed pose updated");
+    }
+
+    /// <summary>
+    /// Check MCL convergence based on pose stability and quality criteria
+    /// </summary>
+    /// <returns>True if converged (for any reason), false otherwise</returns>
+    public bool CheckConvergence(Pose currentPose, double reliability, double? mae, out int iterationCount)
+    {
+        return CheckConvergenceWithReason(currentPose, reliability, mae, out iterationCount) != MclConvergenceReason.NotConverged;
+    }
+
+    /// <summary>
+    /// Check MCL convergence and return the reason
+    /// </summary>
+    /// <returns>Convergence reason indicating why MCL stopped or NotConverged if still running</returns>
+    public MclConvergenceReason CheckConvergenceWithReason(Pose currentPose, double reliability, double? mae, out int iterationCount)
+    {
+        lock (_lock)
+        {
+            iterationCount = ++_iterationCount;
+
+            if (_iterationCount < _config.Mcl.ConvergenceMinIterations)
+            {
+                _lastPose = currentPose;
+                return MclConvergenceReason.NotConverged;
+            }
+
+            // Calculate pose change
+            double poseDist = double.MaxValue;
+            double yawDiff = double.MaxValue;
+
+            if (_lastPose.HasValue)
+            {
+                double dx = currentPose.Position.X - _lastPose.Value.Position.X;
+                double dy = currentPose.Position.Y - _lastPose.Value.Position.Y;
+                poseDist = Math.Sqrt(dx * dx + dy * dy);
+
+                double yawCur = GetYawFromPose(currentPose);
+                double yawLast = GetYawFromPose(_lastPose.Value);
+                double dyaw = yawCur - yawLast;
+                while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
+                while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
+                yawDiff = Math.Abs(dyaw);
+            }
+
+            _lastPose = currentPose;
+
+            // Check stability criteria
+            bool poseStable = poseDist < _config.Mcl.ConvergencePoseChangeThresholdMeters &&
+                            yawDiff < _config.Mcl.ConvergenceYawChangeThresholdRad;
+
+            bool qualityGood = !_config.Mcl.EstimateReliability ||
+                            (reliability >= _config.Mcl.ConvergenceReliabilityMin &&
+                             (!mae.HasValue || mae.Value <= _config.Mcl.ConvergenceMaeMaxMeters));
+
+            var now = DateTime.UtcNow;
+            double runningSec = (now - _runningSinceUtc).TotalSeconds;
+            double stableDurationSec = _config.Mcl.ConvergenceStableDurationSeconds;
+            double timeoutSec = _config.Mcl.ConvergenceTimeoutSeconds;
+
+            if (qualityGood)
+            {
+                if (poseStable)
+                {
+                    if (!_stableSinceUtc.HasValue)
+                        _stableSinceUtc = now;
+                    else if ((now - _stableSinceUtc.Value).TotalSeconds >= stableDurationSec)
+                        return MclConvergenceReason.PoseStable;
+                }
+                else
+                {
+                    _stableSinceUtc = null;
+                }
+            }
+            else
+            {
+                _stableSinceUtc = null;
+                if (runningSec >= timeoutSec)
+                    return MclConvergenceReason.Timeout;
+            }
+
+            if (_iterationCount >= _config.Mcl.ConvergenceMaxIterations)
+                return MclConvergenceReason.MaxIterations;
+
+            return MclConvergenceReason.NotConverged;
+        }
+    }
+
+    /// <summary>
+    /// Process odometry data for MCL motion model
+    /// </summary>
+    public void ProcessOdometry(OdometryData odomData)
+    {
+        lock (_lock)
+        {
+            if (!_running || !_lastOdometryData.HasValue)
+            {
+                _lastOdometryData = odomData;
+                _lastOdometryTimestampSec = odomData.Time / 10_000_000.0;
+                return;
+            }
+
+            double currentTimeSec = odomData.Time / 10_000_000.0;
+            double deltaTimeSec = currentTimeSec - _lastOdometryTimestampSec;
+
+            if (deltaTimeSec > 0 && deltaTimeSec < 1.0)
+            {
+                var lastPose = _lastOdometryData.Value.Pose;
+                var currentOdomPose = odomData.Pose;
+
+                double dx = currentOdomPose.Translation.X - lastPose.Translation.X;
+                double dy = currentOdomPose.Translation.Y - lastPose.Translation.Y;
+                double linearX = dx / deltaTimeSec;
+                double linearY = dy / deltaTimeSec;
+
+                double yawLast = ((RobotNet10.Shared.Numbers.Quaternion)lastPose.Rotation).ToYawRadian();
+                double yawCurrent = ((RobotNet10.Shared.Numbers.Quaternion)currentOdomPose.Rotation).ToYawRadian();
+                double dyaw = yawCurrent - yawLast;
+                while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
+                while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
+                double angularZ = dyaw / deltaTimeSec;
+
+                // Store for use by MCL (would be passed to mcl.OnOdom in actual implementation)
+                OnOdometryProcessed?.Invoke(deltaTimeSec, linearX, linearY, angularZ);
+            }
+
+            _lastOdometryData = odomData;
+            _lastOdometryTimestampSec = currentTimeSec;
+        }
+    }
+
+    // Event to signal odometry processing
+    public event Action<double, double, double, double>? OnOdometryProcessed;
+
+    private static double GetYawFromPose(Pose p)
+    {
+        var q = p.Orientation;
+        return Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z));
+    }
+
+    public void Dispose()
+    {
+        lock (_lock)
+        {
+            Stop();
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/OccupancyGridFileHelper.cs

@@ -0,0 +1,243 @@
+using RobotNet10.Shared.Localization;
+using SkiaSharp;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for saving OccupancyGrid to various file formats.
+/// Consolidates file saving logic used by both CartographerService and MapSaveProcessor.
+/// </summary>
+public static class OccupancyGridFileHelper
+{
+    #region Public API
+
+    /// <summary>
+    /// Save all map file formats (PGM, YAML, PNG, JPG) to the specified directory.
+    /// </summary>
+    public static async Task SaveAllFormatsAsync(
+        OccupancyGrid occupancyGrid,
+        string mapPath,
+        CancellationToken cancellationToken = default,
+        ILogger? logger = null)
+    {
+        var pgmPath = Path.Combine(mapPath, "map.pgm");
+        var yamlPath = Path.Combine(mapPath, "map.yaml");
+        var pngPath = Path.Combine(mapPath, "map.png");
+        var jpgPath = Path.Combine(mapPath, "map.jpg");
+
+        SaveAsPgm(occupancyGrid, pgmPath, logger);
+        SaveAsYaml(occupancyGrid, yamlPath, "map.png", logger);
+        await Task.Run(() => SaveAsPng(occupancyGrid, pngPath, cancellationToken, logger), cancellationToken);
+        await Task.Run(() => SaveAsJpg(occupancyGrid, jpgPath, cancellationToken, logger), cancellationToken);
+    }
+
+    /// <summary>
+    /// Save occupancy grid as PGM file (binary format P5)
+    /// </summary>
+    public static void SaveAsPgm(OccupancyGrid occupancyGrid, string pgmPath, ILogger? logger = null)
+    {
+        try
+        {
+            using var fileStream = new FileStream(pgmPath, FileMode.Create, FileAccess.Write);
+            using var writer = new StreamWriter(fileStream);
+
+            // Write PGM header
+            writer.WriteLine("P5"); // Binary format
+            writer.WriteLine($"{occupancyGrid.Width} {occupancyGrid.Height}");
+            writer.WriteLine("255"); // Max value
+
+            // Flush header before writing binary data
+            writer.Flush();
+
+            // Write pixel data directly without Y-flip.
+            // Both OccupancyGrid and PGM file use same convention for consistency with PgmLoader.
+            var buffer = new byte[occupancyGrid.Width * occupancyGrid.Height];
+            for (int y = 0; y < occupancyGrid.Height; y++)
+            {
+                for (int x = 0; x < occupancyGrid.Width; x++)
+                {
+                    int index = y * occupancyGrid.Width + x;
+                    var occupancyValue = occupancyGrid.Data[index];
+
+                    if (occupancyValue == -1)
+                    {
+                        buffer[index] = 205; // Unknown (gray)
+                    }
+                    else
+                    {
+                        // Convert occupancy (0-100) to PGM (0-255)
+                        // occupancy 0 (free) -> PGM 254 (white)
+                        // occupancy 100 (occupied) -> PGM 0 (black)
+                        buffer[index] = (byte)(254 - (occupancyValue * 254 / 100));
+                    }
+                }
+            }
+
+            fileStream.Write(buffer, 0, buffer.Length);
+            fileStream.Flush();
+        }
+        catch (Exception ex)
+        {
+            logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save PGM file: {Path}", pgmPath);
+            throw;
+        }
+    }
+
+    /// <summary>
+    /// Save occupancy grid as YAML file (ROS map format)
+    /// </summary>
+    public static void SaveAsYaml(OccupancyGrid occupancyGrid, string yamlPath, string imageFilename, ILogger? logger = null)
+    {
+        try
+        {
+            var originX = occupancyGrid.Origin.Position.X;
+            var originY = occupancyGrid.Origin.Position.Y;
+
+            // Write YAML file (ROS map format)
+            var yamlContent = $"image: {imageFilename}\n" +
+                            $"resolution: {occupancyGrid.Resolution:F10}\n" +
+                            $"origin: [{originX:F10}, {originY:F10}, 0.0]\n" +
+                            $"negate: 0\n" +
+                            $"occupied_thresh: 0.65\n" +
+                            $"free_thresh: 0.196\n";
+
+            File.WriteAllText(yamlPath, yamlContent);
+        }
+        catch (Exception ex)
+        {
+            logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save YAML file: {Path}", yamlPath);
+            throw;
+        }
+    }
+
+    /// <summary>
+    /// Save occupancy grid as PNG file
+    /// </summary>
+    public static void SaveAsPng(OccupancyGrid occupancyGrid, string pngPath, CancellationToken cancellationToken = default, ILogger? logger = null)
+    {
+        RenderAndSaveImage(occupancyGrid, pngPath, SKEncodedImageFormat.Png, 100, cancellationToken, logger);
+    }
+
+    /// <summary>
+    /// Save occupancy grid as JPG file
+    /// </summary>
+    public static void SaveAsJpg(OccupancyGrid occupancyGrid, string jpgPath, CancellationToken cancellationToken = default, ILogger? logger = null)
+    {
+        RenderAndSaveImage(occupancyGrid, jpgPath, SKEncodedImageFormat.Jpeg, 95, cancellationToken, logger);
+    }
+
+    #endregion
+
+    #region Image Rendering
+
+    /// <summary>
+    /// Render occupancy grid to an image and save in the specified format.
+    /// Shared implementation for both PNG and JPG output.
+    /// </summary>
+    private static void RenderAndSaveImage(
+        OccupancyGrid occupancyGrid,
+        string outputPath,
+        SKEncodedImageFormat format,
+        int quality,
+        CancellationToken cancellationToken,
+        ILogger? logger)
+    {
+        var formatName = format == SKEncodedImageFormat.Png ? "PNG" : "JPG";
+        try
+        {
+            if (occupancyGrid == null || occupancyGrid.Width <= 0 || occupancyGrid.Height <= 0)
+            {
+                logger?.LogWarning("OccupancyGridFileHelper: Cannot save {Format} - invalid occupancy grid", formatName);
+                return;
+            }
+
+            cancellationToken.ThrowIfCancellationRequested();
+
+            var width = occupancyGrid.Width;
+            var height = occupancyGrid.Height;
+
+            // Create SKBitmap with RGBA_8888 format
+            using var bitmap = new SKBitmap(width, height, SKColorType.Rgba8888, SKAlphaType.Opaque);
+
+            // Get pixel buffer pointer for direct memory access
+            var pixelsPtr = bitmap.GetPixels();
+            if (pixelsPtr == IntPtr.Zero)
+            {
+                throw new InvalidOperationException("Failed to get pixel buffer from bitmap");
+            }
+
+            // Convert occupancy grid data to image pixels
+            // Flip Y: OccupancyGrid uses ROS convention (row 0 = world bottom, Y up)
+            // but PNG/JPG image convention is row 0 = top, Y down.
+            unsafe
+            {
+                var pixels = (byte*)pixelsPtr.ToPointer();
+
+                for (int y = 0; y < height; y++)
+                {
+                    for (int x = 0; x < width; x++)
+                    {
+                        // Read from flipped Y in grid (bottom-up) to write top-down in image
+                        var srcIndex = (height - 1 - y) * width + x;
+                        var occupancyValue = occupancyGrid.Data[srcIndex];
+
+                        byte intensity;
+                        if (occupancyValue == -1)
+                        {
+                            intensity = 205; // Unknown (gray)
+                        }
+                        else if (occupancyValue == 0)
+                        {
+                            intensity = 254; // Free space (white)
+                        }
+                        else
+                        {
+                            // Occupied space: Convert occupancy (0-100) to pixel intensity (0-255)
+                            var intensityValue = 254.0 - (occupancyValue * 254.0 / 100.0);
+                            intensity = (byte)Math.Clamp((int)Math.Round(intensityValue), 0, 254);
+                        }
+
+                        // Write RGBA bytes directly (destination index uses image row y)
+                        var dstIndex = y * width + x;
+                        var pixelOffset = dstIndex * 4;
+                        pixels[pixelOffset] = intensity;     // R
+                        pixels[pixelOffset + 1] = intensity; // G
+                        pixels[pixelOffset + 2] = intensity; // B
+                        pixels[pixelOffset + 3] = 255;       // A (opaque)
+                    }
+                }
+            }
+
+            // Encode and save
+            using var image = SKImage.FromBitmap(bitmap) ?? throw new InvalidOperationException("Failed to create SKImage from bitmap");
+            using var data = image.Encode(format, quality) ?? throw new InvalidOperationException($"Failed to encode {formatName} image");
+
+            var directory = Path.GetDirectoryName(outputPath);
+            if (!string.IsNullOrEmpty(directory))
+            {
+                Directory.CreateDirectory(directory);
+            }
+
+            if (File.Exists(outputPath))
+            {
+                File.Delete(outputPath);
+            }
+
+            using var stream = File.Create(outputPath);
+            data.SaveTo(stream);
+            stream.Flush();
+        }
+        catch (OperationCanceledException)
+        {
+            logger?.LogWarning("OccupancyGridFileHelper: {Format} save cancelled: {Path}", formatName, outputPath);
+            throw;
+        }
+        catch (Exception ex)
+        {
+            logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save {Format} file: {Path}", formatName, outputPath);
+            // Don't throw - image files are optional
+        }
+    }
+
+    #endregion
+}

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

@@ -0,0 +1,297 @@
+using CartographerSharp.Mapping;
+using CartographerSharp.Mapping.D2D;
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+//using Pose = RobotNet10.Shared.Geometry.Pose;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for occupancy grid generation and manipulation.
+/// Contains pure functions extracted from OccupancyGridProvider.
+/// </summary>
+public static class OccupancyGridHelper
+{
+    #region Constants
+
+    /// <summary>
+    /// Initial size for empty occupancy grid (cells)
+    /// </summary>
+    public const int InitialEmptyGridSize = 100;
+
+    /// <summary>
+    /// Probability threshold for free space
+    /// </summary>
+    public const double FreeSpaceProbabilityThreshold = 0.01;
+
+    /// <summary>
+    /// Probability threshold for occupied space
+    /// </summary>
+    public const double OccupiedSpaceProbabilityThreshold = 0.99;
+
+    /// <summary>
+    /// Lower bound for unknown space probability range
+    /// </summary>
+    public const double UnknownSpaceLowerBound = 0.4;
+
+    /// <summary>
+    /// Upper bound for unknown space probability range
+    /// </summary>
+    public const double UnknownSpaceUpperBound = 0.6;
+
+    #endregion
+
+    #region Bounds Calculation
+
+    /// <summary>
+    /// Calculate global bounds of a submap by transforming its corners to global frame
+    /// </summary>
+    public static (double minX, double minY, double maxX, double maxY) CalculateSubmapGlobalBounds(
+        Submap2D submap2D,
+        Rigid3d globalPose)
+    {
+        var grid = submap2D.Grid;
+        if (grid == null)
+        {
+            return (double.MaxValue, double.MaxValue, double.MinValue, double.MinValue);
+        }
+
+        var limits = grid.Limits;
+
+        // Use cropped limits if available (only known cells) for more accurate bounds
+        grid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
+
+        // If cropped limits are valid (has known cells), use them; otherwise use full limits
+        var hasCroppedLimits = croppedLimits.NumXCells > 0 && croppedLimits.NumYCells > 0;
+
+        // Get bounds of cropped (known) cells in submap local frame using GetCellCenter
+        var cornerIndices = new[]
+        {
+            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
+                                                      hasCroppedLimits ? croppedOffset.Y : 0),
+            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
+                                                      hasCroppedLimits ? croppedOffset.Y : 0),
+            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
+                                                      hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1),
+            new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
+                                                      hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1)
+        };
+
+        double croppedMinX = double.MaxValue, croppedMinY = double.MaxValue;
+        double croppedMaxX = double.MinValue, croppedMaxY = double.MinValue;
+
+        foreach (var cornerIndex in cornerIndices)
+        {
+            var cellCenter = limits.GetCellCenter(cornerIndex);
+            croppedMinX = Math.Min(croppedMinX, cellCenter.X);
+            croppedMinY = Math.Min(croppedMinY, cellCenter.Y);
+            croppedMaxX = Math.Max(croppedMaxX, cellCenter.X);
+            croppedMaxY = Math.Max(croppedMaxY, cellCenter.Y);
+        }
+
+        // Transform corners to global frame
+        var corners = new[]
+        {
+            new Vector2(croppedMinX, croppedMinY),
+            new Vector2(croppedMaxX, croppedMinY),
+            new Vector2(croppedMaxX, croppedMaxY),
+            new Vector2(croppedMinX, croppedMaxY)
+        };
+
+        double submapGlobalMinX = double.MaxValue, submapGlobalMinY = double.MaxValue;
+        double submapGlobalMaxX = double.MinValue, submapGlobalMaxY = double.MinValue;
+
+        foreach (var corner in corners)
+        {
+            var corner3D = new Vector3(corner.X, corner.Y, 0.0);
+            var globalCorner = globalPose.TransformPoint(corner3D);
+
+            submapGlobalMinX = Math.Min((float)submapGlobalMinX, (float)globalCorner.X);
+            submapGlobalMinY = Math.Min((float)submapGlobalMinY, (float)globalCorner.Y);
+            submapGlobalMaxX = Math.Max((float)submapGlobalMaxX, (float)globalCorner.X);
+            submapGlobalMaxY = Math.Max((float)submapGlobalMaxY, (float)globalCorner.Y);
+        }
+
+        return (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY);
+    }
+
+    /// <summary>
+    /// Get current grid bounds from origin and dimensions
+    /// </summary>
+    public static (double minX, double minY, double maxX, double maxY) GetGridBounds(OccupancyGrid grid)
+    {
+        var gridMinX = grid.Origin.Position.X;
+        var gridMinY = grid.Origin.Position.Y;
+        var gridMaxX = gridMinX + (grid.Width * grid.Resolution);
+        var gridMaxY = gridMinY + (grid.Height * grid.Resolution);
+        return (gridMinX, gridMinY, gridMaxX, gridMaxY);
+    }
+
+    /// <summary>
+    /// Calculate bounds from a list of submaps with padding
+    /// </summary>
+    public static (double minX, double minY, double maxX, double maxY) CalculateBounds(
+        List<(Submap2D Submap, Rigid3d GlobalPose)> submapList,
+        double mapPadding)
+    {
+        double minX = double.MaxValue, minY = double.MaxValue;
+        double maxX = double.MinValue, maxY = double.MinValue;
+
+        foreach (var (submap, globalPose) in submapList)
+        {
+            var (submapMinX, submapMinY, submapMaxX, submapMaxY) =
+                CalculateSubmapGlobalBounds(submap, globalPose);
+
+            minX = Math.Min(minX, submapMinX);
+            minY = Math.Min(minY, submapMinY);
+            maxX = Math.Max(maxX, submapMaxX);
+            maxY = Math.Max(maxY, submapMaxY);
+        }
+
+        return (minX - mapPadding, minY - mapPadding, maxX + mapPadding, maxY + mapPadding);
+    }
+
+    #endregion
+
+    #region Probability Conversion
+
+    /// <summary>
+    /// Convert probability value to occupancy grid value
+    /// </summary>
+    public static sbyte ConvertProbabilityToOccupancyValue(double probability)
+    {
+        if (probability < FreeSpaceProbabilityThreshold)
+        {
+            return 0;
+        }
+        if (probability > OccupiedSpaceProbabilityThreshold)
+        {
+            return 100;
+        }
+        if (probability < UnknownSpaceLowerBound || probability > UnknownSpaceUpperBound)
+        {
+            var occupancyValue = (sbyte)Math.Round(probability * 100.0);
+            return (sbyte)Math.Clamp(occupancyValue, (sbyte)0, (sbyte)100);
+        }
+        return -1; // Unknown space
+    }
+
+    #endregion
+
+    #region Grid Creation
+
+    /// <summary>
+    /// Create an empty occupancy grid with default size
+    /// </summary>
+    public static OccupancyGrid CreateEmptyOccupancyGrid(double resolution)
+    {
+        var origin = new Pose
+        {
+            Position = new Vector3(0, 0, 0),
+            Orientation = new Quaternion(0, 0, 0, 1)
+        };
+
+        return new OccupancyGrid(resolution, InitialEmptyGridSize, InitialEmptyGridSize, origin);
+    }
+
+    // NOTE: GenerateOccupancyGridFromSubmapList and MergeSubmapIntoOccupancyGrid
+    // have been moved to OccupancyGridGenerator for unified grid generation.
+    // Use OccupancyGridGenerator.GenerateFromMapBuilder instead.
+
+    #endregion
+
+    #region Grid Expansion
+
+    /// <summary>
+    /// Expand grid if needed to fit multiple submaps (optimized version)
+    /// </summary>
+    public static OccupancyGrid? ExpandGridToFitSubmaps(
+        OccupancyGrid currentGrid,
+        List<(Submap2D Submap, Rigid3d GlobalPose, SubmapId SubmapId)> submaps,
+        double resolution,
+        double mapPadding)
+    {
+        if (currentGrid == null || submaps == null || submaps.Count == 0)
+            return null;
+
+        // Calculate combined bounds of all submaps
+        double combinedMinX = double.MaxValue, combinedMinY = double.MaxValue;
+        double combinedMaxX = double.MinValue, combinedMaxY = double.MinValue;
+        bool hasValidBounds = false;
+
+        foreach (var (submap2D, globalPose, _) in submaps)
+        {
+            var grid = submap2D.Grid;
+            if (grid == null)
+                continue;
+
+            var (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY) =
+                CalculateSubmapGlobalBounds(submap2D, globalPose);
+
+            if (submapGlobalMinX < submapGlobalMaxX && submapGlobalMinY < submapGlobalMaxY &&
+                submapGlobalMinX != double.MaxValue && submapGlobalMinY != double.MaxValue &&
+                submapGlobalMaxX != double.MinValue && submapGlobalMaxY != double.MinValue)
+            {
+                combinedMinX = Math.Min(combinedMinX, submapGlobalMinX);
+                combinedMinY = Math.Min(combinedMinY, submapGlobalMinY);
+                combinedMaxX = Math.Max(combinedMaxX, submapGlobalMaxX);
+                combinedMaxY = Math.Max(combinedMaxY, submapGlobalMaxY);
+                hasValidBounds = true;
+            }
+        }
+
+        if (!hasValidBounds)
+        {
+            return null;
+        }
+
+        var (gridMinX, gridMinY, gridMaxX, gridMaxY) = GetGridBounds(currentGrid);
+
+        var needsExpansion = combinedMinX < gridMinX || combinedMinY < gridMinY ||
+                             combinedMaxX > gridMaxX || combinedMaxY > gridMaxY;
+
+        if (!needsExpansion)
+        {
+            return null;
+        }
+
+        var newMinX = Math.Min(gridMinX, combinedMinX) - mapPadding;
+        var newMinY = Math.Min(gridMinY, combinedMinY) - mapPadding;
+        var newMaxX = Math.Max(gridMaxX, combinedMaxX) + mapPadding;
+        var newMaxY = Math.Max(gridMaxY, combinedMaxY) + mapPadding;
+
+        var newWidth = (int)Math.Ceiling((newMaxX - newMinX) / resolution);
+        var newHeight = (int)Math.Ceiling((newMaxY - newMinY) / resolution);
+
+        var newOrigin = new Pose
+        {
+            Position = new Vector3(newMinX, newMinY, 0),
+            Orientation = new Quaternion(0, 0, 0, 1)
+        };
+
+        var expandedGrid = new OccupancyGrid(resolution, newWidth, newHeight, newOrigin);
+
+        // Copy existing grid data to expanded grid
+        for (int y = 0; y < currentGrid.Height; y++)
+        {
+            for (int x = 0; x < currentGrid.Width; x++)
+            {
+                var (worldX, worldY) = currentGrid.GridToWorld(x, y);
+                var (newGridX, newGridY) = expandedGrid.WorldToGrid(worldX, worldY);
+
+                if (newGridX >= 0 && newGridX < expandedGrid.Width &&
+                    newGridY >= 0 && newGridY < expandedGrid.Height)
+                {
+                    var cellValue = currentGrid.GetCell(x, y);
+                    expandedGrid.SetCell(newGridX, newGridY, cellValue);
+                }
+            }
+        }
+
+        return expandedGrid;
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/OccupancyGridManager.cs

@@ -0,0 +1,306 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.Shared;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+using RobotNet10.Shared.Localization;
+using System.Text.Json;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Manages occupancy grid loading, updating, and caching
+/// Handles throttled updates during ScanMapping using counter-based logic
+/// </summary>
+public class OccupancyGridManager(CartographerConfiguration _config, ILogger<OccupancyGridManager> _logger)
+{
+    private readonly Lock _lock = new();
+
+    private OccupancyGrid? _occupancyGrid;
+    private OccupancyGrid? _occupancyGridMcl;
+    private DateTime _lastUpdatedOccupancyGrid = DateTime.MinValue;
+    private volatile int _generationInProgress = 0; // 1 = generation running
+
+    // Version tracking: PoseGraph node insertion version at last successful generation.
+    // Compared against the live PoseGraph version to skip redundant regenerations.
+    private int _lastGeneratedVersion = -1;
+
+    // Configuration: time-based throttling (~3 seconds between updates)
+    private readonly System.Diagnostics.Stopwatch _updateStopwatch = System.Diagnostics.Stopwatch.StartNew();
+    private const double UpdateIntervalSeconds = 3.0;
+
+    // Insertion tracking: flag to track if at least one scan was inserted into submap since last update
+    // Used in ScanMapping mode to ensure we only update when there's new data
+    private volatile bool _hasInsertionSinceLastUpdate = false;
+
+    public OccupancyGrid? OccupancyGrid
+    {
+        get
+        {
+            lock (_lock) { return _occupancyGrid; }
+        }
+    }
+
+    public OccupancyGrid? OccupancyGridMcl
+    {
+        get
+        {
+            lock (_lock) { return _occupancyGridMcl; }
+        }
+    }
+
+    public DateTime LastUpdated
+    {
+        get
+        {
+            lock (_lock) { return _lastUpdatedOccupancyGrid; }
+        }
+    }
+
+    /// <summary>
+    /// Get occupancy grid, optionally checking if updated since a given time
+    /// </summary>
+    public OccupancyGrid? GetGrid(DateTime since = default)
+    {
+        lock (_lock)
+        {
+            if (since == DateTime.MinValue)
+                return _occupancyGrid;
+            return _lastUpdatedOccupancyGrid > since ? _occupancyGrid : null;
+        }
+    }
+
+    /// <summary>
+    /// Signal that a scan was inserted into a submap (InsertionResult.HasValue = true).
+    /// Called from ProcessWithTrajectoryBuilder when AddSensorData returns insertion result.
+    /// This flag is required for ShouldUpdateGrid to return true.
+    /// </summary>
+    public void SignalInsertion()
+    {
+        _hasInsertionSinceLastUpdate = true;
+    }
+
+    /// <summary>
+    /// Check if grid should be updated based on two conditions:
+    /// 1. Enough time has elapsed since last update (3 seconds) OR grid is null
+    /// 2. At least one scan was inserted into submap since last update (SignalInsertion was called)
+    /// Both conditions must be met for update to proceed.
+    /// Thread-safe: prevents concurrent generation via _generationInProgress flag.
+    /// </summary>
+    public bool ShouldUpdateGrid()
+    {
+        // Fast check: skip if generation is already in progress
+        if (Interlocked.CompareExchange(ref _generationInProgress, 0, 0) == 1)
+            return false;
+
+        lock (_lock)
+        {
+            bool gridIsNull = _occupancyGrid == null;
+            bool timeElapsed = _updateStopwatch.Elapsed.TotalSeconds >= UpdateIntervalSeconds;
+            bool hasInsertion = _hasInsertionSinceLastUpdate;
+
+            // Require BOTH: (time elapsed OR grid null) AND has insertion
+            if ((gridIsNull || timeElapsed) && hasInsertion)
+            {
+                _logger.LogDebug(
+                    "OccupancyGridManager: ShouldUpdateGrid=true, gridIsNull={GridIsNull}, elapsed={Elapsed:F1}s, hasInsertion={HasInsertion}",
+                    gridIsNull, _updateStopwatch.Elapsed.TotalSeconds, hasInsertion);
+                _updateStopwatch.Restart();
+                _hasInsertionSinceLastUpdate = false; // Reset insertion flag after update
+                return true;
+            }
+            return false;
+        }
+    }
+
+    /// <summary>
+    /// Update occupancy grid from MapBuilder submaps during ScanMapping
+    /// Only generates Display grid for visualization (MCL not used during mapping)
+    /// </summary>
+    public void UpdateFromMapBuilder(IMapBuilder mapBuilder)
+    {
+        // Prevent concurrent generation
+        if (Interlocked.CompareExchange(ref _generationInProgress, 1, 0) != 0)
+        {
+            _logger.LogDebug("OccupancyGridManager: Skipping - generation already in progress");
+            return;
+        }
+
+        try
+        {
+            if (mapBuilder == null)
+            {
+                _logger.LogWarning("OccupancyGridManager: UpdateFromMapBuilder called with null mapBuilder");
+                return;
+            }
+
+            var resolution = _config.MapStorage.OccupancyGridResolution;
+            var padding = _config.MapStorage.MapPadding;
+            var strategy = _config.OccupancyGrid?.MergeStrategy ?? SubmapMergeStrategy.LogOddsSum;
+
+            var sw = System.Diagnostics.Stopwatch.StartNew();
+
+            // Generate display grid using configured merge strategy.
+            // Pass the last generated version so the generator can skip if no new nodes
+            // have been inserted (avoids redundant regeneration of identical data).
+            var displayGrid = OccupancyGridGenerator.Generate(
+                mapBuilder, resolution, padding,
+                Volatile.Read(ref _lastGeneratedVersion),
+                out var snapshotVersion,
+                _logger, _config.OccupancyGrid);
+
+            sw.Stop();
+
+            if (displayGrid == null)
+            {
+                lock (_lock)
+                {
+                    _occupancyGrid = null;
+                    _occupancyGridMcl = null;
+                    _lastUpdatedOccupancyGrid = DateTime.MinValue;
+                }
+                return;
+            }
+
+            // Count cell statistics for debugging
+            int freeCells = 0, occupiedCells = 0, unknownCells = 0;
+            for (int i = 0; i < displayGrid.Data.Length; i++)
+            {
+                if (displayGrid.Data[i] < 0) unknownCells++;
+                else if (displayGrid.Data[i] == 0) freeCells++;
+                else occupiedCells++;
+            }
+
+            lock (_lock)
+            {
+                _occupancyGrid = displayGrid;
+                // Keep existing mclGrid (if any) - don't regenerate during mapping
+                _lastUpdatedOccupancyGrid = DateTime.UtcNow;
+            }
+
+            // Cache the version so the next cycle can skip if no new data was added.
+            Volatile.Write(ref _lastGeneratedVersion, snapshotVersion);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "OccupancyGridManager: Failed to update grid from MapBuilder");
+        }
+        finally
+        {
+            Interlocked.Exchange(ref _generationInProgress, 0);
+        }
+    }
+
+    /// <summary>
+    /// Load occupancy grid from PGM file for localization
+    /// </summary>
+    public void LoadFromPgm(string mapName)
+    {
+        try
+        {
+            var mapPath = Path.Combine(_config.MapStorage.Directory, mapName);
+            var pgmPath = Path.Combine(mapPath, "map.pgm");
+
+            if (!File.Exists(pgmPath))
+            {
+                var pgmFiles = Directory.GetFiles(mapPath, "*.pgm", SearchOption.TopDirectoryOnly);
+                if (pgmFiles.Length == 0)
+                {
+                    _logger.LogWarning("OccupancyGridManager: No PGM file found for map: {MapName}", mapName);
+                    Clear();
+                    return;
+                }
+                pgmPath = pgmFiles[0];
+            }
+
+            // Load grid once and clone for MCL (Fix 2: avoid loading PGM file twice)
+            // UNIFIED CONVENTION: Both use ROS convention (row 0 = world BOTTOM, Y-axis pointing UP)
+            var displayGrid = PgmLoader.LoadFromPgm(pgmPath, logger: _logger);
+            OccupancyGrid? mclGrid = null;
+            if (displayGrid != null)
+            {
+                mclGrid = new OccupancyGrid
+                {
+                    Resolution = displayGrid.Resolution,
+                    Width = displayGrid.Width,
+                    Height = displayGrid.Height,
+                    Origin = displayGrid.Origin,
+                    Data = (sbyte[])displayGrid.Data.Clone()
+                };
+            }
+
+            // Load origin from map.json if available
+            if (displayGrid != null && mclGrid != null)
+            {
+                var mapJsonPath = Path.Combine(mapPath, "map.json");
+                if (File.Exists(mapJsonPath))
+                {
+                    try
+                    {
+                        var jsonContent = File.ReadAllText(mapJsonPath);
+                        var metadata = JsonSerializer.Deserialize<MapInfo>(jsonContent);
+                        if (metadata?.Origin != null)
+                        {
+                            displayGrid.Origin = metadata.Origin;
+                            mclGrid.Origin = metadata.Origin;
+                        }
+                    }
+                    catch (Exception ex)
+                    {
+                        _logger.LogWarning(ex, "OccupancyGridManager: Could not read origin from map.json");
+                    }
+                }
+            }
+
+            lock (_lock)
+            {
+                _occupancyGrid = displayGrid;
+                _occupancyGridMcl = mclGrid;
+                _lastUpdatedOccupancyGrid = DateTime.UtcNow;
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "OccupancyGridManager: Failed to load PGM for map: {MapName}", mapName);
+        }
+    }
+
+    /// <summary>
+    /// Clear grids for new scanning session
+    /// </summary>
+    public void Clear()
+    {
+        lock (_lock)
+        {
+            _occupancyGrid = null;
+            _occupancyGridMcl = null;
+            _lastUpdatedOccupancyGrid = DateTime.MinValue;
+            _updateStopwatch.Restart();
+        }
+        _hasInsertionSinceLastUpdate = false;
+        Volatile.Write(ref _lastGeneratedVersion, -1);
+    }
+
+    /// <summary>
+    /// Reset timer and insertion flag for new state (ScanMapping/Localizing)
+    /// </summary>
+    public void ResetCounter()
+    {
+        lock (_lock)
+        {
+            _updateStopwatch.Restart();
+        }
+        _hasInsertionSinceLastUpdate = false;
+        Volatile.Write(ref _lastGeneratedVersion, -1);
+    }
+
+    /// <summary>
+    /// Dispose resources
+    /// </summary>
+    public void Dispose()
+    {
+        lock (_lock)
+        {
+            _occupancyGrid = null;
+            _occupancyGridMcl = null;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/PbstreamTransformHelper.cs

@@ -0,0 +1,163 @@
+using CartographerSharp.IO;
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for transforming pbstream files.
+/// Centralizes the logic for updating TransformToMap in PoseGraph proto.
+/// </summary>
+public static class PbstreamTransformHelper
+{
+    /// <summary>
+    /// Transform pbstream file by updating TransformToMap in PoseGraph proto.
+    /// This applies a coordinate transform to shift the map origin.
+    /// </summary>
+    /// <param name="pbstreamPath">Path to the pbstream file</param>
+    /// <param name="newOrigin">New origin pose to apply</param>
+    /// <param name="logger">Optional logger for logging</param>
+    /// <exception cref="InvalidOperationException">Thrown when pbstream read/write fails</exception>
+    /// <exception cref="ArgumentException">Thrown when newOrigin contains invalid values (NaN/Infinity)</exception>
+    public static void TransformPbstreamOrigin(string pbstreamPath, Pose newOrigin, ILogger? logger = null)
+    {
+        // Validate newOrigin for NaN/Infinity
+        ValidatePose(newOrigin);
+
+        var tempPbstreamPath = pbstreamPath + ".tmp";
+        try
+        {
+            using (var reader = new ProtoStreamReader(pbstreamPath))
+            using (var writer = new ProtoStreamWriter(tempPbstreamPath))
+            {
+                // Read and write header
+                if (!reader.ReadProto<CartographerSharp.Models.Mapping.SerializedData>(out var headerData))
+                {
+                    throw new InvalidOperationException("Failed to read serialization header");
+                }
+                if (!headerData.SerializationHeader.HasValue)
+                {
+                    throw new InvalidOperationException("Invalid serialization header");
+                }
+                writer.WriteProto(headerData);
+
+                // Read PoseGraph, update TransformToMap, and write
+                if (!reader.ReadProto<CartographerSharp.Models.Mapping.SerializedData>(out var poseGraphData))
+                {
+                    throw new InvalidOperationException("Failed to read pose graph");
+                }
+                if (!poseGraphData.PoseGraph.HasValue)
+                {
+                    throw new InvalidOperationException("Invalid pose graph data");
+                }
+
+                var poseGraphProto = poseGraphData.PoseGraph.Value;
+
+                // Convert Pose to Rigid3d and compose with current transform
+                // Key logic from xloc.cc ChangeMapOrigin:
+                // SetTransformToMap(GetTransformToMap() * Rigid3d(position, orientation));
+                //
+                // In Cartographer:
+                // - TransformToMap: converts map frame -> internal frame
+                // - TransformToMapInverse: converts internal frame -> map frame (used in OccupancyGridGenerator)
+                //
+                // We want: newOrigin.Position (T) in old map becomes (0,0) in new map
+                // Formula: newMapPoint = R^-1 * (oldMapPoint - T) where T = newOrigin.Position, R = newOrigin.Orientation
+                var newOriginRigid = new Rigid3d(
+                    new Vector3(newOrigin.Position.X, newOrigin.Position.Y, newOrigin.Position.Z),
+                    new Quaternion(newOrigin.Orientation.X, newOrigin.Orientation.Y, newOrigin.Orientation.Z, newOrigin.Orientation.W));
+
+                var currentTransform = poseGraphProto.TransformToMap.HasValue
+                    ? (Rigid3d)poseGraphProto.TransformToMap.Value
+                    : Rigid3d.Identity;
+                var newTransform = currentTransform * newOriginRigid;
+
+                // Update TransformToMap in the proto
+                poseGraphProto.TransformToMap = (CartographerSharp.Models.Transform.Rigid3dProto)newTransform;
+
+                // Write updated pose graph
+                var updatedPoseGraphData = new CartographerSharp.Models.Mapping.SerializedData { PoseGraph = poseGraphProto };
+                writer.WriteProto(updatedPoseGraphData);
+
+                // Copy all remaining data unchanged (AllTrajectoryBuilderOptions, submaps, nodes, etc.)
+                while (!reader.Eof)
+                {
+                    if (!reader.ReadProto<CartographerSharp.Models.Mapping.SerializedData>(out var data))
+                    {
+                        break;
+                    }
+                    writer.WriteProto(data);
+                }
+            }
+
+            // Replace original file with updated file
+            File.Move(tempPbstreamPath, pbstreamPath, overwrite: true);
+            logger?.LogInformation("PbstreamTransformHelper: Transformed pbstream origin successfully");
+        }
+        catch (Exception)
+        {
+            // Clean up temp file on error
+            if (File.Exists(tempPbstreamPath))
+            {
+                try { File.Delete(tempPbstreamPath); } catch { }
+            }
+            throw;
+        }
+    }
+
+    /// <summary>
+    /// Check if pose is identity (no transform needed)
+    /// </summary>
+    public static bool IsIdentityPose(Pose pose)
+    {
+        const double tolerance = 1e-6;
+
+        // Check position is (0, 0, 0)
+        var posIsZero = Math.Abs(pose.Position.X) < tolerance &&
+                        Math.Abs(pose.Position.Y) < tolerance &&
+                        Math.Abs(pose.Position.Z) < tolerance;
+
+        // Check orientation is identity quaternion (0, 0, 0, 1)
+        var quatIsIdentity = Math.Abs(pose.Orientation.X) < tolerance &&
+                             Math.Abs(pose.Orientation.Y) < tolerance &&
+                             Math.Abs(pose.Orientation.Z) < tolerance &&
+                             Math.Abs(pose.Orientation.W - 1.0) < tolerance;
+
+        return posIsZero && quatIsIdentity;
+    }
+
+    /// <summary>
+    /// Validate pose for NaN/Infinity values
+    /// </summary>
+    /// <exception cref="ArgumentException">Thrown when pose contains invalid values</exception>
+    public static void ValidatePose(Pose pose)
+    {
+        // Check position
+        if (double.IsNaN(pose.Position.X) || double.IsInfinity(pose.Position.X) ||
+            double.IsNaN(pose.Position.Y) || double.IsInfinity(pose.Position.Y) ||
+            double.IsNaN(pose.Position.Z) || double.IsInfinity(pose.Position.Z))
+        {
+            throw new ArgumentException($"Pose position contains invalid values (NaN/Infinity): ({pose.Position.X}, {pose.Position.Y}, {pose.Position.Z})", nameof(pose));
+        }
+
+        // Check orientation
+        if (double.IsNaN(pose.Orientation.X) || double.IsInfinity(pose.Orientation.X) ||
+            double.IsNaN(pose.Orientation.Y) || double.IsInfinity(pose.Orientation.Y) ||
+            double.IsNaN(pose.Orientation.Z) || double.IsInfinity(pose.Orientation.Z) ||
+            double.IsNaN(pose.Orientation.W) || double.IsInfinity(pose.Orientation.W))
+        {
+            throw new ArgumentException($"Pose orientation contains invalid values (NaN/Infinity): ({pose.Orientation.X}, {pose.Orientation.Y}, {pose.Orientation.Z}, {pose.Orientation.W})", nameof(pose));
+        }
+    }
+
+    /// <summary>
+    /// Convert Pose to Rigid3d
+    /// </summary>
+    public static Rigid3d PoseToRigid3d(Pose pose)
+    {
+        var translation = new Vector3(pose.Position.X, pose.Position.Y, pose.Position.Z);
+        var rotation = new Quaternion(pose.Orientation.X, pose.Orientation.Y, pose.Orientation.Z, pose.Orientation.W);
+        return new Rigid3d(translation, rotation);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/PoseConverter.cs

@@ -0,0 +1,53 @@
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for converting between Cartographer Rigid3d and RobotNet10 Pose
+/// </summary>
+public static class PoseConverter
+{
+    /// <summary>
+    /// Convert Rigid3d to Pose
+    /// </summary>
+    public static Pose ToPose(Rigid3d rigid3d)
+    {
+        return new Pose
+        {
+            Position = new Vector3
+            {
+                X = rigid3d.Translation.X,
+                Y = rigid3d.Translation.Y,
+                Z = rigid3d.Translation.Z
+            },
+            Orientation = new Quaternion
+            {
+                X = rigid3d.Rotation.X,
+                Y = rigid3d.Rotation.Y,
+                Z = rigid3d.Rotation.Z,
+                W = rigid3d.Rotation.W
+            }
+        };
+    }
+
+    /// <summary>
+    /// Convert Pose to Rigid3d
+    /// </summary>
+    public static Rigid3d ToRigid3d(Pose pose)
+    {
+        var translation = new Vector3(
+            (float)pose.Position.X,
+            (float)pose.Position.Y,
+            (float)pose.Position.Z);
+
+        var rotation = new Quaternion(
+            (float)pose.Orientation.X,
+            (float)pose.Orientation.Y,
+            (float)pose.Orientation.Z,
+            (float)pose.Orientation.W);
+
+        return new Rigid3d(translation, rotation);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/RangeDataPayload.cs

@@ -0,0 +1,25 @@
+using CartographerSharp.Sensor;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Point cloud data in both base_link and sensor (lidar) frame.
+/// Pipeline produces both so Cartographer gets base_link for trajectory and MCL gets sensor_frame without re-transform.
+/// </summary>
+public sealed class RangeDataPayload
+{
+    /// <summary>Point cloud in base_link (robot) frame — for Cartographer ITrajectoryBuilder.AddSensorData.</summary>
+    public TimedPointCloudData BaseLink { get; set; }
+
+    /// <summary>Point cloud in sensor (lidar) frame — for MCL OnScan (beam angles relative to sensor). SensorPipeline always produces this.</summary>
+    public TimedPointCloudData SensorFrame { get; set; }
+
+    /// <summary>Actual minimum angle (radians) of filtered point cloud in sensor frame — for MCL scan conversion.</summary>
+    public double ActualAngleMin { get; set; }
+
+    /// <summary>Actual maximum angle (radians) of filtered point cloud in sensor frame — for MCL scan conversion.</summary>
+    public double ActualAngleMax { get; set; }
+
+    /// <summary>Actual angle increment (radians) from original scan — for MCL scan conversion.</summary>
+    public double ActualAngleIncrement { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/ScanMatchingQualityEvaluator.cs

@@ -0,0 +1,414 @@
+using Microsoft.Extensions.Options;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+using System.Diagnostics;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Evaluates scan matching quality at a given pose against occupancy grid.
+/// Provides reliability [0,1] and MAE metrics without running particle filter.
+/// Directly evaluates the quality of pose from CartographerSharp.
+/// </summary>
+public class ScanMatchingQualityEvaluator : IDisposable
+{
+    #region Fields and Constructor
+
+    private readonly CartographerConfiguration _config;
+    private readonly ILogger<ScanMatchingQualityEvaluator> _logger;
+    private readonly Lock _lock = new();
+
+    // Occupancy grid and distance map
+    private OccupancyGrid? _occupancyGrid;
+    private double[,]? _distanceMap; // Distance to nearest occupied cell (meters)
+    private double _mapResolution;
+    private double _mapOriginX, _mapOriginY, _mapOriginYaw;
+    private int _mapWidth, _mapHeight;
+
+    // Likelihood field model constants
+    private double _normConstHit, _denomHit, _measurementModelRandom;
+
+    // Latest scan and pose for periodic evaluation
+    private RangeDataPayload? _latestScan;
+    private Pose? _latestPose;
+    private string? _latestDeviceId;
+    private DateTime _lastScanTime = DateTime.MinValue;
+
+    // Periodic evaluation state
+    private bool _running;
+    private Timer? _periodicTimer;
+    private readonly TimeSpan _evaluationInterval;
+
+    // Latest metrics
+    private double _reliability = 0.5;
+    private double? _mae;
+    private DateTime _lastUpdateTime = DateTime.MinValue;
+
+    // Primary lidar ID for filtering
+    private string? _primaryLidarId;
+
+    public ScanMatchingQualityEvaluator(
+        IOptions<CartographerConfiguration> configuration,
+        ILogger<ScanMatchingQualityEvaluator> logger)
+    {
+        _config = configuration?.Value ?? throw new ArgumentNullException(nameof(configuration));
+        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
+
+        // Get evaluation interval from configuration
+        _evaluationInterval = TimeSpan.FromSeconds(_config.Mcl.ReliabilityMonitoring.MonitoringIntervalSeconds);
+    }
+
+    #endregion
+
+    #region Public API
+
+    /// <summary>
+    /// Start periodic scan quality evaluation
+    /// </summary>
+    public void Start(OccupancyGrid occupancyGrid, Pose? initialPose, string? primaryLidarId)
+    {
+        lock (_lock)
+        {
+            if (_running)
+            {
+                _logger.LogWarning("ScanMatchingQualityEvaluator: Already running, ignoring Start call");
+                return;
+            }
+
+            try
+            {
+                _occupancyGrid = occupancyGrid ?? throw new ArgumentNullException(nameof(occupancyGrid));
+                _primaryLidarId = primaryLidarId;
+
+                // Build distance map from occupancy grid
+                BuildDistanceMap(occupancyGrid);
+
+                // Initialize likelihood field model constants
+                InitializeMeasurementModel();
+
+                // Create periodic timer
+                _periodicTimer = new Timer(
+                    EvaluateScanQuality,
+                    null,
+                    _evaluationInterval,
+                    _evaluationInterval);
+
+                _running = true;
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "ScanMatchingQualityEvaluator: Failed to start");
+                _running = false;
+                throw;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Stop periodic evaluation
+    /// </summary>
+    public void Stop()
+    {
+        lock (_lock)
+        {
+            if (!_running)
+            {
+                return;
+            }
+
+            _running = false;
+
+            _periodicTimer?.Dispose();
+            _periodicTimer = null;
+
+            _latestScan = null;
+            _latestPose = null;
+            _latestDeviceId = null;
+            _occupancyGrid = null;
+            _distanceMap = null;
+        }
+    }
+
+    /// <summary>
+    /// Cache latest scan and pose for periodic evaluation
+    /// </summary>
+    public void OnScanReceived(string deviceId, RangeDataPayload payload, Pose? currentPose)
+    {
+        lock (_lock)
+        {
+            if (!_running)
+            {
+                return;
+            }
+
+            // Filter by primary lidar if specified
+            if (!string.IsNullOrEmpty(_primaryLidarId) && deviceId != _primaryLidarId)
+            {
+                return;
+            }
+
+            _latestScan = payload;
+            _latestPose = currentPose;
+            _latestDeviceId = deviceId;
+            _lastScanTime = DateTime.UtcNow;
+        }
+    }
+
+    /// <summary>
+    /// Get latest evaluation metrics
+    /// </summary>
+    public EvaluationMetrics GetLatestMetrics()
+    {
+        lock (_lock)
+        {
+            return new EvaluationMetrics(_reliability, _mae, _lastUpdateTime);
+        }
+    }
+
+    #endregion
+
+    #region Distance Map Setup
+
+    /// <summary>
+    /// Build distance map from occupancy grid using Felzenszwalb-Huttenlocher distance transform
+    /// Distance map stores distance (in meters) to nearest occupied cell for each grid cell
+    /// </summary>
+    private void BuildDistanceMap(OccupancyGrid grid)
+    {
+        _mapWidth = grid.Width;
+        _mapHeight = grid.Height;
+        _mapResolution = grid.Resolution;
+        _mapOriginX = grid.Origin.Position.X;
+        _mapOriginY = grid.Origin.Position.Y;
+        _mapOriginYaw = grid.Origin.Orientation.ToYawRadian();
+
+        // Create binary map: 0 = occupied (100), 1 = free
+        var binaryMap = new byte[_mapHeight, _mapWidth];
+        for (int v = 0; v < _mapHeight; v++)
+        {
+            for (int u = 0; u < _mapWidth; u++)
+            {
+                int idx = v * _mapWidth + u;
+                binaryMap[v, u] = grid.Data[idx] == 100 ? (byte)0 : (byte)1;
+            }
+        }
+
+        // Compute Euclidean distance transform
+        _distanceMap = DistanceTransformHelper.ComputeEuclidean(binaryMap, _mapWidth, _mapHeight, _mapResolution);
+
+        _logger.LogDebug("ScanMatchingQualityEvaluator: Built distance map {Width}x{Height} at {Res}m resolution",
+            _mapWidth, _mapHeight, _mapResolution);
+    }
+
+    #endregion
+
+    #region Measurement Model
+
+    /// <summary>
+    /// Initialize measurement model constants (likelihood field model)
+    /// </summary>
+    private void InitializeMeasurementModel()
+    {
+        double varHit = _config.Mcl.VarHit;
+        double zHit = _config.Mcl.ZHit;
+        double zRand = _config.Mcl.ZRand;
+
+        _normConstHit = 1.0 / Math.Sqrt(2.0 * Math.PI * varHit);
+        _denomHit = 2.0 * varHit;
+        _measurementModelRandom = zRand;
+    }
+
+    #endregion
+
+    #region Scan Evaluation
+
+    /// <summary>
+    /// Timer callback: Evaluate scan quality at current pose
+    /// </summary>
+    private void EvaluateScanQuality(object? state)
+    {
+        try
+        {
+            // Read cached scan and pose
+            RangeDataPayload? scan;
+            Pose? pose;
+            bool running;
+
+            lock (_lock)
+            {
+                running = _running;
+                scan = _latestScan;
+                pose = _latestPose;
+            }
+
+            if (!running || scan == null || pose == null)
+            {
+                return;
+            }
+
+            var stopwatch = Stopwatch.StartNew();
+
+            // Convert scan to ranges
+            var pointsForScan = scan.SensorFrame.Ranges
+                .Select(r => new Vector2(r.Position.X, r.Position.Y))
+                .ToList();
+
+            double angleMinDeg = scan.ActualAngleMin * (180.0 / Math.PI);
+            double angleMaxDeg = scan.ActualAngleMax * (180.0 / Math.PI);
+
+            // Evaluate scan at current pose
+            var result = EvaluateScanAtPose(pose.Value, pointsForScan, angleMinDeg, angleMaxDeg);
+
+            stopwatch.Stop();
+
+            // Update metrics
+            lock (_lock)
+            {
+                if (_running)
+                {
+                    _reliability = result.Reliability;
+                    _mae = result.Mae;
+                    _lastUpdateTime = DateTime.UtcNow;
+
+                    _logger.LogDebug(
+                        "ScanMatchingQualityEvaluator: reliability={Reliability:F3}, mae={Mae:F4}m, validBeams={Beams}, elapsed={Elapsed}ms",
+                        result.Reliability,
+                        result.Mae,
+                        result.ValidBeams,
+                        stopwatch.ElapsedMilliseconds);
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogWarning(ex, "ScanMatchingQualityEvaluator: Evaluation failed, keeping previous metrics");
+        }
+    }
+
+    /// <summary>
+    /// Evaluate scan quality at given pose using likelihood field model
+    /// </summary>
+    private EvaluationResult EvaluateScanAtPose(Pose pose, List<Vector2> points, double angleMinDeg, double angleMaxDeg)
+    {
+        if (_distanceMap == null)
+        {
+            return new EvaluationResult(0.5, 1.0, 0.0, 0);
+        }
+
+        double totalError = 0;
+        double totalLikelihood = 0;
+        int validBeams = 0;
+
+        // Pose in map frame
+        double poseX = pose.Position.X;
+        double poseY = pose.Position.Y;
+        double poseYaw = pose.Orientation.ToYawRadian();
+
+        int numPoints = points.Count;
+        double angleRangeDeg = angleMaxDeg - angleMinDeg;
+        int scanStep = _config.Mcl.ScanStep;
+
+        for (int i = 0; i < numPoints; i += scanStep)
+        {
+            var point = points[i];
+            double range = Math.Sqrt(point.X * point.X + point.Y * point.Y);
+
+            // Skip invalid ranges
+            if (range < _config.TrajectoryBuilder.MinRange || range > _config.TrajectoryBuilder.MaxRange)
+            {
+                continue;
+            }
+
+            // Beam angle in sensor frame
+            double beamAngleDeg = angleMinDeg + (angleRangeDeg * i / numPoints);
+            double beamAngleRad = beamAngleDeg * (Math.PI / 180.0);
+
+            // Transform beam endpoint to map frame
+            double cosYaw = Math.Cos(poseYaw);
+            double sinYaw = Math.Sin(poseYaw);
+            double beamEndX = poseX + (point.X * cosYaw - point.Y * sinYaw);
+            double beamEndY = poseY + (point.X * sinYaw + point.Y * cosYaw);
+
+            // Convert to grid coordinates
+            double dx = beamEndX - _mapOriginX;
+            double dy = beamEndY - _mapOriginY;
+            double cosOrigin = Math.Cos(_mapOriginYaw);
+            double sinOrigin = Math.Sin(_mapOriginYaw);
+            double gridX = (dx * cosOrigin + dy * sinOrigin) / _mapResolution;
+            double gridY = (-dx * sinOrigin + dy * cosOrigin) / _mapResolution;
+
+            int u = (int)Math.Round(gridX);
+            int v = (int)Math.Round(gridY);
+
+            // Check bounds
+            if (u < 0 || u >= _mapWidth || v < 0 || v >= _mapHeight)
+            {
+                continue;
+            }
+
+            // Get distance to nearest obstacle
+            double dist = _distanceMap[v, u];
+
+            // Calculate error (MAE)
+            totalError += dist;
+
+            // Calculate likelihood using likelihood field model
+            double pHit = _normConstHit * Math.Exp(-(dist * dist) / _denomHit);
+            double likelihood = _config.Mcl.ZHit * pHit + _measurementModelRandom;
+            totalLikelihood += likelihood;
+
+            validBeams++;
+        }
+
+        // Calculate metrics
+        double mae = validBeams > 0 ? totalError / validBeams : 1.0;
+        double avgLikelihood = validBeams > 0 ? totalLikelihood / validBeams : 0.0;
+
+        // Calculate reliability from MAE and likelihood
+        double reliability = CalculateReliability(mae, avgLikelihood);
+
+        return new EvaluationResult(reliability, mae, avgLikelihood, validBeams);
+    }
+
+    /// <summary>
+    /// Calculate reliability [0,1] from MAE and average likelihood
+    /// </summary>
+    private double CalculateReliability(double mae, double avgLikelihood)
+    {
+        // MAE-based component (exponential decay)
+        // Good: < 0.05m → 1.0
+        // Poor: > 0.5m → ~0.0
+        double maeScore = Math.Exp(-10.0 * mae);
+
+        // Likelihood-based component (already normalized 0-1)
+        double likelihoodScore = Math.Clamp(avgLikelihood, 0.0, 1.0);
+
+        // Combine: MAE more important (70%), likelihood 30%
+        double reliability = maeScore * 0.7 + likelihoodScore * 0.3;
+
+        return Math.Clamp(reliability, 0.0, 1.0);
+    }
+
+    #endregion
+
+    #region IDisposable
+
+    public void Dispose()
+    {
+        Stop();
+        GC.SuppressFinalize(this);
+    }
+
+    #endregion
+
+    /// <summary>
+    /// Evaluation result
+    /// </summary>
+    private record EvaluationResult(double Reliability, double Mae, double AvgLikelihood, int ValidBeams);
+
+    /// <summary>
+    /// Evaluation metrics (public)
+    /// </summary>
+    public record EvaluationMetrics(double Reliability, double? Mae, DateTime UpdateTime);
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/SensorDataTransformHelper.cs

@@ -0,0 +1,172 @@
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Sensor;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for converting sensor data (Lidar, IMU, Odometry) to CartographerSharp types.
+/// Used by SensorPipeline when feeding data into ITrajectoryBuilder.AddSensorData.
+/// </summary>
+public static class SensorDataTransformHelper
+{
+    /// <summary>
+    /// Transforms one Lidar scan into both base_link and sensor-frame point clouds in a single pass.
+    /// Filters by range and optional angle limits; applies sensor transform for base_link. Use for RangeDataPayload.
+    /// </summary>
+    /// <param name="timestamp">Scan timestamp</param>
+    /// <param name="scan">LaserScan from ILidar</param>
+    /// <param name="sensorTransform">Transform from lidar frame to base_link (Rigid3f)</param>
+    /// <param name="angleMinDeg">Optional minimum angle in degrees (points below filtered out)</param>
+    /// <param name="angleMaxDeg">Optional maximum angle in degrees (points above filtered out)</param>
+    /// <returns>(BaseLink, SensorFrame, actualAngleMin, actualAngleMax, angleIncrement) for Cartographer and MCL</returns>
+    public static (TimedPointCloudData BaseLink, TimedPointCloudData SensorFrame, double ActualAngleMin, double ActualAngleMax, double AngleIncrement) ToTimedPointCloudDataBaseAndSensorFrame(
+        DateTime timestamp,
+        LaserScan scan,
+        Transform sensorTransform,
+        double? angleMinDeg = null,
+        double? angleMaxDeg = null)
+    {
+        var rotation = sensorTransform.Rotation;
+        var translation = sensorTransform.Translation;
+
+        double r00 = 1.0 - 2.0 * (rotation.Y * rotation.Y + rotation.Z * rotation.Z);
+        double r01 = 2.0 * (rotation.X * rotation.Y - rotation.Z * rotation.W);
+        double r02 = 2.0 * (rotation.X * rotation.Z + rotation.Y * rotation.W);
+        double r10 = 2.0 * (rotation.X * rotation.Y + rotation.Z * rotation.W);
+        double r11 = 1.0 - 2.0 * (rotation.X * rotation.X + rotation.Z * rotation.Z);
+        double r12 = 2.0 * (rotation.Y * rotation.Z - rotation.X * rotation.W);
+        double r20 = 2.0 * (rotation.X * rotation.Z - rotation.Y * rotation.W);
+        double r21 = 2.0 * (rotation.Y * rotation.Z + rotation.X * rotation.W);
+        double r22 = 1.0 - 2.0 * (rotation.X * rotation.X + rotation.Y * rotation.Y);
+
+        int estimatedCount = (int)(scan.Ranges.Length * 0.8);
+        var basePoints = new TimedPointCloud(estimatedCount);
+        var sensorPoints = new TimedPointCloud(estimatedCount);
+
+        var ranges = scan.Ranges;
+        var rangeMin = scan.RangeMin;
+        var rangeMax = scan.RangeMax;
+        var angleMin = scan.AngleMin;
+        var angleIncrement = scan.AngleIncrement;
+        var useTimeIncrement = scan.TimeIncrement > 0 && !double.IsNaN(scan.TimeIncrement) && !double.IsInfinity(scan.TimeIncrement);
+        var timeIncrement = scan.TimeIncrement;
+
+        double? angleMinRad = angleMinDeg.HasValue ? (angleMinDeg.Value * Math.PI / 180.0) : null;
+        double? angleMaxRad = angleMaxDeg.HasValue ? (angleMaxDeg.Value * Math.PI / 180.0) : null;
+
+        double angle = angleMin;
+        double? firstValidAngle = null;
+        double? lastValidAngle = null;
+
+        for (int i = 0; i < ranges.Length; i++)
+        {
+            var range = ranges[i];
+
+            if (double.IsNaN(range) || double.IsInfinity(range) ||
+                range < rangeMin || range > rangeMax)
+            {
+                angle += angleIncrement;
+                continue;
+            }
+
+            if (angleMinRad.HasValue && angle < angleMinRad.Value)
+            {
+                angle += angleIncrement;
+                continue;
+            }
+            if (angleMaxRad.HasValue && angle > angleMaxRad.Value)
+            {
+                angle += angleIncrement;
+                continue;
+            }
+
+            var cosAngle = Math.Cos(angle);
+            var sinAngle = Math.Sin(angle);
+            var x = range * cosAngle;
+            var y = range * sinAngle;
+            var z = 0.0;
+
+            double time = useTimeIncrement ? i * timeIncrement : 0.0;
+
+            sensorPoints.Add(new TimedRangefinderPoint(new Vector3(x, y, z), time));
+
+            var tx = r00 * x + r01 * y + r02 * z + translation.X;
+            var ty = r10 * x + r11 * y + r12 * z + translation.Y;
+            var tz = r20 * x + r21 * y + r22 * z + translation.Z;
+            basePoints.Add(new TimedRangefinderPoint(new Vector3(tx, ty, tz), time));
+
+            // Track actual angle range of filtered points
+            if (!firstValidAngle.HasValue)
+                firstValidAngle = angle;
+            lastValidAngle = angle;
+
+            angle += angleIncrement;
+        }
+
+        if (basePoints.Count == 0)
+            throw new InvalidOperationException("No valid points in scan after filtering");
+
+        long adjustedTimestamp = timestamp.Ticks;
+        if (useTimeIncrement)
+        {
+            double duration = basePoints[^1].Time;
+            adjustedTimestamp = timestamp.Ticks + (long)(duration * TimeSpan.TicksPerSecond);
+            for (int i = 0; i < basePoints.Count; i++)
+            {
+                var bp = basePoints[i];
+                var sp = sensorPoints[i];
+                basePoints[i] = new TimedRangefinderPoint(bp.Position, bp.Time - duration);
+                sensorPoints[i] = new TimedRangefinderPoint(sp.Position, sp.Time - duration);
+            }
+        }
+
+        // Calculate actual angle metadata from filtered points
+        double actualAngleMin = firstValidAngle ?? angleMin;
+        double actualAngleMax = lastValidAngle ?? angleMin;
+
+        return (
+            new TimedPointCloudData(adjustedTimestamp, translation, basePoints),
+            new TimedPointCloudData(adjustedTimestamp, Vector3.Zero, sensorPoints),
+            actualAngleMin,
+            actualAngleMax,
+            angleIncrement);
+    }
+
+    /// <summary>
+    /// Transform IMU data from primitive values to Cartographer ImuData.
+    /// Applies sensor transform (rotation only) to express in base_link frame.
+    /// </summary>
+    public static ImuData ToImuData(
+        Vector3 linearAcceleration,
+        Vector3 angularVelocity,
+        DateTime timestamp,
+        Transform sensorTransform)
+    {
+        return new ImuData(timestamp.Ticks, 
+            Vector3.Transform(linearAcceleration, sensorTransform.Rotation), 
+            Vector3.Transform(angularVelocity, sensorTransform.Rotation));
+    }
+
+    /// <summary>
+    /// Transform odometry Pose to Cartographer OdometryData.
+    /// </summary>
+    /// <param name="pose">Pose in base_link frame</param>
+    /// <param name="timestamp">Timestamp in ticks</param>
+    /// <returns>OdometryData for AddSensorData</returns>
+    public static OdometryData ToOdometryData(Pose pose, long timestamp)
+    {
+        const double QuaternionEpsilon = 1e-6;
+        if (pose.Orientation.LengthSquared() < QuaternionEpsilon)
+        {
+            throw new ArgumentException($"Quaternion is invalid (near zero length: {pose.Orientation.LengthSquared()})", nameof(pose));
+        }
+
+        pose.Orientation = pose.Orientation.Normalize();
+        var rigid3d = new Rigid3d(pose.Position, pose.Orientation);
+
+        return new OdometryData(timestamp, rigid3d);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/SensorPipeline.cs

@@ -0,0 +1,674 @@
+using CartographerSharp.Sensor;
+using Microsoft.Extensions.Options;
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.Shared.Geometry;
+using System.Collections.Concurrent;
+using System.Diagnostics;
+using RobotNet10.Shared.Numbers;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Sensor pipeline: subscribes to Lidar/IMU/Odometry, queues data, and forwards to caller via actions.
+/// Does not reference ITrajectoryBuilder or sample point cloud; CartographerService provides AddRangeData, AddImuData, AddOdometryData.
+/// </summary>
+internal sealed class SensorPipeline(
+    IDeviceProvider _deviceProvider,
+    IOdometryEstimator _odometryEstimator,
+    IOptions<CartographerConfiguration> configuration,
+    ILogger _logger,
+    Action<string, RangeDataPayload> _addRangeData,
+    Action<string, ImuData> _addImuData,
+    Action<string, OdometryData> _addOdometryData) : IDisposable
+{
+    #region Fields
+
+    private readonly CartographerConfiguration _config = configuration.Value;
+    private readonly Lock _lock = new();
+
+    private readonly List<ILidar> _subscribedLidars = [];
+    private IInertialMeasurementUnit? _subscribedImu;
+    private bool _odometrySubscribed;
+
+    private volatile bool _active;
+
+    private Dictionary<string, LidarSensorConfiguration> _lidarConfigs = [];
+    private Dictionary<string, Transform> _lidarTransforms = [];
+    private Transform? _imuTransform;
+
+    private const int NotProcessing = 0;
+    private const int Processing = 1;
+
+    // Per-device processing flags: each lidar has its own flag
+    private readonly ConcurrentDictionary<string, int> _isProcessingLidar = new();
+    private long _lastValidEnqueueTimeTicks; // Use Volatile.Read/Write for thread-safe access
+    private const int LidarRoundTimeoutMs = 300;
+
+    private volatile List<string> _lidarDeviceOrder = [];
+
+    private struct LidarScanQueueItem
+    {
+        public string DeviceId { get; set; }
+        public LidarScanDataEventArgs ScanData { get; set; }
+    }
+
+    private readonly ConcurrentQueue<LidarScanQueueItem> _scanDataQueue = new();
+
+    // Transformed lidar data queue item (output of transform thread, input to processing thread)
+    private struct TransformedLidarQueueItem
+    {
+        public string DeviceId { get; set; }
+        public RangeDataPayload Payload { get; set; }
+    }
+
+    private readonly ConcurrentQueue<TransformedLidarQueueItem> _transformedQueue = new();
+
+    private struct ImuDataQueueItem
+    {
+        public string DeviceId { get; set; }
+        public Vector3 LinearAcceleration { get; set; }
+        public Vector3 AngularVelocity { get; set; }
+        public DateTime Timestamp { get; set; }
+    }
+
+    private readonly ConcurrentQueue<ImuDataQueueItem> _imuDataQueue = new();
+
+    private Thread? _lidarTransformThread;
+    private volatile bool _lidarTransformThreadRunning;
+
+    private Thread? _lidarProcessingThread;
+    private volatile bool _lidarProcessingThreadRunning;
+
+    private Thread? _imuProcessingThread;
+    private volatile bool _imuThreadRunning;
+
+    private Thread? _odometryProcessingThread;
+    private volatile bool _odometryThreadRunning;
+
+    private volatile bool _disposed;
+
+    // ManualResetEventSlim for efficient blocking when queues are empty
+    private readonly ManualResetEventSlim _lidarDataAvailable = new(false);
+    private readonly ManualResetEventSlim _transformedDataAvailable = new(false);
+    private readonly ManualResetEventSlim _imuDataAvailable = new(false);
+
+    // Queue capacity limits (Fix 9)
+    private const int MaxScanQueueSize = 5;
+    private const int MaxImuQueueSize = 50;
+
+    #endregion
+
+    #region Initialization
+
+    public async Task InitializeAsync()
+    {
+        await DiscoverAndSubscribeLidarsAsync();
+        await SubscribeToImuAsync();
+        SubscribeToOdometry();
+    }
+
+    public void ResumeSubscriptions()
+    {
+        lock (_lock)
+        {
+            if (_active)
+            {
+                _logger.LogDebug("SensorPipeline.ResumeSubscriptions: Already active, skipping");
+                return;
+            }
+
+            _logger.LogInformation("SensorPipeline.ResumeSubscriptions: Resuming {LidarCount} lidars", _subscribedLidars.Count);
+
+            foreach (var lidar in _subscribedLidars)
+            {
+                lidar.ScanDataReceived += OnLidarScanDataReceived;
+            }
+            _subscribedImu?.ImuDataChanged += OnImuDataChanged;
+
+            if (_config.UseOdometry && _odometrySubscribed)
+                StartOdometryProcessingThread();
+            StartLidarProcessingThread();
+            if (_subscribedImu != null)
+                StartImuProcessingThread();
+
+            _active = true;
+            _logger.LogInformation("SensorPipeline.ResumeSubscriptions: Sensors resumed, active=true");
+        }
+    }
+
+    public void PauseSubscriptions()
+    {
+        lock (_lock)
+        {
+            if (!_active)
+            {
+                _logger.LogDebug("SensorPipeline.PauseSubscriptions: Already inactive, skipping");
+                return;
+            }
+
+            _logger.LogInformation("SensorPipeline.PauseSubscriptions: Pausing sensors");
+
+            foreach (var lidar in _subscribedLidars)
+            {
+                lidar.ScanDataReceived -= OnLidarScanDataReceived;
+            }
+
+            _subscribedImu?.ImuDataChanged -= OnImuDataChanged;
+
+            if (_odometrySubscribed)
+                StopOdometryProcessingThread();
+            StopLidarProcessingThread();
+            if (_subscribedImu != null)
+                StopImuProcessingThread();
+
+            _active = false;
+            _logger.LogInformation("SensorPipeline.PauseSubscriptions: Sensors paused, active=false");
+        }
+    }
+
+    #endregion
+
+    #region Subscriptions
+
+    private async Task DiscoverAndSubscribeLidarsAsync()
+    {
+        var allLidars = await _deviceProvider.GetDevicesByTypeAsync(DeviceType.Lidar);
+        var lidarList = allLidars.OfType<ILidar>().ToList();
+        if (lidarList.Count == 0)
+            throw new InvalidOperationException("No Lidars found in device provider");
+
+        var lidarConfigsDict = new Dictionary<string, LidarSensorConfiguration>();
+        var lidarTransformsDict = new Dictionary<string, Transform>();
+        foreach (var lidar in lidarList)
+        {
+            var deviceId = (lidar as DeviceBase)?.DeviceId;
+            if (string.IsNullOrEmpty(deviceId))
+                continue;
+            var sensorConfig = _config.Sensors.Lidars.FirstOrDefault(cfg => cfg.DeviceId == deviceId && cfg.Enabled);
+            if (sensorConfig != null)
+            {
+                lidarConfigsDict[deviceId] = sensorConfig;
+                lidarTransformsDict[deviceId] = sensorConfig.Transform;
+                _subscribedLidars.Add(lidar);
+            }
+            else
+            {
+                // FIXED: Skip lidars not configured in Cartographer.Sensors.Lidars
+                // This prevents unintended lidars (e.g., those used only for Detection)
+                // from being subscribed to CartographerSharp SLAM pipeline
+                _logger.LogDebug("SensorPipeline: Lidar {DeviceId} found but not enabled in Cartographer.Sensors.Lidars config, skipping", deviceId);
+            }
+        }
+        if (_subscribedLidars.Count == 0)
+            throw new InvalidOperationException("No Lidars to subscribe to");
+        _lidarConfigs = lidarConfigsDict;
+        _lidarTransforms = lidarTransformsDict;
+
+        // Initialize lidar device order and per-device processing flags
+        _lidarDeviceOrder = [.. _subscribedLidars
+            .Select(lidar => (lidar as DeviceBase)?.DeviceId)
+            .Where(id => !string.IsNullOrEmpty(id))
+            .Cast<string>()];
+
+        foreach (var lidarId in _lidarDeviceOrder)
+        {
+            _isProcessingLidar[lidarId] = NotProcessing;
+        }
+    }
+
+    private async Task SubscribeToImuAsync()
+    {
+        if (!_config.Sensors.Imu.Enabled || string.IsNullOrEmpty(_config.Sensors.Imu.DeviceId))
+            return;
+        _imuTransform = _config.Sensors.Imu.Transform;
+        var imu = await _deviceProvider.GetDeviceAsync(_config.Sensors.Imu.DeviceId);
+        if (imu is IInertialMeasurementUnit imuDevice)
+            _subscribedImu = imuDevice;
+        else
+            _logger.LogWarning("SensorPipeline: IMU device {DeviceId} not found or not an IMU", _config.Sensors.Imu.DeviceId);
+    }
+
+    private void SubscribeToOdometry()
+    {
+        if (!_config.UseOdometry || _odometrySubscribed)
+            return;
+        _odometrySubscribed = true;
+    }
+
+    #endregion
+
+    #region Lidar Processing
+
+    private void StartLidarProcessingThread()
+    {
+        if (_lidarTransformThreadRunning || _lidarProcessingThreadRunning)
+            return;
+
+        // Start transform thread
+        _lidarTransformThreadRunning = true;
+        _lidarTransformThread = new Thread(LidarTransformThreadProc) { Name = "LidarTransform", Priority = ThreadPriority.Highest, IsBackground = true };
+        _lidarTransformThread.Start();
+
+        // Start processing thread
+        _lidarProcessingThreadRunning = true;
+        _lidarProcessingThread = new Thread(LidarProcessingThreadProc) { Name = "LidarProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
+        _lidarProcessingThread.Start();
+    }
+
+    private void StopLidarProcessingThread()
+    {
+        // Stop transform thread
+        if (_lidarTransformThreadRunning)
+        {
+            _lidarTransformThreadRunning = false;
+            _lidarDataAvailable.Set(); // Wake thread to check flag
+            if (_lidarTransformThread != null)
+            {
+                _lidarTransformThread.Join(2000);
+                if (_lidarTransformThread.IsAlive)
+                    _logger.LogWarning("SensorPipeline: Lidar transform thread did not stop in time");
+                _lidarTransformThread = null;
+            }
+        }
+
+        // Stop processing thread
+        if (_lidarProcessingThreadRunning)
+        {
+            _lidarProcessingThreadRunning = false;
+            _transformedDataAvailable.Set(); // Wake thread to check flag
+            if (_lidarProcessingThread != null)
+            {
+                _lidarProcessingThread.Join(2000);
+                if (_lidarProcessingThread.IsAlive)
+                    _logger.LogWarning("SensorPipeline: Lidar processing thread did not stop in time");
+                _lidarProcessingThread = null;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Transform thread: dequeues raw scan data, transforms to RangeDataPayload, enqueues to transformed queue.
+    /// Does NOT check _isProcessingLidar flags - just processes everything in _scanDataQueue.
+    /// </summary>
+    private void LidarTransformThreadProc()
+    {
+        Thread.BeginThreadAffinity();
+        while (_lidarTransformThreadRunning)
+        {
+            _lidarDataAvailable.Wait(50); // timeout to check _lidarTransformThreadRunning
+            _lidarDataAvailable.Reset();
+
+            if (_scanDataQueue.IsEmpty)
+                continue;
+
+            while (_scanDataQueue.TryDequeue(out var queueItem))
+            {
+                if (_disposed || !_active ||
+                    !_lidarConfigs.TryGetValue(queueItem.DeviceId, out var lidarConfig) ||
+                    !_lidarTransforms.TryGetValue(queueItem.DeviceId, out var lidarTransform))
+                    continue;
+
+                try
+                {
+                    var (baseLink, sensorFrame, actualAngleMin, actualAngleMax, angleIncrement) = SensorDataTransformHelper.ToTimedPointCloudDataBaseAndSensorFrame(
+                        queueItem.ScanData.Timestamp, queueItem.ScanData.MeasurementData,
+                        lidarTransform, lidarConfig.AngleMin, lidarConfig.AngleMax);
+
+                    _transformedQueue.Enqueue(new TransformedLidarQueueItem
+                    {
+                        DeviceId = queueItem.DeviceId,
+                        Payload = new RangeDataPayload
+                        {
+                            BaseLink = baseLink,
+                            SensorFrame = sensorFrame,
+                            ActualAngleMin = actualAngleMin,
+                            ActualAngleMax = actualAngleMax,
+                            ActualAngleIncrement = angleIncrement
+                        }
+                    });
+                    _transformedDataAvailable.Set();
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogError(ex, "SensorPipeline: Error transforming Lidar from {DeviceId}", queueItem.DeviceId);
+                }
+            }
+        }
+        Thread.EndThreadAffinity();
+    }
+
+    /// <summary>
+    /// Processing thread: dequeues transformed data, calls _addRangeData, handles round tracking and timeout.
+    /// </summary>
+    private void LidarProcessingThreadProc()
+    {
+        Thread.BeginThreadAffinity();
+        var processedLidarsInRound = new HashSet<string>();
+        var expectedLidarCount = _lidarDeviceOrder.Count;
+
+        while (_lidarProcessingThreadRunning)
+        {
+            _transformedDataAvailable.Wait(50); // timeout to check _lidarProcessingThreadRunning
+            _transformedDataAvailable.Reset();
+
+            if (_transformedQueue.IsEmpty)
+            {
+                // Check timeout if we're in the middle of a round (at least one lidar processed)
+                if (processedLidarsInRound.Count > 0)
+                {
+                    var lastEnqueueTicks = Volatile.Read(ref _lastValidEnqueueTimeTicks);
+                    var elapsedMs = (DateTime.UtcNow.Ticks - lastEnqueueTicks) / TimeSpan.TicksPerMillisecond;
+                    if (elapsedMs >= LidarRoundTimeoutMs)
+                    {
+                        var missingLidars = _lidarDeviceOrder.Except(processedLidarsInRound).ToList();
+                        _logger.LogError(
+                            "SensorPipeline: Lidar round timeout after {ElapsedMs}ms. Processed {ProcessedCount}/{ExpectedCount} lidars. Processed: [{ProcessedList}]. Missing: [{MissingList}]",
+                            elapsedMs, processedLidarsInRound.Count, expectedLidarCount,
+                            string.Join(", ", processedLidarsInRound),
+                            string.Join(", ", missingLidars));
+                        throw new TimeoutException($"SensorPipeline: Lidar round timeout - processed {processedLidarsInRound.Count}/{expectedLidarCount} lidars, missing: [{string.Join(", ", missingLidars)}]");
+                    }
+                }
+                continue;
+            }
+
+            while (_transformedQueue.TryDequeue(out var queueItem))
+            {
+                if (_disposed || !_active)
+                    continue;
+
+                try
+                {
+                    _addRangeData(queueItem.DeviceId, queueItem.Payload);
+
+                    // Track processed lidar in this round
+                    processedLidarsInRound.Add(queueItem.DeviceId);
+
+                    // Check if all lidars have been processed in this round
+                    if (processedLidarsInRound.Count >= expectedLidarCount)
+                    {
+                        // Reset all per-device flags to allow new round
+                        foreach (var lidarId in _lidarDeviceOrder)
+                        {
+                            _isProcessingLidar[lidarId] = NotProcessing;
+                        }
+                        processedLidarsInRound.Clear();
+                    }
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogError(ex, "SensorPipeline: Error processing Lidar from {DeviceId}", queueItem.DeviceId);
+                }
+            }
+        }
+        Thread.EndThreadAffinity();
+    }
+
+    #endregion
+
+    #region IMU Processing
+
+    private void StartImuProcessingThread()
+    {
+        if (_imuThreadRunning)
+            return;
+        _imuThreadRunning = true;
+        _imuProcessingThread = new Thread(ImuProcessingThreadProc) { Name = "ImuProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
+        _imuProcessingThread.Start();
+    }
+
+    private void StopImuProcessingThread()
+    {
+        if (!_imuThreadRunning)
+            return;
+        _imuThreadRunning = false;
+        _imuDataAvailable.Set(); // Wake thread to check flag
+        if (_imuProcessingThread != null)
+        {
+            _imuProcessingThread.Join(2000);
+            if (_imuProcessingThread.IsAlive)
+                _logger.LogWarning("SensorPipeline: IMU processing thread did not stop in time");
+            _imuProcessingThread = null;
+        }
+    }
+
+    private void ImuProcessingThreadProc()
+    {
+        Thread.BeginThreadAffinity();
+        var imuUpdateIntervalMs = _config.Sensors.ImuUpdateIntervalMs;
+        var imuMinIntervalTicks = imuUpdateIntervalMs * TimeSpan.TicksPerMillisecond;
+        var lastProcessTimeTicks = 0L;
+        while (_imuThreadRunning)
+        {
+            _imuDataAvailable.Wait(50); // timeout to check _imuThreadRunning
+            _imuDataAvailable.Reset();
+
+            while (_imuDataQueue.TryDequeue(out var queueItem))
+            {
+                if (_disposed || !_active || _imuTransform == null)
+                    continue;
+
+                // Rate limiting: skip if too soon since last process (when interval > 0)
+                var currentTicks = queueItem.Timestamp.Ticks;
+                if (imuMinIntervalTicks > 0 && lastProcessTimeTicks > 0 &&
+                    currentTicks - lastProcessTimeTicks < imuMinIntervalTicks)
+                    continue;
+
+                try
+                {
+                    var imuData = SensorDataTransformHelper.ToImuData(
+                        queueItem.LinearAcceleration, queueItem.AngularVelocity, queueItem.Timestamp, _imuTransform.Value);
+                    _addImuData(queueItem.DeviceId, imuData);
+                    lastProcessTimeTicks = currentTicks;
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogError(ex, "SensorPipeline: Error processing IMU from {DeviceId}", queueItem.DeviceId);
+                }
+            }
+        }
+        Thread.EndThreadAffinity();
+    }
+
+    #endregion
+
+    #region Odometry Processing
+
+    private void StartOdometryProcessingThread()
+    {
+        if (_odometryThreadRunning)
+            return;
+        _odometryThreadRunning = true;
+        _odometryProcessingThread = new Thread(OdometryProcessingThreadProc) { Name = "OdometryProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
+        _odometryProcessingThread.Start();
+    }
+
+    private void StopOdometryProcessingThread()
+    {
+        if (!_odometryThreadRunning)
+            return;
+        _odometryThreadRunning = false;
+        if (_odometryProcessingThread != null)
+        {
+            _odometryProcessingThread.Join(1000);
+            if (_odometryProcessingThread.IsAlive)
+                _logger.LogWarning("SensorPipeline: Odometry processing thread did not stop in time");
+            _odometryProcessingThread = null;
+        }
+    }
+
+    private void OdometryProcessingThreadProc()
+    {
+        Thread.BeginThreadAffinity();
+        try
+        {
+            var spinWait = new SpinWait();
+            var stopwatch = Stopwatch.StartNew();
+            var odometryUpdateIntervalMs = _config.Sensors.OdometryUpdateIntervalMs;
+            var targetTicksPerInterval = odometryUpdateIntervalMs * Stopwatch.Frequency / 1000;
+            var lastProcessTimeTicks = stopwatch.ElapsedTicks;
+            while (_odometryThreadRunning)
+            {
+                var currentTicks = stopwatch.ElapsedTicks;
+                var remaining = targetTicksPerInterval - (currentTicks - lastProcessTimeTicks);
+                if (remaining > Stopwatch.Frequency / 100) // > 10ms remaining
+                    Thread.Sleep(1);
+                else
+                    spinWait.SpinOnce();
+
+                currentTicks = stopwatch.ElapsedTicks;
+                if (currentTicks - lastProcessTimeTicks >= targetTicksPerInterval)
+                {
+                    if (!_disposed && _active)
+                    {
+                        try
+                        {
+                            var currentPose = _odometryEstimator.CurrentPose;
+                            var currentTimeTicks = DateTime.UtcNow.Ticks;
+
+                            // Extract theta from quaternion
+                            var currentTheta = currentPose.Orientation.ToYawRadian();
+                            var currentX = currentPose.Position.X;
+                            var currentY = currentPose.Position.Y;
+
+                            var odometryData = SensorDataTransformHelper.ToOdometryData(currentPose, currentTimeTicks);
+                            _addOdometryData("odometry", odometryData);
+                        }
+                        catch (Exception ex)
+                        {
+                            _logger.LogError(ex, "SensorPipeline: Error processing odometry at {Ticks}", DateTime.UtcNow.Ticks);
+                        }
+                    }
+                    lastProcessTimeTicks = currentTicks;
+                }
+            }
+        }
+        finally
+        {
+            Thread.EndThreadAffinity();
+        }
+    }
+
+    #endregion
+
+    #region Event Handlers
+
+    private void OnLidarScanDataReceived(object? sender, LidarScanDataEventArgs e)
+    {
+        if (_disposed || !_active || sender is not DeviceBase device)
+            return;
+        if (_scanDataQueue.Count >= MaxScanQueueSize)
+            return; // Drop: processing can't keep up
+
+        var deviceId = device.DeviceId;
+
+        // Check if this lidar already has data in the current round
+        if (_isProcessingLidar.TryGetValue(deviceId, out var status) && status == Processing)
+            return; // Drop: this lidar already has data in current round
+
+        // Set flag to indicate this lidar has data in current round
+        _isProcessingLidar[deviceId] = Processing;
+
+        try
+        {
+            _scanDataQueue.Enqueue(new LidarScanQueueItem { DeviceId = deviceId, ScanData = e });
+            Volatile.Write(ref _lastValidEnqueueTimeTicks, DateTime.UtcNow.Ticks);
+            _lidarDataAvailable.Set();
+        }
+        catch (Exception ex)
+        {
+            // Reset flag on error so this lidar can try again
+            _isProcessingLidar[deviceId] = NotProcessing;
+            _logger.LogError(ex, "SensorPipeline: Error enqueueing Lidar from {DeviceId}", deviceId);
+        }
+    }
+
+    private void OnImuDataChanged(object? sender, ImuDataChangedEventArgs e)
+    {
+        if (sender is not DeviceBase device || _disposed || string.IsNullOrEmpty(device.DeviceId) || _imuTransform == null || !_active)
+            return;
+        if (_imuDataQueue.Count >= MaxImuQueueSize)
+            return; // Drop: processing can't keep up
+        try
+        {
+            _imuDataQueue.Enqueue(new ImuDataQueueItem
+            {
+                DeviceId = device.DeviceId,
+                LinearAcceleration = e.Acceleration.Accel.Linear,
+                AngularVelocity = e.AngularVelocity.Vector,
+                Timestamp = e.Timestamp
+            });
+            _imuDataAvailable.Set();
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "SensorPipeline: Error enqueueing IMU from {DeviceId}", device.DeviceId);
+        }
+    }
+
+    #endregion
+
+    #region Processing Status
+
+    /// <summary>
+    /// Wait for all queued data to be processed, with timeout.
+    /// Returns true if all queues are drained within the timeout.
+    /// </summary>
+    public bool WaitForProcessingComplete(int timeoutMs = 200)
+    {
+        var sw = Stopwatch.StartNew();
+        var spin = new SpinWait();
+        while (sw.ElapsedMilliseconds < timeoutMs)
+        {
+            // Check if all queues are empty and no lidar is being processed
+            var allLidarsNotProcessing = _isProcessingLidar.Values.All(status => status == NotProcessing);
+            if (_scanDataQueue.IsEmpty && _transformedQueue.IsEmpty && _imuDataQueue.IsEmpty && allLidarsNotProcessing)
+                return true;
+            spin.SpinOnce();
+        }
+        return false;
+    }
+
+    #endregion
+
+    #region IDisposable
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+        _disposed = true;
+        try
+        {
+            PauseSubscriptions();
+            foreach (var lidar in _subscribedLidars)
+                lidar.ScanDataReceived -= OnLidarScanDataReceived;
+            _subscribedLidars.Clear();
+            _subscribedImu?.ImuDataChanged -= OnImuDataChanged;
+            _subscribedImu = null;
+            if (_odometrySubscribed)
+            {
+                StopOdometryProcessingThread();
+                _odometrySubscribed = false;
+            }
+            StopLidarProcessingThread();
+            StopImuProcessingThread();
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "SensorPipeline: Error during Dispose");
+        }
+        finally
+        {
+            _lidarDataAvailable.Dispose();
+            _transformedDataAvailable.Dispose();
+            _imuDataAvailable.Dispose();
+            GC.SuppressFinalize(this);
+        }
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/SlamResultProcessor.cs

@@ -0,0 +1,153 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Processes SLAM results from sensor data
+/// Handles both ScanMapping and Localization modes with appropriate pose/covariance updates
+/// </summary>
+public class SlamResultProcessor(ILogger<SlamResultProcessor> _logger)
+{
+    private readonly Lock _lock = new();
+
+    private int _totalSubmapsCreated;
+    private int _scanMappingTrajectoryNodeCount;
+    private Matrix3x3? _poseCovariance;
+    private int _constraintCount;
+    private double _averageConstraintQuality;
+
+    public int TotalSubmapsCreated
+    {
+        get { lock (_lock) { return _totalSubmapsCreated; } }
+    }
+
+    public int TrajectoryNodeCount
+    {
+        get { lock (_lock) { return _scanMappingTrajectoryNodeCount; } }
+    }
+
+    public Matrix3x3? PoseCovariance
+    {
+        get { lock (_lock) { return _poseCovariance; } }
+    }
+
+    public int ConstraintCount
+    {
+        get { lock (_lock) { return _constraintCount; } }
+    }
+
+    public double AverageConstraintQuality
+    {
+        get { lock (_lock) { return _averageConstraintQuality; } }
+    }
+
+    /// <summary>
+    /// Process SLAM result for ScanMapping mode
+    /// Tracks submap creation and trajectory nodes
+    /// </summary>
+    public bool ProcessScanMappingResult(
+        ITrajectoryBuilder.MatchingResult result,
+        SLAMState currentState,
+        out IReadOnlyList<CartographerSharp.Mapping.Submap>? newSubmaps)
+    {
+        newSubmaps = null;
+
+        if (result.InsertionResult?.InsertionSubmaps == null || result.InsertionResult.Value.InsertionSubmaps.Count == 0)
+            return false;
+
+        var insertionSubmaps = result.InsertionResult.Value.InsertionSubmaps;
+        bool shouldFireSubmapsUpdated = false;
+
+        lock (_lock)
+        {
+            _totalSubmapsCreated += insertionSubmaps.Count;
+
+            if (currentState == SLAMState.ScanMapping)
+            {
+                shouldFireSubmapsUpdated = true;
+                newSubmaps = insertionSubmaps;
+
+                // Track nodes
+                if (result.InsertionResult.Value.ConstantData != null)
+                {
+                    _scanMappingTrajectoryNodeCount++;
+                }
+            }
+        }
+
+        if (shouldFireSubmapsUpdated)
+        {
+            _logger.LogDebug("SlamResultProcessor: {Count} new submaps created (Total: {Total})",
+                insertionSubmaps.Count, _totalSubmapsCreated);
+        }
+
+        return shouldFireSubmapsUpdated;
+    }
+
+    /// <summary>
+    /// Process SLAM result for Localization mode
+    /// Updates covariance and constraint tracking
+    /// </summary>
+    public void ProcessLocalizationResult(
+        Pose currentPose,
+        Matrix3x3? cachedCovariance,
+        int cachedConstraintCount,
+        double cachedAverageQuality)
+    {
+        lock (_lock)
+        {
+            _poseCovariance = cachedCovariance;
+            _constraintCount = cachedConstraintCount;
+            _averageConstraintQuality = cachedAverageQuality;
+        }
+    }
+
+    /// <summary>
+    /// Update covariance from constraint calculation
+    /// </summary>
+    public void UpdateCovariance(Matrix3x3? covariance, int constraintCount, double averageQuality)
+    {
+        lock (_lock)
+        {
+            _poseCovariance = covariance;
+            _constraintCount = constraintCount;
+            _averageConstraintQuality = averageQuality;
+        }
+    }
+
+    /// <summary>
+    /// Reset counters for new ScanMapping session
+    /// </summary>
+    public void ResetForNewSession()
+    {
+        lock (_lock)
+        {
+            _totalSubmapsCreated = 0;
+            _scanMappingTrajectoryNodeCount = 0;
+            _poseCovariance = null;
+            _constraintCount = 0;
+            _averageConstraintQuality = 0.0;
+        }
+    }
+
+    /// <summary>
+    /// Clear localization data
+    /// </summary>
+    public void ClearLocalizationData()
+    {
+        lock (_lock)
+        {
+            _poseCovariance = null;
+            _constraintCount = 0;
+            _averageConstraintQuality = 0.0;
+        }
+    }
+
+    public void Dispose()
+    {
+        // Nothing to dispose currently
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/TrajectoryConstraintCache.cs

@@ -0,0 +1,99 @@
+using CartographerSharp.Mapping;
+using Microsoft.Extensions.Logging;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Cache for pose graph constraints of a single trajectory.
+/// Used by CartographerService to avoid repeatedly querying constraints when calculating covariance.
+/// </summary>
+/// <remarks>
+/// Creates a constraint cache with the given refresh interval.
+/// </remarks>
+public sealed class TrajectoryConstraintCache(TimeSpan _updateInterval)
+{
+    private readonly Lock _lock = new();
+    private List<IPoseGraph.Constraint>? _cachedConstraints;
+    private DateTime _lastUpdate = DateTime.MinValue;
+    private IMapBuilder? _lastMapBuilder;
+    private int _lastTrajectoryId = -1;
+
+    /// <summary>
+    /// Gets constraints for the given trajectory, refreshing from the pose graph when the cache is stale
+    /// or when map builder / trajectory id change.
+    /// </summary>
+    /// <param name="mapBuilder">Current map builder (may be null)</param>
+    /// <param name="trajectoryId">Trajectory id to filter constraints</param>
+    /// <param name="logger">Optional logger for warnings</param>
+    /// <returns>Cached or freshly fetched constraints, or null if mapBuilder is null or fetch failed</returns>
+    public List<IPoseGraph.Constraint>? GetOrUpdate(
+        IMapBuilder? mapBuilder,
+        int trajectoryId,
+        ILogger? logger = null)
+    {
+        var now = DateTime.UtcNow;
+        bool needsUpdate = false;
+
+        lock (_lock)
+        {
+            if (mapBuilder != _lastMapBuilder || trajectoryId != _lastTrajectoryId)
+            {
+                needsUpdate = true;
+            }
+            else if (_cachedConstraints == null || (now - _lastUpdate) > _updateInterval)
+            {
+                needsUpdate = true;
+            }
+        }
+
+        if (!needsUpdate)
+        {
+            lock (_lock)
+            {
+                return _cachedConstraints;
+            }
+        }
+
+        if (mapBuilder == null)
+        {
+            return null;
+        }
+
+        try
+        {
+            var constraints = mapBuilder.PoseGraph.Constraints().Where(c => c.NodeId.TrajectoryId == trajectoryId).ToList();
+
+            lock (_lock)
+            {
+                _cachedConstraints = constraints;
+                _lastUpdate = now;
+                _lastMapBuilder = mapBuilder;
+                _lastTrajectoryId = trajectoryId;
+            }
+
+            return constraints;
+        }
+        catch (Exception ex)
+        {
+            logger?.LogWarning(ex, "TrajectoryConstraintCache: Failed to update constraint cache");
+            lock (_lock)
+            {
+                return _cachedConstraints;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Clears the cache (e.g. when switching map or trajectory).
+    /// </summary>
+    public void Invalidate()
+    {
+        lock (_lock)
+        {
+            _cachedConstraints = null;
+            _lastUpdate = DateTime.MinValue;
+            _lastMapBuilder = null;
+            _lastTrajectoryId = -1;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/TrajectoryHelper.cs

@@ -0,0 +1,48 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for trajectory-related operations
+/// </summary>
+public static class TrajectoryHelper
+{
+    
+
+    /// <summary>
+    /// Build sensor IDs from configuration
+    /// </summary>
+    /// <param name="config">Cartographer configuration</param>
+    /// <returns>HashSet of sensor IDs</returns>
+    public static HashSet<ITrajectoryBuilder.SensorId> BuildSensorIds(CartographerConfiguration config)
+    {
+        var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>();
+
+        // Add Lidar sensor IDs
+        foreach (var lidarConfig in config.Sensors.Lidars.Where(l => l.Enabled))
+        {
+            sensorIds.Add(new ITrajectoryBuilder.SensorId(
+                ITrajectoryBuilder.SensorId.SensorType.Range,
+                lidarConfig.DeviceId));
+        }
+
+        // Add IMU sensor ID
+        if (config.Sensors.Imu.Enabled && !string.IsNullOrEmpty(config.Sensors.Imu.DeviceId))
+        {
+            sensorIds.Add(new ITrajectoryBuilder.SensorId(
+                ITrajectoryBuilder.SensorId.SensorType.Imu,
+                config.Sensors.Imu.DeviceId));
+        }
+
+        // Add Odometry sensor ID
+        if (config.UseOdometry)
+        {
+            sensorIds.Add(new ITrajectoryBuilder.SensorId(
+                ITrajectoryBuilder.SensorId.SensorType.Odometry,
+                "odometry"));
+        }
+
+        return sensorIds;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/WallAlignmentHelper.cs

@@ -0,0 +1,359 @@
+using Microsoft.Extensions.Logging;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for detecting walls from point cloud data and calculating alignment angles
+/// </summary>
+public static class WallAlignmentHelper
+{
+    private const double MIN_WALL_LENGTH = 1.0; // Minimum wall length in meters
+    private const double RANSAC_INLIER_THRESHOLD = 0.05; // 5cm tolerance for RANSAC
+    private const int RANSAC_ITERATIONS = 100;
+    private const int MIN_INLIERS = 20; // Minimum points to consider a valid wall
+
+    #region Internal Types
+
+    /// <summary>
+    /// Line representation: ax + by + c = 0 (normalized: a^2 + b^2 = 1)
+    /// </summary>
+    private struct Line
+    {
+        public double A { get; set; }
+        public double B { get; set; }
+        public double C { get; set; }
+
+        /// <summary>
+        /// Get angle of line relative to X-axis in radians
+        /// Line equation: ax + by + c = 0
+        /// Direction vector: (-b, a)
+        /// Angle = atan2(a, -b)
+        /// </summary>
+        public double GetAngle() => Math.Atan2(A, -B);
+
+        /// <summary>
+        /// Get perpendicular distance from a point to this line
+        /// </summary>
+        public double DistanceToPoint(Vector3 point)
+        {
+            return Math.Abs(A * point.X + B * point.Y + C);
+        }
+    }
+
+    /// <summary>
+    /// Detected wall information
+    /// </summary>
+    private struct Wall
+    {
+        public Line Line { get; set; }
+        public int InlierCount { get; set; }
+        public double Length { get; set; }
+        public Vector3 StartPoint { get; set; }
+        public Vector3 EndPoint { get; set; }
+    }
+
+    #endregion
+
+    #region Public API
+
+    /// <summary>
+    /// Detect the longest wall from a collection of 2D points and calculate the minimum
+    /// rotation angle needed to align it with either the X or Y axis
+    /// </summary>
+    /// <param name="points">Point cloud in base_link frame</param>
+    /// <param name="logger">Logger for debug information</param>
+    /// <returns>
+    /// Compensation angle in radians, or null if no valid wall found.
+    /// This angle should be applied to robot orientation to make the wall parallel to X or Y axis.
+    /// </returns>
+    public static double? DetectWallAndCalculateCompensation(
+        IReadOnlyList<Vector3> points,
+        ILogger logger)
+    {
+        if (points == null || points.Count < MIN_INLIERS)
+        {
+            logger.LogWarning("WallAlignment: Insufficient points for wall detection (count: {Count})", points?.Count ?? 0);
+            return null;
+        }
+
+        logger.LogInformation("WallAlignment: Processing {Count} points for wall detection", points.Count);
+
+        // Detect all walls using RANSAC
+        var walls = DetectWallsRANSAC(points, logger);
+
+        if (walls.Count == 0)
+        {
+            logger.LogWarning("WallAlignment: No walls detected");
+            return null;
+        }
+
+        // Find the longest wall
+        var longestWall = walls.OrderByDescending(w => w.Length).First();
+
+        logger.LogInformation(
+            "WallAlignment: Longest wall found - Length: {Length:F2}m, Inliers: {Inliers}, Angle: {Angle:F2}rad ({AngleDeg:F2}°)",
+            longestWall.Length,
+            longestWall.InlierCount,
+            longestWall.Line.GetAngle(),
+            longestWall.Line.GetAngle() * 180.0 / Math.PI);
+
+        // Calculate compensation angle
+        var wallAngle = longestWall.Line.GetAngle();
+        var compensationAngle = CalculateMinimumRotationToAxis(wallAngle);
+
+        logger.LogInformation(
+            "WallAlignment: Compensation angle: {Angle:F2}rad ({AngleDeg:F2}°)",
+            compensationAngle,
+            compensationAngle * 180.0 / Math.PI);
+
+        return compensationAngle;
+    }
+
+    #endregion
+
+    #region RANSAC Wall Detection
+
+    /// <summary>
+    /// Detect walls using RANSAC line fitting algorithm
+    /// </summary>
+    private static List<Wall> DetectWallsRANSAC(IReadOnlyList<Vector3> points, ILogger logger)
+    {
+        var walls = new List<Wall>();
+        var unusedPoints = points.ToList();
+        var random = new Random(DateTime.Now.Millisecond);
+
+        // Iteratively find walls until not enough points remain
+        while (unusedPoints.Count >= MIN_INLIERS)
+        {
+            Line bestLine = default;
+            int bestInlierCount = 0;
+            List<Vector3> bestInliers = [];
+
+            // RANSAC iterations
+            for (int iter = 0; iter < RANSAC_ITERATIONS; iter++)
+            {
+                // Randomly select 2 points
+                if (unusedPoints.Count < 2) break;
+
+                var idx1 = random.Next(unusedPoints.Count);
+                var idx2 = random.Next(unusedPoints.Count);
+
+                if (idx1 == idx2) continue;
+
+                var p1 = unusedPoints[idx1];
+                var p2 = unusedPoints[idx2];
+
+                // Skip if points are too close
+                var dx = p2.X - p1.X;
+                var dy = p2.Y - p1.Y;
+                var dist = Math.Sqrt(dx * dx + dy * dy);
+                if (dist < 0.1) continue; // Minimum 10cm distance
+
+                // Fit line through these 2 points
+                var line = FitLineThroughPoints(p1, p2);
+
+                // Count inliers
+                var inliers = new List<Vector3>();
+                foreach (var point in unusedPoints)
+                {
+                    if (line.DistanceToPoint(point) < RANSAC_INLIER_THRESHOLD)
+                    {
+                        inliers.Add(point);
+                    }
+                }
+
+                // Update best model
+                if (inliers.Count > bestInlierCount)
+                {
+                    bestInlierCount = inliers.Count;
+                    bestInliers = inliers;
+                    bestLine = line;
+                }
+            }
+
+            // Check if we found a valid wall
+            if (bestInlierCount < MIN_INLIERS)
+            {
+                break; // No more walls to find
+            }
+
+            // Calculate wall length (distance between furthest inlier points)
+            var (startPoint, endPoint, length) = CalculateWallExtent(bestInliers);
+
+            if (length < MIN_WALL_LENGTH)
+            {
+                // Wall too short, remove inliers and continue
+                foreach (var inlier in bestInliers)
+                {
+                    unusedPoints.Remove(inlier);
+                }
+                continue;
+            }
+
+            // Valid wall found
+            walls.Add(new Wall
+            {
+                Line = bestLine,
+                InlierCount = bestInlierCount,
+                Length = length,
+                StartPoint = startPoint,
+                EndPoint = endPoint
+            });
+
+            logger.LogDebug(
+                "WallAlignment: Wall detected - Length: {Length:F2}m, Inliers: {Inliers}",
+                length, bestInlierCount);
+
+            // Remove inliers from unused points
+            foreach (var inlier in bestInliers)
+            {
+                unusedPoints.Remove(inlier);
+            }
+        }
+
+        return walls;
+    }
+
+    /// <summary>
+    /// Fit a line through two points using line equation: ax + by + c = 0
+    /// where a^2 + b^2 = 1 (normalized)
+    /// </summary>
+    private static Line FitLineThroughPoints(Vector3 p1, Vector3 p2)
+    {
+        var dx = p2.X - p1.X;
+        var dy = p2.Y - p1.Y;
+        var length = Math.Sqrt(dx * dx + dy * dy);
+
+        if (length < 1e-6)
+        {
+            // Points are identical, return arbitrary line
+            return new Line { A = 1, B = 0, C = -p1.X };
+        }
+
+        // Normal to line: (dy, -dx) / length (perpendicular to direction vector)
+        var a = dy / length;
+        var b = -dx / length;
+        var c = -(a * p1.X + b * p1.Y);
+
+        return new Line { A = a, B = b, C = c };
+    }
+
+    /// <summary>
+    /// Calculate wall extent (start point, end point, and length)
+    /// </summary>
+    private static (Vector3 StartPoint, Vector3 EndPoint, double Length) CalculateWallExtent(
+        List<Vector3> inliers)
+    {
+        if (inliers.Count < 2)
+        {
+            return (Vector3.Zero, Vector3.Zero, 0);
+        }
+
+        // Find two points that are furthest apart
+        var maxDist = 0.0;
+        var startIdx = 0;
+        var endIdx = 0;
+
+        for (int i = 0; i < inliers.Count; i++)
+        {
+            for (int j = i + 1; j < inliers.Count; j++)
+            {
+                var dx = inliers[j].X - inliers[i].X;
+                var dy = inliers[j].Y - inliers[i].Y;
+                var dist = Math.Sqrt(dx * dx + dy * dy);
+
+                if (dist > maxDist)
+                {
+                    maxDist = dist;
+                    startIdx = i;
+                    endIdx = j;
+                }
+            }
+        }
+
+        return (inliers[startIdx], inliers[endIdx], maxDist);
+    }
+
+    #endregion
+
+    #region Angle Compensation
+
+    /// <summary>
+    /// Calculate minimum rotation angle to align the wall with X or Y axis
+    /// </summary>
+    /// <param name="wallAngle">Wall angle in radians (relative to X-axis)</param>
+    /// <returns>Compensation angle in radians</returns>
+    private static double CalculateMinimumRotationToAxis(double wallAngle)
+    {
+        // Normalize angle to [-pi, pi]
+        while (wallAngle > Math.PI) wallAngle -= 2 * Math.PI;
+        while (wallAngle < -Math.PI) wallAngle += 2 * Math.PI;
+
+        // Calculate rotation needed for each axis
+        // For X-axis: wall should be at 0° or ±180°
+        // For Y-axis: wall should be at ±90°
+
+        var rotations = new[]
+        {
+            -wallAngle,                    // Align with X-axis (0°)
+            Math.PI - wallAngle,           // Align with X-axis (180°)
+            -Math.PI - wallAngle,          // Align with X-axis (-180°)
+            Math.PI / 2 - wallAngle,       // Align with Y-axis (90°)
+            -Math.PI / 2 - wallAngle       // Align with Y-axis (-90°)
+        };
+
+        // Find the smallest absolute rotation
+        var minRotation = rotations.OrderBy(Math.Abs).First();
+
+        // Normalize result to [-pi, pi]
+        while (minRotation > Math.PI) minRotation -= 2 * Math.PI;
+        while (minRotation < -Math.PI) minRotation += 2 * Math.PI;
+
+        return minRotation;
+    }
+
+    #endregion
+
+    #region Quaternion Helpers
+
+    /// <summary>
+    /// Create a quaternion from a yaw angle (rotation around Z-axis)
+    /// </summary>
+    public static Quaternion CreateQuaternionFromYaw(double yawRadians)
+    {
+        // Quaternion for rotation around Z-axis:
+        // q = [0, 0, sin(yaw/2), cos(yaw/2)]
+        var halfYaw = yawRadians / 2.0;
+        return new Quaternion(
+            x: 0,
+            y: 0,
+            z: Math.Sin(halfYaw),
+            w: Math.Cos(halfYaw)
+        );
+    }
+
+    /// <summary>
+    /// Extract yaw angle from a quaternion
+    /// </summary>
+    public static double GetYawFromQuaternion(Quaternion q)
+    {
+        // Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
+        return Math.Atan2(
+            2.0 * (q.W * q.Z + q.X * q.Y),
+            1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z)
+        );
+    }
+
+    /// <summary>
+    /// Combine current robot yaw with compensation angle
+    /// </summary>
+    public static Quaternion ApplyCompensation(Quaternion currentOrientation, double compensationAngle)
+    {
+        var currentYaw = GetYawFromQuaternion(currentOrientation);
+        var newYaw = currentYaw + compensationAngle;
+        return CreateQuaternionFromYaw(newYaw);
+    }
+
+    #endregion
+}