Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerConfiguration.cs

@@ -0,0 +1,48 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Root configuration for Cartographer integration
+/// </summary>
+public class CartographerConfiguration
+{
+    /// <summary>
+    /// Enable or disable Cartographer service
+    /// </summary>
+    public bool Enable { get; set; } = true;
+
+    /// <summary>
+    /// Use odometry data if available
+    /// </summary>
+    public bool UseOdometry { get; set; } = true;
+
+    /// <summary>
+    /// Sensor configuration
+    /// </summary>
+    public SensorConfiguration Sensors { get; set; } = new();
+
+    /// <summary>
+    /// Trajectory builder configuration
+    /// </summary>
+    public TrajectoryBuilderConfiguration TrajectoryBuilder { get; set; } = new();
+
+    /// <summary>
+    /// Map builder configuration
+    /// </summary>
+    public MapBuilderConfiguration MapBuilder { get; set; } = new();
+
+    /// <summary>
+    /// Map storage configuration
+    /// </summary>
+    public MapStorageConfiguration MapStorage { get; set; } = new();
+
+    /// <summary>
+    /// MCL (Monte Carlo Localization) configuration. When Enabled, SetInitialPoseAsync uses MCL to refine pose before starting Cartographer trajectory (xloc flow).
+    /// </summary>
+    public MclConfiguration Mcl { get; set; } = new();
+
+    /// <summary>
+    /// Occupancy grid generation configuration.
+    /// Controls probability thresholds, output mode, wall thinning, and ambiguous cell handling.
+    /// </summary>
+    public OccupancyGridConfiguration OccupancyGrid { get; set; } = new();
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerExtensions.cs

@@ -0,0 +1,26 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public static class CartographerExtensions
+{
+    extension(IServiceCollection services)
+    {
+        public IServiceCollection AddCartographer(IConfigurationSection configuration)
+        {
+            // Cartographer / Localization services
+            services.Configure<CartographerConfiguration>(configuration);
+
+            // Core Cartographer service (singleton - manages state machine, MapBuilder, sensor pipeline, localization, and occupancy grid)
+            // Also implements IHostedService to start automatically
+            services.AddSingleton<CartographerService>();
+            services.AddSingleton<ISLAMService>(sp => sp.GetRequiredService<CartographerService>());
+            services.AddHostedService(sp => sp.GetRequiredService<CartographerService>());
+
+            return services;
+        }
+    }
+
+    
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.ApiMethods.cs

@@ -0,0 +1,166 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public partial class CartographerService
+{
+    #region ISLAMService -- Localization
+
+    public void StartLocalization(string mapName, Pose? initialPose = null)
+    {
+        // Validate state
+        var currentState = State;
+        if (currentState != SLAMState.Ready)
+        {
+            throw new InvalidOperationException($"Cannot start localization from state: {currentState}");
+        }
+
+        // Dispose old MapBuilder if exists
+        lock (_lock)
+        {
+            if (_mapBuilder != null)
+            {
+                _logger.LogInformation("CartographerService: Disposing existing MapBuilder before starting localization");
+                (_mapBuilder as IDisposable)?.Dispose();
+                _mapBuilder = null;
+                _trajectoryBuilder = null;
+                _trajectoryId = -1;
+                _currentMapName = null;
+            }
+        }
+
+        // Store pending data for InitializingLocalizing state
+        lock (_localizationLock)
+        {
+            _pendingLocalizationMapName = mapName;
+            _pendingLocalizationInitialPose = initialPose;
+        }
+
+        // Fire StartLocalization trigger
+        // Actual work will be done in InitializingLocalizing.ExecuteOnEntry
+        FireStateMachine(CartographerTrigger.StartLocalization);
+
+        _logger.LogInformation("CartographerService: StartLocalization trigger fired for map: {MapName}", mapName);
+    }
+
+    public void SetInitialPose(Pose pose)
+    {
+        // Validate state and get current map name
+        string? currentMapName;
+        lock (_lock)
+        {
+            if (_currentState != SLAMState.Localizing && _currentState != SLAMState.Relocalizing)
+            {
+                throw new InvalidOperationException(
+                    $"SetInitialPose is only allowed during Localizing or InitializingLocalizing. Current state: {_currentState}");
+            }
+            if (_mapBuilder == null)
+            {
+                if (_config.Enable)
+                {
+                    FireStateMachine(CartographerTrigger.ErrorOccurred);
+                    throw new InvalidOperationException(
+                        "Localization not started. Call StartLocalization first.");
+                }
+            }
+            currentMapName = _currentMapName;
+        }
+
+        if (string.IsNullOrEmpty(currentMapName))
+        {
+            throw new InvalidOperationException("No map name available for SetInitialPose");
+        }
+
+        // Store pending data for InitializingLocalizing state
+        lock (_localizationLock)
+        {
+            _pendingLocalizationMapName = currentMapName; // Reuse current map
+            _pendingLocalizationInitialPose = pose;
+            _isSetInitialPoseFlow = true; // Mark as SetInitialPose flow
+        }
+
+        _logger.LogInformation("CartographerService: SetInitialPose called with pose [{X}, {Y}, {Z}], transitioning to InitializingLocalizing",
+            pose.Position.X, pose.Position.Y, pose.Position.Z);
+
+        // Fire StartLocalization trigger to transition to InitializingLocalizing
+        // The InitializingLocalizing state handler will process the restart with new pose
+        FireStateMachine(CartographerTrigger.StartLocalization);
+    }
+
+    public void StopLocalization() => FireStateMachine(CartographerTrigger.StopLocalization);
+
+    #endregion
+
+    #region ISLAMService -- ScanMapping
+
+    public void StartScanMapping(string mapName)
+    {
+        // Validate that SLAM is enabled
+        if (!_config.Enable) return;
+
+        lock (_lock)
+        {
+            _currentMapName = mapName;
+        }
+        FireStateMachine(CartographerTrigger.StartScanMapping);
+    }
+
+    public void SaveScanMap()
+    {
+        // Validate that SLAM is enabled
+        if (!_config.Enable) return;
+
+        FireStateMachine(CartographerTrigger.SaveMap);
+    }
+
+    #endregion
+
+    #region ISLAMService -- Query
+
+    public List<(int NodeId, Pose Pose)>? GetTrajectoryNodes()
+    {
+        var trajectoryNodePoses = _mapBuilder?.PoseGraph.GetTrajectoryNodePoses() ?? null;
+
+        if (trajectoryNodePoses == null || trajectoryNodePoses.IsEmpty)
+        {
+            return [];
+        }
+
+        var result = new List<(int NodeId, Pose Pose)>();
+        var trajectoryId = _trajectoryId;
+
+        foreach (var kvp in trajectoryNodePoses)
+        {
+            if (kvp.Id.TrajectoryId == trajectoryId)
+            {
+                var globalPose = kvp.Data.GlobalPose;
+                var pose = Helpers.PoseConverter.ToPose(globalPose);
+                result.Add((kvp.Id.NodeIndex, pose));
+            }
+        }
+        return result;
+    }
+
+    public IReadOnlyList<Vector3> GetAggregatedSamplePointCloud()
+    {
+        lock (_pointCloudLock)
+        {
+            var count = _completedAccumulatedSamplePointClouds.Count;
+            return [.. _completedAccumulatedSamplePointClouds];
+        }
+    }
+
+    public OccupancyGrid? GetOccupancyGrid() => _occupancyGridManager.OccupancyGrid;
+
+    public OccupancyGrid? GetOccupancyGrid(DateTime since) => _occupancyGridManager.GetGrid(since);
+
+    public DateTime LastUpdatedOccupancyGrid => _occupancyGridManager.LastUpdated;
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.Lifecycle.cs

@@ -0,0 +1,307 @@
+using System.Globalization;
+using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public partial class CartographerService
+{
+    #region IHostedService
+
+    public async Task StartAsync(CancellationToken cancellationToken)
+    {
+        try
+        {
+            // Start trigger thread first (handles deferred state machine triggers from sensor processing)
+            StartTriggerThread();
+
+            // Start state machine
+            lock (_stateMachineLock) { _stateMachine.Start(); }
+
+            // Fire Start trigger to begin initialization
+            FireStateMachine(CartographerTrigger.Start);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Failed to start");
+            throw;
+        }
+    }
+
+    public async Task StopAsync(CancellationToken cancellationToken)
+    {
+        try
+        {
+            // Mark as disposed to prevent new operations
+            _disposed = true;
+
+            // Pause sensor subscriptions to stop receiving new data
+            _sensorPipeline?.PauseSubscriptions();
+
+            // Stop background threads
+            StopPoseThread();
+            StopTriggerThread();
+
+            // Give a small delay to ensure any in-flight sensor data is processed
+            // (PoseGraph2D work queue will handle processing)
+            await Task.Delay(100, CancellationToken.None);
+            _sensorPipeline?.Dispose();
+
+            // Stop MCL if running
+            _mclProcessor?.Stop();
+
+            // Finish trajectory if active, then dispose MapBuilder (stops background threads before state machine shutdown)
+            _mapBuilder?.FinishTrajectoryAndDispose(_trajectoryId);
+            _trajectoryId = -1;
+            _trajectoryBuilder = null;
+            _mapBuilder = null;
+
+            FireStateMachine(CartographerTrigger.Shutdown);
+
+            // Stop state machine
+            try
+            {
+                lock (_stateMachineLock)
+                {
+                    if (_stateMachine.IsRunning)
+                    {
+                        _stateMachine.Stop();
+                    }
+                }
+            }
+            catch (Exception ex)
+            {
+                _logger.LogWarning(ex, "CartographerService: Error stopping state machine");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Error during stop");
+            throw;
+        }
+    }
+
+    #endregion
+
+    #region State Persistence
+
+    /// <summary>
+    /// Ensures the localizing state directory exists
+    /// </summary>
+    private string EnsureLocalizingStateDirectory()
+    {
+        var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
+        if (!Directory.Exists(directory))
+        {
+            Directory.CreateDirectory(directory);
+            _logger.LogInformation("CartographerService: Created localizing state directory: {Directory}", directory);
+        }
+        return directory;
+    }
+
+    /// <summary>
+    /// Saves the current map name to CurrentMap.name file
+    /// </summary>
+    private void SaveCurrentMapName(string mapName)
+    {
+        try
+        {
+            var directory = EnsureLocalizingStateDirectory();
+            var filePath = Path.Combine(directory, "CurrentMap.name");
+            File.WriteAllText(filePath, mapName);
+            _logger.LogDebug("CartographerService: Saved current map name '{MapName}' to {FilePath}", mapName, filePath);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogWarning(ex, "CartographerService: Failed to save current map name");
+        }
+    }
+
+    /// <summary>
+    /// Loads the current map name from CurrentMap.name file
+    /// </summary>
+    /// <returns>Map name if file exists and is valid, null otherwise</returns>
+    private string? LoadCurrentMapName()
+    {
+        try
+        {
+            var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
+            var filePath = Path.Combine(directory, "CurrentMap.name");
+            if (!File.Exists(filePath))
+            {
+                return null;
+            }
+
+            var mapName = File.ReadAllText(filePath).Trim();
+            if (string.IsNullOrWhiteSpace(mapName))
+            {
+                return null;
+            }
+
+            _logger.LogDebug("CartographerService: Loaded current map name '{MapName}' from {FilePath}", mapName, filePath);
+            return mapName;
+        }
+        catch (Exception ex)
+        {
+            _logger.LogWarning(ex, "CartographerService: Failed to load current map name");
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Saves the pose to {mapName}.pose file in text format: x y z qx qy qz qw
+    /// </summary>
+    private void SavePoseToFile(string mapName, Pose pose)
+    {
+        try
+        {
+            var directory = EnsureLocalizingStateDirectory();
+            var sanitizedMapName = MapNameHelper.Sanitize(mapName);
+            var filePath = Path.Combine(directory, $"{sanitizedMapName}.pose");
+
+            // Format: x y z qx qy qz qw (space-separated, invariant culture for decimal point)
+            var content = string.Format(
+                CultureInfo.InvariantCulture,
+                "{0} {1} {2} {3} {4} {5} {6}",
+                pose.Position.X,
+                pose.Position.Y,
+                pose.Position.Z,
+                pose.Orientation.X,
+                pose.Orientation.Y,
+                pose.Orientation.Z,
+                pose.Orientation.W);
+
+            File.WriteAllText(filePath, content);
+            _logger.LogDebug("CartographerService: Saved pose to {FilePath}: {Content}", filePath, content);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogWarning(ex, "CartographerService: Failed to save pose for map '{MapName}'", mapName);
+        }
+    }
+
+    /// <summary>
+    /// Loads the pose from {mapName}.pose file
+    /// </summary>
+    /// <returns>Pose if file exists and is valid, null otherwise</returns>
+    private Pose? LoadPoseFromFile(string mapName)
+    {
+        try
+        {
+            var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
+            var sanitizedMapName = MapNameHelper.Sanitize(mapName);
+            var filePath = Path.Combine(directory, $"{sanitizedMapName}.pose");
+
+            if (!File.Exists(filePath))
+            {
+                return null;
+            }
+
+            var content = File.ReadAllText(filePath).Trim();
+            var parts = content.Split(' ', StringSplitOptions.RemoveEmptyEntries);
+
+            if (parts.Length != 7)
+            {
+                _logger.LogWarning("CartographerService: Invalid pose file format in {FilePath}, expected 7 values, got {Count}", filePath, parts.Length);
+                return null;
+            }
+
+            var pose = new Pose
+            {
+                Position = new Vector3(
+                    double.Parse(parts[0], CultureInfo.InvariantCulture),
+                    double.Parse(parts[1], CultureInfo.InvariantCulture),
+                    double.Parse(parts[2], CultureInfo.InvariantCulture)),
+                Orientation = new Quaternion(
+                    double.Parse(parts[3], CultureInfo.InvariantCulture),
+                    double.Parse(parts[4], CultureInfo.InvariantCulture),
+                    double.Parse(parts[5], CultureInfo.InvariantCulture),
+                    double.Parse(parts[6], CultureInfo.InvariantCulture))
+            };
+
+            _logger.LogDebug("CartographerService: Loaded pose from {FilePath}: ({X}, {Y}, {Z})",
+                filePath, pose.Position.X, pose.Position.Y, pose.Position.Z);
+            return pose;
+        }
+        catch (Exception ex)
+        {
+            _logger.LogWarning(ex, "CartographerService: Failed to load pose for map '{MapName}'", mapName);
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Checks if a pose file exists for the given map
+    /// </summary>
+    private bool PoseFileExists(string mapName)
+    {
+        try
+        {
+            var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
+            var sanitizedMapName = MapNameHelper.Sanitize(mapName);
+            var filePath = Path.Combine(directory, $"{sanitizedMapName}.pose");
+            return File.Exists(filePath);
+        }
+        catch
+        {
+            return false;
+        }
+    }
+
+    #endregion
+
+    #region IDisposable
+
+    public void Dispose()
+    {
+        if (_disposed) return;
+
+        try
+        {
+            _disposed = true;
+
+            // Cancel any ongoing save operation
+            _savingMapCts?.Cancel();
+            _savingMapCts?.Dispose();
+            _savingMapCts = null;
+
+            _sensorPipeline?.PauseSubscriptions();
+            _sensorPipeline?.WaitForProcessingComplete(200);
+
+            // Stop background threads
+            StopPoseThread();
+            StopTriggerThread();
+
+            // Dispose pending trigger event
+            _pendingTriggerEvent.Dispose();
+
+            _sensorPipeline?.Dispose();
+
+            // Finish trajectory if active, then dispose MapBuilder (safety net if Dispose is called before StopAsync)
+            _mapBuilder?.FinishTrajectoryAndDispose(_trajectoryId);
+            _trajectoryBuilder = null;
+            _trajectoryId = -1;
+            _mapBuilder = null;
+
+            _occupancyGridManager.Dispose();
+            _slamResultProcessor?.Dispose();
+            _mclProcessor?.Dispose();
+
+            lock (_stateMachineLock) { _stateMachine.Stop(); }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Error during Dispose()");
+            // Don't throw in Dispose - log and continue cleanup
+        }
+        finally
+        {
+            GC.SuppressFinalize(this);
+        }
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.MapManagement.cs

@@ -0,0 +1,772 @@
+using CartographerSharp.Mapping;
+using Microsoft.AspNetCore.SignalR;
+using RobotNet10.RobotApp.Client.Shared.SLAM;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Hubs;
+using RobotNet10.RobotApp.Shared;
+using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public partial class CartographerService
+{
+    #region Map Listing and Info
+
+    /// <summary>
+    /// Liệt kê các maps có sẵn trong thư mục maps
+    /// </summary>
+    public IReadOnlyList<MapInfo> ListMaps()
+    {
+        var mapsDirectory = Path.GetFullPath(_config.MapStorage.Directory);
+        if (!Directory.Exists(mapsDirectory))
+            return [];
+
+        try
+        {
+            var maps = new List<MapInfo>();
+            var mapDirectories = Directory.GetDirectories(mapsDirectory);
+
+            foreach (var mapDir in mapDirectories)
+            {
+                try
+                {
+                    var metadataPath = MapPathHelper.ResolveMetadataPath(mapDir);
+                    if (metadataPath != null)
+                    {
+                        var jsonContent = File.ReadAllText(metadataPath);
+                        var metadata = System.Text.Json.JsonSerializer.Deserialize<MapInfo>(jsonContent);
+                        if (metadata != null)
+                        {
+                            metadata = MapNameHelper.EnsureMapSize(metadata);
+                            maps.Add(metadata);
+                        }
+                    }
+                    else
+                    {
+                        // Create basic metadata from directory name
+                        var mapName = Path.GetFileName(mapDir);
+                        maps.Add(new MapInfo
+                        {
+                            Name = mapName,
+                            FolderPath = mapDir,
+                            CreatedDate = Directory.GetCreationTime(mapDir),
+                            Resolution = _config.MapStorage.OccupancyGridResolution
+                        });
+                    }
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogWarning(ex, "CartographerService: Failed to read metadata for map in: {MapDir}", mapDir);
+                }
+            }
+
+            return maps;
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Failed to list maps");
+            return [];
+        }
+    }
+
+    /// <summary>
+    /// Lấy thông tin chi tiết của một map theo tên
+    /// </summary>
+    public MapInfo? GetMapInfo(string mapName)
+    {
+        try
+        {
+            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
+
+            if (!Directory.Exists(mapPath))
+            {
+                _logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
+                return null;
+            }
+
+            var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
+
+            if (metadataPath == null)
+            {
+                // Create basic metadata from directory name
+                var sanitizedMapName = MapNameHelper.Sanitize(mapName);
+                return new MapInfo
+                {
+                    Name = sanitizedMapName,
+                    FolderPath = mapPath,
+                    CreatedDate = Directory.GetCreationTime(mapPath),
+                    Resolution = _config.MapStorage.OccupancyGridResolution
+                };
+            }
+
+            var jsonContent = File.ReadAllText(metadataPath);
+            var metadata = System.Text.Json.JsonSerializer.Deserialize<MapInfo>(jsonContent);
+            if (metadata == null)
+                return null;
+
+            metadata = MapNameHelper.EnsureMapSize(metadata);
+
+            return metadata;
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Failed to get map info: {MapName}", mapName);
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Lấy trạng thái xử lý của map
+    /// </summary>
+    /// <param name="mapName">Tên map</param>
+    /// <returns>True nếu map đang được xử lý, false nếu không</returns>
+    public bool GetMapProcessingStatus(string mapName)
+    {
+        return _mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing;
+    }
+
+    #endregion
+
+    #region Map Operations
+
+    /// <summary>
+    /// Lấy đường dẫn đến file ảnh PNG của map
+    /// </summary>
+    public string? GetMapImagePath(string mapName)
+    {
+        try
+        {
+            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
+
+            if (!Directory.Exists(mapPath))
+            {
+                _logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
+                return null;
+            }
+
+            var imagePath = MapPathHelper.ResolveImagePath(mapPath);
+            if (imagePath != null)
+                return imagePath;
+
+            _logger.LogWarning("CartographerService: Map image not found for: {MapName}", mapName);
+            return null;
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Failed to get map image path: {MapName}", mapName);
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Xóa map folder
+    /// </summary>
+    public Task<bool> DeleteMapAsync(string mapName, CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
+
+            if (!Directory.Exists(mapPath))
+            {
+                _logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
+                return Task.FromResult(false);
+            }
+
+            Directory.Delete(mapPath, recursive: true);
+            return Task.FromResult(true);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Failed to delete map: {MapName}", mapName);
+            return Task.FromResult(false);
+        }
+    }
+
+    /// <summary>
+    /// Transform map để chọn lại gốc tọa độ.
+    /// Kiểm tra nếu map đang được xử lý thì return false.
+    /// Nếu không, bắt đầu xử lý trên thread riêng và return true ngay lập tức.
+    /// </summary>
+    public Task<bool> TransformMapOriginAsync(string mapName, Pose newOrigin, CancellationToken cancellationToken = default)
+    {
+        // Check if map is already being processed
+        if (_mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing)
+        {
+            _logger.LogWarning("CartographerService: Map {MapName} is already being processed", mapName);
+            return Task.FromResult(false);
+        }
+
+        // Set processing status to true
+        _mapProcessingStatus[mapName] = true;
+
+        // Notify clients that processing has started
+        _ = _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, true, cancellationToken);
+
+        // Start processing on a separate thread
+        _ = Task.Run(async () =>
+        {
+            try
+            {
+                await TransformMapOriginInternalAsync(mapName, newOrigin, cancellationToken);
+            }
+            finally
+            {
+                // Set processing status to false
+                _mapProcessingStatus[mapName] = false;
+
+                // Notify clients that processing has completed
+                await _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, false, cancellationToken);
+            }
+        }, cancellationToken);
+
+        // Return true immediately to indicate processing has started
+        return Task.FromResult(true);
+    }
+
+    /// <summary>
+    /// Internal method that performs the actual map transformation.
+    /// Based on xloc.cc ChangeMapOrigin logic: updates TransformToMap in the PoseGraph proto
+    /// while preserving all other data (trajectories, submaps, options, etc.).
+    /// </summary>
+    private async Task<bool> TransformMapOriginInternalAsync(string mapName, Pose newOrigin, CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
+            var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
+            var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);
+
+            if (metadataPath == null)
+            {
+                _logger.LogError("TransformMapOrigin: Metadata file not found in: {MapPath}", mapPath);
+                return false;
+            }
+
+            if (pbstreamPath == null)
+            {
+                _logger.LogError("TransformMapOrigin: Pbstream file not found in: {MapPath}", mapPath);
+                return false;
+            }
+
+            // Load metadata
+            var jsonContent = await File.ReadAllTextAsync(metadataPath, cancellationToken);
+            var metadata = System.Text.Json.JsonSerializer.Deserialize<MapCartographerInfo>(jsonContent);
+
+            if (metadata == null)
+            {
+                _logger.LogError("TransformMapOrigin: Failed to deserialize metadata");
+                return false;
+            }
+
+            // === Update pbstream file using shared helper ===
+            // This preserves all original data (AllTrajectoryBuilderOptions, submaps, nodes, etc.)
+            PbstreamTransformHelper.TransformPbstreamOrigin(pbstreamPath, newOrigin, _logger);
+
+            // Reload transformed map to regenerate occupancy grid and image files
+            var transformedLoadResult = LoadMapFromDirectory(mapName);
+            if (transformedLoadResult == null)
+            {
+                _logger.LogError("TransformMapOrigin: Failed to load transformed map for file regeneration");
+                return false;
+            }
+
+            MapBuilder transformedMapBuilder;
+            try
+            {
+                transformedMapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
+                    transformedLoadResult,
+                    _config,
+                    _logger,
+                    loadFrozenState: false);
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "TransformMapOrigin: Failed to create MapBuilder from transformed pbstream");
+                return false;
+            }
+
+            try
+            {
+                // Use MapSaveProcessor to regenerate occupancy grid files and update metadata
+                if (_mapSaveProcessor == null)
+                {
+                    _logger.LogError("TransformMapOrigin: MapSaveProcessor is not available");
+                    return false;
+                }
+
+                await _mapSaveProcessor.SaveMapMetadataAsync(
+                    mapName,
+                    transformedMapBuilder,
+                    mapPath,
+                    cancellationToken);
+
+                return true;
+            }
+            finally
+            {
+                transformedMapBuilder.Dispose();
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "TransformMapOrigin: Failed for map {MapName}", mapName);
+            return false;
+        }
+    }
+
+    /// <summary>
+    /// Rerender map image files (PNG, JPG, PGM) with custom OccupancyGridConfiguration.
+    /// Kiểm tra nếu map đang được xử lý thì return false.
+    /// Nếu không, bắt đầu xử lý trên thread riêng và return true ngay lập tức.
+    /// </summary>
+    public Task<bool> RerenderMapWithConfigAsync(string mapName, OccupancyGridConfigurationDto configDto, CancellationToken cancellationToken = default)
+    {
+        // Check if map is already being processed
+        if (_mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing)
+        {
+            _logger.LogWarning("CartographerService: Map {MapName} is already being processed", mapName);
+            return Task.FromResult(false);
+        }
+
+        // Set processing status to true
+        _mapProcessingStatus[mapName] = true;
+
+        // Notify clients that processing has started
+        _ = _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, true, cancellationToken);
+
+        // Start processing on a separate thread
+        _ = Task.Run(async () =>
+        {
+            try
+            {
+                await RerenderMapWithConfigInternalAsync(mapName, configDto, cancellationToken);
+            }
+            finally
+            {
+                // Set processing status to false
+                _mapProcessingStatus[mapName] = false;
+
+                // Notify clients that processing has completed
+                await _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, false, cancellationToken);
+            }
+        }, cancellationToken);
+
+        // Return true immediately to indicate processing has started
+        return Task.FromResult(true);
+    }
+
+    /// <summary>
+    /// Internal method that performs the actual map rerendering.
+    /// Loads pbstream, generates occupancy grid with custom config, and saves image files.
+    /// </summary>
+    private async Task<bool> RerenderMapWithConfigInternalAsync(string mapName, OccupancyGridConfigurationDto configDto, CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
+            var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);
+
+            if (pbstreamPath == null)
+            {
+                _logger.LogError("RerenderMapWithConfig: Pbstream file not found in: {MapPath}", mapPath);
+                return false;
+            }
+
+            // Load map from pbstream
+            var loadResult = LoadMapFromDirectory(mapName);
+            if (loadResult == null)
+            {
+                _logger.LogError("RerenderMapWithConfig: Failed to load map from directory");
+                return false;
+            }
+
+            MapBuilder mapBuilder;
+            try
+            {
+                mapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
+                    loadResult,
+                    _config,
+                    _logger,
+                    loadFrozenState: false);
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "RerenderMapWithConfig: Failed to create MapBuilder from pbstream");
+                return false;
+            }
+
+            try
+            {
+                // === DEBUG: Log PoseGraph state after loading ===
+                var poseGraph = mapBuilder.PoseGraph;
+                var transformToMap = poseGraph.GetTransformToMap();
+                _logger.LogWarning(
+                    "RerenderMapWithConfig DEBUG: TransformToMap = T[{TX:F4}, {TY:F4}, {TZ:F4}], R[{RW:F4}, {RX:F4}, {RY:F4}, {RZ:F4}]",
+                    transformToMap.Translation.X, transformToMap.Translation.Y, transformToMap.Translation.Z,
+                    transformToMap.Rotation.W, transformToMap.Rotation.X, transformToMap.Rotation.Y, transformToMap.Rotation.Z);
+
+                var allSubmapData = poseGraph.GetAllSubmapData();
+                _logger.LogWarning("RerenderMapWithConfig DEBUG: Total submaps = {Count}", allSubmapData?.Count ?? 0);
+
+                // Log LocalToGlobalTransform for each trajectory
+                try
+                {
+                    var trajectoryStates = poseGraph.GetTrajectoryStates();
+                    _logger.LogWarning("RerenderMapWithConfig DEBUG: Found {Count} trajectories", trajectoryStates?.Count ?? 0);
+                    if (trajectoryStates != null)
+                    {
+                        foreach (var trajState in trajectoryStates)
+                        {
+                            var trajId = trajState.Key;
+                            var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajId);
+                            _logger.LogWarning(
+                                "RerenderMapWithConfig DEBUG: Trajectory[{TrajId}] State={State}, LocalToGlobalTransform = " +
+                                "T[{TX:F4},{TY:F4},{TZ:F4}], R[{RW:F4},{RX:F4},{RY:F4},{RZ:F4}]",
+                                trajId, trajState.Value,
+                                localToGlobal.Translation.X, localToGlobal.Translation.Y, localToGlobal.Translation.Z,
+                                localToGlobal.Rotation.W, localToGlobal.Rotation.X, localToGlobal.Rotation.Y, localToGlobal.Rotation.Z);
+                        }
+                    }
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogWarning("RerenderMapWithConfig DEBUG: Failed to get trajectory states: {Error}", ex.Message);
+                }
+
+                int submapIdx = 0;
+                foreach (var idDataRef in allSubmapData)
+                {
+                    var submapId = idDataRef.Id;
+                    var submapData = idDataRef.Data;
+                    var submapPose = submapData.Pose;
+
+                    // Calculate globalPose as OccupancyGridGenerator does
+                    var transformToMapInverse = transformToMap.Inverse();
+                    var globalPose = transformToMapInverse * submapPose;
+
+                    if (submapData.Submap is CartographerSharp.Mapping.D2D.Submap2D submap2D)
+                    {
+                        var localPose = submap2D.LocalPose;
+                        var grid = submap2D.Grid;
+                        var gridInfo = grid != null
+                            ? $"cells={grid.Limits.CellLimits.NumXCells}x{grid.Limits.CellLimits.NumYCells}, res={grid.Limits.Resolution:F4}"
+                            : "null";
+
+                        // Calculate yaw from quaternion components
+                        static double GetYawDegreesFromComponents(double qw, double qx, double qy, double qz)
+                        {
+                            var siny_cosp = 2.0 * (qw * qz + qx * qy);
+                            var cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
+                            return Math.Atan2(siny_cosp, cosy_cosp) * 180.0 / Math.PI;
+                        }
+
+                        var localYaw = GetYawDegreesFromComponents(
+                            localPose.Rotation.W, localPose.Rotation.X, localPose.Rotation.Y, localPose.Rotation.Z);
+                        var submapPoseYaw = GetYawDegreesFromComponents(
+                            submapPose.Rotation.W, submapPose.Rotation.X, submapPose.Rotation.Y, submapPose.Rotation.Z);
+                        var globalPoseYaw = GetYawDegreesFromComponents(
+                            globalPose.Rotation.W, globalPose.Rotation.X, globalPose.Rotation.Y, globalPose.Rotation.Z);
+
+                        _logger.LogWarning(
+                            "RerenderMapWithConfig DEBUG: Submap[{Idx}] Id={TrajId}:{SubIdx}, " +
+                            "LocalPose=T[{LPX:F4},{LPY:F4}] Yaw={LPYaw:F2}°, " +
+                            "SubmapData.Pose=T[{SPX:F4},{SPY:F4}] Yaw={SPYaw:F2}°, " +
+                            "GlobalPose=T[{GPX:F4},{GPY:F4}] Yaw={GPYaw:F2}°, " +
+                            "Grid={GridInfo}",
+                            submapIdx, submapId.TrajectoryId, submapId.SubmapIndex,
+                            localPose.Translation.X, localPose.Translation.Y, localYaw,
+                            submapPose.Translation.X, submapPose.Translation.Y, submapPoseYaw,
+                            globalPose.Translation.X, globalPose.Translation.Y, globalPoseYaw,
+                            gridInfo);
+                    }
+                    submapIdx++;
+                }
+                // === END DEBUG ===
+
+                // Convert DTO to OccupancyGridConfiguration
+                var config = ConvertDtoToConfig(configDto);
+
+                // Generate occupancy grid with custom config
+                var resolution = _config.MapStorage.OccupancyGridResolution;
+                var padding = _config.MapStorage.MapPadding;
+                var occupancyGrid = OccupancyGridGenerator.Generate(mapBuilder, resolution, padding, _logger, config);
+
+                if (occupancyGrid == null)
+                {
+                    _logger.LogError("RerenderMapWithConfig: Failed to generate occupancy grid");
+                    return false;
+                }
+
+                // === DEBUG: Log OccupancyGrid result ===
+                _logger.LogWarning(
+                    "RerenderMapWithConfig DEBUG: OccupancyGrid size={W}x{H}, resolution={Res:F4}, " +
+                    "Origin=T[{OX:F4},{OY:F4}]",
+                    occupancyGrid.Width, occupancyGrid.Height, occupancyGrid.Resolution,
+                    occupancyGrid.Origin.Position.X, occupancyGrid.Origin.Position.Y);
+                // === END DEBUG ===
+
+                // Save all image formats (PGM, PNG, JPG, YAML)
+                await OccupancyGridFileHelper.SaveAllFormatsAsync(occupancyGrid, mapPath, cancellationToken, _logger);
+
+                _logger.LogInformation("RerenderMapWithConfig: Successfully rerendered map {MapName} with custom config", mapName);
+                return true;
+            }
+            finally
+            {
+                mapBuilder.Dispose();
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "RerenderMapWithConfig: Failed for map {MapName}", mapName);
+            return false;
+        }
+    }
+
+    /// <summary>
+    /// Convert OccupancyGridConfigurationDto to OccupancyGridConfiguration
+    /// </summary>
+    private static OccupancyGridConfiguration ConvertDtoToConfig(OccupancyGridConfigurationDto dto)
+    {
+        return new OccupancyGridConfiguration
+        {
+            MergeStrategy = dto.MergeStrategy switch
+            {
+                SubmapMergeStrategyDto.PorterDuff => SubmapMergeStrategy.PorterDuff,
+                SubmapMergeStrategyDto.LogOddsSum => SubmapMergeStrategy.LogOddsSum,
+                SubmapMergeStrategyDto.MaxProbability => SubmapMergeStrategy.MaxProbability,
+                _ => SubmapMergeStrategy.LogOddsSum
+            },
+            LogOddsClamp = dto.LogOddsClamp,
+            UseLogOddsAverage = dto.UseLogOddsAverage,
+            FreeSpaceThreshold = dto.FreeSpaceThreshold,
+            OccupiedSpaceThreshold = dto.OccupiedSpaceThreshold,
+            UseBinaryOutput = dto.UseBinaryOutput,
+            EnableWallThinning = dto.EnableWallThinning,
+            WallThinningIterations = dto.WallThinningIterations,
+            MinWallThicknessPixels = dto.MinWallThicknessPixels,
+            AmbiguousCellValue = dto.AmbiguousCellValue,
+            AmbiguousRangeLower = dto.AmbiguousRangeLower,
+            AmbiguousRangeUpper = dto.AmbiguousRangeUpper,
+            EnableMedianFilter = dto.EnableMedianFilter,
+            MedianFilterKernelSize = dto.MedianFilterKernelSize
+        };
+    }
+
+    #endregion
+
+    #region Map Loading
+
+    /// <summary>
+    /// Load map files directly from map directory
+    /// </summary>
+    private MapCartographerLoadResult? LoadMapFromDirectory(string mapName)
+    {
+        try
+        {
+            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
+
+            if (!Directory.Exists(mapPath))
+            {
+                _logger.LogError("CartographerService: Map directory not found: {MapPath}", mapPath);
+                return null;
+            }
+
+            // Find pbstream file (map.pbstream or legacy)
+            var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);
+            if (pbstreamPath == null)
+            {
+                _logger.LogError("CartographerService: No pbstream file found in: {MapPath}", mapPath);
+                return null;
+            }
+
+            if (Path.GetFileName(pbstreamPath) != "map.pbstream")
+            {
+                _logger.LogWarning("CartographerService: Using legacy pbstream file: {Path}", pbstreamPath);
+            }
+
+            // Load metadata to get saved config (map.json)
+            MapCartographerConfigSnapshot? savedConfig = null;
+            var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
+            if (metadataPath != null)
+            {
+                if (Path.GetFileName(metadataPath) == "metadata.json")
+                {
+                    _logger.LogWarning("CartographerService: Using legacy metadata.json file");
+                }
+
+                try
+                {
+                    var jsonContent = File.ReadAllText(metadataPath);
+                    var metadata = System.Text.Json.JsonSerializer.Deserialize<MapCartographerInfo>(jsonContent);
+                    savedConfig = metadata?.MapConfig;
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogWarning(ex, "CartographerService: Failed to load metadata, will use current config");
+                }
+            }
+
+            return new MapCartographerLoadResult
+            {
+                PbstreamPath = pbstreamPath,
+                MapPath = mapPath,
+                SavedConfig = savedConfig
+            };
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Failed to load map: {MapName}", mapName);
+            return null;
+        }
+    }
+
+    private bool MapExists(string mapName)
+    {
+        try
+        {
+            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
+            return Directory.Exists(mapPath);
+        }
+        catch
+        {
+            return false;
+        }
+    }
+
+    #endregion
+
+    #region Wall Alignment
+
+    /// <summary>
+    /// Calculate wall-aligned pose by detecting walls from lidar scans.
+    /// This method should be called AFTER sensors are resumed to ensure fresh lidar data.
+    /// The aligned pose should be set on LocalTrajectoryBuilder2D using SetInitialPose()
+    /// BEFORE resuming sensors for scan mapping.
+    /// </summary>
+    /// <returns>Wall-aligned pose if successful, null otherwise</returns>
+    private Pose CalculateWallAlignedPoseAsync()
+    {
+        _logger.LogInformation("CartographerService: Starting wall alignment calculation");
+
+        // Step 1: Get all lidar devices
+        var allDevices = _deviceProvider.GetDevicesByType(RobotNet10.RobotApp.Client.Shared.Devices.DeviceType.Lidar);
+        var lidarList = allDevices.OfType<ILidar>().ToList();
+
+        if (lidarList.Count == 0)
+        {
+            _logger.LogWarning("CartographerService: No lidar devices found for wall alignment");
+            return new Pose();
+        }
+
+        _logger.LogInformation("CartographerService: Found {Count} lidar device(s)", lidarList.Count);
+
+        // Step 2: Collect point clouds from all lidars and transform to base_link frame
+        var allPoints = new List<Vector3>();
+
+        foreach (var lidar in lidarList)
+        {
+            try
+            {
+                var deviceId = (lidar as DeviceBase)?.DeviceId;
+                if (string.IsNullOrEmpty(deviceId))
+                {
+                    _logger.LogWarning("CartographerService: Lidar device has no ID, skipping");
+                    continue;
+                }
+
+                var laserScan = lidar.CurrentMeasurementData;
+                if (!laserScan.HasValue)
+                {
+                    _logger.LogWarning("CartographerService: No scan data from lidar {DeviceId}", deviceId);
+                    continue;
+                }
+
+                // Get sensor configuration and transform
+                var sensorConfig = _config.Sensors.Lidars
+                    .FirstOrDefault(cfg => cfg.DeviceId == deviceId && cfg.Enabled);
+
+                if (sensorConfig == null)
+                {
+                    _logger.LogWarning("CartographerService: No configuration found for lidar {DeviceId}", deviceId);
+                    continue;
+                }
+
+                var scan = laserScan.Value;
+
+                // Convert LaserScan to point cloud in base_link frame
+                var (baseLink, _, _, _, _) = SensorDataTransformHelper.ToTimedPointCloudDataBaseAndSensorFrame(
+                    scan.Header.Stamp,
+                    scan,
+                    sensorConfig.Transform,
+                    sensorConfig.AngleMin,
+                    sensorConfig.AngleMax
+                );
+
+                // Extract points in base_link frame (use .Ranges instead of .Points)
+                int pointsAdded = 0;
+                foreach (var point in baseLink.Ranges)
+                {
+                    allPoints.Add(point.Position);
+                    pointsAdded++;
+                }
+
+                _logger.LogInformation(
+                    "CartographerService: Collected {Count} points from lidar {DeviceId}",
+                    pointsAdded, deviceId);
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "CartographerService: Error processing lidar data for wall alignment");
+            }
+        }
+
+        if (allPoints.Count == 0)
+        {
+            _logger.LogWarning("CartographerService: No points collected for wall alignment");
+            return new Pose();
+        }
+
+        _logger.LogInformation("CartographerService: Total points collected: {Count}", allPoints.Count);
+
+        // Step 3: Detect wall and calculate compensation angle
+        var compensationAngle = WallAlignmentHelper.DetectWallAndCalculateCompensation(allPoints, _logger);
+
+        if (!compensationAngle.HasValue)
+        {
+            _logger.LogWarning("CartographerService: Could not detect wall for alignment");
+            return new Pose();
+        }
+
+        // Step 4: Calculate wall-aligned pose
+        // Start with identity pose (robot at origin, facing along X-axis)
+        // Then apply the compensation angle to align with detected wall
+        var alignedOrientation = WallAlignmentHelper.CreateQuaternionFromYaw(compensationAngle.Value);
+
+        var alignedPose = new Pose
+        {
+            Position = Vector3.Zero,          // Start at origin
+            Orientation = alignedOrientation   // Oriented to align map with wall
+        };
+
+        _logger.LogInformation(
+            "CartographerService: Calculated wall-aligned pose - " +
+            "Position: [{X:F3}, {Y:F3}, {Z:F3}], Yaw: {Yaw:F3}rad ({YawDeg:F1}°), " +
+            "Compensation angle: {CompAngle:F3}rad ({CompAngleDeg:F1}°)",
+            alignedPose.Position.X,
+            alignedPose.Position.Y,
+            alignedPose.Position.Z,
+            WallAlignmentHelper.GetYawFromQuaternion(alignedPose.Orientation),
+            WallAlignmentHelper.GetYawFromQuaternion(alignedPose.Orientation) * 180.0 / Math.PI,
+            compensationAngle.Value,
+            compensationAngle.Value * 180.0 / Math.PI);
+
+        return alignedPose;
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.SensorProcessing.cs

