809 lines
31 KiB
C#
809 lines
31 KiB
C#
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
|
|
}
|