415 lines
13 KiB
C#
415 lines
13 KiB
C#
using Microsoft.Extensions.Options;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Localization;
|
|
using RobotNet10.Shared.Numbers;
|
|
using System.Diagnostics;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
|
|
/// <summary>
|
|
/// Evaluates scan matching quality at a given pose against occupancy grid.
|
|
/// Provides reliability [0,1] and MAE metrics without running particle filter.
|
|
/// Directly evaluates the quality of pose from CartographerSharp.
|
|
/// </summary>
|
|
public class ScanMatchingQualityEvaluator : IDisposable
|
|
{
|
|
#region Fields and Constructor
|
|
|
|
private readonly CartographerConfiguration _config;
|
|
private readonly ILogger<ScanMatchingQualityEvaluator> _logger;
|
|
private readonly Lock _lock = new();
|
|
|
|
// Occupancy grid and distance map
|
|
private OccupancyGrid? _occupancyGrid;
|
|
private double[,]? _distanceMap; // Distance to nearest occupied cell (meters)
|
|
private double _mapResolution;
|
|
private double _mapOriginX, _mapOriginY, _mapOriginYaw;
|
|
private int _mapWidth, _mapHeight;
|
|
|
|
// Likelihood field model constants
|
|
private double _normConstHit, _denomHit, _measurementModelRandom;
|
|
|
|
// Latest scan and pose for periodic evaluation
|
|
private RangeDataPayload? _latestScan;
|
|
private Pose? _latestPose;
|
|
private string? _latestDeviceId;
|
|
private DateTime _lastScanTime = DateTime.MinValue;
|
|
|
|
// Periodic evaluation state
|
|
private bool _running;
|
|
private Timer? _periodicTimer;
|
|
private readonly TimeSpan _evaluationInterval;
|
|
|
|
// Latest metrics
|
|
private double _reliability = 0.5;
|
|
private double? _mae;
|
|
private DateTime _lastUpdateTime = DateTime.MinValue;
|
|
|
|
// Primary lidar ID for filtering
|
|
private string? _primaryLidarId;
|
|
|
|
public ScanMatchingQualityEvaluator(
|
|
IOptions<CartographerConfiguration> configuration,
|
|
ILogger<ScanMatchingQualityEvaluator> logger)
|
|
{
|
|
_config = configuration?.Value ?? throw new ArgumentNullException(nameof(configuration));
|
|
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
|
|
|
// Get evaluation interval from configuration
|
|
_evaluationInterval = TimeSpan.FromSeconds(_config.Mcl.ReliabilityMonitoring.MonitoringIntervalSeconds);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public API
|
|
|
|
/// <summary>
|
|
/// Start periodic scan quality evaluation
|
|
/// </summary>
|
|
public void Start(OccupancyGrid occupancyGrid, Pose? initialPose, string? primaryLidarId)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_running)
|
|
{
|
|
_logger.LogWarning("ScanMatchingQualityEvaluator: Already running, ignoring Start call");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
_occupancyGrid = occupancyGrid ?? throw new ArgumentNullException(nameof(occupancyGrid));
|
|
_primaryLidarId = primaryLidarId;
|
|
|
|
// Build distance map from occupancy grid
|
|
BuildDistanceMap(occupancyGrid);
|
|
|
|
// Initialize likelihood field model constants
|
|
InitializeMeasurementModel();
|
|
|
|
// Create periodic timer
|
|
_periodicTimer = new Timer(
|
|
EvaluateScanQuality,
|
|
null,
|
|
_evaluationInterval,
|
|
_evaluationInterval);
|
|
|
|
_running = true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "ScanMatchingQualityEvaluator: Failed to start");
|
|
_running = false;
|
|
throw;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop periodic evaluation
|
|
/// </summary>
|
|
public void Stop()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_running)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_running = false;
|
|
|
|
_periodicTimer?.Dispose();
|
|
_periodicTimer = null;
|
|
|
|
_latestScan = null;
|
|
_latestPose = null;
|
|
_latestDeviceId = null;
|
|
_occupancyGrid = null;
|
|
_distanceMap = null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cache latest scan and pose for periodic evaluation
|
|
/// </summary>
|
|
public void OnScanReceived(string deviceId, RangeDataPayload payload, Pose? currentPose)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_running)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Filter by primary lidar if specified
|
|
if (!string.IsNullOrEmpty(_primaryLidarId) && deviceId != _primaryLidarId)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_latestScan = payload;
|
|
_latestPose = currentPose;
|
|
_latestDeviceId = deviceId;
|
|
_lastScanTime = DateTime.UtcNow;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get latest evaluation metrics
|
|
/// </summary>
|
|
public EvaluationMetrics GetLatestMetrics()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return new EvaluationMetrics(_reliability, _mae, _lastUpdateTime);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Distance Map Setup
|
|
|
|
/// <summary>
|
|
/// Build distance map from occupancy grid using Felzenszwalb-Huttenlocher distance transform
|
|
/// Distance map stores distance (in meters) to nearest occupied cell for each grid cell
|
|
/// </summary>
|
|
private void BuildDistanceMap(OccupancyGrid grid)
|
|
{
|
|
_mapWidth = grid.Width;
|
|
_mapHeight = grid.Height;
|
|
_mapResolution = grid.Resolution;
|
|
_mapOriginX = grid.Origin.Position.X;
|
|
_mapOriginY = grid.Origin.Position.Y;
|
|
_mapOriginYaw = grid.Origin.Orientation.ToYawRadian();
|
|
|
|
// Create binary map: 0 = occupied (100), 1 = free
|
|
var binaryMap = new byte[_mapHeight, _mapWidth];
|
|
for (int v = 0; v < _mapHeight; v++)
|
|
{
|
|
for (int u = 0; u < _mapWidth; u++)
|
|
{
|
|
int idx = v * _mapWidth + u;
|
|
binaryMap[v, u] = grid.Data[idx] == 100 ? (byte)0 : (byte)1;
|
|
}
|
|
}
|
|
|
|
// Compute Euclidean distance transform
|
|
_distanceMap = DistanceTransformHelper.ComputeEuclidean(binaryMap, _mapWidth, _mapHeight, _mapResolution);
|
|
|
|
_logger.LogDebug("ScanMatchingQualityEvaluator: Built distance map {Width}x{Height} at {Res}m resolution",
|
|
_mapWidth, _mapHeight, _mapResolution);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Measurement Model
|
|
|
|
/// <summary>
|
|
/// Initialize measurement model constants (likelihood field model)
|
|
/// </summary>
|
|
private void InitializeMeasurementModel()
|
|
{
|
|
double varHit = _config.Mcl.VarHit;
|
|
double zHit = _config.Mcl.ZHit;
|
|
double zRand = _config.Mcl.ZRand;
|
|
|
|
_normConstHit = 1.0 / Math.Sqrt(2.0 * Math.PI * varHit);
|
|
_denomHit = 2.0 * varHit;
|
|
_measurementModelRandom = zRand;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Scan Evaluation
|
|
|
|
/// <summary>
|
|
/// Timer callback: Evaluate scan quality at current pose
|
|
/// </summary>
|
|
private void EvaluateScanQuality(object? state)
|
|
{
|
|
try
|
|
{
|
|
// Read cached scan and pose
|
|
RangeDataPayload? scan;
|
|
Pose? pose;
|
|
bool running;
|
|
|
|
lock (_lock)
|
|
{
|
|
running = _running;
|
|
scan = _latestScan;
|
|
pose = _latestPose;
|
|
}
|
|
|
|
if (!running || scan == null || pose == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var stopwatch = Stopwatch.StartNew();
|
|
|
|
// Convert scan to ranges
|
|
var pointsForScan = scan.SensorFrame.Ranges
|
|
.Select(r => new Vector2(r.Position.X, r.Position.Y))
|
|
.ToList();
|
|
|
|
double angleMinDeg = scan.ActualAngleMin * (180.0 / Math.PI);
|
|
double angleMaxDeg = scan.ActualAngleMax * (180.0 / Math.PI);
|
|
|
|
// Evaluate scan at current pose
|
|
var result = EvaluateScanAtPose(pose.Value, pointsForScan, angleMinDeg, angleMaxDeg);
|
|
|
|
stopwatch.Stop();
|
|
|
|
// Update metrics
|
|
lock (_lock)
|
|
{
|
|
if (_running)
|
|
{
|
|
_reliability = result.Reliability;
|
|
_mae = result.Mae;
|
|
_lastUpdateTime = DateTime.UtcNow;
|
|
|
|
_logger.LogDebug(
|
|
"ScanMatchingQualityEvaluator: reliability={Reliability:F3}, mae={Mae:F4}m, validBeams={Beams}, elapsed={Elapsed}ms",
|
|
result.Reliability,
|
|
result.Mae,
|
|
result.ValidBeams,
|
|
stopwatch.ElapsedMilliseconds);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "ScanMatchingQualityEvaluator: Evaluation failed, keeping previous metrics");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Evaluate scan quality at given pose using likelihood field model
|
|
/// </summary>
|
|
private EvaluationResult EvaluateScanAtPose(Pose pose, List<Vector2> points, double angleMinDeg, double angleMaxDeg)
|
|
{
|
|
if (_distanceMap == null)
|
|
{
|
|
return new EvaluationResult(0.5, 1.0, 0.0, 0);
|
|
}
|
|
|
|
double totalError = 0;
|
|
double totalLikelihood = 0;
|
|
int validBeams = 0;
|
|
|
|
// Pose in map frame
|
|
double poseX = pose.Position.X;
|
|
double poseY = pose.Position.Y;
|
|
double poseYaw = pose.Orientation.ToYawRadian();
|
|
|
|
int numPoints = points.Count;
|
|
double angleRangeDeg = angleMaxDeg - angleMinDeg;
|
|
int scanStep = _config.Mcl.ScanStep;
|
|
|
|
for (int i = 0; i < numPoints; i += scanStep)
|
|
{
|
|
var point = points[i];
|
|
double range = Math.Sqrt(point.X * point.X + point.Y * point.Y);
|
|
|
|
// Skip invalid ranges
|
|
if (range < _config.TrajectoryBuilder.MinRange || range > _config.TrajectoryBuilder.MaxRange)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Beam angle in sensor frame
|
|
double beamAngleDeg = angleMinDeg + (angleRangeDeg * i / numPoints);
|
|
double beamAngleRad = beamAngleDeg * (Math.PI / 180.0);
|
|
|
|
// Transform beam endpoint to map frame
|
|
double cosYaw = Math.Cos(poseYaw);
|
|
double sinYaw = Math.Sin(poseYaw);
|
|
double beamEndX = poseX + (point.X * cosYaw - point.Y * sinYaw);
|
|
double beamEndY = poseY + (point.X * sinYaw + point.Y * cosYaw);
|
|
|
|
// Convert to grid coordinates
|
|
double dx = beamEndX - _mapOriginX;
|
|
double dy = beamEndY - _mapOriginY;
|
|
double cosOrigin = Math.Cos(_mapOriginYaw);
|
|
double sinOrigin = Math.Sin(_mapOriginYaw);
|
|
double gridX = (dx * cosOrigin + dy * sinOrigin) / _mapResolution;
|
|
double gridY = (-dx * sinOrigin + dy * cosOrigin) / _mapResolution;
|
|
|
|
int u = (int)Math.Round(gridX);
|
|
int v = (int)Math.Round(gridY);
|
|
|
|
// Check bounds
|
|
if (u < 0 || u >= _mapWidth || v < 0 || v >= _mapHeight)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Get distance to nearest obstacle
|
|
double dist = _distanceMap[v, u];
|
|
|
|
// Calculate error (MAE)
|
|
totalError += dist;
|
|
|
|
// Calculate likelihood using likelihood field model
|
|
double pHit = _normConstHit * Math.Exp(-(dist * dist) / _denomHit);
|
|
double likelihood = _config.Mcl.ZHit * pHit + _measurementModelRandom;
|
|
totalLikelihood += likelihood;
|
|
|
|
validBeams++;
|
|
}
|
|
|
|
// Calculate metrics
|
|
double mae = validBeams > 0 ? totalError / validBeams : 1.0;
|
|
double avgLikelihood = validBeams > 0 ? totalLikelihood / validBeams : 0.0;
|
|
|
|
// Calculate reliability from MAE and likelihood
|
|
double reliability = CalculateReliability(mae, avgLikelihood);
|
|
|
|
return new EvaluationResult(reliability, mae, avgLikelihood, validBeams);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate reliability [0,1] from MAE and average likelihood
|
|
/// </summary>
|
|
private double CalculateReliability(double mae, double avgLikelihood)
|
|
{
|
|
// MAE-based component (exponential decay)
|
|
// Good: < 0.05m → 1.0
|
|
// Poor: > 0.5m → ~0.0
|
|
double maeScore = Math.Exp(-10.0 * mae);
|
|
|
|
// Likelihood-based component (already normalized 0-1)
|
|
double likelihoodScore = Math.Clamp(avgLikelihood, 0.0, 1.0);
|
|
|
|
// Combine: MAE more important (70%), likelihood 30%
|
|
double reliability = maeScore * 0.7 + likelihoodScore * 0.3;
|
|
|
|
return Math.Clamp(reliability, 0.0, 1.0);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region IDisposable
|
|
|
|
public void Dispose()
|
|
{
|
|
Stop();
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// Evaluation result
|
|
/// </summary>
|
|
private record EvaluationResult(double Reliability, double Mae, double AvgLikelihood, int ValidBeams);
|
|
|
|
/// <summary>
|
|
/// Evaluation metrics (public)
|
|
/// </summary>
|
|
public record EvaluationMetrics(double Reliability, double? Mae, DateTime UpdateTime);
|
|
}
|