@@ -0,0 +1,566 @@
+using System.Diagnostics;
+using CartographerSharp.Mapping;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using RobotNet10.RobotApp.Shared;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+using RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public partial class CartographerService
+{
+    #region Sensor Data Callbacks
+
+    private void OnAddRangeData(string deviceId, RangeDataPayload payload)
+    {
+        // Get current state and relevant objects
+        var currentState = State;
+        var tb = _trajectoryBuilder;
+        bool mclRunning = _mclProcessor?.IsRunning ?? false;
+        MclService? mcl = _mcl;
+
+        // Handle data based on current state
+        switch (currentState)
+        {
+            case SLAMState.Relocalizing:
+                // InitializingLocalizing: MCL is computing initial pose
+                // - If MCL enabled and running: Process with MCL (no trajectory yet)
+                // - If MCL disabled: Trajectory already added, process with CartographerSharp
+                if (mclRunning && mcl != null && _mclProcessor != null)
+                {
+                    // MCL path: Feed scan to MCL from primary lidar only
+                    string? primaryLidarId = _mclProcessor.PrimaryLidarId;
+                    if (string.IsNullOrEmpty(primaryLidarId) || deviceId == primaryLidarId)
+                    {
+                        try
+                        {
+                            RunMclOnScan(payload, mcl);
+                        }
+                        catch (Exception ex)
+                        {
+                            _logger.LogError(ex, "CartographerService: OnAddRangeData MCL failed for {DeviceId}", deviceId);
+                        }
+                    }
+                }
+                else if (tb != null)
+                {
+                    // Non-MCL path: Trajectory already added in StartLocalizationAsync, process with CartographerSharp
+                    ProcessWithTrajectoryBuilder(deviceId, payload, tb);
+                }
+                break;
+
+            case SLAMState.Localizing:
+                // Localizing: Use CartographerSharp for localization
+                if (tb != null)
+                {
+                    ProcessWithTrajectoryBuilder(deviceId, payload, tb);
+                }
+                else
+                {
+                    _logger.LogWarning("CartographerService.OnAddRangeData: Localizing but _trajectoryBuilder is null!");
+                }
+                break;
+
+            case SLAMState.ScanMapping:
+                // ScanMapping: Use CartographerSharp for SLAM
+                if (tb != null)
+                {
+                    ProcessWithTrajectoryBuilder(deviceId, payload, tb);
+                }
+                break;
+
+            default:
+                // Other states (Idle, Ready, SavingMap, Error): Ignore sensor data
+                break;
+        }
+    }
+
+    private void OnAddImuData(string deviceId, ImuData data) => _trajectoryBuilder?.AddSensorData(deviceId, data);
+
+    private void OnAddOdometryData(string sensorId, OdometryData data)
+    {
+        // Feed to trajectory builder (existing)
+        _trajectoryBuilder?.AddSensorData(sensorId, data);
+
+        // Feed to MCL when running (xloc flow: mcl_->CallOdomCallback continuously)
+        if (_mclProcessor?.IsRunning ?? false)
+        {
+            _mclProcessor.ProcessOdometry(data);
+        }
+    }
+
+    #endregion
+
+    #region Trajectory Builder Processing
+
+    /// <summary>
+    /// Process range data with trajectory builder (CartographerSharp)
+    /// Used for both Localizing and ScanMapping states
+    /// </summary>
+    private void ProcessWithTrajectoryBuilder(string deviceId, RangeDataPayload payload, ITrajectoryBuilder tb)
+    {
+        try
+        {
+            var result = tb.AddSensorData(deviceId, payload.BaseLink);
+            
+            // Skip if disposed to prevent issues during shutdown
+            if (_disposed)
+            {
+                return;
+            }
+
+            // Get current state and pose for operations that don't depend on result.HasValue
+            var currentState = State;
+            var currentPose = Volatile.Read(ref _poseSnapshot).Pose;
+
+            // === SAMPLE POINT CLOUD (does NOT depend on hasValue) ===
+            // Process sample point cloud even when result.HasValue is false
+            // Use SamplePointCloudGlobal from result if available, otherwise transform raw payload to global frame
+            if (_pointCloudUpdateStopwatch.ElapsedMilliseconds >= 1000)
+            {
+                // Capture variables for Task.Run closure
+                var capturedPayload = payload;
+                var capturedPose = currentPose;
+                var capturedResult = result;
+                var logger = _logger;
+
+                // Move ALL calculation into Task.Run to avoid blocking sensor thread
+                _ = Task.Run(() =>
+                {
+                    List<Vector3>? pointsToStore = null;
+
+                    // Check if result has SamplePointCloudGlobal (preferred)
+                    if (capturedResult.HasValue && capturedResult.Value.SamplePointCloudGlobal is { } pcFromResult)
+                    {
+                        // Use CartographerSharp's sample point cloud (already in global frame)
+                        pointsToStore = new List<Vector3>(pcFromResult.Count);
+                        foreach (var p in pcFromResult.Points)
+                            pointsToStore.Add(new Vector3(p.Position.X, p.Position.Y, p.Position.Z));
+
+                        if (pointsToStore != null && pointsToStore.Count > 0)
+                        {
+                            lock (_pointCloudLock)
+                            {
+                                _completedAccumulatedSamplePointClouds = pointsToStore;
+                            }
+                        }
+
+                        _pointCloudUpdateStopwatch.Restart();
+                    }
+                });
+            }
+
+            // === POSE SYNC (does NOT depend on hasValue) ===
+            // Periodic pose sync to file (for auto-resume on next startup)
+            var syncInterval = _config.MapStorage.PoseSyncIntervalSeconds;
+            if (syncInterval > 0 && _poseSyncStopwatch.Elapsed.TotalSeconds >= syncInterval)
+            {
+                var mapName = _currentMapName;
+                if (!string.IsNullOrEmpty(mapName) && (currentState == SLAMState.Localizing || currentState == SLAMState.ScanMapping))
+                {
+                    // Use cached global-frame pose from snapshot
+                    SavePoseToFile(mapName, currentPose);
+                    _poseSyncStopwatch.Restart();
+                }
+            }
+
+            // === CARTOGRAPHER RESULT PROCESSING (DEPENDS on hasValue) ===
+            // Skip result processing if no valid result
+            if (!result.HasValue)
+            {
+                return;
+            }
+
+            var r = result.Value;
+
+            // Process based on mode
+            if (currentState == SLAMState.ScanMapping)
+            {
+                // Store matching score for pose thread (volatile write, no lock needed)
+                Volatile.Write(ref _lastMatchingScore, r.PoseConfidence);
+
+                // Process scan mapping result (only if processor is available)
+                _slamResultProcessor?.ProcessScanMappingResult(r, currentState, out _);
+
+                // Signal insertion to OccupancyGridManager when a scan was inserted into submap.
+                // This sets a flag that ShouldUpdateGrid() will check.
+                if (r.InsertionResult.HasValue)
+                {
+                    _occupancyGridManager.SignalInsertion();
+                }
+
+                // Update occupancy grid when BOTH conditions are met:
+                // 1. At least 3 seconds have elapsed since last update
+                // 2. At least one scan was inserted since last update (SignalInsertion was called)
+                if (_mapBuilder != null && _occupancyGridManager.ShouldUpdateGrid())
+                {
+                    var mapBuilder = _mapBuilder;
+                    _ = Task.Run(() => _occupancyGridManager.UpdateFromMapBuilder(mapBuilder));
+                }
+            }
+            else if (currentState == SLAMState.Localizing)
+            {
+                // Store scan match score for drift detection (same as ScanMapping but also used in LocalizationScore)
+                // This is critical for detecting map drift during localization
+                Volatile.Write(ref _lastMatchingScore, r.PoseConfidence);
+
+                // Get cached covariance data
+                var cachedCovariance = _slamResultProcessor?.PoseCovariance;
+                var cachedConstraintCount = _slamResultProcessor?.ConstraintCount ?? 0;
+                var cachedAverageQuality = _slamResultProcessor?.AverageConstraintQuality ?? 0.0;
+
+                // Update confidence with scan match score (pose is handled by background thread at 100Hz)
+                UpdateConfidenceMetrics(cachedCovariance, cachedConstraintCount, cachedAverageQuality, r.PoseConfidence);
+
+                // Calculate covariance asynchronously (throttled: skip if already computing)
+                IMapBuilder? mbSnapshot;
+                int trajIdSnapshot;
+                lock (_lock)
+                {
+                    mbSnapshot = _mapBuilder;
+                    trajIdSnapshot = _trajectoryId;
+                }
+                if (mbSnapshot != null && Interlocked.CompareExchange(ref _covarianceComputing, 1, 0) == 0)
+                {
+                    _ = Task.Run(() =>
+                    {
+                        try
+                        {
+                            var constraints = _constraintCache.GetOrUpdate(mbSnapshot, trajIdSnapshot, _logger);
+
+                            CovarianceCalculator.Result covResult;
+                            try
+                            {
+                                covResult = constraints != null
+                                    ? CovarianceCalculator.Calculate(constraints)
+                                    : new CovarianceCalculator.Result(null, 0, 0.0);
+                            }
+                            catch (Exception ex)
+                            {
+                                _logger.LogWarning(ex, "CartographerService: Failed to calculate covariance from constraints");
+                                covResult = new CovarianceCalculator.Result(null, 0, 0.0);
+                            }
+
+                            _slamResultProcessor?.UpdateCovariance(covResult.Covariance, covResult.ConstraintCount, covResult.AverageConstraintQuality);
+
+                            lock (_lock)
+                            {
+                                _poseCovariance = covResult.Covariance;
+                            }
+                        }
+                        catch (Exception ex)
+                        {
+                            _logger.LogWarning(ex, "CartographerService: Error calculating covariance asynchronously");
+                        }
+                        finally
+                        {
+                            Interlocked.Exchange(ref _covarianceComputing, 0);
+                        }
+                    });
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogDebug(ex, "CartographerService: ProcessWithTrajectoryBuilder failed for {DeviceId}", deviceId);
+        }
+    }
+
+    #endregion
+
+    #region MCL Scan Processing
+
+    /// <summary>Runs MCL with one scan from the primary lidar. Convergence logic matches xloc MCLThread: pose stable (dist+yaw) + stable_duration + reliability/MAE, or timeout.</summary>
+    private void RunMclOnScan(RangeDataPayload payload, MclService mcl)
+    {
+        // Base_link to laser was set at MCL start (SetMclBaseLinkToLaser). SensorPipeline always provides SensorFrame; use it for MCL (beam angles in sensor frame).
+        var ranges = payload.SensorFrame.Ranges;
+        _mclScanBuffer ??= new List<Vector2>(ranges.Count);
+        _mclScanBuffer.Clear();
+        foreach (var r in ranges)
+            _mclScanBuffer.Add(new Vector2(r.Position.X, r.Position.Y));
+        var pointsForScan = _mclScanBuffer;
+
+        // Use actual angle information from filtered point cloud (SensorPipeline provides accurate angles after filtering)
+        // Convert from radians to degrees for MclScanHelper.ConvertToScan
+        double angleMinDeg = payload.ActualAngleMin * (180f / Math.PI);
+        double angleMaxDeg = payload.ActualAngleMax * (180f / Math.PI);
+
+        // Use actual number of points from scan data
+        int numBins = pointsForScan.Count;
+
+        double rangeMin = _config.TrajectoryBuilder.MinRange;
+        double rangeMax = _config.TrajectoryBuilder.MaxRange;
+        var (angleMin, angleMax, angleIncrement, _, _, mclRanges) = MclScanHelper.ConvertToScan(pointsForScan, angleMinDeg, angleMaxDeg, numBins, rangeMin, rangeMax);
+
+        mcl.OnScan(angleMin, angleMax, angleIncrement, rangeMin, rangeMax, mclRanges);
+
+        const int iterationsPerScan = 5;
+        for (int i = 0; i < iterationsPerScan; i++)
+            mcl.RunOneIteration();
+
+        Pose currentPose = mcl.GetPose();
+        double reliability = mcl.Reliability;
+        double? mae = mcl.MaeForBestParticle;
+        double totalLikelihood = mcl.TotalLikelihood;
+
+        // Use MclProcessor to check convergence with reason
+        var convergenceReason = _mclProcessor?.CheckConvergenceWithReason(currentPose, reliability, mae, out var _)
+            ?? MclConvergenceReason.NotConverged;
+        
+        // Note: OnPoseUpdated is called in all paths below (converged/not-converged),
+        // which publishes pose to _poseSnapshot with MCL reliability and MAE for proper score calculation.
+
+        if (convergenceReason != MclConvergenceReason.NotConverged)
+        {
+            _mclProcessor?.Stop();
+
+            // Disable global localization mode after convergence to switch to normal tracking mode
+            mcl.DisableGlobalLocalizationMode();
+
+            if (_mapBuilder == null)
+            {
+                _logger.LogWarning("CartographerService: MCL converged but MapBuilder is null; trajectory not added.");
+                return;
+            }
+
+            // Verify no active trajectory before adding new one (defensive check)
+            int currentTrajId;
+            lock (_lock)
+            {
+                currentTrajId = _trajectoryId;
+            }
+
+            if (currentTrajId != -1)
+            {
+                _logger.LogWarning("CartographerService: MCL converged but active trajectory {TrajectoryId} exists. " +
+                    "Waiting for trajectory to finish before adding new one.", currentTrajId);
+
+                // Don't add trajectory yet, MCL will check again on next scan
+                // This should never happen if first defensive check is working, but being defensive
+                OnPoseUpdated(currentPose, null, mclReliability: reliability, mclMae: mae);
+                return;
+            }
+
+            // Determine which pose to use for trajectory:
+            // - PoseStable: MCL converged successfully, use MCL's current pose
+            // - Timeout/MaxIterations: MCL failed to converge, fall back to initial pose
+            Pose poseForTrajectory;
+            if (convergenceReason == MclConvergenceReason.PoseStable)
+            {
+                poseForTrajectory = currentPose;
+                _logger.LogInformation("CartographerService: MCL converged successfully, using MCL pose ({X:F3}, {Y:F3})",
+                    currentPose.Position.X, currentPose.Position.Y);
+            }
+            else
+            {
+                // Timeout or MaxIterations - use initial pose (saved from auto-resume)
+                poseForTrajectory = _mclProcessor?.InitialPose ?? currentPose;
+                _logger.LogWarning("CartographerService: MCL did not converge (reason={Reason}), falling back to initial pose ({X:F3}, {Y:F3})",
+                    convergenceReason, poseForTrajectory.Position.X, poseForTrajectory.Position.Y);
+            }
+
+            var (trajId, trajBuilder, poseInMapFrame) = _mapBuilder.AddLocalizationTrajectoryBuilder(_config, poseForTrajectory);
+            lock (_lock)
+            {
+                _trajectoryId = trajId;
+                _trajectoryBuilder = trajBuilder;
+            }
+
+            if (poseInMapFrame.HasValue)
+            {
+                var p = PoseConverter.ToPose(poseInMapFrame.Value);
+                _logger.LogDebug("CartographerService: AddLocalizationTrajectoryBuilder trajId: {TrajId}, poseInMapFrame: [{X}, {Y}, {Z}] yaw: {Yaw:F4} ({YawDeg:F2})",
+                    trajId, poseInMapFrame.Value.Translation.X, poseInMapFrame.Value.Translation.Y, poseInMapFrame.Value.Translation.Z, GetYawFromPose(p), GetYawFromPose(p) * 180.0 / Math.PI);
+                // NOTE: Don't call StartRelocalization here - MCL already found the pose.
+                // StartRelocalization triggers constraint-based relocalization which is redundant
+                // and causes excessive optimization runs (sets _isRelocalized=false).
+            }
+
+            // Pass pose and MCL reliability/MAE for proper score calculation
+            // Use poseForTrajectory to be consistent with what we're using for the trajectory
+            OnPoseUpdated(poseForTrajectory, null, mclReliability: reliability, mclMae: mae);
+
+            // Use deferred trigger to avoid deadlock: FireStateMachine would call PauseSubscriptions
+            // which tries to stop the lidar thread, but we're currently running inside that thread.
+            // QueueStateMachineTrigger fires the trigger from a separate thread.
+            QueueStateMachineTrigger(CartographerTrigger.MclConverged);
+        }
+        else
+        {
+            // Pass MCL reliability and MAE for proper score calculation during convergence
+            OnPoseUpdated(currentPose, null, mclReliability: reliability, mclMae: mae);
+        }
+    }
+
+    /// <summary>Sets MCL base_link to laser transform from primary lidar config. Call once when starting MCL (after SetMap and SetInitialPose).</summary>
+    /// <remarks>Config Transform is "lidar frame to base_link frame": P_base = R*P_lidar + Translation, so Translation = lidar origin in base_link (sensor pose in base_link). MCL expects baseLink2Laser = sensor pose in base_link (same meaning), so we pass (tx, ty, yaw) as-is; no inverse needed.</remarks>
+    private void SetMclBaseLinkToLaser(string primaryLidarId, MclService mcl)
+    {
+        var lidarConfig = string.IsNullOrEmpty(primaryLidarId) ? null : _config.Sensors.Lidars.FirstOrDefault(l => l.DeviceId == primaryLidarId);
+        if (lidarConfig?.Transform == null)
+            return;
+        var lidarTransform = lidarConfig.Transform;
+        double tx = lidarTransform.Translation.X;
+        double ty = lidarTransform.Translation.Y;
+        double yawLidarInBase = lidarTransform.Rotation.ToYawRadian();
+        mcl.SetBaseLinkToLaser(tx, ty, yawLidarInBase);
+    }
+
+    /// <summary>Resolves effective primary lidar for MCL (PrimaryLidarId if present in config, else first lidar). Logs and validates; returns empty if no lidars.</summary>
+    private string GetEffectiveMclPrimaryLidarIdAndLog()
+    {
+        var lidars = _config.Sensors.Lidars;
+        string? configured = string.IsNullOrEmpty(_config.Mcl.PrimaryLidarId) ? null : _config.Mcl.PrimaryLidarId;
+        if (lidars.Count == 0)
+        {
+            if (!string.IsNullOrEmpty(configured))
+                _logger.LogWarning("CartographerService: MCL PrimaryLidarId is set to {PrimaryLidarId} but Sensors.Lidars is empty; MCL will accept any device.", configured);
+            return string.Empty;
+        }
+        if (!string.IsNullOrEmpty(configured))
+        {
+            var found = lidars.Any(l => l.DeviceId == configured);
+            if (found)
+            {
+                _logger.LogInformation("CartographerService: MCL using primary lidar: {PrimaryLidarId}", configured);
+                return configured;
+            }
+            _logger.LogWarning(
+                "CartographerService: MCL PrimaryLidarId '{PrimaryLidarId}' not found in Sensors.Lidars; using first lidar: {FirstId}",
+                configured, lidars[0].DeviceId);
+        }
+        var firstId = lidars[0].DeviceId ?? string.Empty;
+        if (string.IsNullOrEmpty(configured))
+            _logger.LogInformation("CartographerService: MCL using first lidar: {FirstId}", firstId);
+        return firstId;
+    }
+
+    #endregion
+
+    #region Pose Update
+
+    /// <summary>
+    /// Updates pose snapshot and confidence metrics.
+    /// Used by MCL (Relocalizing) and state entry handlers when no background pose thread is running.
+    /// When background pose thread IS running (ScanMapping/Localizing), use UpdateConfidenceMetrics instead.
+    /// </summary>
+    /// <param name="pose">Current pose in global frame</param>
+    /// <param name="covariance">Pose covariance (optional)</param>
+    /// <param name="constraintCount">Number of constraints (default 0)</param>
+    /// <param name="constraintQuality">Average constraint quality 0-1 (default 0)</param>
+    /// <param name="mclReliability">MCL reliability 0-1 (optional, for Relocalizing state)</param>
+    /// <param name="mclMae">MCL mean absolute error in meters (optional, for Relocalizing state)</param>
+    private void OnPoseUpdated(
+        Pose pose,
+        Matrix3x3? covariance,
+        int constraintCount = 0,
+        double constraintQuality = 0.0,
+        double? mclReliability = null,
+        double? mclMae = null)
+    {
+        // Publish pose snapshot (atomic volatile write)
+        // For Relocalizing: use MCL reliability and MAE for score calculation
+        var score = LocalizationScoreCalculator.Calculate(
+            covariance,
+            constraintCount,
+            constraintQuality,
+            mclReliability: mclReliability,
+            mclMae: mclMae);
+
+        // Determine drift status based on MCL reliability
+        var driftStatus = DriftDetector.DriftStatus.Stable;
+        if (mclReliability.HasValue)
+        {
+            if (mclReliability.Value < 0.3)
+                driftStatus = DriftDetector.DriftStatus.Critical;
+            else if (mclReliability.Value < 0.5)
+                driftStatus = DriftDetector.DriftStatus.Warning;
+        }
+
+        Volatile.Write(ref _poseSnapshot, new PoseSnapshot(
+            pose,
+            covariance,
+            score,
+            scanMatchScore: null, // Not available during Relocalizing
+            driftStatus));
+
+        // Also update covariance fields for when pose thread takes over
+        lock (_lock)
+        {
+            _poseCovariance = covariance;
+            _constraintCount = constraintCount;
+            _averageConstraintQuality = constraintQuality;
+        }
+    }
+
+    /// <summary>
+    /// Updates only confidence metrics (covariance, constraints) without touching pose.
+    /// Used by sensor callback during ScanMapping/Localizing when background pose thread handles pose.
+    /// </summary>
+    private void UpdateConfidenceMetrics(Matrix3x3? covariance, int constraintCount, double constraintQuality)
+    {
+        lock (_lock)
+        {
+            _poseCovariance = covariance;
+            _constraintCount = constraintCount;
+            _averageConstraintQuality = constraintQuality;
+        }
+    }
+
+    /// <summary>
+    /// Updates confidence metrics including scan match score for Localizing state.
+    /// Scan match score is critical for detecting map drift.
+    /// </summary>
+    private void UpdateConfidenceMetrics(Matrix3x3? covariance, int constraintCount, double constraintQuality, double scanMatchScore)
+    {
+        lock (_lock)
+        {
+            _poseCovariance = covariance;
+            _constraintCount = constraintCount;
+            _averageConstraintQuality = constraintQuality;
+        }
+        // Scan match score is stored via volatile write (already done before calling this method)
+        // It will be read by background pose thread for LocalizationScore calculation
+    }
+
+    /// <summary>
+    /// Transform raw payload points to global frame using the given pose.
+    /// Used as fallback when CartographerSharp doesn't return SamplePointCloudGlobal.
+    /// </summary>
+    private static List<Vector3> TransformPayloadToGlobalFrame(RangeDataPayload payload, Pose pose)
+    {
+        var yaw = GetYawFromPose(pose);
+        var cosYaw = Math.Cos(yaw);
+        var sinYaw = Math.Sin(yaw);
+        var poseX = pose.Position.X;
+        var poseY = pose.Position.Y;
+
+        var result = new List<Vector3>(payload.BaseLink.Ranges.Count);
+        foreach (var range in payload.BaseLink.Ranges)
+        {
+            // Transform from base_link frame to global frame
+            var localX = range.Position.X;
+            var localY = range.Position.Y;
+            var globalX = poseX + localX * cosYaw - localY * sinYaw;
+            var globalY = poseY + localX * sinYaw + localY * cosYaw;
+            result.Add(new Vector3(globalX, globalY, 0));
+        }
+        return result;
+    }
+
+    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));
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.StateHandlers.cs

@@ -0,0 +1,739 @@
+using CartographerSharp.Mapping;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using Microsoft.AspNetCore.SignalR;
+using Microsoft.Extensions.Options;
+using RobotNet10.RobotApp.Shared;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public partial class CartographerService
+{
+    #region Idle State
+
+    private void OnIdleStateEntry()
+    {
+        lock (_lock)
+        {
+            _currentState = SLAMState.Idle;
+            _lastError = null;
+            _lastException = null;
+        }
+        _ = NotifyStateChangedAsync(SLAMState.Idle);
+    }
+
+    #endregion
+
+    #region Initializing State
+
+    private async void OnInitializingStateEntry()
+    {
+        lock (_lock)
+        {
+            _currentState = SLAMState.Initializing;
+            _lastError = null;
+        }
+        _ = NotifyStateChangedAsync(SLAMState.Initializing);
+
+        try
+        {
+            if (_config.Enable)
+            {
+                // Full SLAM mode: initialize devices and sensor pipeline
+                // Wait for devices to be loaded and connected
+                var devicesLoaded = await _deviceProvider.WaitForDevicesLoadedAsync(TimeSpan.FromSeconds(30));
+                if (!devicesLoaded)
+                {
+                    _logger.LogError("CartographerService: Timeout waiting for devices to be loaded");
+                    throw new TimeoutException("Timeout waiting for devices to be loaded");
+                }
+
+                var devicesConnected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(60));
+                if (!devicesConnected)
+                {
+                    _logger.LogWarning("Some devices are not connected, but continuing initialization");
+                }
+
+                // Initialize sensor manager (subscribes to all sensors)
+                if (_sensorPipeline != null)
+                {
+                    await _sensorPipeline.InitializeAsync();
+                }
+
+                // Create MapBuilder and add trajectory builder using helper
+                _mapBuilder = MapBuilderHelper.CreateMapBuilder(_config);
+                (_trajectoryId, _trajectoryBuilder) = _mapBuilder.AddTrajectoryBuilder(_config);
+
+                // Check for auto-resume: if CurrentMap.name exists with valid map and pose, auto-start localization
+                var savedMapName = LoadCurrentMapName();
+                if (!string.IsNullOrEmpty(savedMapName) && MapExists(savedMapName) && PoseFileExists(savedMapName))
+                {
+                    var savedPose = LoadPoseFromFile(savedMapName);
+                    if (savedPose.HasValue)
+                    {
+                        _logger.LogInformation(
+                            "CartographerService: Auto-resume detected - Map: {MapName}, Pose: ({X:F3}, {Y:F3}, {Z:F3})",
+                            savedMapName, savedPose.Value.Position.X, savedPose.Value.Position.Y, savedPose.Value.Position.Z);
+
+                        // Set pending localization data
+                        lock (_localizationLock)
+                        {
+                            _pendingLocalizationMapName = savedMapName;
+                            _pendingLocalizationInitialPose = savedPose;
+                            _isSetInitialPoseFlow = false;
+                        }
+
+                        // Fire InitializationComplete to transition to Ready, then StartLocalization to auto-resume
+                        FireStateMachine(CartographerTrigger.InitializationComplete);
+                        FireStateMachine(CartographerTrigger.StartLocalization);
+                        return;
+                    }
+                }
+            }
+            FireStateMachine(CartographerTrigger.InitializationComplete);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Initialization failed");
+            lock (_lock)
+            {
+                _lastError = ex.Message;
+                _lastException = ex;
+            }
+            FireStateMachine(CartographerTrigger.InitializationFailed);
+        }
+    }
+
+    #endregion
+
+    #region Ready State
+
+    private void OnReadyStateEntry()
+    {
+        lock (_lock)
+        {
+            _currentState = SLAMState.Ready;
+        }
+
+        // Pause sensor subscriptions to stop receiving new data
+        _sensorPipeline?.PauseSubscriptions();
+
+        // Wait for in-flight sensor data to be processed
+        _sensorPipeline?.WaitForProcessingComplete(200);
+
+        // Notify state change (fire-and-forget)
+        _ = NotifyStateChangedAsync(SLAMState.Ready);
+    }
+
+    #endregion
+
+    #region Relocalizing State
+
+    private void OnInitializingLocalizingStateEntry()
+    {
+        lock (_lock)
+        {
+            _currentState = SLAMState.Relocalizing;
+        }
+
+        // Notify state change
+        _ = NotifyStateChangedAsync(SLAMState.Relocalizing);
+
+        if (_config.Enable)
+        {
+            // Start background task to handle localization setup
+            _ = Task.Run(async () =>
+            {
+                try
+                {
+                    // Get pending data and determine flow type
+                    string? mapName;
+                    Pose initialPose;
+                    bool isSetInitialPoseFlow;
+
+                    lock (_localizationLock)
+                    {
+                        mapName = _pendingLocalizationMapName;
+                        initialPose = _pendingLocalizationInitialPose ?? new();
+                        isSetInitialPoseFlow = _isSetInitialPoseFlow;
+                        _isSetInitialPoseFlow = false; // Reset flag after reading
+                    }
+
+                    if (string.IsNullOrEmpty(mapName))
+                    {
+                        throw new InvalidOperationException("No map name set for localization");
+                    }
+
+                    _logger.LogInformation("CartographerService: Background localization task started for map: {MapName}, isSetInitialPoseFlow: {IsSetInitialPoseFlow}", mapName, isSetInitialPoseFlow);
+
+                    IMapBuilder? mapBuilder;
+                    bool useMcl = _config.Mcl.Enabled && _mcl != null;
+
+                    // =================================================================================
+                    // STEP 1: Prepare MapBuilder
+                    // =================================================================================
+                    if (isSetInitialPoseFlow)
+                    {
+                        // SetInitialPose flow: Finish old trajectory, reuse existing MapBuilder
+                        int oldTrajectoryId;
+                        lock (_lock)
+                        {
+                            mapBuilder = _mapBuilder ?? throw new InvalidOperationException("MapBuilder is null during SetInitialPose flow");
+
+                            oldTrajectoryId = _trajectoryId;
+
+                            // Finish old trajectory if exists
+                            if (oldTrajectoryId >= 0)
+                            {
+                                _logger.LogInformation("CartographerService: Finishing old trajectory {TrajectoryId} before restarting with new pose", oldTrajectoryId);
+                                mapBuilder.FinishTrajectory(oldTrajectoryId);
+                                _trajectoryBuilder = null;
+                                _trajectoryId = -1;
+                            }
+                        }
+
+                        // Wait for trajectory to finish
+                        if (oldTrajectoryId >= 0)
+                        {
+                            await mapBuilder.WaitForTrajectoryFinishedAsync(oldTrajectoryId, CancellationToken.None);
+
+                            // Verify trajectory is finished (defensive check)
+                            int currentTrajId;
+                            lock (_lock)
+                            {
+                                currentTrajId = _trajectoryId;
+                            }
+
+                            if (currentTrajId != -1)
+                            {
+                                _logger.LogError("CartographerService: Trajectory {OldId} not finished after wait, current trajectory: {CurrentId}",
+                                    oldTrajectoryId, currentTrajId);
+                                throw new InvalidOperationException($"Old trajectory {oldTrajectoryId} not finished properly");
+                            }
+
+                            _logger.LogInformation("CartographerService: Verified trajectory {TrajectoryId} is finished", oldTrajectoryId);
+                        }
+                    }
+                    else
+                    {
+                        // StartLocalization flow: Load map from storage
+                        _logger.LogInformation("CartographerService: Loading map from storage for map: {MapName}", mapName);
+
+                        // Load map files directly from configuration
+                        var loadResult = LoadMapFromDirectory(mapName)
+                                            ?? throw new InvalidOperationException($"CartographerService: Failed to load map files: {mapName}");
+
+                        mapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
+                            loadResult,
+                            _config,
+                            _logger,
+                            loadFrozenState: true);
+
+                        lock (_lock)
+                        {
+                            // Reset trajectory from old MapBuilder (created in Initializing state)
+                            // before assigning new MapBuilder from pbstream
+                            _trajectoryId = -1;
+                            _trajectoryBuilder = null;
+
+                            _mapBuilder = mapBuilder;
+                            _currentMapName = mapName;
+                        }
+
+                        // Load occupancy grid from PGM
+                        _occupancyGridManager.LoadFromPgm(mapName);
+                    }
+
+                    // =================================================================================
+                    // STEP 2: Determine if we can use MCL
+                    // =================================================================================
+                    bool canUseMcl = useMcl && _occupancyGridManager.OccupancyGridMcl != null;
+                    _logger.LogInformation("CartographerService: MCL check - useMcl: {UseMcl}, OccupancyGridMcl != null: {GridNotNull}, canUseMcl: {CanUseMcl}",
+                        useMcl, _occupancyGridManager.OccupancyGridMcl != null, canUseMcl);
+
+                    // =================================================================================
+                    // STEP 3: Resume sensors AFTER map is prepared (only if sensor pipeline exists)
+                    // =================================================================================
+                    _sensorPipeline?.ResumeSubscriptions();
+
+                    // =================================================================================
+                    // STEP 4: Start localization - MCL path or non-MCL path
+                    // =================================================================================
+                    if (canUseMcl && _mcl != null && _mclProcessor != null)
+                    {
+                        if (_occupancyGridManager.OccupancyGridMcl is null)
+                        {
+                            throw new InvalidOperationException("OccupancyGridMcl is null despite canUseMcl being true");
+                        }
+                        // MCL path: Start MCL processor, trajectory will be added when MCL converges
+                        var grid = _occupancyGridManager.OccupancyGridMcl;
+                        var seedPose = initialPose;
+
+                        _mcl.SetMap(grid);
+                        _mcl.SetInitialPose(seedPose, useTightNoise: true);
+
+                        var effectivePrimary = GetEffectiveMclPrimaryLidarIdAndLog();
+                        SetMclBaseLinkToLaser(effectivePrimary, _mcl);
+
+                        // Start MCL processor (will run until convergence, then fire MclConverged trigger in RunMclOnScan)
+                        _mclProcessor.Start(seedPose, effectivePrimary, grid);
+
+                        _logger.LogInformation(
+                            "CartographerService: MCL started for map {MapName} with pose [{X}, {Y}, {Z}]; trajectory will be added when MCL converges.",
+                            mapName, seedPose.Position.X, seedPose.Position.Y, seedPose.Position.Z);
+
+                        OnPoseUpdated(seedPose, null);
+
+                        // Note: MclConverged trigger will be fired by RunMclOnScan when MCL converges
+                    }
+                    else
+                    {
+                        // Non-MCL path: Add trajectory immediately, then fire MclSkipped
+                        lock (_lock)
+                        {
+                            if (_mapBuilder != null)
+                            {
+                                _logger.LogInformation("CartographerService: Adding trajectory without MCL (MCL disabled or no occupancy grid) with initial pose: [{X}, {Y}, {Z}]",
+                                    initialPose.Position.X, initialPose.Position.Y, initialPose.Position.Z);
+                                var (trajId, trajBuilder, poseInMapFrame) = _mapBuilder.AddLocalizationTrajectoryBuilder(_config, initialPose);
+                                _trajectoryId = trajId;
+                                _trajectoryBuilder = trajBuilder;
+                                // NOTE: Don't call StartRelocalization - AddLocalizationTrajectoryBuilder already
+                                // sets InitialTrajectoryPose with the correct pose. StartRelocalization would
+                                // trigger redundant constraint-based relocalization and cause excessive optimization.
+
+                                OnPoseUpdated(initialPose, null);
+                            }
+                        }
+
+                        // Fire MclSkipped to transition to Localizing
+                        _logger.LogInformation("CartographerService: Trajectory added without MCL; firing MclSkipped to transition to Localizing");
+                        FireStateMachine(CartographerTrigger.MclSkipped);
+                    }
+
+                    _logger.LogInformation("CartographerService: Background localization task completed successfully for map: {MapName}", mapName);
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogError(ex, "CartographerService: Background localization task failed");
+
+                    // Update error tracking
+                    lock (_lock)
+                    {
+                        _lastError = ex.Message;
+                        _lastException = ex;
+                        _mapBuilder = null;
+                        _trajectoryBuilder = null;
+                        _trajectoryId = -1;
+                        _currentMapName = null;
+                    }
+
+                    // Fire ErrorOccurred trigger to transition to Error state
+                    FireStateMachine(CartographerTrigger.ErrorOccurred);
+                }
+            });
+        }
+        else
+        {
+            if (string.IsNullOrEmpty(_pendingLocalizationMapName))
+            {
+                FireStateMachine(CartographerTrigger.ErrorOccurred);
+                _logger.LogError("Start localization with no map name");
+            }
+            else
+            {
+                _currentMapName = _pendingLocalizationMapName;
+                if (_pendingLocalizationInitialPose.HasValue)
+                {
+                    var initPose = _pendingLocalizationInitialPose ?? new();
+                    OnPoseUpdated(initPose, null);
+                }
+                // Load occupancy grid from PGM
+                _occupancyGridManager.LoadFromPgm(_pendingLocalizationMapName);
+                FireStateMachine(CartographerTrigger.MclSkipped);
+            }
+        }
+    }
+
+    private void OnInitializingLocalizingStateExit()
+    {
+        // Pause sensors when leaving this state (will be resumed again in Localizing if transitioning there)
+        _sensorPipeline?.PauseSubscriptions();
+        _sensorPipeline?.WaitForProcessingComplete(200);
+    }
+
+    #endregion
+
+    #region Localizing State
+
+    private void OnLocalizingStateEntry()
+    {
+        lock (_lock)
+        {
+            _currentState = SLAMState.Localizing;
+        }
+
+        // Log trajectory builder status for debugging
+        _logger.LogInformation(
+            "CartographerService: Entered Localizing state - trajectoryBuilder={TbNotNull}, trajectoryId={TrajId}",
+            _trajectoryBuilder != null, _trajectoryId);
+
+        try
+        {
+            // Save current map name to file for auto-resume on next startup
+            var mapName = _currentMapName;
+            if (!string.IsNullOrEmpty(mapName))
+            {
+                SaveCurrentMapName(mapName);
+            }
+
+            // Reset drift detector for new localization session
+            _driftDetector.Reset();
+
+            // Start pose sync stopwatch for periodic pose saving
+            _poseSyncStopwatch.Restart();
+
+            // Start point cloud update stopwatch for throttled point cloud updates
+            _pointCloudUpdateStopwatch.Restart();
+
+            // Just resume subscriptions - no special handling needed
+            // (SetInitialPose flow is handled in OnInitializingLocalizingStateEntry)
+            _logger.LogInformation("CartographerService: Resuming sensor subscriptions for Localizing state");
+            _sensorPipeline?.ResumeSubscriptions();
+            _logger.LogInformation("CartographerService: Sensor subscriptions resumed successfully");
+
+            // Start background pose extrapolation thread at 100Hz
+            StartPoseThread(10);
+            _logger.LogInformation("CartographerService: Pose thread started for Localizing state");
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Error during OnLocalizingStateEntry");
+            throw;
+        }
+
+        _ = NotifyStateChangedAsync(SLAMState.Localizing);
+    }
+
+    private void OnLocalizingStateExit()
+    {
+        // Pause sensor subscriptions when leaving Localizing state
+        _sensorPipeline?.PauseSubscriptions();
+        _sensorPipeline?.WaitForProcessingComplete(200);
+
+        // Stop background pose thread before cleanup
+        StopPoseThread();
+
+        // Stop MCL processor if running
+        _mclProcessor?.Stop();
+
+        // Stop pose sync stopwatch and save final pose
+        _poseSyncStopwatch.Stop();
+        var mapName = _currentMapName;
+        if (!string.IsNullOrEmpty(mapName))
+        {
+            SavePoseToFile(mapName, Volatile.Read(ref _poseSnapshot).Pose);
+        }
+
+        // Check if this is SetInitialPose flow (transition back to InitializingLocalizing)
+        bool isSetInitialPoseFlow;
+        lock (_localizationLock)
+        {
+            isSetInitialPoseFlow = _isSetInitialPoseFlow;
+        }
+
+        // Cleanup localization resources when transitioning to Ready (StopLocalization trigger)
+        // But keep resources when transitioning back to InitializingLocalizing (SetInitialPose) or self-transitioning
+        var nextState = State; // Will be updated after this exits
+        if (!isSetInitialPoseFlow && (nextState == SLAMState.Ready || _currentState == SLAMState.Ready))
+        {
+            lock (_lock)
+            {
+                _mapBuilder?.FinishTrajectoryAndDispose(_trajectoryId);
+                _trajectoryBuilder = null;
+                _trajectoryId = -1;
+                _mapBuilder = null;
+                _currentMapName = null;
+
+                _poseCovariance = null;
+
+                _constraintCache.Invalidate();
+            }
+
+            Volatile.Write(ref _poseSnapshot, PoseSnapshot.Empty);
+
+            // Clear localization data from processors
+            _slamResultProcessor?.ClearLocalizationData();
+
+            _logger.LogInformation("CartographerService: Localization resources cleaned up");
+        }
+        else if (isSetInitialPoseFlow)
+        {
+            _logger.LogInformation("CartographerService: SetInitialPose flow detected, keeping MapBuilder for restart");
+        }
+    }
+
+    #endregion
+
+    #region ScanMapping State
+
+    private void OnScanMappingStateEntry()
+    {
+        if (!_config.Enable) return;
+        lock (_lock)
+        {
+            _currentState = SLAMState.ScanMapping;
+        }
+        Volatile.Write(ref _poseSnapshot, PoseSnapshot.Empty);
+        Volatile.Write(ref _lastMatchingScore, -1.0);
+        _ = NotifyStateChangedAsync(SLAMState.ScanMapping);
+
+        _ = Task.Run(async () =>
+        {
+            // IMPORTANT: Sensors are already PAUSED from previous state (OnScanMappingStateExit or OnReadyStateEntry)
+            // This ensures no sensor data is processed before we set the initial pose
+
+            // Wait for in-flight sensor data to be processed
+            _sensorPipeline?.WaitForProcessingComplete(200);
+
+            // Reset processors for new scan mapping session
+            _slamResultProcessor?.ResetForNewSession();
+            _occupancyGridManager.ResetCounter();
+
+            // Reset MapBuilder if there are existing trajectory states (theo xloc flow)
+            if (_mapBuilder != null && _mapBuilder.DisposeIfHasTrajectories())
+            {
+                _trajectoryBuilder = null;
+                _trajectoryId = -1;
+                _mapBuilder = null;
+            }
+
+            // STEP 2: Perform wall alignment to calculate aligned pose
+            Pose wallAlignedPose = CalculateWallAlignedPoseAsync();
+            lock (_wallAlignedPoseLock)
+            {
+                _wallAlignedPose = wallAlignedPose; // Store for use when saving map
+            }
+
+            // STEP 3: Pause sensors again before creating trajectory
+            // This prevents sensor data from being processed before we set initial pose
+            _sensorPipeline?.PauseSubscriptions();
+            _logger.LogInformation("CartographerService: Sensors paused before creating trajectory");
+
+            // Wait for in-flight sensor data to be processed
+            _sensorPipeline?.WaitForProcessingComplete(200);
+
+            // STEP 4: Create MapBuilder and add trajectory builder
+            if (_mapBuilder == null || _trajectoryBuilder == null)
+            {
+                _mapBuilder ??= MapBuilderHelper.CreateMapBuilder(_config);
+
+                // Add trajectory builder (without sensor data, extrapolator not initialized yet)
+                (_trajectoryId, _trajectoryBuilder) = _mapBuilder.AddTrajectoryBuilder(_config);
+
+                // Log wall-aligned pose (will be applied when saving map via TransformPbstreamOrigin)
+                _logger.LogInformation(
+                    "CartographerService: Wall alignment calculated for trajectory {TrajectoryId} - " +
+                    "Position: [{X:F3}, {Y:F3}, {Z:F3}], Yaw: {Yaw:F3}rad ({YawDeg:F1}°). Will be applied when saving map.",
+                    _trajectoryId,
+                    wallAlignedPose.Position.X,
+                    wallAlignedPose.Position.Y,
+                    wallAlignedPose.Position.Z,
+                    WallAlignmentHelper.GetYawFromQuaternion(wallAlignedPose.Orientation),
+                    WallAlignmentHelper.GetYawFromQuaternion(wallAlignedPose.Orientation) * 180.0 / Math.PI);
+            }
+
+            if (_trajectoryBuilder == null)
+            {
+                _logger.LogWarning("CartographerService: Trajectory builder is null when entering ScanMapping state");
+            }
+
+            // STEP 6: Now resume sensors - extrapolator will be initialized with correct initial pose
+            _sensorPipeline?.ResumeSubscriptions();
+            _logger.LogInformation("CartographerService: Sensors resumed for scan mapping");
+
+            // Start background pose extrapolation thread at 20Hz
+            StartPoseThread(50);
+
+            // Start pose sync stopwatch for periodic pose saving
+            _poseSyncStopwatch.Restart();
+
+            // Start point cloud update stopwatch for throttled point cloud updates
+            _pointCloudUpdateStopwatch.Restart();
+        });
+
+    }
+
+    private void OnScanMappingStateExit()
+    {
+        if (!_config.Enable) return;
+        // Pause sensor subscriptions immediately when leaving ScanMapping state
+        _sensorPipeline?.PauseSubscriptions();
+
+        // Stop background pose thread before cleanup
+        StopPoseThread();
+
+        // Note: Do NOT manually update _currentState here - state machine will handle state transition
+        // The state will be updated when ExecuteOnEntry of next state is called
+
+        // Stop pose sync stopwatch and save final pose
+        _poseSyncStopwatch.Stop();
+        var mapName = _currentMapName;
+        if (!string.IsNullOrEmpty(mapName))
+        {
+            SavePoseToFile(mapName, Volatile.Read(ref _poseSnapshot).Pose);
+        }
+
+        // Wait for in-flight sensor data to be processed
+        _sensorPipeline?.WaitForProcessingComplete(200);
+    }
+
+    #endregion
+
+    #region SavingMap State
+
+    private void OnSavingMapStateEntry()
+    {
+        if (!_config.Enable) return;
+        lock (_lock)
+        {
+            _currentState = SLAMState.SavingMap;
+        }
+
+        // Pause sensor subscriptions to stop receiving new data
+        _sensorPipeline?.PauseSubscriptions();
+
+        // Wait for in-flight sensor data to be processed
+        // This prevents "Cannot add node to finished trajectory" errors
+        _sensorPipeline?.WaitForProcessingComplete(2000);
+
+        // Notify state change (fire-and-forget)
+        _ = NotifyStateChangedAsync(SLAMState.SavingMap);
+
+        // Create CancellationTokenSource for save operation (allows cancellation if needed)
+        _savingMapCts?.Cancel();
+        _savingMapCts?.Dispose();
+        _savingMapCts = new CancellationTokenSource();
+        var cancellationToken = _savingMapCts.Token;
+
+        // Start background save task to handle async save workflow
+        _ = Task.Run(async () =>
+        {
+            try
+            {
+                string? mapName;
+                lock (_lock)
+                {
+                    mapName = _currentMapName;
+                }
+
+                if (string.IsNullOrEmpty(mapName))
+                {
+                    throw new InvalidOperationException("No map name set for save operation");
+                }
+
+                var mapBuilder = MapBuilder ?? throw new InvalidOperationException("MapBuilder is not available for save operation");
+                var trajectoryId = TrajectoryId;
+
+                // Get wall-aligned pose with proper synchronization
+                Pose? wallAlignedPose;
+                lock (_wallAlignedPoseLock)
+                {
+                    wallAlignedPose = _wallAlignedPose;
+                }
+
+                _logger.LogInformation("CartographerService: Background save task started for map: {MapName}", mapName);
+
+                // Execute save map logic using MapSaveProcessor
+                // This handles: finish trajectory, optimization, pbstream, PGM, PNG, JPG, metadata
+                if (_mapSaveProcessor == null)
+                {
+                    throw new InvalidOperationException("MapSaveProcessor is not available (CartographerConfiguration.Enable = false)");
+                }
+
+                // Create progress callback to send updates via SignalR
+                async Task ProgressCallback(int workItemsAdded, int workItemsCompleted, int percentComplete)
+                {
+                    try
+                    {
+                        await _hubContext.Clients.All.SendAsync("OnMapSaveProgress",
+                            workItemsAdded, workItemsCompleted, percentComplete, cancellationToken);
+                    }
+                    catch (Exception ex)
+                    {
+                        _logger.LogWarning(ex, "CartographerService: Failed to send map save progress update");
+                    }
+                }
+
+                var mapPath = await _mapSaveProcessor.SaveMapAsync(mapName, mapBuilder, trajectoryId, ProgressCallback, wallAlignedPose, cancellationToken);
+
+                _logger.LogInformation("CartographerService: Background save task completed successfully for map: {MapName}, path: {MapPath}", mapName, mapPath);
+
+                // Save current map name to file for auto-resume on next startup
+                SaveCurrentMapName(mapName);
+
+                // Fire MapSaved trigger to transition to Ready state
+                FireStateMachine(CartographerTrigger.MapSaved);
+            }
+            catch (OperationCanceledException)
+            {
+                _logger.LogWarning("CartographerService: Background save task was cancelled");
+
+                // Update error tracking
+                lock (_lock)
+                {
+                    _lastError = "Save operation was cancelled";
+                    _lastException = null;
+                }
+
+                // Fire ErrorOccurred trigger to transition to Error state
+                FireStateMachine(CartographerTrigger.ErrorOccurred);
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "CartographerService: Background save task failed");
+
+                // Update error tracking
+                lock (_lock)
+                {
+                    _lastError = ex.Message;
+                    _lastException = ex;
+                }
+
+                // Fire ErrorOccurred trigger to transition to Error state
+                FireStateMachine(CartographerTrigger.ErrorOccurred);
+            }
+        }, cancellationToken);
+    }
+
+    #endregion
+
+    #region Error State
+
+    private void OnErrorStateEntry()
+    {
+        lock (_lock)
+        {
+            _currentState = SLAMState.Error;
+            _logger.LogError(
+                "CartographerService: Entered Error state. Error: {Error}. " +
+                "Call Reset() to return to Idle state and reinitialize.",
+                _lastError);
+
+            // Log exception details if available
+            if (_lastException != null)
+            {
+                _logger.LogError(_lastException,
+                    "CartographerService: Exception details for Error state");
+            }
+        }
+        _ = NotifyStateChangedAsync(SLAMState.Error);
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.StateMachine.cs

@@ -0,0 +1,127 @@
+using Appccelerate.StateMachine;
+using Appccelerate.StateMachine.Machine;
+using Microsoft.AspNetCore.SignalR;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public partial class CartographerService
+{
+    #region State Machine Definition
+
+    private PassiveStateMachine<SLAMState, CartographerTrigger> BuildStateMachine()
+    {
+        var builder = new StateMachineDefinitionBuilder<SLAMState, CartographerTrigger>();
+
+        // Idle state
+        builder.In(SLAMState.Idle)
+            .ExecuteOnEntry(OnIdleStateEntry)
+            .On(CartographerTrigger.Start)
+            .Goto(SLAMState.Initializing);
+
+        // Initializing state
+        builder.In(SLAMState.Initializing)
+            .ExecuteOnEntry(OnInitializingStateEntry)
+            .On(CartographerTrigger.InitializationComplete)
+            .Goto(SLAMState.Ready)
+            .On(CartographerTrigger.InitializationFailed)
+            .Goto(SLAMState.Error);
+
+        // Ready state
+        builder.In(SLAMState.Ready)
+            .ExecuteOnEntry(OnReadyStateEntry)
+            .On(CartographerTrigger.StartLocalization)
+            .Goto(SLAMState.Relocalizing)
+            .On(CartographerTrigger.StartScanMapping)
+            .Goto(SLAMState.ScanMapping)
+            .On(CartographerTrigger.Shutdown)
+            .Goto(SLAMState.Idle);
+
+        // InitializingLocalizing state - with background task for async localization setup
+        // Background task will load map, start MCL (if enabled), or add trajectory (if MCL disabled), then fire MclConverged/MclSkipped trigger
+        builder.In(SLAMState.Relocalizing)
+            .ExecuteOnEntry(OnInitializingLocalizingStateEntry)
+            .ExecuteOnExit(OnInitializingLocalizingStateExit)
+            .On(CartographerTrigger.MclConverged)
+            .Goto(SLAMState.Localizing)
+            .On(CartographerTrigger.MclSkipped)
+            .Goto(SLAMState.Localizing)
+            .On(CartographerTrigger.StopLocalization)
+            .Goto(SLAMState.Ready)
+            .On(CartographerTrigger.ErrorOccurred)
+            .Goto(SLAMState.Error);
+
+        // Localizing state
+        builder.In(SLAMState.Localizing)
+            .ExecuteOnEntry(OnLocalizingStateEntry)
+            .ExecuteOnExit(OnLocalizingStateExit)
+            .On(CartographerTrigger.MclConverged)
+            .Goto(SLAMState.Localizing)  // Self-transition: handle MCL convergence after it converges
+            .On(CartographerTrigger.StartLocalization)
+            .Goto(SLAMState.Relocalizing)  // Allow transition back to InitializingLocalizing for SetInitialPose
+            .On(CartographerTrigger.StopLocalization)
+            .Goto(SLAMState.Ready)
+            .On(CartographerTrigger.ErrorOccurred)
+            .Goto(SLAMState.Error);
+
+        // ScanMapping state
+        builder.In(SLAMState.ScanMapping)
+            .ExecuteOnEntry(OnScanMappingStateEntry)
+            .ExecuteOnExit(OnScanMappingStateExit)
+            .On(CartographerTrigger.SaveMap)
+            .Goto(SLAMState.SavingMap)
+            .On(CartographerTrigger.ErrorOccurred)
+            .Goto(SLAMState.Error);
+
+        // SavingMap state - with background task for async save workflow
+        builder.In(SLAMState.SavingMap)
+            .ExecuteOnEntry(OnSavingMapStateEntry)
+            .On(CartographerTrigger.MapSaved)
+            .Goto(SLAMState.Ready)
+            .On(CartographerTrigger.ErrorOccurred)
+            .Goto(SLAMState.Error);
+
+        // Error state
+        builder.In(SLAMState.Error)
+            .ExecuteOnEntry(OnErrorStateEntry)
+            .On(CartographerTrigger.Reset)
+            .Goto(SLAMState.Idle);
+
+        return builder
+            .WithInitialState(SLAMState.Idle)
+            .Build()
+            .CreatePassiveStateMachine();
+    }
+
+    #endregion
+
+    #region State Machine Helpers
+
+    /// <summary>
+    /// Thread-safe wrapper for state machine Fire() (Fix 7).
+    /// PassiveStateMachine is not thread-safe; all Fire/Start/Stop calls must be synchronized.
+    /// </summary>
+    private void FireStateMachine(CartographerTrigger trigger)
+    {
+        lock (_stateMachineLock)
+        {
+            _stateMachine.Fire(trigger);
+        }
+    }
+
+    private async Task NotifyStateChangedAsync(SLAMState state)
+    {
+        try
+        {
+            await _hubContext.Clients.All.SendAsync("OnStateChanged", state);
+            _logger.LogDebug("CartographerService: Notified state change to {State}", state);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "CartographerService: Failed to notify state change to {State}", state);
+        }
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.cs

@@ -0,0 +1,500 @@
+using Appccelerate.StateMachine;
+using Appccelerate.StateMachine.Machine;
+using CartographerSharp.Mapping;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Sensor;
+using Microsoft.AspNetCore.SignalR;
+using Microsoft.Extensions.Options;
+using System.Collections.Concurrent;
+using System.Diagnostics;
+using System.Globalization;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Hubs;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.RobotApp.Shared;
+using RobotNet10.RobotApp.Shared.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+using RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+using SkiaSharp;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Main service for Cartographer SLAM integration.
+/// Manages state machine, sensor subscriptions, and handles both localization and scan mapping.
+/// Implements IHostedService to start automatically when application starts.
+///
+/// Split into partial classes:
+///   CartographerService.cs              - Fields, properties, constructor
+///   CartographerService.StateMachine.cs - State machine definition and helpers
+///   CartographerService.StateHandlers.cs- State entry/exit handlers
+///   CartographerService.SensorProcessing.cs - Sensor data callbacks and MCL processing
+///   CartographerService.ApiMethods.cs   - ISLAMService method implementations
+///   CartographerService.MapManagement.cs- Map CRUD, loading, wall alignment
+///   CartographerService.Lifecycle.cs    - IHostedService, state persistence, IDisposable
+/// </summary>
+public partial class CartographerService : ISLAMService, IHostedService, IDisposable
+{
+    #region Fields
+
+    private readonly PassiveStateMachine<SLAMState, CartographerTrigger> _stateMachine;
+    private readonly CartographerConfiguration _config;
+    private readonly IDeviceProvider _deviceProvider;
+    private readonly SensorPipeline? _sensorPipeline;
+    private readonly IHubContext<SLAMHub> _hubContext;
+    private readonly ILogger<CartographerService> _logger;
+    private readonly Lock _lock = new();
+    private readonly Lock _stateMachineLock = new();
+
+    // State tracking (synchronized with state machine in ExecuteOnEntry handlers)
+    private SLAMState _currentState = SLAMState.Idle;
+
+    // Error tracking
+    private string? _lastError;
+    private Exception? _lastException;
+
+    // Unified MapBuilder and TrajectoryBuilder for both ScanMapping and Localization
+    private IMapBuilder? _mapBuilder;
+    private ITrajectoryBuilder? _trajectoryBuilder;
+    private int _trajectoryId = -1;
+    private string? _currentMapName; // Store current map name for both ScanMapping and Localization
+    private Pose? _wallAlignedPose; // Store wall-aligned pose calculated during ScanMapping entry for use when saving
+    private readonly Lock _wallAlignedPoseLock = new(); // Synchronization for _wallAlignedPose
+    private CancellationTokenSource? _savingMapCts; // CancellationTokenSource for save map operation
+
+    // Covariance computation throttle (Fix 3)
+    private int _covarianceComputing; // 0 = idle, 1 = computing
+
+    // Pose sync: Stopwatch for periodic pose saving during Localizing/ScanMapping
+    private readonly Stopwatch _poseSyncStopwatch = new();
+
+    // Pose snapshot: lock-free cached pose + confidence (updated by background thread or MCL)
+    // Access via Volatile.Read/Write for memory ordering guarantees
+    private PoseSnapshot _poseSnapshot = PoseSnapshot.Empty;
+
+    // Background pose extrapolation thread (runs at 20Hz for ScanMapping, 100Hz for Localizing)
+    private Thread? _poseThread;
+    private volatile bool _poseThreadRunning;
+
+    // Background trigger thread: handles deferred state machine triggers from sensor processing
+    private Thread? _triggerThread;
+    private volatile bool _triggerThreadRunning;
+
+    // Confidence metrics: written by sensor callback, read by pose thread
+    private Matrix3x3? _poseCovariance;
+    private int _constraintCount = 0;
+    private double _averageConstraintQuality = 0.0;
+    private double _lastMatchingScore = -1.0; // ScanMapping: PoseConfidence from scan matcher (use Volatile.Read/Write)
+
+    // Localization: Constraint caching for performance
+    private readonly TrajectoryConstraintCache _constraintCache = new(TimeSpan.FromMilliseconds(100));
+
+    // MCL (Monte Carlo Localization): when Enabled, SetInitialPoseAsync runs MCL until convergence then adds trajectory (xloc flow). Created internally, only used here.
+    private readonly MclService? _mcl;
+    private readonly MclProcessor? _mclProcessor;
+
+    // Pending state machine trigger: used to defer FireStateMachine calls from lidar processing thread
+    // This prevents deadlock when MCL converges and tries to stop the lidar thread from within itself
+    // Uses int because volatile cannot be used with nullable enums; -1 = no pending, >= 0 = trigger value
+    private int _pendingTrigger = -1;
+    private readonly ManualResetEventSlim _pendingTriggerEvent = new(false);
+
+    // Sample point cloud in map/global frame from AddSensorData SamplePointCloudGlobal; returned by GetAggregatedSamplePointCloud
+    private List<Vector3> _completedAccumulatedSamplePointClouds = [];
+    private readonly Lock _pointCloudLock = new();
+    private readonly Stopwatch _pointCloudUpdateStopwatch = Stopwatch.StartNew();
+
+    // Reusable buffer for MCL scan conversion (Fix 6a)
+    private List<Vector2>? _mclScanBuffer;
+
+    // Pending localization data (set by StartLocalization, consumed by state handlers)
+    private readonly Lock _localizationLock = new();
+    private string? _pendingLocalizationMapName;
+    private Pose? _pendingLocalizationInitialPose;
+    private bool _isSetInitialPoseFlow; // Flag to differentiate SetInitialPose from StartLocalization
+
+    // Helper processors
+    private readonly SlamResultProcessor? _slamResultProcessor;
+    private readonly OccupancyGridManager _occupancyGridManager;
+    private readonly MapSaveProcessor? _mapSaveProcessor;
+
+    // Drift detection for Localizing state
+    private readonly DriftDetector _driftDetector;
+
+    // Map processing status tracking
+    private readonly ConcurrentDictionary<string, bool> _mapProcessingStatus = new();
+
+    private readonly IOdometryEstimator? _odometryEstimator;
+
+    // Dispose tracking
+    private volatile bool _disposed = false;
+
+    #endregion
+
+    #region Properties
+
+    /// <summary>
+    /// Gets the MapBuilder instance (internal - for OccupancyGridProvider).
+    /// Works for both ScanMapping and Localization modes.
+    /// </summary>
+    internal IMapBuilder? MapBuilder => _mapBuilder;
+
+    /// <summary>
+    /// Gets the current trajectory ID (internal).
+    /// Works for both ScanMapping and Localization modes.
+    /// </summary>
+    internal int TrajectoryId => _trajectoryId;
+
+    /// <summary>
+    /// Gets the current pose in global (map) frame.
+    /// Lock-free: returns cached snapshot updated by background pose thread (20Hz ScanMapping, 100Hz Localizing)
+    /// or directly by MCL during Relocalizing.
+    /// </summary>
+    public Pose CurrentPose => Volatile.Read(ref _poseSnapshot).Pose;
+
+    /// <summary>
+    /// Gets the covariance of pose estimate (only available during localization).
+    /// Lock-free: reads from cached snapshot.
+    /// </summary>
+    public Matrix3x3? PoseCovariance => Volatile.Read(ref _poseSnapshot).Covariance;
+
+    /// <summary>
+    /// Gets the current map name (available during Localization or ScanMapping)
+    /// </summary>
+    public string? CurrentMap
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _currentMapName;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Gets the confidence score. Meaning varies by state:
+    /// ScanMapping: PoseConfidence from scan matcher.
+    /// Localizing: LocalizationScore from covariance/constraints/scanMatchScore.
+    /// Relocalizing: MCL reliability.
+    /// Lock-free: reads from cached snapshot.
+    /// </summary>
+    public double? LocalizationScore => Volatile.Read(ref _poseSnapshot).Score;
+
+    /// <summary>
+    /// Gets the current drift detection metrics (only meaningful during Localizing state).
+    /// Provides detailed breakdown of localization quality factors including scan match score,
+    /// odometry residual, and drift status.
+    /// </summary>
+    public DriftDetector.DriftMetrics DriftMetrics => _driftDetector.GetLatestMetrics();
+
+    /// <summary>
+    /// Gets the current drift status (only meaningful during Localizing state).
+    /// Returns Stable, Warning, Critical, or Lost based on combined metrics analysis.
+    /// </summary>
+    public DriftDetector.DriftStatus DriftStatus => _driftDetector.GetLatestMetrics().Status;
+
+    /// <summary>
+    /// Gets the current state (synchronized with state machine)
+    /// </summary>
+    public SLAMState State
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _currentState;
+            }
+        }
+    }
+
+    #endregion
+
+    #region Pose Thread
+
+    /// <summary>
+    /// Starts the background pose extrapolation thread.
+    /// </summary>
+    /// <param name="intervalMs">Polling interval: 50ms for ScanMapping (20Hz), 10ms for Localizing (100Hz)</param>
+    private void StartPoseThread(int intervalMs)
+    {
+        if (_poseThreadRunning) return;
+        _poseThreadRunning = true;
+        _poseThread = new Thread(() => PoseThreadProc(intervalMs))
+        {
+            Name = "PoseExtrapolator",
+            IsBackground = true,
+            Priority = ThreadPriority.AboveNormal
+        };
+        _poseThread.Start();
+    }
+
+    /// <summary>
+    /// Stops the background pose extrapolation thread and waits for it to finish.
+    /// </summary>
+    private void StopPoseThread()
+    {
+        if (!_poseThreadRunning) return;
+        _poseThreadRunning = false;
+        _poseThread?.Join(500);
+        if (_poseThread?.IsAlive == true)
+            _logger.LogWarning("CartographerService: Pose thread did not stop within timeout");
+        _poseThread = null;
+    }
+
+    /// <summary>
+    /// Background thread loop: polls TryGetExtrapolatedPoseFilter, applies localToGlobal transform,
+    /// and publishes global-frame PoseSnapshot via volatile write.
+    /// </summary>
+    private void PoseThreadProc(int intervalMs)
+    {
+        Thread.BeginThreadAffinity();
+        try
+        {
+            while (_poseThreadRunning && !_disposed)
+            {
+                try
+                {
+                    // Read references without lock (tolerate stale by 1 iteration)
+                    var tb = _trajectoryBuilder;
+                    var mb = _mapBuilder;
+                    var trajId = _trajectoryId;
+
+                    if (tb != null)
+                    {
+                        var extrapolated = tb.TryGetExtrapolatedPose(DateTime.UtcNow.Ticks);
+                        if (extrapolated.HasValue)
+                        {
+                            Pose globalPose;
+                            if (mb != null && trajId >= 0)
+                            {
+                                var localToGlobal = mb.PoseGraph.GetLocalToGlobalTransform(trajId);
+                                globalPose = PoseConverter.ToPose(localToGlobal * extrapolated.Value);
+                            }
+                            else
+                            {
+                                globalPose = PoseConverter.ToPose(extrapolated.Value);
+                            }
+
+                            // Build confidence-aware snapshot based on current state
+                            var state = _currentState;
+                            PoseSnapshot snapshot;
+
+                            if (state == SLAMState.ScanMapping)
+                            {
+                                var matchScore = Volatile.Read(ref _lastMatchingScore);
+                                double? score = null;
+                                if (matchScore >= 0)
+                                {
+                                    // Normalize to [0, 1] range if input is in [0, 100] range
+                                    // PoseConfidence from Cartographer returns 0-100 (percentage)
+                                    score = matchScore > 1.0 ? matchScore / 100.0 : matchScore;
+                                    score = Math.Clamp(score.Value, 0.0, 1.0);
+                                }
+                                snapshot = new PoseSnapshot(globalPose, score: score);
+                            }
+                            else
+                            {
+                                // Localizing: read covariance fields and compute score with scan match
+                                Matrix3x3? cov;
+                                int cc;
+                                double cq;
+                                lock (_lock)
+                                {
+                                    cov = _poseCovariance;
+                                    cc = _constraintCount;
+                                    cq = _averageConstraintQuality;
+                                }
+
+                                // Include scan match score for drift detection
+                                var matchScore = Volatile.Read(ref _lastMatchingScore);
+                                double? scanMatchScore = matchScore >= 0 ? matchScore : null;
+
+                                // Get odometry pose for cross-validation (helps distinguish drift vs dynamic obstacles)
+                                Pose? odometryPose = _odometryEstimator?.CurrentPose;
+
+                                // Update drift detector with all available metrics
+                                var driftMetrics = _driftDetector.Update(
+                                    cartographerPose: globalPose,
+                                    odometryPose: odometryPose,
+                                    scanMatchScore: scanMatchScore ?? 0.5,
+                                    covariance: cov,
+                                    constraintCount: cc,
+                                    constraintQuality: cq,
+                                    mclPose: null, // MCL not running during Localizing
+                                    mclReliability: null);
+
+                                // Use drift detector's combined score as the localization score
+                                // This includes scan match score with proper weighting
+                                var locScore = driftMetrics.CombinedScore;
+
+                                snapshot = new PoseSnapshot(
+                                    globalPose,
+                                    cov,
+                                    locScore,
+                                    scanMatchScore,
+                                    driftMetrics.Status);
+                            }
+
+                            Volatile.Write(ref _poseSnapshot, snapshot);
+                        }
+                    }
+                }
+                catch (Exception ex)
+                {
+                    _logger.LogDebug(ex, "CartographerService: PoseThread iteration failed");
+                }
+
+                Thread.Sleep(intervalMs);
+            }
+        }
+        finally
+        {
+            Thread.EndThreadAffinity();
+        }
+    }
+
+    #endregion
+
+    #region Trigger Thread
+
+    /// <summary>
+    /// Queues a state machine trigger to be fired from a dedicated thread.
+    /// This prevents deadlock when MCL converges inside the lidar processing thread
+    /// and tries to stop the same thread via state transition.
+    /// </summary>
+    private void QueueStateMachineTrigger(CartographerTrigger trigger)
+    {
+        Interlocked.Exchange(ref _pendingTrigger, (int)trigger);
+        _pendingTriggerEvent.Set();
+    }
+
+    /// <summary>
+    /// Starts the background trigger thread that handles deferred state machine transitions.
+    /// </summary>
+    private void StartTriggerThread()
+    {
+        if (_triggerThreadRunning) return;
+        _triggerThreadRunning = true;
+        _triggerThread = new Thread(TriggerThreadProc)
+        {
+            Name = "StateMachineTrigger",
+            IsBackground = true,
+            Priority = ThreadPriority.AboveNormal
+        };
+        _triggerThread.Start();
+    }
+
+    /// <summary>
+    /// Stops the background trigger thread.
+    /// </summary>
+    private void StopTriggerThread()
+    {
+        if (!_triggerThreadRunning) return;
+        _triggerThreadRunning = false;
+        _pendingTriggerEvent.Set(); // Wake thread to exit
+        _triggerThread?.Join(1000);
+        if (_triggerThread?.IsAlive == true)
+            _logger.LogWarning("CartographerService: Trigger thread did not stop within timeout");
+        _triggerThread = null;
+    }
+
+    /// <summary>
+    /// Background thread that processes deferred state machine triggers.
+    /// </summary>
+    private void TriggerThreadProc()
+    {
+        while (_triggerThreadRunning && !_disposed)
+        {
+            try
+            {
+                _pendingTriggerEvent.Wait(100); // Wait with timeout to check running flag
+                _pendingTriggerEvent.Reset();
+
+                var triggerValue = Interlocked.Exchange(ref _pendingTrigger, -1);
+                if (triggerValue >= 0)
+                {
+                    var trigger = (CartographerTrigger)triggerValue;
+                    _logger.LogDebug("CartographerService: Firing deferred trigger {Trigger}", trigger);
+                    FireStateMachine(trigger);
+                }
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "CartographerService: Error in TriggerThreadProc");
+            }
+        }
+    }
+
+    #endregion
+
+    #region Constructor
+
+    public CartographerService(
+        IOptions<CartographerConfiguration> configuration,
+        IDeviceProvider deviceProvider,
+        IHubContext<SLAMHub> hubContext,
+        ILogger<CartographerService> logger,
+        IOdometryEstimator odometryEstimator,
+        ILoggerFactory loggerFactory)
+    {
+        _config = configuration?.Value ?? throw new ArgumentNullException(nameof(configuration));
+        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
+        _hubContext = hubContext ?? throw new ArgumentNullException(nameof(hubContext));
+        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
+        _odometryEstimator = odometryEstimator;
+
+        // Initialize components based on Enable flag
+        if (_config.Enable)
+        {
+            // Full SLAM mode: create all components
+            _mcl = _config.Mcl.Enabled ? new MclService(configuration) : null;
+            _sensorPipeline = new SensorPipeline(
+                deviceProvider,
+                odometryEstimator,
+                configuration,
+                logger,
+                OnAddRangeData,
+                OnAddImuData,
+                OnAddOdometryData);
+            _mclProcessor = _config.Mcl.Enabled ? new MclProcessor(_config, loggerFactory.CreateLogger<MclProcessor>()) : null;
+            _slamResultProcessor = new SlamResultProcessor(loggerFactory.CreateLogger<SlamResultProcessor>());
+
+            // Wire up MclProcessor odometry event if enabled
+            if (_mclProcessor != null && _mcl != null)
+            {
+                _mclProcessor.OnOdometryProcessed += (dt, vx, vy, vtheta) => _mcl.OnOdom(dt, vx, vy, vtheta);
+            }
+        }
+        else
+        {
+            // Localization-only mode: minimal components
+            _mcl = null;
+            _sensorPipeline = null;
+            _mclProcessor = null;
+            _slamResultProcessor = null;
+
+            _logger.LogInformation("CartographerService: Running in localization-only mode (Enable = false). Only StartLocalization, SetInitialPose, and StopLocalization are available.");
+        }
+
+        // Always create OccupancyGridManager (needed for both modes to load occupancy grid from file)
+        _occupancyGridManager = new OccupancyGridManager(_config, loggerFactory.CreateLogger<OccupancyGridManager>());
+
+        // Always create MapSaveProcessor (needed for both modes to transform maps and save metadata)
+        _mapSaveProcessor = new MapSaveProcessor(_config, loggerFactory.CreateLogger<MapSaveProcessor>());
+
+        // Always create DriftDetector for monitoring localization quality
+        _driftDetector = new DriftDetector();
+
+        // Build state machine
+        _stateMachine = BuildStateMachine();
+        // Note: State machine will be started in StartAsync (IHostedService)
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/MapBuilderConfiguration.cs

@@ -0,0 +1,89 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Map builder configuration
+/// </summary>
+public class MapBuilderConfiguration
+{
+    public bool? UseTrajectoryBuilder2D { get; set; }
+    public bool? UseTrajectoryBuilder3D { get; set; }
+    public int NumBackgroundThreads { get; set; } = 4;
+    public int OptimizeEveryNNodes { get; set; } = 90;
+    public double MatcherTranslationWeight { get; set; } = 5e2;
+    public double MatcherRotationWeight { get; set; } = 1.6e3;
+    public int? MaxNumFinalIterations { get; set; }
+    public double? GlobalSamplingRatio { get; set; }
+    public bool? LogResidualHistograms { get; set; }
+    public double? GlobalConstraintSearchAfterNSeconds { get; set; }
+    public bool EnableSingleTrajectoryLoopClosure { get; set; } = true;
+    public double SingleTrajectoryLoopClosureDistanceThreshold { get; set; } = 3.0;
+    public OptimizationProblemOptions PoseGraphOptimizationProblemOptions { get; set; } = new();
+    public ConstraintBuilderOptionsConfiguration? ConstraintBuilderOptions { get; set; }
+    public OverlappingSubmapsTrimmerOptions2DConfiguration? OverlappingSubmapsTrimmer2D { get; set; }
+    public bool CollateByTrajectory { get; set; } = false;
+}
+
+/// <summary>
+/// Optimization problem options
+/// </summary>
+public class OptimizationProblemOptions
+{
+    public double HuberScale { get; set; } = 1e1;
+    public double OdometryHuberScale { get; set; } = 1e1;
+    public double LocalSlamPoseHuberScale { get; set; } = 1e1;
+    public double AccelerationWeight { get; set; } = 1.1e2;
+    public double RotationWeight { get; set; } = 1.6e4;
+    public double LocalSlamPoseTranslationWeight { get; set; } = 1e5;
+    public double LocalSlamPoseRotationWeight { get; set; } = 1e5;
+    public double OdometryTranslationWeight { get; set; } = 1e5;
+    public double OdometryRotationWeight { get; set; } = 1e5;
+    public double FixedFramePoseTranslationWeight { get; set; } = 1e1;
+    public double FixedFramePoseRotationWeight { get; set; } = 1e2;
+    public bool FixedFramePoseUseTolerantLoss { get; set; } = false;
+    public double FixedFramePoseTolerantLossParamA { get; set; } = 1.0;
+    public double FixedFramePoseTolerantLossParamB { get; set; } = 1.0;
+    public bool LogSolverSummary { get; set; } = false;
+    public int MaxNumIterations { get; set; } = 50;
+    public CeresSolverOptionsConfiguration? CeresSolverOptions { get; set; }
+}
+
+/// <summary>
+/// Map storage configuration
+/// </summary>
+public class MapStorageConfiguration
+{
+    public string Directory { get; set; } = "./maps";
+    public double OccupancyGridResolution { get; set; } = 0.05;
+    public double MapPadding { get; set; } = 1.0;
+    public bool IncludeUnfinishedSubmaps { get; set; } = false;
+    public string LocalizingStateDirectory { get; set; } = "./localizing_state";
+    public double PoseSyncIntervalSeconds { get; set; } = 5.0;
+    public PngVisualizationOptions PngVisualizationOptions { get; set; } = new();
+}
+
+/// <summary>
+/// PNG visualization options
+/// </summary>
+public class PngVisualizationOptions
+{
+    public bool ShowGrid { get; set; } = true;
+    public bool ShowTrajectory { get; set; } = true;
+    public string GridColor { get; set; } = "#CCCCCC";
+    public string TrajectoryColor { get; set; } = "#FF0000";
+}
+
+/// <summary>
+/// Constraint builder options configuration
+/// </summary>
+public class ConstraintBuilderOptionsConfiguration
+{
+    public double SamplingRatio { get; set; } = 0.3;
+    public double MaxConstraintDistance { get; set; } = 15.0;
+    public double MinScore { get; set; } = 0.55;
+    public double GlobalLocalizationMinScore { get; set; } = 0.6;
+    public double LoopClosureTranslationWeight { get; set; } = 1.1e4;
+    public double LoopClosureRotationWeight { get; set; } = 1.0;
+    public bool LogMatches { get; set; } = true;
+    public FastCorrelativeScanMatcherOptions2DConfiguration? FastCorrelativeScanMatcherOptions { get; set; }
+    public CeresScanMatcherOptions2DConfiguration? CeresScanMatcherOptions { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/MclConfiguration.cs

@@ -0,0 +1,127 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// MCL configuration (from mcl.yaml). Used when SetInitialPoseAsync runs MCL before adding localization trajectory.
+/// </summary>
+public class MclConfiguration
+{
+    public bool Enabled { get; set; }
+    public double InitialNoiseX { get; set; } = 1.0;
+    public double InitialNoiseY { get; set; } = 1.0;
+    public double InitialNoiseYaw { get; set; } = 0.3;
+    /// <summary>When user provides initial pose (SetInitialPoseAsync), use these smaller noise values so particles stay near the pose and iteration 1 does not jump far. If zero, fall back to InitialNoiseX/Y/Yaw.</summary>
+    public double InitialNoiseWhenPoseGivenX { get; set; } = 0.15;
+    public double InitialNoiseWhenPoseGivenY { get; set; } = 0.15;
+    public double InitialNoiseWhenPoseGivenYaw { get; set; } = 0.05;
+    public int ParticlesNum { get; set; } = 1000;
+    public bool UseAugmentedMcl { get; set; }
+    public bool AddRandomParticlesInResampling { get; set; } = true;
+    public double RandomParticlesRate { get; set; } = 0.1;
+    public double[] RandomParticlesNoise { get; set; } = [0.05f, 0.05f, 0.1];
+    public double[] OdomNoiseDdm { get; set; } = [1.0, 0.5, 0.5, 1.5];
+    /// <summary>Odometry noise for omni-directional model (9 params: xx, xy, xyaw, yx, yy, yyaw, yawx, yawy, yawyaw). Used when UseOmniDirectionalModel=true.</summary>
+    public double[] OdomNoiseOdm { get; set; } = [4.0, 1.0, 1.0, 1.0, 4.0, 1.0, 1.0, 1.0, 8.0];
+    public bool UseOmniDirectionalModel { get; set; }
+    public int MeasurementModelType { get; set; } = 0;
+    /// <summary>Scan step for likelihood calculation (xloc: scanStep_ = 10). Use every Nth beam to avoid underflow with many beams. 1 = use all beams (slow, underflow risk), 10 = every 10th beam (recommended).</summary>
+    public int ScanStep { get; set; } = 10;
+    public double ZHit { get; set; } = 0.9;
+    public double ZShort { get; set; } = 0.2;
+    public double ZMax { get; set; } = 0.05;
+    public double ZRand { get; set; } = 0.05;
+    public double VarHit { get; set; } = 0.08;
+    public double LambdaShort { get; set; } = 1.0;
+    public double LambdaUnknown { get; set; } = 0.01;
+    /// <summary>Prior probability for known obstacles (class-conditional model). Default: 0.5. xloc: pKnownPrior_.</summary>
+    public double KnownClassPrior { get; set; } = 0.5;
+    /// <summary>Computed: Prior probability for known obstacles (same as KnownClassPrior, for C# code compatibility).</summary>
+    public double PKnownPrior => KnownClassPrior;
+    /// <summary>Computed: Prior probability for unknown obstacles (1 - KnownClassPrior).</summary>
+    public double PUnknownPrior => 1.0 - KnownClassPrior;
+    public double UnknownScanProbThreshold { get; set; } = 0.9;
+    public double AlphaSlow { get; set; } = 0.001;
+    public double AlphaFast { get; set; } = 0.99;
+    public bool RejectUnknownScan { get; set; } = true;
+    public double ResampleThresholdEss { get; set; } = 0.5;
+    public double[] ResampleThresholds { get; set; } = [0.2, 0.2, 0.2, 0.02f, -99999.0];
+    /// <summary>Reliability transition decay for differential drive (2 params: dist_coeff, yaw_coeff). Used when UseOmniDirectionalModel=false.</summary>
+    public double[] RelTransDdm { get; set; } = [0.0, 0.0];
+    /// <summary>Reliability transition decay for omni-directional (3 params: x_coeff, y_coeff, yaw_coeff). Used when UseOmniDirectionalModel=true.</summary>
+    public double[] RelTransOdm { get; set; } = [0.0, 0.0, 0.0];
+    public int ClassifierType { get; set; }
+    /// <summary>Estimate reliability per particle (MAE-based); used by decision model (xloc: estimateReliability_).</summary>
+    public bool EstimateReliability { get; set; }
+    /// <summary>MAE failure threshold in meters for simple decision model when classifier files not used (xloc: ~0.12).</summary>
+    public double FailureThreshold { get; set; } = 0.12;
+    /// <summary>Use global-localization pose sampler (merge external poses as extra particles).</summary>
+    public bool UseGLPoseSampler { get; set; }
+    /// <summary>Max time diff (sec) between scan and GL poses to fuse (xloc: glSampledPoseTimeTH_).</summary>
+    public double GLSampledPoseTimeTH { get; set; } = 0.5;
+    /// <summary>GMM positional variance for GL predictive likelihood (xloc: gmmPositionalVariance_).</summary>
+    public double GmmPositionalVariance { get; set; } = 0.1;
+    /// <summary>GMM angular variance for GL predictive likelihood (xloc: gmmAngularVariance_).</summary>
+    public double GmmAngularVariance { get; set; } = 0.1;
+    /// <summary>Uniform rate in predictive distribution for GL (xloc: predDistUnifRate_).</summary>
+    public double PredDistUnifRate { get; set; } = 0.05;
+    /// <summary>Pose change below this (meters) for convergence (xloc: pose_change_threshold 0.03).</summary>
+    public double ConvergencePoseChangeThresholdMeters { get; set; } = 0.03;
+    /// <summary>Yaw change below this (radians) for convergence (xloc: yaw_change_threshold 0.05).</summary>
+    public double ConvergenceYawChangeThresholdRad { get; set; } = 0.05;
+    /// <summary>Pose must stay stable for this duration (seconds) before convergence (xloc: stable_duration 0.3).</summary>
+    public double ConvergenceStableDurationSeconds { get; set; } = 0.3;
+    /// <summary>Min reliability [0,1] to allow pose-stable convergence (xloc: reliability_ >= 0.9).</summary>
+    public double ConvergenceReliabilityMin { get; set; } = 0.9;
+    /// <summary>Max MAE (meters) for best particle to allow pose-stable convergence (xloc: mae <= 0.12).</summary>
+    public double ConvergenceMaeMaxMeters { get; set; } = 0.12;
+    /// <summary>Timeout in seconds; after this, force convergence with initial pose (for Relocalizing state auto-resume fallback).</summary>
+    public double ConvergenceTimeoutSeconds { get; set; } = 30.0;
+    /// <summary>
+    /// Max MCL iterations before forcing convergence.
+    /// Note: C# does 5 iterations per scan. At 10Hz lidar = 50 iter/sec.
+    /// 1500 iterations = 30 seconds at 10Hz, 60 seconds at 5Hz (matches ConvergenceTimeoutSeconds).
+    /// </summary>
+    public int ConvergenceMaxIterations { get; set; } = 1500;
+    /// <summary>
+    /// Min MCL iterations before allowing convergence (at least 2 scans at 10Hz).
+    /// </summary>
+    public int ConvergenceMinIterations { get; set; } = 10;
+    /// <summary>
+    /// DeviceId of the lidar used for MCL scan (xloc uses a single "scan" topic).
+    /// If null or empty, the first lidar in Sensors.Lidars is used.
+    /// When multiple lidars exist, only this lidar's range data is passed to MCL; others are ignored during MCL phase.
+    /// </summary>
+    public string? PrimaryLidarId { get; set; }
+
+    /// <summary>
+    /// MCL reliability monitoring configuration for Localizing state
+    /// </summary>
+    public MclReliabilityMonitoringConfiguration ReliabilityMonitoring { get; set; } = new();
+}
+
+/// <summary>
+/// MCL reliability monitoring configuration for Localizing state
+/// Runs MCL periodically (every 2 seconds by default) to provide reliability and MAE metrics
+/// </summary>
+public class MclReliabilityMonitoringConfiguration
+{
+    /// <summary>
+    /// Enable MCL reliability monitoring during Localizing state
+    /// </summary>
+    public bool EnableReliabilityMonitoring { get; set; } = true;
+
+    /// <summary>
+    /// Interval between MCL updates (seconds)
+    /// </summary>
+    public double MonitoringIntervalSeconds { get; set; } = 2.0;
+
+    /// <summary>
+    /// Number of particles for monitoring (less than initialization for performance)
+    /// Recommended: 500 (vs 1000 for initialization)
+    /// </summary>
+    public int MonitoringParticlesNum { get; set; } = 500;
+
+    /// <summary>
+    /// Number of MCL iterations per monitoring cycle
+    /// </summary>
+    public int MonitoringIterationsPerCycle { get; set; } = 5;
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/OccupancyGridConfiguration.cs

@@ -0,0 +1,206 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Strategy for merging multiple submaps into a single occupancy grid.
+/// </summary>
+public enum SubmapMergeStrategy
+{
+    /// <summary>
+    /// Porter-Duff Source-Over compositing (Cairo-style).
+    /// Matches original Cartographer C++ behavior.
+    /// </summary>
+    PorterDuff,
+
+    /// <summary>
+    /// Sum log-odds from all submaps (Bayesian approach).
+    /// Provides clearer free/occupied distinction with multiple observations.
+    /// LogOdds = Σ log(p_i / (1 - p_i)), then convert back to probability.
+    /// </summary>
+    LogOddsSum,
+
+    /// <summary>
+    /// Take maximum probability (most pessimistic/conservative).
+    /// Good for navigation safety - any occupied observation dominates.
+    /// </summary>
+    MaxProbability
+}
+
+/// <summary>
+/// Configuration for occupancy grid generation and filtering.
+/// Controls how probability values are converted to occupancy values (0=free, 100=occupied, -1=unknown).
+/// </summary>
+public class OccupancyGridConfiguration
+{
+    #region Merge Strategy
+
+    /// <summary>
+    /// Strategy for merging overlapping cells from multiple submaps.
+    /// Default: LogOddsSum (clearer free/occupied distinction)
+    /// </summary>
+    public SubmapMergeStrategy MergeStrategy { get; set; } = SubmapMergeStrategy.LogOddsSum;
+
+    /// <summary>
+    /// Clamp log-odds to prevent extreme values from dominating.
+    /// Range: [1, 20], Default: 10 (corresponds to probability ~0.00005 to ~0.99995)
+    /// </summary>
+    public double LogOddsClamp { get; set; } = 10.0;
+
+    /// <summary>
+    /// When true, use average log-odds instead of sum.
+    /// This prevents amplification when a cell is observed by many submaps.
+    /// Default: true (average is more stable for visualization)
+    /// </summary>
+    public bool UseLogOddsAverage { get; set; } = true;
+
+    #endregion
+
+    #region Threshold Configuration
+
+    /// <summary>
+    /// Threshold for classifying a cell as FREE (white, occupancy=0).
+    /// In texture-based conversion: textureValue >= FreeSpaceThreshold → FREE
+    /// Higher value = stricter (fewer free cells), Lower value = more permissive (more free cells).
+    /// Range: [0, 255], Default: 100
+    ///
+    /// Technical detail:
+    /// - textureValue = max(0, 128 - logOddsInteger)
+    /// - logOddsInteger maps probability [0.1, 0.9] to [1, 255]
+    /// - FreeSpaceThreshold=100 requires probability ≤ ~0.15 (very confident free)
+    /// - FreeSpaceThreshold=50 requires probability ≤ ~0.30 (moderately confident free)
+    /// </summary>
+    public int FreeSpaceThreshold { get; set; } = 100;
+
+    /// <summary>
+    /// Threshold for classifying a cell as OCCUPIED (black, occupancy=100).
+    /// In texture-based conversion: textureAlpha > OccupiedSpaceThreshold → OCCUPIED
+    /// Higher value = stricter (fewer occupied cells, thinner walls), Lower value = more permissive.
+    /// Range: [0, 255], Default: 0
+    ///
+    /// Technical detail:
+    /// - textureAlpha = max(0, logOddsInteger - 128)
+    /// - OccupiedSpaceThreshold=0 requires probability > 0.5 (any occupied tendency)
+    /// - OccupiedSpaceThreshold=50 requires probability > ~0.70 (confident occupied)
+    /// - OccupiedSpaceThreshold=100 requires probability > ~0.85 (very confident occupied)
+    /// </summary>
+    public int OccupiedSpaceThreshold { get; set; } = 0;
+
+    #endregion
+
+    #region Output Mode
+
+    /// <summary>
+    /// When true, output only binary values (0=free, 100=occupied, -1=unknown).
+    /// When false, output gradient values (0-100) based on probability.
+    /// Default: true (binary output for compatibility with most navigation systems).
+    /// </summary>
+    public bool UseBinaryOutput { get; set; } = true;
+
+    #endregion
+
+    #region Wall Thinning (Post-processing)
+
+    /// <summary>
+    /// Enable morphological erosion to thin walls in the occupancy grid.
+    /// Useful for reducing wall thickness caused by sensor noise or multiple observations.
+    /// Default: false
+    /// </summary>
+    public bool EnableWallThinning { get; set; } = false;
+
+    /// <summary>
+    /// Number of erosion iterations for wall thinning.
+    /// Each iteration removes one pixel layer from occupied regions.
+    /// Higher value = thinner walls, but may disconnect thin walls.
+    /// Range: [1, 5], Default: 1
+    /// </summary>
+    public int WallThinningIterations { get; set; } = 1;
+
+    /// <summary>
+    /// Minimum wall thickness to preserve (in pixels) during wall thinning.
+    /// Walls thinner than this will not be eroded further.
+    /// Range: [1, 10], Default: 1
+    /// </summary>
+    public int MinWallThicknessPixels { get; set; } = 1;
+
+    #endregion
+
+    #region Ambiguous Cell Handling
+
+    /// <summary>
+    /// How to handle ambiguous cells (probability ~0.5, neither clearly free nor occupied).
+    /// Values: -1 = Unknown, 0 = Free, 100 = Occupied
+    /// Default: -1 (mark as unknown)
+    ///
+    /// Note: This applies when UseBinaryOutput=true and the cell doesn't meet
+    /// either FreeSpaceThreshold or OccupiedSpaceThreshold.
+    /// </summary>
+    public sbyte AmbiguousCellValue { get; set; } = -1;
+
+    /// <summary>
+    /// Lower bound of the ambiguous range (probability).
+    /// Cells with probability between AmbiguousRangeLower and AmbiguousRangeUpper
+    /// are considered ambiguous and handled according to AmbiguousCellValue.
+    /// Default: 0.35 (corresponding to ~neither free nor occupied)
+    /// </summary>
+    public double AmbiguousRangeLower { get; set; } = 0.35;
+
+    /// <summary>
+    /// Upper bound of the ambiguous range (probability).
+    /// Default: 0.65
+    /// </summary>
+    public double AmbiguousRangeUpper { get; set; } = 0.65;
+
+    #endregion
+
+    #region Advanced Options
+
+    /// <summary>
+    /// Apply median filter to reduce noise in the occupancy grid.
+    /// Useful for removing salt-and-pepper noise.
+    /// Default: false
+    /// </summary>
+    public bool EnableMedianFilter { get; set; } = false;
+
+    /// <summary>
+    /// Kernel size for median filter (must be odd number).
+    /// Range: [3, 7], Default: 3
+    /// </summary>
+    public int MedianFilterKernelSize { get; set; } = 3;
+
+    #endregion
+
+    #region TSDF-Specific Options
+
+    /// <summary>
+    /// TSD threshold for classifying a cell as FREE space (meters).
+    /// Cells with TSD > TsdfFreeThreshold are considered free.
+    /// Lower value = more aggressive free space detection (closer to walls).
+    /// Range: [0.01, 0.3], Default: 0.05 (5cm from surface)
+    /// </summary>
+    public double TsdfFreeThreshold { get; set; } = 0.05;
+
+    /// <summary>
+    /// TSD threshold for classifying a cell as OCCUPIED (meters, negative value).
+    /// Cells with TSD < TsdfOccupiedThreshold are considered occupied.
+    /// Higher value (closer to 0) = more aggressive obstacle detection.
+    /// Range: [-0.3, 0], Default: -0.02 (2cm inside surface)
+    /// </summary>
+    public double TsdfOccupiedThreshold { get; set; } = -0.02;
+
+    /// <summary>
+    /// Minimum weight required for a TSDF cell to be considered valid.
+    /// Cells with weight < TsdfMinWeight are skipped (treated as unknown).
+    /// Lower value = include more cells but with less confidence.
+    /// Range: [0.01, 5.0], Default: 0.1
+    /// </summary>
+    public double TsdfMinWeight { get; set; } = 0.1;
+
+    /// <summary>
+    /// Maximum TSD value for normalization (meters).
+    /// Should match the TruncationDistance in TsdfRangeDataInserterOptions.
+    /// Used to convert TSD to probability: probability = 0.5 * (1 - tsd/TsdfMaxTsd)
+    /// Range: [0.1, 1.0], Default: 0.3
+    /// </summary>
+    public double TsdfMaxTsd { get; set; } = 0.3;
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/SensorConfiguration.cs

@@ -0,0 +1,115 @@
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Sensor configuration
+/// </summary>
+public class SensorConfiguration
+{
+    /// <summary>
+    /// Lidar sensor configurations (multiple lidars supported)
+    /// </summary>
+    public List<LidarSensorConfiguration> Lidars { get; set; } = [];
+
+    /// <summary>
+    /// IMU sensor configuration (only one IMU supported by CartographerSharp)
+    /// </summary>
+    public ImuSensorConfiguration Imu { get; set; } = new();
+
+    /// <summary>
+    /// Sampling ratio for sensor data (0.0 to 1.0)
+    /// - 1.0 = process all messages (default)
+    /// - 0.5 = process 50% of messages
+    /// - 0.0 = process no messages
+    /// Based on xloc flow: sampling ratio to reduce message overload
+    /// </summary>
+    public double? SamplingRatio { get; set; }
+
+    /// <summary>
+    /// Sampling ratio for Lidar data (overrides global SamplingRatio if set)
+    /// </summary>
+    public double? LidarSamplingRatio { get; set; }
+
+    /// <summary>
+    /// Sampling ratio for IMU data (overrides global SamplingRatio if set)
+    /// </summary>
+    public double? ImuSamplingRatio { get; set; }
+
+    /// <summary>
+    /// Sampling ratio for Odometry data (overrides global SamplingRatio if set)
+    /// </summary>
+    public double? OdometrySamplingRatio { get; set; }
+
+    /// <summary>
+    /// Odometry update interval in milliseconds.
+    /// Odometry is polled at this interval from the odometry estimator.
+    /// Default: 5ms (200Hz). Lower values = higher frequency, more CPU usage.
+    /// Recommended: 5-20ms (50-200Hz)
+    /// </summary>
+    public int OdometryUpdateIntervalMs { get; set; } = 5;
+
+    /// <summary>
+    /// IMU update interval in milliseconds.
+    /// IMU data is processed at this minimum interval (rate limiting).
+    /// Default: 5ms (200Hz). Set to 0 to process all IMU events without rate limiting.
+    /// Recommended: 0-10ms (100Hz-unlimited)
+    /// </summary>
+    public int ImuUpdateIntervalMs { get; set; } = 5;
+}
+
+/// <summary>
+/// Lidar sensor configuration
+/// </summary>
+public class LidarSensorConfiguration
+{
+    /// <summary>
+    /// Device ID from IDeviceProvider
+    /// </summary>
+    public string DeviceId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Enable or disable this lidar
+    /// </summary>
+    public bool Enabled { get; set; } = true;
+
+    /// <summary>
+    /// Transform from lidar frame to base_link frame
+    /// </summary>
+    public Transform Transform { get; set; }
+
+    /// <summary>
+    /// Minimum angle for filtering point cloud (degrees).
+    /// Points with angle less than this will be filtered out.
+    /// If null, no minimum angle filtering is applied.
+    /// </summary>
+    public double? AngleMin { get; set; }
+
+    /// <summary>
+    /// Maximum angle for filtering point cloud (degrees).
+    /// Points with angle greater than this will be filtered out.
+    /// If null, no maximum angle filtering is applied.
+    /// </summary>
+    public double? AngleMax { get; set; }
+}
+
+/// <summary>
+/// IMU sensor configuration
+/// </summary>
+public class ImuSensorConfiguration
+{
+    /// <summary>
+    /// Device ID from IDeviceProvider
+    /// </summary>
+    public string DeviceId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Enable or disable this IMU
+    /// </summary>
+    public bool Enabled { get; set; } = false;
+
+    /// <summary>
+    /// Transform from IMU frame to base_link frame
+    /// </summary>
+    public Transform Transform { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/TrajectoryBuilderConfiguration.cs

@@ -0,0 +1,398 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Trajectory builder configuration
+/// </summary>
+public class TrajectoryBuilderConfiguration
+{
+    /// <summary>
+    /// Use 2D SLAM (true) or 3D SLAM (false)
+    /// </summary>
+    public bool Use2D { get; set; } = true;
+
+    /// <summary>
+    /// Minimum range for lidar points (meters)
+    /// </summary>
+    public double MinRange { get; set; } = 0.1;
+
+    /// <summary>
+    /// Maximum range for lidar points (meters)
+    /// </summary>
+    public double MaxRange { get; set; } = 30.0;
+
+    /// <summary>
+    /// Minimum Z coordinate for lidar points (meters). Points below this will be filtered out.
+    /// </summary>
+    public double? MinZ { get; set; }
+
+    /// <summary>
+    /// Maximum Z coordinate for lidar points (meters). Points above this will be filtered out.
+    /// </summary>
+    public double? MaxZ { get; set; }
+
+    /// <summary>
+    /// Points beyond 'max_range' will be inserted with this length as empty space (meters).
+    /// </summary>
+    public double? MissingDataRayLength { get; set; }
+
+    /// <summary>
+    /// Voxel filter size (meters)
+    /// </summary>
+    public double VoxelFilterSize { get; set; } = 0.025;
+
+    /// <summary>
+    /// Number of accumulated range data before processing
+    /// </summary>
+    public int NumAccumulatedRangeData { get; set; } = 1;
+
+    /// <summary>
+    /// Use IMU data if available. If null, will be auto-detected from sensor configuration.
+    /// </summary>
+    public bool? UseImuData { get; set; }
+
+    /// <summary>
+    /// Use online correlative scan matching
+    /// </summary>
+    public bool UseOnlineCorrelativeScanMatching { get; set; } = false;
+
+    /// <summary>
+    /// Real-time correlative scan matcher options
+    /// </summary>
+    public RealTimeCorrelativeScanMatcherOptionsConfiguration? RealTimeCorrelativeScanMatcherOptions { get; set; }
+
+    /// <summary>
+    /// Ceres scan matcher options for 2D
+    /// </summary>
+    public CeresScanMatcherOptions2DConfiguration? CeresScanMatcherOptions { get; set; }
+
+    /// <summary>
+    /// Motion filter options
+    /// </summary>
+    public MotionFilterOptionsConfiguration? MotionFilterOptions { get; set; }
+
+    /// <summary>
+    /// Adaptive voxel filter options (optional, if not provided uses fixed voxel_filter_size)
+    /// </summary>
+    public AdaptiveVoxelFilterOptionsConfiguration? AdaptiveVoxelFilterOptions { get; set; }
+
+    /// <summary>
+    /// Loop closure adaptive voxel filter options (optional)
+    /// Used to compute a sparser point cloud for finding loop closures.
+    /// If not provided, uses the same filter as AdaptiveVoxelFilterOptions or fixed voxel_filter_size.
+    /// </summary>
+    public AdaptiveVoxelFilterOptionsConfiguration? LoopClosureAdaptiveVoxelFilterOptions { get; set; }
+
+    /// <summary>
+    /// Pose extrapolator options
+    /// </summary>
+    public PoseExtrapolatorOptionsConfiguration? PoseExtrapolatorOptions { get; set; }
+
+    /// <summary>
+    /// Debug options for scan matching convergence
+    /// </summary>
+    public ScanMatchingDebugOptionsConfiguration? ScanMatchingDebugOptions { get; set; }
+
+    /// <summary>
+    /// Submaps options for 2D
+    /// </summary>
+    public SubmapsOptions2DConfiguration? SubmapsOptions { get; set; }
+
+    /// <summary>
+    /// High resolution grid options for 2D (optional)
+    /// Used for higher precision scan matching
+    /// </summary>
+    public GridOptions2DConfiguration? HighResGridOptions { get; set; }
+
+    /// <summary>
+    /// High resolution adaptive voxel filter options (optional)
+    /// Used to filter point cloud for high resolution grid
+    /// </summary>
+    public AdaptiveVoxelFilterOptionsConfiguration? HighResAdaptiveVoxelFilterOptions { get; set; }
+
+    /// <summary>
+    /// High resolution voxel filter size (meters)
+    /// Used when HighResAdaptiveVoxelFilterOptions is not provided
+    /// </summary>
+    public double? HighResVoxelFilterSize { get; set; }
+
+    /// <summary>
+    /// Landmark threshold options (optional)
+    /// Used for landmark-based localization
+    /// </summary>
+    public LandmarkThresholdOptionsConfiguration? LandmarkThreshold { get; set; }
+
+    /// <summary>
+    /// Number of duplicate inserts to accurate submap
+    /// </summary>
+    public int? NumDuplicateInsertToAccurateSubmap { get; set; }
+
+    /// <summary>
+    /// True score cell threshold
+    /// Threshold for determining if a cell has a true score
+    /// </summary>
+    public double? TrueScoreCellThreshold { get; set; }
+
+    /// <summary>
+    /// If true, provides a confidence score for the pose estimate based on
+    /// real-time correlative scan matching.
+    /// </summary>
+    public bool ProvideConfidenceScore { get; set; } = false;
+
+    /// <summary>
+    /// Soft limit for Ceres scan match cost. When exceeded, pose is trusted but
+    /// scan is NOT inserted into submap (prevents map corruption).
+    /// Set to 0 to disable. Default: 0 (disabled).
+    /// </summary>
+    public double CeresScoreSoftLimit { get; set; } = 0;
+
+    /// <summary>
+    /// Hard limit for Ceres scan match cost. When exceeded, the pose is considered
+    /// unreliable and odometry prediction is used instead.
+    /// Set to 0 to disable. Default: 0 (disabled).
+    /// </summary>
+    public double CeresScoreHardLimit { get; set; } = 0;
+
+    /// <summary>
+    /// After this many consecutive hard-limit failures, a new submap is forced
+    /// to break the map-growth deadlock.
+    /// Set to 0 to disable. Default: 5.
+    /// </summary>
+    public int MaxConsecutiveHighCostBeforeNewSubmap { get; set; } = 5;
+
+    /// <summary>
+    /// Real-time correlative scan matcher options for initial pose (SetInitialPose).
+    /// Uses wider search window for relocalization scenarios.
+    /// </summary>
+    public RealTimeCorrelativeScanMatcherOptionsConfiguration? InitialPoseRealTimeCorrelativeScanMatcherOptions { get; set; }
+
+    /// <summary>
+    /// Ceres scan matcher options for initial pose (SetInitialPose).
+    /// Uses stricter weights for better accuracy in relocalization scenarios.
+    /// </summary>
+    public CeresScanMatcherOptions2DConfiguration? InitialPoseCeresScanMatcherOptions { get; set; }
+}
+
+#region Scan Matching Options
+
+/// <summary>
+/// Ceres scan matcher options for 2D
+/// </summary>
+public class CeresScanMatcherOptions2DConfiguration
+{
+    public double OccupiedSpaceWeight { get; set; } = 1.0;
+    public double TranslationWeight { get; set; } = 10.0;
+    public double RotationWeight { get; set; } = 40.0;
+    public double? HighResOccupiedSpaceWeight { get; set; }
+    public double? HighResTranslationWeight { get; set; }
+    public double? HighResRotationWeight { get; set; }
+    public double? LandmarkWeight { get; set; }
+    public CeresSolverOptionsConfiguration? CeresSolverOptions { get; set; }
+}
+
+/// <summary>
+/// Real-time correlative scan matcher options
+/// </summary>
+public class RealTimeCorrelativeScanMatcherOptionsConfiguration
+{
+    public double LinearSearchWindow { get; set; } = 0.1;
+    public double AngularSearchWindow { get; set; } = 0.349066; // ~20 degrees
+    public double TranslationDeltaCostWeight { get; set; } = 0.1;
+    public double RotationDeltaCostWeight { get; set; } = 0.1;
+    public int NumThreads { get; set; } = 1;
+}
+
+/// <summary>
+/// Fast correlative scan matcher options for 2D
+/// </summary>
+public class FastCorrelativeScanMatcherOptions2DConfiguration
+{
+    public double LinearSearchWindow { get; set; } = 7.0;
+    public double AngularSearchWindow { get; set; } = 0.523599; // ~30 degrees
+    public int BranchAndBoundDepth { get; set; } = 7;
+    public double? LocalizationLinearSearchWindow { get; set; }
+    public double? LocalizationAngularSearchWindow { get; set; }
+}
+
+/// <summary>
+/// Ceres solver options
+/// </summary>
+public class CeresSolverOptionsConfiguration
+{
+    public bool UseNonmonotonicSteps { get; set; } = false;
+    public int MaxNumIterations { get; set; } = 20;
+    public int NumThreads { get; set; } = 1;
+    public double? FunctionTolerance { get; set; }
+    public double? GradientTolerance { get; set; }
+    public double? ParameterTolerance { get; set; }
+}
+
+#endregion
+
+#region Motion and Filter Options
+
+/// <summary>
+/// Motion filter options
+/// </summary>
+public class MotionFilterOptionsConfiguration
+{
+    public double MaxTimeSeconds { get; set; } = 5.0;
+    public double MaxDistanceMeters { get; set; } = 0.2;
+    public double MaxAngleRadians { get; set; } = 0.0174533; // ~1 degree
+}
+
+/// <summary>
+/// Adaptive voxel filter options
+/// </summary>
+public class AdaptiveVoxelFilterOptionsConfiguration
+{
+    public double MaxLength { get; set; } = 0.5;
+    public double MinNumPoints { get; set; } = 200;
+    public double MaxRange { get; set; } = 50.0;
+}
+
+#endregion
+
+#region Submap Options
+
+/// <summary>
+/// Submaps options for 2D
+/// </summary>
+public class SubmapsOptions2DConfiguration
+{
+    public int NumRangeData { get; set; } = 90;
+    public GridOptions2DConfiguration GridOptions { get; set; } = new();
+    public RangeDataInserterOptionsConfiguration RangeDataInserterOptions { get; set; } = new();
+}
+
+/// <summary>
+/// Grid options for 2D
+/// </summary>
+public class GridOptions2DConfiguration
+{
+    public int GridType { get; set; } = 1; // ProbabilityGrid
+    public double Resolution { get; set; } = 0.05;
+}
+
+/// <summary>
+/// Range data inserter options
+/// </summary>
+public class RangeDataInserterOptionsConfiguration
+{
+    public int RangeDataInserterType { get; set; } = 1; // ProbabilityGridInserter2D
+    public ProbabilityGridRangeDataInserterOptions2DConfiguration? ProbabilityGridRangeDataInserterOptions { get; set; }
+    public TsdfRangeDataInserterOptions2DConfiguration? TsdfRangeDataInserterOptions { get; set; }
+}
+
+/// <summary>
+/// TSDF range data inserter options for 2D
+/// </summary>
+public class TsdfRangeDataInserterOptions2DConfiguration
+{
+    public double TruncationDistance { get; set; } = 0.3;
+    public double MaximumWeight { get; set; } = 10.0;
+    public bool UpdateFreeSpace { get; set; } = false;
+    public NormalEstimationOptions2DConfiguration NormalEstimationOptions { get; set; } = new();
+    public bool ProjectSdfDistanceToScanNormal { get; set; } = true;
+    public int UpdateWeightRangeExponent { get; set; } = 0;
+    public double UpdateWeightAngleScanNormalToRayKernelBandwidth { get; set; } = 0.5;
+    public double UpdateWeightDistanceCellToHitKernelBandwidth { get; set; } = 0.5;
+}
+
+/// <summary>
+/// Normal estimation options for 2D TSDF
+/// </summary>
+public class NormalEstimationOptions2DConfiguration
+{
+    public int NumNormalSamples { get; set; } = 4;
+    public double SampleRadius { get; set; } = 0.5;
+}
+
+/// <summary>
+/// Probability grid range data inserter options for 2D
+/// </summary>
+public class ProbabilityGridRangeDataInserterOptions2DConfiguration
+{
+    public double HitProbability { get; set; } = 0.55;
+    public double MissProbability { get; set; } = 0.49;
+    public bool InsertFreeSpace { get; set; } = true;
+}
+
+/// <summary>
+/// Overlapping submaps trimmer options for 2D
+/// </summary>
+public class OverlappingSubmapsTrimmerOptions2DConfiguration
+{
+    public int FreshSubmapsCount { get; set; } = 2;
+    public double MinCoveredArea { get; set; } = 1.0;
+    public int MinAddedSubmapsCount { get; set; } = 5;
+}
+
+#endregion
+
+#region Pose Extrapolator Options
+
+/// <summary>
+/// Pose extrapolator options
+/// </summary>
+public class PoseExtrapolatorOptionsConfiguration
+{
+    public bool UseImuBased { get; set; } = false;
+    public ConstantVelocityPoseExtrapolatorOptionsConfiguration ConstantVelocity { get; set; } = new();
+    public ImuBasedPoseExtrapolatorOptionsConfiguration? ImuBased { get; set; }
+    public double? VelocityThreshold { get; set; }
+    public double? AccelerationThreshold { get; set; }
+    public double? GravityDeviationThreshold { get; set; }
+    public bool? UseOdometryDirectly { get; set; }
+}
+
+/// <summary>
+/// Constant velocity pose extrapolator options
+/// </summary>
+public class ConstantVelocityPoseExtrapolatorOptionsConfiguration
+{
+    public double ImuGravityTimeConstant { get; set; } = 10.0;
+    public double PoseQueueDuration { get; set; } = 0.001;
+}
+
+/// <summary>
+/// IMU-based pose extrapolator options
+/// </summary>
+public class ImuBasedPoseExtrapolatorOptionsConfiguration
+{
+    public double PoseQueueDuration { get; set; } = 5.0;
+    public double GravityConstant { get; set; } = 9.806;
+    public double PoseTranslationWeight { get; set; } = 1.0;
+    public double PoseRotationWeight { get; set; } = 1.0;
+    public double ImuAccelerationWeight { get; set; } = 1.0;
+    public double ImuRotationWeight { get; set; } = 1.0;
+    public double OdometryTranslationWeight { get; set; } = 1.0;
+    public double OdometryRotationWeight { get; set; } = 1.0;
+    public CeresSolverOptionsConfiguration? SolverOptions { get; set; }
+}
+
+#endregion
+
+#region Debug Options
+
+/// <summary>
+/// Debug options for scan matching convergence
+/// </summary>
+public class ScanMatchingDebugOptionsConfiguration
+{
+    public bool EnableDebugMode { get; set; } = false;
+    public double MinCostReductionPercent { get; set; } = 80.0;
+    public double MaxPoseChangeDistanceMeters { get; set; } = 0.5;
+    public double MaxPoseChangeRotationDegrees { get; set; } = 5.0;
+}
+
+/// <summary>
+/// Landmark threshold options
+/// </summary>
+public class LandmarkThresholdOptionsConfiguration
+{
+    public double MirrorLandmarkMatchingDistance { get; set; } = 0.5;
+    public double NewLandmarkDistance { get; set; } = 2.0;
+    public int LocalLandmarksHistory { get; set; } = 80;
+}
+
+#endregion

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Enums/CartographerTrigger.cs

@@ -0,0 +1,23 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Enums;
+
+/// <summary>
+/// Triggers cho CartographerService state machine
+/// </summary>
+public enum CartographerTrigger
+{
+    Start,
+    InitializationComplete,
+    InitializationFailed,
+    StartLocalization,
+    /// <summary>MCL converged; initial pose ready, trajectory added. Transition from InitializingLocalizing to Localizing.</summary>
+    MclConverged,
+    /// <summary>Initial pose set without MCL (trajectory already added). Transition from InitializingLocalizing to Localizing.</summary>
+    MclSkipped,
+    StartScanMapping,
+    StopLocalization,
+    SaveMap,
+    MapSaved,
+    ErrorOccurred,
+    Reset,
+    Shutdown
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Geometry/Matrix3x3.cs

@@ -0,0 +1,118 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+
+/// <summary>
+/// 3x3 matrix for representing covariance matrices
+/// </summary>
+public readonly struct Matrix3x3
+{
+    private readonly double[,] _data;
+
+    /// <summary>
+    /// Creates a new 3x3 matrix with all zeros
+    /// </summary>
+    public Matrix3x3()
+    {
+        _data = new double[3, 3];
+    }
+
+    /// <summary>
+    /// Creates a new 3x3 matrix from a 2D array
+    /// </summary>
+    /// <param name="data">3x3 array of values</param>
+    /// <exception cref="ArgumentException">Thrown if array is not 3x3</exception>
+    public Matrix3x3(double[,] data)
+    {
+        if (data.GetLength(0) != 3 || data.GetLength(1) != 3)
+        {
+            throw new ArgumentException("Matrix must be 3x3", nameof(data));
+        }
+        _data = new double[3, 3];
+        Array.Copy(data, _data, 9);
+    }
+
+    /// <summary>
+    /// Gets or sets the value at the specified row and column
+    /// </summary>
+    public double this[int row, int col]
+    {
+        get => _data[row, col];
+        set => _data[row, col] = value;
+    }
+
+    /// <summary>
+    /// Gets the determinant of the matrix
+    /// </summary>
+    public double Determinant
+    {
+        get
+        {
+            return _data[0, 0] * (_data[1, 1] * _data[2, 2] - _data[1, 2] * _data[2, 1])
+                 - _data[0, 1] * (_data[1, 0] * _data[2, 2] - _data[1, 2] * _data[2, 0])
+                 + _data[0, 2] * (_data[1, 0] * _data[2, 1] - _data[1, 1] * _data[2, 0]);
+        }
+    }
+
+    /// <summary>
+    /// Creates an identity matrix
+    /// </summary>
+    public static Matrix3x3 Identity()
+    {
+        var matrix = new Matrix3x3();
+        matrix[0, 0] = 1.0;
+        matrix[1, 1] = 1.0;
+        matrix[2, 2] = 1.0;
+        return matrix;
+    }
+
+    /// <summary>
+    /// Creates a covariance matrix for 2D pose (x, y, theta)
+    /// </summary>
+    /// <param name="xx">Variance in x</param>
+    /// <param name="yy">Variance in y</param>
+    /// <param name="tt">Variance in theta</param>
+    /// <param name="xy">Covariance between x and y</param>
+    /// <param name="xt">Covariance between x and theta</param>
+    /// <param name="yt">Covariance between y and theta</param>
+    public static Matrix3x3 Covariance(double xx, double yy, double tt, double xy = 0.0, double xt = 0.0, double yt = 0.0)
+    {
+        var matrix = new Matrix3x3();
+        matrix[0, 0] = xx; // Var(x)
+        matrix[1, 1] = yy; // Var(y)
+        matrix[2, 2] = tt; // Var(theta)
+        matrix[0, 1] = matrix[1, 0] = xy; // Cov(x,y)
+        matrix[0, 2] = matrix[2, 0] = xt; // Cov(x,theta)
+        matrix[1, 2] = matrix[2, 1] = yt; // Cov(y,theta)
+        return matrix;
+    }
+
+    /// <summary>
+    /// Gets all values as a 2D array
+    /// </summary>
+    public double[,] ToArray()
+    {
+        var result = new double[3, 3];
+        Array.Copy(_data, result, 9);
+        return result;
+    }
+
+    /// <summary>
+    /// Gets the matrix as a flattened array (row-major order)
+    /// </summary>
+    public double[] ToFlatArray()
+    {
+        return
+        [
+            _data[0, 0], _data[0, 1], _data[0, 2],
+            _data[1, 0], _data[1, 1], _data[1, 2],
+            _data[2, 0], _data[2, 1], _data[2, 2]
+        ];
+    }
+
+    public override string ToString()
+    {
+        return $"[{_data[0, 0]:F3}, {_data[0, 1]:F3}, {_data[0, 2]:F3}]\n" +
+               $"[{_data[1, 0]:F3}, {_data[1, 1]:F3}, {_data[1, 2]:F3}]\n" +
+               $"[{_data[2, 0]:F3}, {_data[2, 1]:F3}, {_data[2, 2]:F3}]";
+    }
+}
+

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

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/MapCartographerInfo.cs

@@ -0,0 +1,117 @@
+using RobotNet10.RobotApp.Shared;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+
+public class MapCartographerInfo : MapInfo
+{
+
+    /// <summary>
+    /// Cartographer configuration snapshot khi map được save
+    /// Dùng để validate khi load map
+    /// </summary>
+    public MapCartographerConfigSnapshot? MapConfig { get; init; }
+
+}
+
+/// <summary>
+/// Snapshot của CartographerConfiguration khi save map
+/// Chỉ lưu các phần cần thiết để recreate MapBuilder
+/// </summary>
+public class MapCartographerConfigSnapshot
+{
+    /// <summary>
+    /// Use 2D or 3D trajectory builder
+    /// </summary>
+    public bool Use2D { get; init; }
+
+    /// <summary>
+    /// MapBuilder configuration snapshot
+    /// </summary>
+    public MapCarographerBuilderConfigSnapshot MapBuilder { get; init; } = new();
+
+    /// <summary>
+    /// TrajectoryBuilder configuration snapshot (chỉ các phần quan trọng)
+    /// </summary>
+    public TrajectoryBuilderConfigSnapshot TrajectoryBuilder { get; init; } = new();
+}
+
+/// <summary>
+/// Snapshot của MapBuilderConfiguration
+/// </summary>
+public class MapCarographerBuilderConfigSnapshot
+{
+    public bool? UseTrajectoryBuilder2D { get; init; }
+    public bool? UseTrajectoryBuilder3D { get; init; }
+    public int NumBackgroundThreads { get; init; }
+    public int OptimizeEveryNNodes { get; init; }
+    public double MatcherTranslationWeight { get; init; }
+    public double MatcherRotationWeight { get; init; }
+    public int? MaxNumFinalIterations { get; init; }
+    public double? GlobalSamplingRatio { get; init; }
+    public bool? LogResidualHistograms { get; init; }
+    public double? GlobalConstraintSearchAfterNSeconds { get; init; }
+    public bool EnableSingleTrajectoryLoopClosure { get; init; } = true;
+    public double SingleTrajectoryLoopClosureDistanceThreshold { get; init; } = 3.0;
+    public OptimizationProblemOptionsSnapshot OptimizationProblemOptions { get; init; } = new();
+}
+
+/// <summary>
+/// Snapshot của OptimizationProblemOptions
+/// </summary>
+public class OptimizationProblemOptionsSnapshot
+{
+    public double HuberScale { get; init; }
+    public double AccelerationWeight { get; init; }
+    public double RotationWeight { get; init; }
+    public double LocalSlamPoseTranslationWeight { get; init; }
+    public double LocalSlamPoseRotationWeight { get; init; }
+    public double OdometryTranslationWeight { get; init; }
+    public double OdometryRotationWeight { get; init; }
+    public double FixedFramePoseTranslationWeight { get; init; }
+    public double FixedFramePoseRotationWeight { get; init; }
+    public bool FixedFramePoseUseTolerantLoss { get; init; }
+    public double FixedFramePoseTolerantLossParamA { get; init; }
+    public double FixedFramePoseTolerantLossParamB { get; init; }
+    public bool LogSolverSummary { get; init; }
+    public int MaxNumIterations { get; init; }
+}
+
+/// <summary>
+/// Snapshot của TrajectoryBuilderConfiguration (chỉ các phần quan trọng)
+/// </summary>
+public class TrajectoryBuilderConfigSnapshot
+{
+    public bool Use2D { get; init; }
+    public double MinRange { get; init; }
+    public double MaxRange { get; init; }
+    public double? MinZ { get; init; }
+    public double? MaxZ { get; init; }
+    public double? MissingDataRayLength { get; init; }
+    public double VoxelFilterSize { get; init; }
+    public int NumAccumulatedRangeData { get; init; }
+    public bool? UseImuData { get; init; }
+    public bool UseOnlineCorrelativeScanMatching { get; init; }
+}
+
+/// <summary>
+/// Result from loading map files (pbstream path and saved config)
+/// Used to separate file loading from CartographerSharp MapBuilder creation
+/// </summary>
+public class MapCartographerLoadResult
+{
+    /// <summary>
+    /// Đường dẫn đến file .pbstream
+    /// </summary>
+    public string PbstreamPath { get; init; } = string.Empty;
+
+    /// <summary>
+    /// Đường dẫn đến map folder
+    /// </summary>
+    public string MapPath { get; init; } = string.Empty;
+
+    /// <summary>
+    /// Cartographer configuration snapshot khi map được save (nếu có)
+    /// </summary>
+    public MapCartographerConfigSnapshot? SavedConfig { get; init; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Mcl/MclScanHelper.cs

@@ -0,0 +1,75 @@
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
+
+/// <summary>
+/// Converts point cloud (x,y) to MCL scan format (angle_min, angle_max, angle_increment, range_min, range_max, ranges[]).
+/// Used when feeding TimedPointCloudData to MCL (bin by angle, take min range per bin).
+/// </summary>
+public static class MclScanHelper
+{
+    /// <summary>
+    /// Transforms points from base_link frame to sensor (lidar) frame.
+    /// baseFromLidar: p_base = R(yaw)*p_lidar + (tx, ty). So p_lidar = R(-yaw)*(p_base - (tx, ty)).
+    /// </summary>
+    /// <param name="pointsBase">Points (x, y) in base_link frame</param>
+    /// <param name="tx">X of lidar origin in base_link (baseFromLidar translation)</param>
+    /// <param name="ty">Y of lidar origin in base_link</param>
+    /// <param name="yawRad">Yaw of lidar frame in base_link (radians)</param>
+    /// <returns>Points (x, y) in sensor/lidar frame</returns>
+    public static IEnumerable<(double x, double y)> BaseToSensorFrame(
+        IEnumerable<(double x, double y)> pointsBase,
+        double tx, double ty, double yawRad)
+    {
+        double c = Math.Cos(-yawRad);
+        double s = Math.Sin(-yawRad);
+        foreach (var (xb, yb) in pointsBase)
+        {
+            double dx = xb - tx;
+            double dy = yb - ty;
+            yield return (c * dx - s * dy, s * dx + c * dy);
+        }
+    }
+
+    /// <summary>
+    /// Bins points by angle and returns ranges (min range per bin). Angles in radians; ranges in meters.
+    /// Points should be in sensor (lidar) frame so that angles are relative to sensor (required by MCL likelihood).
+    /// </summary>
+    public static (double angleMin, double angleMax, double angleIncrement, double rangeMin, double rangeMax, double[] ranges) ConvertToScan(
+        IEnumerable<Vector2> points,
+        double angleMinDeg,
+        double angleMaxDeg,
+        int numBins,
+        double rangeMin,
+        double rangeMax)
+    {
+        double angleMin = angleMinDeg * Math.PI / 180.0;
+        double angleMax = angleMaxDeg * Math.PI / 180.0;
+        double angleIncrement = (angleMax - angleMin) / Math.Max(1, numBins);
+        var bins = new List<double>[numBins];
+        for (int i = 0; i < numBins; i++)
+            bins[i] = [];
+
+        foreach (var point in points)
+        {
+            var range = Math.Sqrt(point.X * point.X + point.Y * point.Y);
+            if (range < 1e-6) continue;
+            var angle = Math.Atan2(point.Y, point.X);
+            int bin = (int)Math.Floor((angle - angleMin) / angleIncrement);
+            if (bin < 0) bin = 0;
+            if (bin >= numBins) bin = numBins - 1;
+            bins[bin].Add(range);
+        }
+
+        var ranges = new double[numBins];
+        for (int i = 0; i < numBins; i++)
+        {
+            if (bins[i].Count == 0)
+                ranges[i] = rangeMax;
+            else
+                ranges[i] = bins[i].Min();
+        }
+        return (angleMin, angleMax, angleIncrement, rangeMin, rangeMax, ranges);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Mcl/MclTypes.cs

@@ -0,0 +1,56 @@
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
+
+#region MclPose2d
+
+/// <summary>
+/// 2D pose (x, y, yaw) for MCL. Matches xloc mcl_2d_localization::Pose.
+/// </summary>
+public struct MclPose2d(double x, double y, double yaw)
+{
+    public double X { get; set; } = x;
+    public double Y { get; set; } = y;
+    public double Yaw { get; set; } = NormalizeYaw(yaw);
+
+    public static double NormalizeYaw(double yaw)
+    {
+        while (yaw < -Math.PI) yaw += 2.0 * Math.PI;
+        while (yaw > Math.PI) yaw -= 2.0 * Math.PI;
+        return yaw;
+    }
+
+    public void SetPose(double x, double y, double yaw)
+    {
+        X = x;
+        Y = y;
+        Yaw = NormalizeYaw(yaw);
+    }
+}
+
+#endregion
+
+#region MclParticle
+
+/// <summary>
+/// Particle (pose + weight) for MCL. Matches xloc mcl_2d_localization::Particle.
+/// </summary>
+public class MclParticle
+{
+    public MclPose2d Pose { get; set; }
+    public double W { get; set; }
+
+    public MclParticle() : this(0, 0, 0, 0) { }
+
+    public MclParticle(double x, double y, double yaw, double w)
+    {
+        Pose = new MclPose2d(x, y, yaw);
+        W = w;
+    }
+
+    public MclParticle(MclPose2d pose, double w)
+    {
+        Pose = pose;
+        W = w;
+    }
+}
+
+#endregion

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/PgmLoader.cs

@@ -0,0 +1,409 @@
+using Microsoft.Extensions.Logging;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Localization;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Helper class để load occupancy grid từ PGM file (ROS map format)
+/// PGM format: Portable Gray Map
+/// </summary>
+internal static class PgmLoader
+{
+    /// <summary>
+    /// Load occupancy grid từ PGM file và YAML metadata file.
+    /// UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM, Y-axis pointing UP).
+    /// This ensures consistency across all map formats (PGM, PNG, JPG) and in-memory grids.
+    /// </summary>
+    /// <param name="pgmPath">Path to PGM file.</param>
+    /// <param name="yamlPath">Optional path to YAML metadata.</param>
+    /// <param name="logger">Optional logger.</param>
+    public static OccupancyGrid? LoadFromPgm(
+        string pgmPath,
+        string? yamlPath = null,
+        ILogger? logger = null)
+    {
+        try
+        {
+            if (!File.Exists(pgmPath))
+            {
+                logger?.LogError("PgmLoader: PGM file not found: {PgmPath}", pgmPath);
+                return null;
+            }
+
+            // Try to find YAML file if not provided
+            if (string.IsNullOrEmpty(yamlPath))
+            {
+                var yamlPathCandidate = Path.ChangeExtension(pgmPath, ".yaml");
+                if (File.Exists(yamlPathCandidate))
+                {
+                    yamlPath = yamlPathCandidate;
+                }
+            }
+
+            // Read YAML metadata if available
+            double resolution = 0.05; // Default resolution
+            Pose origin = new()
+            {
+                Position = new Vector3(0, 0, 0),
+                Orientation = new Quaternion(0, 0, 0, 1)
+            };
+
+            if (!string.IsNullOrEmpty(yamlPath) && File.Exists(yamlPath))
+            {
+                var metadata = ParseYamlMetadata(yamlPath, logger);
+                resolution = metadata.resolution;
+                origin = metadata.origin;
+            }
+
+            // Read PGM file
+            // Strategy: Read header using StreamReader to parse, then read file again byte-by-byte to find exact binary start position
+            string magic = "";
+            int width = 0, height = 0, maxValue = 0;
+            long binaryDataStartPosition = 0;
+
+            // First pass: Read header using StreamReader to parse values
+            using (var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read))
+            {
+                using var reader = new StreamReader(fileStream, leaveOpen: true);
+                
+                // Read magic number (P5 or P2)
+                magic = reader.ReadLine() ?? "";
+                if (magic != "P5" && magic != "P2")
+                {
+                    logger?.LogError("PgmLoader: Invalid PGM format. Expected P5 or P2, got: {Magic}", magic);
+                    return null;
+                }
+
+                // Skip comments
+                string? line;
+                while ((line = reader.ReadLine()) != null && line.StartsWith('#'))
+                {
+                    // Skip comment lines
+                }
+
+                // Read dimensions
+                var dimensions = line?.Split(' ', StringSplitOptions.RemoveEmptyEntries);
+                if (dimensions == null || dimensions.Length != 2)
+                {
+                    logger?.LogError("PgmLoader: Invalid dimensions line: {Line}", line);
+                    return null;
+                }
+
+                width = int.Parse(dimensions[0]);
+                height = int.Parse(dimensions[1]);
+
+                // Read max value
+                var maxValueLine = reader.ReadLine();
+                if (string.IsNullOrEmpty(maxValueLine))
+                {
+                    logger?.LogError("PgmLoader: Missing max value");
+                    return null;
+                }
+
+                maxValue = int.Parse(maxValueLine);
+            }
+
+            // Second pass: Read file from beginning byte-by-byte to find exact binary data start position
+            // This is necessary because StreamReader buffering makes Position unreliable
+            // Strategy: Read lines and count non-comment lines until we find the 3rd non-comment line (max value)
+            using (var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read))
+            {
+                var lineBuffer = new List<byte>();
+                var nonCommentLinesFound = 0;
+                
+                while (true)
+                {
+                    var buffer = new byte[1];
+                    var bytesRead = fileStream.Read(buffer, 0, 1);
+                    if (bytesRead == 0)
+                    {
+                        break; // End of file
+                    }
+                    
+                    var currentChar = (char)buffer[0];
+                    
+                    // Check for newline (handle both \n and \r\n)
+                    if (currentChar == '\n')
+                    {
+                        // Process line
+                        if (lineBuffer.Count > 0)
+                        {
+                            var line = System.Text.Encoding.ASCII.GetString([.. lineBuffer]).TrimStart();
+                            
+                            if (!line.StartsWith('#'))
+                            {
+                                nonCommentLinesFound++;
+                                
+                                if (nonCommentLinesFound == 3)
+                                {
+                                    // Max value line - binary data starts after this newline
+                                    binaryDataStartPosition = fileStream.Position;
+                                    break;
+                                }
+                            }
+                        }
+                        
+                        lineBuffer.Clear();
+                    }
+                    else if (currentChar == '\r')
+                    {
+                        // Handle \r\n - read next byte
+                        var nextBytesRead = fileStream.Read(buffer, 0, 1);
+                        if (nextBytesRead > 0 && buffer[0] == '\n')
+                        {
+                            // Process line
+                            if (lineBuffer.Count > 0)
+                            {
+                                var line = System.Text.Encoding.ASCII.GetString([.. lineBuffer]).TrimStart();
+                                
+                                if (!line.StartsWith('#'))
+                                {
+                                    nonCommentLinesFound++;
+                                    
+                                    if (nonCommentLinesFound == 3)
+                                    {
+                                        // Max value line - binary data starts after this \r\n
+                                        binaryDataStartPosition = fileStream.Position;
+                                        break;
+                                    }
+                                }
+                            }
+                            
+                            lineBuffer.Clear();
+                        }
+                        else
+                        {
+                            // Just \r without \n, add to line buffer
+                            lineBuffer.Add((byte)currentChar);
+                            if (nextBytesRead > 0)
+                            {
+                                lineBuffer.Add(buffer[0]);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        // Add character to line buffer
+                        lineBuffer.Add(buffer[0]);
+                    }
+                }
+                
+                if (binaryDataStartPosition == 0)
+                {
+                    logger?.LogError("PgmLoader: Failed to find max value line end position. Found {LinesFound} non-comment lines.", nonCommentLinesFound);
+                    return null;
+                }
+            }
+
+            // Create occupancy grid
+            var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);
+
+            // Second pass: Read pixel data
+            if (magic == "P5")
+            {
+                // Binary format - read directly from file
+                using var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read);
+                fileStream.Position = binaryDataStartPosition;
+                
+                var buffer = new byte[width * height];
+                var totalBytesRead = 0;
+                
+                // Read in chunks to handle large files
+                while (totalBytesRead < buffer.Length)
+                {
+                    var bytesRead = fileStream.Read(buffer, totalBytesRead, buffer.Length - totalBytesRead);
+                    if (bytesRead == 0)
+                    {
+                        // End of file reached
+                        break;
+                    }
+                    totalBytesRead += bytesRead;
+                }
+
+                if (totalBytesRead != buffer.Length)
+                {
+                    logger?.LogError("PgmLoader: Unexpected end of file. Expected {Expected} bytes, got {Actual}. File may be corrupted or incomplete.", buffer.Length, totalBytesRead);
+                    return null;
+                }
+
+                // Convert to occupancy values
+                // UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM, Y-axis pointing UP)
+                // Both PGM file and output grid use same convention → direct copy, no Y-flip needed
+                for (int y = 0; y < height; y++)
+                {
+                    for (int x = 0; x < width; x++)
+                    {
+                        int index = y * width + x;
+                        var pixelValue = buffer[index];
+                        var occupancyValue = ConvertPixelToOccupancy(pixelValue, maxValue);
+                        occupancyGrid.Data[index] = occupancyValue;
+                    }
+                }
+            }
+            else // P2 - ASCII format
+            {
+                // ASCII format - read from file starting at binaryDataStartPosition
+                using var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read);
+                fileStream.Position = binaryDataStartPosition;
+                using var reader = new StreamReader(fileStream);
+                
+                // Read ASCII values
+                var data = new List<int>();
+                string? line;
+                while ((line = reader.ReadLine()) != null)
+                {
+                    var values = line.Split(' ', StringSplitOptions.RemoveEmptyEntries);
+                    foreach (var value in values)
+                    {
+                        if (int.TryParse(value, out var intValue))
+                        {
+                            data.Add(intValue);
+                        }
+                    }
+                }
+
+                if (data.Count != width * height)
+                {
+                    logger?.LogError("PgmLoader: Invalid data count. Expected {Expected}, got {Actual}", width * height, data.Count);
+                    return null;
+                }
+
+                // Convert to occupancy values (same Y convention as P5 branch)
+                // UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM)
+                for (int y = 0; y < height; y++)
+                {
+                    for (int x = 0; x < width; x++)
+                    {
+                        int index = y * width + x;
+                        var pixelValue = data[index];
+                        var occupancyValue = ConvertPixelToOccupancy((byte)pixelValue, maxValue);
+                        occupancyGrid.Data[index] = occupancyValue;
+                    }
+                }
+            }
+
+            return occupancyGrid;
+        }
+        catch (Exception ex)
+        {
+            logger?.LogError(ex, "PgmLoader: Failed to load PGM file: {PgmPath}", pgmPath);
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Convert PGM pixel value to occupancy grid value
+    /// - 0 (black) = occupied (100)
+    /// - 254 (white) = free (0)
+    /// - 205 (gray) = unknown (-1)
+    /// </summary>
+    private static sbyte ConvertPixelToOccupancy(int pixelValue, int maxValue)
+    {
+        // ROS map_server convention:
+        // - 0 = occupied
+        // - 254 = free
+        // - 205 = unknown
+        // Scale to 0-255 range
+        var normalizedValue = pixelValue * 255 / maxValue;
+
+        if (normalizedValue == 0)
+        {
+            return 100; // Occupied
+        }
+        else if (normalizedValue == 254 || normalizedValue == 255)
+        {
+            return 0; // Free
+        }
+        else if (normalizedValue >= 200 && normalizedValue <= 210)
+        {
+            return -1; // Unknown
+        }
+        else
+        {
+            // Interpolate: 0-200 = occupied (100-0), 210-254 = free (0)
+            if (normalizedValue < 200)
+            {
+                return (sbyte)Math.Clamp(100 - (normalizedValue * 100 / 200), 0, 100);
+            }
+            else
+            {
+                return 0; // Free
+            }
+        }
+    }
+
+    /// <summary>
+    /// Parse YAML metadata file (ROS map format)
+    /// </summary>
+    private static (double resolution, Pose origin) ParseYamlMetadata(string yamlPath, ILogger? logger)
+    {
+        double resolution = 0.05;
+        Pose origin = new()
+        {
+            Position = new Vector3(0, 0, 0),
+            Orientation = new Quaternion(0, 0, 0, 1)
+        };
+
+        try
+        {
+            var lines = File.ReadAllLines(yamlPath);
+            foreach (var line in lines)
+            {
+                var trimmed = line.Trim();
+                if (trimmed.StartsWith('#') || string.IsNullOrWhiteSpace(trimmed))
+                    continue;
+
+                // Parse key: value
+                var colonIndex = trimmed.IndexOf(':');
+                if (colonIndex < 0)
+                    continue;
+
+                var key = trimmed[..colonIndex].Trim();
+                var value = trimmed[(colonIndex + 1)..].Trim();
+
+                switch (key.ToLowerInvariant())
+                {
+                    case "resolution":
+                        if (double.TryParse(value, out var res))
+                        {
+                            resolution = res;
+                        }
+                        break;
+
+                    case "origin":
+                        // Format: [x, y, yaw] or [x, y, 0]
+                        value = value.TrimStart('[').TrimEnd(']');
+                        var coords = value.Split(',');
+                        if (coords.Length >= 2)
+                        {
+                            if (double.TryParse(coords[0].Trim(), out var x) &&
+                                double.TryParse(coords[1].Trim(), out var y))
+                            {
+                                origin.Position = new Vector3(x, y, 0);
+                            }
+                        }
+                        if (coords.Length >= 3)
+                        {
+                            if (double.TryParse(coords[2].Trim(), out var yaw))
+                            {
+                                // Convert yaw (radians) to quaternion
+                                var halfYaw = yaw / 2.0;
+                                origin.Orientation = new Quaternion(0, 0, Math.Sin(halfYaw), Math.Cos(halfYaw));
+                            }
+                        }
+                        break;
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            logger?.LogWarning(ex, "PgmLoader: Failed to parse YAML metadata: {YamlPath}", yamlPath);
+        }
+
+        return (resolution, origin);
+    }
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/PoseSnapshot.cs

@@ -0,0 +1,78 @@
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Immutable snapshot of robot pose with confidence metrics and drift detection status.
+/// Reference type enables lock-free atomic swap via volatile field.
+/// </summary>
+public sealed class PoseSnapshot
+{
+    public static readonly PoseSnapshot Empty = new();
+
+    /// <summary>Pose in GLOBAL (map) frame.</summary>
+    public Pose Pose { get; }
+
+    /// <summary>Pose covariance (Localizing mode only).</summary>
+    public Matrix3x3? Covariance { get; }
+
+    /// <summary>
+    /// Confidence score:
+    /// - ScanMapping: PoseConfidence from scan matcher
+    /// - Localizing: LocalizationScore from covariance/constraints/scanMatchScore
+    /// - Relocalizing: MCL reliability
+    /// </summary>
+    public double? Score { get; }
+
+    /// <summary>
+    /// Scan match score (PoseConfidence) from Cartographer.
+    /// This is the most direct indicator of how well the current scan matches the map.
+    /// Available in both ScanMapping and Localizing states.
+    /// </summary>
+    public double? ScanMatchScore { get; }
+
+    /// <summary>
+    /// Drift detection status (Localizing mode only).
+    /// Indicates whether the robot may be experiencing map drift.
+    /// </summary>
+    public DriftDetector.DriftStatus DriftStatus { get; }
+
+    /// <summary>Timestamp when this snapshot was created.</summary>
+    public long TimestampTicks { get; }
+
+    public PoseSnapshot()
+    {
+        Pose = new Pose();
+        DriftStatus = DriftDetector.DriftStatus.Stable;
+        TimestampTicks = DateTime.UtcNow.Ticks;
+    }
+
+    public PoseSnapshot(Pose pose, Matrix3x3? covariance = null, double? score = null)
+    {
+        Pose = pose;
+        Covariance = covariance;
+        Score = score;
+        DriftStatus = DriftDetector.DriftStatus.Stable;
+        TimestampTicks = DateTime.UtcNow.Ticks;
+    }
+
+    /// <summary>
+    /// Creates a PoseSnapshot with full drift detection information.
+    /// </summary>
+    public PoseSnapshot(
+        Pose pose,
+        Matrix3x3? covariance,
+        double? score,
+        double? scanMatchScore,
+        DriftDetector.DriftStatus driftStatus)
+    {
+        Pose = pose;
+        Covariance = covariance;
+        Score = score;
+        ScanMatchScore = scanMatchScore;
+        DriftStatus = driftStatus;
+        TimestampTicks = DateTime.UtcNow.Ticks;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/TrajectoryBuilderOptionsFactory.cs

@@ -0,0 +1,474 @@
+using CartographerSharp.Mapping;
+using CartographerSharp.Models.Mapping;
+using static CartographerSharp.Mapping.PoseGraph;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+/// <summary>
+/// Factory for creating TrajectoryBuilderOptions to avoid code duplication
+/// </summary>
+public static class TrajectoryBuilderOptionsFactory
+{
+    /// <summary>
+    /// Creates 2D trajectory builder options from configuration.
+    /// </summary>
+    /// <param name="config">Trajectory builder configuration</param>
+    /// <param name="useImuData">Whether to use IMU data</param>
+    /// <param name="gridTypeOverride">Override grid type (null = use config value). ProbabilityGrid for ScanMapping, Tsdf for Localizing.</param>
+    public static TrajectoryBuilderOptions Create2D(
+        TrajectoryBuilderConfiguration config,
+        bool useImuData,
+        GridOptions2D.GridType? gridTypeOverride = null)
+    {
+        if (!config.Use2D)
+        {
+            throw new ArgumentException("Configuration must have Use2D = true", nameof(config));
+        }
+
+        // Use explicit UseImuData from config if provided, otherwise use parameter
+        var finalUseImuData = config.UseImuData ?? useImuData;
+
+        // Create RealTimeCorrelativeScanMatcherOptions
+        RealTimeCorrelativeScanMatcherOptions? realTimeCorrelativeScanMatcherOptions = null;
+        if (config.RealTimeCorrelativeScanMatcherOptions != null)
+        {
+            var rtcConfig = config.RealTimeCorrelativeScanMatcherOptions;
+            realTimeCorrelativeScanMatcherOptions = new RealTimeCorrelativeScanMatcherOptions(
+                linearSearchWindow: rtcConfig.LinearSearchWindow,
+                angularSearchWindow: rtcConfig.AngularSearchWindow,
+                translationDeltaCostWeight: rtcConfig.TranslationDeltaCostWeight,
+                rotationDeltaCostWeight: rtcConfig.RotationDeltaCostWeight,
+                numThreads: rtcConfig.NumThreads);
+        }
+
+        // Create CeresScanMatcherOptions2D
+        CeresScanMatcherOptions2D? ceresScanMatcherOptions = null;
+        if (config.CeresScanMatcherOptions != null)
+        {
+            var csConfig = config.CeresScanMatcherOptions;
+            CartographerSharp.Models.Common.CeresSolverOptions? ceresSolverOptions = null;
+            if (csConfig.CeresSolverOptions != null)
+            {
+                var solverConfig = csConfig.CeresSolverOptions;
+                ceresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
+                    useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
+                    maxNumIterations: solverConfig.MaxNumIterations,
+                    numThreads: solverConfig.NumThreads,
+                    functionTolerance: solverConfig.FunctionTolerance,
+                    gradientTolerance: solverConfig.GradientTolerance,
+                    parameterTolerance: solverConfig.ParameterTolerance);
+            }
+
+            ceresScanMatcherOptions = new CeresScanMatcherOptions2D(
+                occupiedSpaceWeight: csConfig.OccupiedSpaceWeight,
+                translationWeight: csConfig.TranslationWeight,
+                rotationWeight: csConfig.RotationWeight,
+                ceresSolverOptions: ceresSolverOptions,
+                highResOccupiedSpaceWeight: csConfig.HighResOccupiedSpaceWeight,
+                highResTranslationWeight: csConfig.HighResTranslationWeight,
+                highResRotationWeight: csConfig.HighResRotationWeight,
+                landmarkWeight: csConfig.LandmarkWeight);
+        }
+
+        // Create MotionFilterOptions
+        MotionFilterOptions? motionFilterOptions = null;
+        if (config.MotionFilterOptions != null)
+        {
+            var mfConfig = config.MotionFilterOptions;
+            motionFilterOptions = new MotionFilterOptions(
+                maxTimeSeconds: mfConfig.MaxTimeSeconds,
+                maxDistanceMeters: mfConfig.MaxDistanceMeters,
+                maxAngleRadians: mfConfig.MaxAngleRadians);
+        }
+
+        // Create SubmapsOptions2D
+        SubmapsOptions2D? submapsOptions = null;
+        if (config.SubmapsOptions != null)
+        {
+            var subConfig = config.SubmapsOptions;
+
+            // Determine effective grid type (override takes priority over config)
+            var configGridType = (GridOptions2D.GridType)subConfig.GridOptions.GridType;
+            var effectiveGridType = gridTypeOverride ?? configGridType;
+
+            var gridOptions = new GridOptions2D(
+                gridType: effectiveGridType,
+                resolution: subConfig.GridOptions.Resolution);
+
+            // Create RangeDataInserterOptions
+            RangeDataInserterOptions? rangeDataInserterOptions = null;
+            if (subConfig.RangeDataInserterOptions != null)
+            {
+                var rdiConfig = subConfig.RangeDataInserterOptions;
+
+                // Determine inserter type to match effective grid type
+                var effectiveInserterType = effectiveGridType switch
+                {
+                    GridOptions2D.GridType.ProbabilityGrid => RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D,
+                    GridOptions2D.GridType.Tsdf => RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D,
+                    _ => (RangeDataInserterOptions.RangeDataInserterType)rdiConfig.RangeDataInserterType
+                };
+
+                // Read ProbabilityGrid options (use configured or C++ defaults)
+                ProbabilityGridRangeDataInserterOptions2D? probGridOptions = null;
+                if (rdiConfig.ProbabilityGridRangeDataInserterOptions != null)
+                {
+                    var pgConfig = rdiConfig.ProbabilityGridRangeDataInserterOptions;
+                    probGridOptions = new ProbabilityGridRangeDataInserterOptions2D(
+                        hitProbability: pgConfig.HitProbability,
+                        missProbability: pgConfig.MissProbability,
+                        insertFreeSpace: pgConfig.InsertFreeSpace);
+                }
+                else if (effectiveGridType == GridOptions2D.GridType.ProbabilityGrid)
+                {
+                    // C++ Cartographer defaults
+                    probGridOptions = new ProbabilityGridRangeDataInserterOptions2D(0.55, 0.49, true);
+                }
+
+                // Read TSDF options (use configured or C++ defaults)
+                TSDFRangeDataInserterOptions2D? tsdfOptions = null;
+                if (rdiConfig.TsdfRangeDataInserterOptions != null)
+                {
+                    var tsdfConfig = rdiConfig.TsdfRangeDataInserterOptions;
+                    var normalEstOptions = new NormalEstimationOptions2D(
+                        tsdfConfig.NormalEstimationOptions.NumNormalSamples,
+                        tsdfConfig.NormalEstimationOptions.SampleRadius);
+                    tsdfOptions = new TSDFRangeDataInserterOptions2D(
+                        truncationDistance: tsdfConfig.TruncationDistance,
+                        maximumWeight: tsdfConfig.MaximumWeight,
+                        updateFreeSpace: tsdfConfig.UpdateFreeSpace,
+                        normalEstimationOptions: normalEstOptions,
+                        projectSdfDistanceToScanNormal: tsdfConfig.ProjectSdfDistanceToScanNormal,
+                        updateWeightRangeExponent: tsdfConfig.UpdateWeightRangeExponent,
+                        updateWeightAngleScanNormalToRayKernelBandwidth: tsdfConfig.UpdateWeightAngleScanNormalToRayKernelBandwidth,
+                        updateWeightDistanceCellToHitKernelBandwidth: tsdfConfig.UpdateWeightDistanceCellToHitKernelBandwidth);
+                }
+                else if (effectiveGridType == GridOptions2D.GridType.Tsdf)
+                {
+                    // C++ Cartographer defaults for TSDF
+                    tsdfOptions = new TSDFRangeDataInserterOptions2D(
+                        truncationDistance: 0.3,
+                        maximumWeight: 10.0);
+                }
+
+                rangeDataInserterOptions = new RangeDataInserterOptions(
+                    rangeDataInserterType: effectiveInserterType,
+                    probabilityGridRangeDataInserterOptions2D: probGridOptions,
+                    tsdfRangeDataInserterOptions2D: tsdfOptions);
+            }
+
+            // Create HighResGridOptions2D if configured
+            // Apply same gridTypeOverride so HighResGridOptions matches the main grid type
+            GridOptions2D? highResGridOptions = null;
+            if (config.HighResGridOptions != null)
+            {
+                highResGridOptions = new GridOptions2D(
+                    gridType: gridTypeOverride ?? (GridOptions2D.GridType)config.HighResGridOptions.GridType,
+                    resolution: config.HighResGridOptions.Resolution);
+            }
+
+            submapsOptions = new SubmapsOptions2D(
+                numRangeData: subConfig.NumRangeData,
+                gridOptions2D: gridOptions,
+                rangeDataInserterOptions: rangeDataInserterOptions ?? new RangeDataInserterOptions(
+                    RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D),
+                highResGridOptions2D: highResGridOptions);
+        }
+
+        // Create AdaptiveVoxelFilterOptions
+        CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions? adaptiveVoxelFilterOptions = null;
+        if (config.AdaptiveVoxelFilterOptions != null)
+        {
+            var avfConfig = config.AdaptiveVoxelFilterOptions;
+            adaptiveVoxelFilterOptions = new CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions(
+                maxLength: avfConfig.MaxLength,
+                minNumPoints: avfConfig.MinNumPoints,
+                maxRange: avfConfig.MaxRange);
+        }
+
+        // Create LoopClosureAdaptiveVoxelFilterOptions
+        CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions? loopClosureAdaptiveVoxelFilterOptions = null;
+        if (config.LoopClosureAdaptiveVoxelFilterOptions != null)
+        {
+            var lcavfConfig = config.LoopClosureAdaptiveVoxelFilterOptions;
+            loopClosureAdaptiveVoxelFilterOptions = new CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions(
+                maxLength: lcavfConfig.MaxLength,
+                minNumPoints: lcavfConfig.MinNumPoints,
+                maxRange: lcavfConfig.MaxRange);
+        }
+
+        // Create PoseExtrapolatorOptions
+        PoseExtrapolatorOptions? poseExtrapolatorOptions = null;
+        if (config.PoseExtrapolatorOptions != null)
+        {
+            var peConfig = config.PoseExtrapolatorOptions;
+            
+            // Create ConstantVelocityPoseExtrapolatorOptions
+            var constantVelocityOptions = new ConstantVelocityPoseExtrapolatorOptions(
+                imuGravityTimeConstant: peConfig.ConstantVelocity.ImuGravityTimeConstant,
+                poseQueueDuration: peConfig.ConstantVelocity.PoseQueueDuration);
+
+            // Create ImuBasedPoseExtrapolatorOptions
+            ImuBasedPoseExtrapolatorOptions? imuBasedOptions = null;
+            if (peConfig.ImuBased != null)
+            {
+                var imuConfig = peConfig.ImuBased;
+                CartographerSharp.Models.Common.CeresSolverOptions? solverOptions = null;
+                if (imuConfig.SolverOptions != null)
+                {
+                    var solverConfig = imuConfig.SolverOptions;
+                    solverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
+                        useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
+                        maxNumIterations: solverConfig.MaxNumIterations,
+                        numThreads: solverConfig.NumThreads,
+                        functionTolerance: solverConfig.FunctionTolerance,
+                        gradientTolerance: solverConfig.GradientTolerance,
+                        parameterTolerance: solverConfig.ParameterTolerance);
+                }
+
+                imuBasedOptions = new ImuBasedPoseExtrapolatorOptions(
+                    poseQueueDuration: imuConfig.PoseQueueDuration,
+                    gravityConstant: imuConfig.GravityConstant,
+                    poseTranslationWeight: imuConfig.PoseTranslationWeight,
+                    poseRotationWeight: imuConfig.PoseRotationWeight,
+                    imuAccelerationWeight: imuConfig.ImuAccelerationWeight,
+                    imuRotationWeight: imuConfig.ImuRotationWeight,
+                    solverOptions: solverOptions,
+                    odometryTranslationWeight: imuConfig.OdometryTranslationWeight,
+                    odometryRotationWeight: imuConfig.OdometryRotationWeight);
+            }
+
+            poseExtrapolatorOptions = new PoseExtrapolatorOptions(
+                useImuBased: peConfig.UseImuBased,
+                constantVelocity: constantVelocityOptions,
+                imuBased: imuBasedOptions,
+                velocityThreshold: peConfig.VelocityThreshold,
+                accelerationThreshold: peConfig.AccelerationThreshold,
+                gravityDeviationThreshold: peConfig.GravityDeviationThreshold,
+                useOdometryDirectly: peConfig.UseOdometryDirectly ?? false);
+        }
+
+        // Create InitialPoseRealTimeCorrelativeScanMatcherOptions
+        RealTimeCorrelativeScanMatcherOptions? initialPoseRealTimeCorrelativeScanMatcherOptions = null;
+        if (config.InitialPoseRealTimeCorrelativeScanMatcherOptions != null)
+        {
+            var iprtcConfig = config.InitialPoseRealTimeCorrelativeScanMatcherOptions;
+            initialPoseRealTimeCorrelativeScanMatcherOptions = new RealTimeCorrelativeScanMatcherOptions(
+                linearSearchWindow: iprtcConfig.LinearSearchWindow,
+                angularSearchWindow: iprtcConfig.AngularSearchWindow,
+                translationDeltaCostWeight: iprtcConfig.TranslationDeltaCostWeight,
+                rotationDeltaCostWeight: iprtcConfig.RotationDeltaCostWeight,
+                numThreads: iprtcConfig.NumThreads);
+        }
+
+        // Create InitialPoseCeresScanMatcherOptions
+        CeresScanMatcherOptions2D? initialPoseCeresScanMatcherOptions = null;
+        if (config.InitialPoseCeresScanMatcherOptions != null)
+        {
+            var ipcsConfig = config.InitialPoseCeresScanMatcherOptions;
+            CartographerSharp.Models.Common.CeresSolverOptions? ipCeresSolverOptions = null;
+            if (ipcsConfig.CeresSolverOptions != null)
+            {
+                var solverConfig = ipcsConfig.CeresSolverOptions;
+                ipCeresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
+                    useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
+                    maxNumIterations: solverConfig.MaxNumIterations,
+                    numThreads: solverConfig.NumThreads,
+                    functionTolerance: solverConfig.FunctionTolerance,
+                    gradientTolerance: solverConfig.GradientTolerance,
+                    parameterTolerance: solverConfig.ParameterTolerance);
+            }
+
+            initialPoseCeresScanMatcherOptions = new CeresScanMatcherOptions2D(
+                occupiedSpaceWeight: ipcsConfig.OccupiedSpaceWeight,
+                translationWeight: ipcsConfig.TranslationWeight,
+                rotationWeight: ipcsConfig.RotationWeight,
+                ceresSolverOptions: ipCeresSolverOptions,
+                highResOccupiedSpaceWeight: ipcsConfig.HighResOccupiedSpaceWeight,
+                highResTranslationWeight: ipcsConfig.HighResTranslationWeight,
+                highResRotationWeight: ipcsConfig.HighResRotationWeight,
+                landmarkWeight: ipcsConfig.LandmarkWeight);
+        }
+
+        var options2D = new LocalTrajectoryBuilderOptions2D(
+            minRange: config.MinRange,
+            maxRange: config.MaxRange,
+            minZ: (config.MinZ ?? -0.8f), // Default from Cartographer C++
+            maxZ: (config.MaxZ ?? 2.0), // Default from Cartographer C++
+            missingDataRayLength: (config.MissingDataRayLength ?? 5.0), // Default from Cartographer C++
+            numAccumulatedRangeData: config.NumAccumulatedRangeData,
+            voxelFilterSize: config.VoxelFilterSize,
+            useOnlineCorrelativeScanMatching: config.UseOnlineCorrelativeScanMatching,
+            realTimeCorrelativeScanMatcherOptions: realTimeCorrelativeScanMatcherOptions ?? default,
+            ceresScanMatcherOptions: ceresScanMatcherOptions ?? default,
+            motionFilterOptions: motionFilterOptions ?? default,
+            submapsOptions: submapsOptions ?? default,
+            useImuData: finalUseImuData,
+            adaptiveVoxelFilterOptions: adaptiveVoxelFilterOptions ?? default,
+            loopClosureAdaptiveVoxelFilterOptions: loopClosureAdaptiveVoxelFilterOptions ?? default,
+            poseExtrapolatorOptions: poseExtrapolatorOptions ?? default,
+            initialPoseRealTimeCorrelativeScanMatcherOptions: initialPoseRealTimeCorrelativeScanMatcherOptions ?? default,
+            initialPoseCeresScanMatcherOptions: initialPoseCeresScanMatcherOptions ?? default,
+            provideConfidenceScore: config.ProvideConfidenceScore,
+            ceresScoreSoftLimit: config.CeresScoreSoftLimit,
+            ceresScoreHardLimit: config.CeresScoreHardLimit,
+            maxConsecutiveHighCostBeforeNewSubmap: config.MaxConsecutiveHighCostBeforeNewSubmap);
+
+        var jsonOptions = System.Text.Json.JsonSerializer.Serialize(options2D);
+        var jsonElement = System.Text.Json.JsonDocument.Parse(jsonOptions).RootElement;
+
+        return new TrajectoryBuilderOptions(trajectoryBuilder2DOptions: jsonElement);
+    }
+
+    /// <summary>
+    /// Creates 3D trajectory builder options from configuration
+    /// </summary>
+    public static TrajectoryBuilderOptions Create3D(
+        TrajectoryBuilderConfiguration config,
+        bool useImuData)
+    {
+        if (config.Use2D)
+        {
+            throw new ArgumentException("Configuration must have Use2D = false", nameof(config));
+        }
+
+        // TODO: Implement 3D options when needed
+        throw new NotSupportedException("3D trajectory builder not yet implemented");
+    }
+
+    /// <summary>
+    /// Creates PoseGraphOptions from configuration
+    /// </summary>
+    public static PoseGraphOptions CreatePoseGraphOptions(MapBuilderConfiguration config)
+    {
+        // Create OptimizationProblemOptions from configuration
+        // Note: OptimizationProblemOptions proto only supports MaxNumIterations directly,
+        // not the full CeresSolverOptions (UseNonmonotonicSteps, NumThreads).
+        // If CeresSolverOptions.MaxNumIterations is provided, it takes precedence over MaxNumIterations.
+        var maxNumIterations = config.PoseGraphOptimizationProblemOptions.MaxNumIterations;
+        if (config.PoseGraphOptimizationProblemOptions.CeresSolverOptions != null)
+        {
+            // Use MaxNumIterations from CeresSolverOptions if provided, otherwise use direct MaxNumIterations
+            maxNumIterations = config.PoseGraphOptimizationProblemOptions.CeresSolverOptions.MaxNumIterations;
+        }
+
+        var optimizationOptions = new CartographerSharp.Models.Mapping.OptimizationProblemOptions
+        {
+            HuberScale = config.PoseGraphOptimizationProblemOptions.HuberScale,
+            OdometryHuberScale = config.PoseGraphOptimizationProblemOptions.OdometryHuberScale,
+            LocalSlamPoseHuberScale = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseHuberScale,
+            AccelerationWeight = config.PoseGraphOptimizationProblemOptions.AccelerationWeight,
+            RotationWeight = config.PoseGraphOptimizationProblemOptions.RotationWeight,
+            LocalSlamPoseTranslationWeight = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseTranslationWeight,
+            LocalSlamPoseRotationWeight = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseRotationWeight,
+            OdometryTranslationWeight = config.PoseGraphOptimizationProblemOptions.OdometryTranslationWeight,
+            OdometryRotationWeight = config.PoseGraphOptimizationProblemOptions.OdometryRotationWeight,
+            FixedFramePoseTranslationWeight = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTranslationWeight,
+            FixedFramePoseRotationWeight = config.PoseGraphOptimizationProblemOptions.FixedFramePoseRotationWeight,
+            FixedFramePoseUseTolerantLoss = config.PoseGraphOptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
+            FixedFramePoseTolerantLossParamA = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
+            FixedFramePoseTolerantLossParamB = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
+            LogSolverSummary = config.PoseGraphOptimizationProblemOptions.LogSolverSummary,
+            MaxNumIterations = maxNumIterations
+        };
+        
+        // Log mapped values to verify correct assignment
+
+        // Note: Other CeresSolverOptions properties (UseNonmonotonicSteps, NumThreads) are not supported
+        // by OptimizationProblemOptions proto. These options are hardcoded in OptimizationProblem2D/3D classes.
+
+        // Create ConstraintBuilderOptions from configuration
+        CartographerSharp.Models.Mapping.ConstraintBuilderOptions? constraintBuilderOptions = null;
+        if (config.ConstraintBuilderOptions != null)
+        {
+            var cbConfig = config.ConstraintBuilderOptions;
+            
+            // Create FastCorrelativeScanMatcherOptions2D
+            FastCorrelativeScanMatcherOptions2D? fastCorrelativeScanMatcherOptions = null;
+            if (cbConfig.FastCorrelativeScanMatcherOptions != null)
+            {
+                var fcsConfig = cbConfig.FastCorrelativeScanMatcherOptions;
+                fastCorrelativeScanMatcherOptions = new FastCorrelativeScanMatcherOptions2D(
+                    linearSearchWindow: fcsConfig.LinearSearchWindow,
+                    angularSearchWindow: fcsConfig.AngularSearchWindow,
+                    branchAndBoundDepth: fcsConfig.BranchAndBoundDepth,
+                    localizationLinearSearchWindow: fcsConfig.LocalizationLinearSearchWindow,
+                    localizationAngularSearchWindow: fcsConfig.LocalizationAngularSearchWindow);
+            }
+
+            // Create CeresScanMatcherOptions2D for constraint builder
+            CeresScanMatcherOptions2D? ceresScanMatcherOptions = null;
+            if (cbConfig.CeresScanMatcherOptions != null)
+            {
+                var csConfig = cbConfig.CeresScanMatcherOptions;
+                CartographerSharp.Models.Common.CeresSolverOptions? ceresSolverOptions = null;
+                if (csConfig.CeresSolverOptions != null)
+                {
+                    var solverConfig = csConfig.CeresSolverOptions;
+                    ceresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
+                        useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
+                        maxNumIterations: solverConfig.MaxNumIterations,
+                        numThreads: solverConfig.NumThreads,
+                        functionTolerance: solverConfig.FunctionTolerance,
+                        gradientTolerance: solverConfig.GradientTolerance,
+                        parameterTolerance: solverConfig.ParameterTolerance);
+                }
+
+                ceresScanMatcherOptions = new CeresScanMatcherOptions2D(
+                    occupiedSpaceWeight: csConfig.OccupiedSpaceWeight,
+                    translationWeight: csConfig.TranslationWeight,
+                    rotationWeight: csConfig.RotationWeight,
+                    ceresSolverOptions: ceresSolverOptions,
+                    highResOccupiedSpaceWeight: csConfig.HighResOccupiedSpaceWeight,
+                    highResTranslationWeight: csConfig.HighResTranslationWeight,
+                    highResRotationWeight: csConfig.HighResRotationWeight,
+                    landmarkWeight: csConfig.LandmarkWeight);
+            }
+
+            constraintBuilderOptions = new CartographerSharp.Models.Mapping.ConstraintBuilderOptions
+            {
+                SamplingRatio = cbConfig.SamplingRatio,
+                MaxConstraintDistance = cbConfig.MaxConstraintDistance,
+                MinScore = cbConfig.MinScore,
+                GlobalLocalizationMinScore = cbConfig.GlobalLocalizationMinScore,
+                LoopClosureTranslationWeight = cbConfig.LoopClosureTranslationWeight,
+                LoopClosureRotationWeight = cbConfig.LoopClosureRotationWeight,
+                LogMatches = cbConfig.LogMatches,
+                FastCorrelativeScanMatcherOptions = fastCorrelativeScanMatcherOptions,
+                CeresScanMatcherOptions = ceresScanMatcherOptions
+            };
+        }
+
+        // Create OverlappingSubmapsTrimmerOptions2D from configuration
+        PoseGraphOptions.OverlappingSubmapsTrimmerOptions2D? overlappingSubmapsTrimmer2D = null;
+        if (config.OverlappingSubmapsTrimmer2D != null)
+        {
+            var trimmerConfig = config.OverlappingSubmapsTrimmer2D;
+            overlappingSubmapsTrimmer2D = new PoseGraphOptions.OverlappingSubmapsTrimmerOptions2D(
+                freshSubmapsCount: trimmerConfig.FreshSubmapsCount,
+                minCoveredArea: trimmerConfig.MinCoveredArea,
+                minAddedSubmapsCount: trimmerConfig.MinAddedSubmapsCount
+            );
+        }
+
+        // Create PoseGraphOptions with optimization options from configuration
+        return new PoseGraphOptions(
+            optimizeEveryNNodes: config.OptimizeEveryNNodes,
+            matcherTranslationWeight: config.MatcherTranslationWeight,
+            matcherRotationWeight: config.MatcherRotationWeight,
+            maxNumFinalIterations: config.MaxNumFinalIterations ?? 200, // Default from Cartographer C++
+            globalSamplingRatio: config.GlobalSamplingRatio ?? 0.003, // Default from Cartographer C++
+            logResidualHistograms: config.LogResidualHistograms ?? false, // Default
+            globalConstraintSearchAfterNSeconds: config.GlobalConstraintSearchAfterNSeconds ?? 0.0, // Default (disabled)
+            overlappingSubmapsTrimmer2D: overlappingSubmapsTrimmer2D,
+            constraintBuilderOptions: constraintBuilderOptions,
+            optimizationProblemOptions: optimizationOptions
+        )
+        {
+            // Single-trajectory loop closure extension (not in C++ Cartographer)
+            EnableSingleTrajectoryLoopClosure = config.EnableSingleTrajectoryLoopClosure,
+            SingleTrajectoryLoopClosureDistanceThreshold = config.SingleTrajectoryLoopClosureDistanceThreshold
+        };
+    }
+}
+