Initial commit
This commit is contained in:
@@ -0,0 +1,808 @@
|
||||
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
|
||||
}
|
||||
Reference in New Issue
Block a user