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

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