Initial commit

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