Initial commit

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