79 lines
2.5 KiB
C#
79 lines
2.5 KiB
C#
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;
|
|
}
|
|
}
|