Initial commit
This commit is contained in:
@@ -0,0 +1,81 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for calculating covariance from Cartographer constraints
|
||||
/// </summary>
|
||||
public static class CovarianceCalculator
|
||||
{
|
||||
/// <summary>
|
||||
/// Result of covariance calculation
|
||||
/// </summary>
|
||||
public record Result(
|
||||
Matrix3x3? Covariance,
|
||||
int ConstraintCount,
|
||||
double AverageConstraintQuality);
|
||||
|
||||
/// <summary>
|
||||
/// Calculate covariance and quality metrics from constraints
|
||||
/// </summary>
|
||||
/// <param name="constraints">List of pose graph constraints</param>
|
||||
/// <returns>Covariance calculation result</returns>
|
||||
public static Result Calculate(IList<IPoseGraph.Constraint> constraints)
|
||||
{
|
||||
if (constraints == null || constraints.Count == 0)
|
||||
{
|
||||
return new Result(null, 0, 0.0);
|
||||
}
|
||||
|
||||
int constraintCount = constraints.Count;
|
||||
|
||||
// Calculate average constraint weights
|
||||
var avgTranslationWeight = constraints.Average(c => c.ConstraintPose.TranslationWeight);
|
||||
var avgRotationWeight = constraints.Average(c => c.ConstraintPose.RotationWeight);
|
||||
|
||||
// Calculate constraint quality score (0.0 - 1.0)
|
||||
var normalizedTranslationQuality = Math.Min(1.0, avgTranslationWeight / 100.0);
|
||||
var normalizedRotationQuality = Math.Min(1.0, avgRotationWeight / 100.0);
|
||||
var averageConstraintQuality = (normalizedTranslationQuality + normalizedRotationQuality) / 2.0;
|
||||
|
||||
// Calculate covariance from constraint weights
|
||||
var translationWeightVariance = constraints
|
||||
.Select(c => c.ConstraintPose.TranslationWeight)
|
||||
.Select(w => Math.Pow(w - avgTranslationWeight, 2))
|
||||
.Average();
|
||||
|
||||
var rotationWeightVariance = constraints
|
||||
.Select(c => c.ConstraintPose.RotationWeight)
|
||||
.Select(w => Math.Pow(w - avgRotationWeight, 2))
|
||||
.Average();
|
||||
|
||||
// Convert weights to covariance (inverse relationship)
|
||||
var translationCovBase = 1.0 / (avgTranslationWeight + 1e-6);
|
||||
var rotationCovBase = 1.0 / (avgRotationWeight + 1e-6);
|
||||
|
||||
// Adjust covariance based on variance
|
||||
var translationVarianceFactor = 1.0 + (translationWeightVariance / (avgTranslationWeight * avgTranslationWeight + 1e-6));
|
||||
var rotationVarianceFactor = 1.0 + (rotationWeightVariance / (avgRotationWeight * avgRotationWeight + 1e-6));
|
||||
|
||||
var translationCov = translationCovBase * translationVarianceFactor;
|
||||
var rotationCov = rotationCovBase * rotationVarianceFactor;
|
||||
|
||||
// Adjust covariance based on number of constraints
|
||||
var constraintCountFactor = 1.0 / (1.0 + Math.Log10(Math.Max(1, constraintCount)));
|
||||
translationCov *= constraintCountFactor;
|
||||
rotationCov *= constraintCountFactor;
|
||||
|
||||
// Create covariance matrix for 2D pose (x, y, theta)
|
||||
var covariance = Matrix3x3.Covariance(
|
||||
xx: translationCov,
|
||||
yy: translationCov,
|
||||
tt: rotationCov,
|
||||
xy: 0.0,
|
||||
xt: 0.0,
|
||||
yt: 0.0
|
||||
);
|
||||
|
||||
return new Result(covariance, constraintCount, averageConstraintQuality);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Euclidean Distance Transform using Felzenszwalb-Huttenlocher algorithm O(n).
|
||||
/// Shared implementation used by both MclService and ScanMatchingQualityEvaluator.
|
||||
/// Reference: "Distance Transforms of Sampled Functions", Felzenszwalb & Huttenlocher, 2012.
|
||||
/// </summary>
|
||||
public static class DistanceTransformHelper
|
||||
{
|
||||
#region Public API
|
||||
|
||||
/// <summary>
|
||||
/// Compute Euclidean distance (in meters) from each cell to the nearest occupied cell.
|
||||
/// binaryMap[v,u] == 0 → occupied, != 0 → free.
|
||||
/// </summary>
|
||||
public static double[,] ComputeEuclidean(byte[,] binaryMap, int width, int height, double resolution)
|
||||
{
|
||||
// Step 1: Initialize squared distances (0 for occupied, inf for free)
|
||||
const int inf = int.MaxValue / 2;
|
||||
var distSq = new int[height, width];
|
||||
for (int v = 0; v < height; v++)
|
||||
for (int u = 0; u < width; u++)
|
||||
distSq[v, u] = binaryMap[v, u] == 0 ? 0 : inf;
|
||||
|
||||
// Step 2: 1D distance transform along rows (horizontal pass)
|
||||
var tempDist = new int[Math.Max(width, height)];
|
||||
for (int y = 0; y < height; y++)
|
||||
{
|
||||
for (int x = 0; x < width; x++)
|
||||
tempDist[x] = distSq[y, x];
|
||||
|
||||
DistanceTransform1D(tempDist, width);
|
||||
|
||||
for (int x = 0; x < width; x++)
|
||||
distSq[y, x] = tempDist[x];
|
||||
}
|
||||
|
||||
// Step 3: 1D distance transform along columns (vertical pass)
|
||||
for (int x = 0; x < width; x++)
|
||||
{
|
||||
for (int y = 0; y < height; y++)
|
||||
tempDist[y] = distSq[y, x];
|
||||
|
||||
DistanceTransform1D(tempDist, height);
|
||||
|
||||
for (int y = 0; y < height; y++)
|
||||
distSq[y, x] = tempDist[y];
|
||||
}
|
||||
|
||||
// Step 4: Convert squared distance (in pixels) to Euclidean distance (in meters)
|
||||
var result = new double[height, width];
|
||||
for (int v = 0; v < height; v++)
|
||||
for (int u = 0; u < width; u++)
|
||||
result[v, u] = Math.Sqrt(distSq[v, u]) * resolution;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 1D Transform
|
||||
|
||||
/// <summary>
|
||||
/// 1D squared Euclidean distance transform using parabola lower envelope algorithm.
|
||||
/// Operates in-place on the input array.
|
||||
/// </summary>
|
||||
private static void DistanceTransform1D(int[] f, int n)
|
||||
{
|
||||
if (n == 0) return;
|
||||
|
||||
// v stores parabola indices, z stores intersection points
|
||||
var v = new int[n];
|
||||
var z = new double[n + 1];
|
||||
int k = 0; // index of rightmost parabola
|
||||
v[0] = 0;
|
||||
z[0] = double.NegativeInfinity;
|
||||
z[1] = double.PositiveInfinity;
|
||||
|
||||
// Build lower envelope of parabolas
|
||||
for (int q = 1; q < n; q++)
|
||||
{
|
||||
double s;
|
||||
while (true)
|
||||
{
|
||||
int vk = v[k];
|
||||
double fq = f[q];
|
||||
double fvk = f[vk];
|
||||
s = ((fq + q * q) - (fvk + vk * vk)) / (2.0 * (q - vk));
|
||||
|
||||
if (s > z[k])
|
||||
break;
|
||||
|
||||
k--;
|
||||
if (k < 0)
|
||||
{
|
||||
k = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
k++;
|
||||
v[k] = q;
|
||||
z[k] = s;
|
||||
z[k + 1] = double.PositiveInfinity;
|
||||
}
|
||||
|
||||
// Fill in values of distance transform
|
||||
k = 0;
|
||||
var result = new int[n];
|
||||
for (int q = 0; q < n; q++)
|
||||
{
|
||||
while (z[k + 1] < q)
|
||||
k++;
|
||||
int vk = v[k];
|
||||
int dx = q - vk;
|
||||
result[q] = dx * dx + f[vk];
|
||||
}
|
||||
|
||||
// Copy result back
|
||||
Array.Copy(result, f, n);
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -0,0 +1,364 @@
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for calculating localization confidence scores with MCL reliability metrics
|
||||
/// and scan matching quality for drift detection.
|
||||
/// </summary>
|
||||
public static class LocalizationScoreCalculator
|
||||
{
|
||||
private const int MinConstraintsForGoodScore = 5;
|
||||
private const int MaxConstraintsForScaling = 50;
|
||||
|
||||
// Base weights when all data is available (v2 - includes scan match score)
|
||||
private const double ScanMatchWeight = 0.30; // NEW - most important for drift detection
|
||||
private const double CovarianceWeight = 0.15; // Reduced from 0.25
|
||||
private const double ConstraintCountWeight = 0.10; // Reduced from 0.20
|
||||
private const double ConstraintQualityWeight = 0.10; // Reduced from 0.15
|
||||
private const double MclReliabilityWeight = 0.20; // Reduced from 0.25
|
||||
private const double MclMaeWeight = 0.15; // Same as before
|
||||
|
||||
/// <summary>
|
||||
/// Calculate localization confidence score (0.0 - 1.0) from multiple factors including MCL metrics
|
||||
/// and scan matching quality. This is the primary method for Localizing state.
|
||||
/// </summary>
|
||||
/// <param name="covariance">Pose covariance matrix (nullable)</param>
|
||||
/// <param name="constraintCount">Number of constraints</param>
|
||||
/// <param name="constraintQuality">Average constraint quality (0.0 - 1.0)</param>
|
||||
/// <param name="mclReliability">MCL reliability [0,1] from decision model (nullable)</param>
|
||||
/// <param name="mclMae">MCL mean absolute error in meters (nullable)</param>
|
||||
/// <param name="scanMatchScore">Scan matching score (PoseConfidence) from Cartographer (nullable)</param>
|
||||
/// <returns>Combined localization score (0.0 - 1.0)</returns>
|
||||
public static double Calculate(
|
||||
Matrix3x3? covariance,
|
||||
int constraintCount,
|
||||
double constraintQuality,
|
||||
double? mclReliability = null,
|
||||
double? mclMae = null,
|
||||
double? scanMatchScore = null)
|
||||
{
|
||||
var covarianceScore = CalculateCovarianceScoreImproved(covariance);
|
||||
var constraintCountScore = CalculateConstraintCountScore(constraintCount);
|
||||
var constraintQualityScore = constraintQuality;
|
||||
|
||||
// MCL metrics (default to neutral values if not available)
|
||||
var mclReliabilityScore = mclReliability ?? 0.5;
|
||||
var mclMaeScore = CalculateMaeScore(mclMae);
|
||||
|
||||
// Scan match score (critical for drift detection)
|
||||
var normalizedScanMatchScore = CalculateScanMatchScore(scanMatchScore);
|
||||
|
||||
// Adaptive weight adjustment based on data availability
|
||||
var weights = CalculateAdaptiveWeightsV2(
|
||||
hasMcl: mclReliability.HasValue,
|
||||
hasCovariance: covariance.HasValue,
|
||||
hasScanMatch: scanMatchScore.HasValue,
|
||||
constraintCount: constraintCount);
|
||||
|
||||
var combinedScore =
|
||||
(normalizedScanMatchScore * weights.ScanMatch) +
|
||||
(covarianceScore * weights.Covariance) +
|
||||
(constraintCountScore * weights.ConstraintCount) +
|
||||
(constraintQualityScore * weights.ConstraintQuality) +
|
||||
(mclReliabilityScore * weights.MclReliability) +
|
||||
(mclMaeScore * weights.MclMae);
|
||||
|
||||
return Math.Clamp(combinedScore, 0.0, 1.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate score from scan matching confidence (PoseConfidence from Cartographer).
|
||||
/// This is the most direct indicator of how well the current scan matches the map.
|
||||
/// Low scan match score often indicates drift or being in a featureless area.
|
||||
///
|
||||
/// IMPORTANT: PoseConfidence from Cartographer is returned as percentage (0-100),
|
||||
/// not as a normalized value (0-1). This method handles both ranges.
|
||||
/// </summary>
|
||||
private static double CalculateScanMatchScore(double? scanMatchScore)
|
||||
{
|
||||
if (!scanMatchScore.HasValue)
|
||||
{
|
||||
return 0.5; // Neutral if not available
|
||||
}
|
||||
|
||||
var score = scanMatchScore.Value;
|
||||
|
||||
// Normalize to [0, 1] range if input is in [0, 100] range (PoseConfidence is percentage)
|
||||
// Cartographer's LocalPose_Confidence returns 0-100
|
||||
if (score > 1.0)
|
||||
{
|
||||
score = score / 100.0;
|
||||
}
|
||||
|
||||
// Clamp to valid range
|
||||
score = Math.Clamp(score, 0.0, 1.0);
|
||||
|
||||
// Apply sigmoid transformation to make it more sensitive in mid-range
|
||||
// This helps detect gradual degradation before it becomes critical
|
||||
// Sigmoid: score' = 1 / (1 + exp(-10 * (score - 0.5)))
|
||||
// This maps: 0.3 -> ~0.12, 0.5 -> 0.5, 0.7 -> ~0.88
|
||||
return 1.0 / (1.0 + Math.Exp(-10.0 * (score - 0.5)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate score from covariance matrix using trace instead of determinant
|
||||
/// Trace = sum of diagonal elements (sum of variances) - better captures overall uncertainty
|
||||
/// </summary>
|
||||
private static double CalculateCovarianceScoreImproved(Matrix3x3? covariance)
|
||||
{
|
||||
if (covariance == null)
|
||||
{
|
||||
return 0.5; // Default to moderate confidence
|
||||
}
|
||||
|
||||
// Use trace (sum of diagonal elements) instead of determinant
|
||||
// Trace = σ_x² + σ_y² + σ_θ² (sum of variances)
|
||||
// Better captures overall uncertainty and less sensitive to directional bias
|
||||
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 to 0.0-1.0 range using inverse relationship
|
||||
// Typical good localization: trace < 0.1
|
||||
// Typical poor localization: trace > 1.0
|
||||
// Formula: score = 1 / (1 + trace)
|
||||
return 1.0 / (1.0 + trace);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate score from constraint count
|
||||
/// More constraints = more reliable localization
|
||||
/// </summary>
|
||||
private static double CalculateConstraintCountScore(int constraintCount)
|
||||
{
|
||||
if (constraintCount == 0)
|
||||
{
|
||||
return 0.0; // No constraints = no confidence
|
||||
}
|
||||
|
||||
if (constraintCount < MinConstraintsForGoodScore)
|
||||
{
|
||||
// Few constraints: linear scaling from 0 to 0.5
|
||||
return constraintCount / (double)MinConstraintsForGoodScore * 0.5;
|
||||
}
|
||||
|
||||
// Many constraints: logarithmic scaling from 0.5 to 1.0
|
||||
var normalizedCount = Math.Min(constraintCount, MaxConstraintsForScaling);
|
||||
var logFactor = Math.Log10(normalizedCount + 1) / Math.Log10(MaxConstraintsForScaling + 1);
|
||||
return 0.5 + (logFactor * 0.5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate score from MCL MAE (mean absolute error)
|
||||
/// Lower MAE = better scan-map fit = higher score
|
||||
/// </summary>
|
||||
private static double CalculateMaeScore(double? mae)
|
||||
{
|
||||
if (!mae.HasValue)
|
||||
{
|
||||
return 0.5; // Default to moderate confidence if MAE not available
|
||||
}
|
||||
|
||||
var maeValue = mae.Value;
|
||||
|
||||
if (maeValue < 0.01)
|
||||
{
|
||||
return 1.0; // Excellent fit (< 1cm error)
|
||||
}
|
||||
|
||||
// Normalize MAE using exponential decay
|
||||
// 0m = 1.0, 0.1m = 0.61, 0.2m = 0.37, 0.5m = 0.08
|
||||
// Formula: score = exp(-5 * mae)
|
||||
return Math.Exp(-5.0 * maeValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate adaptive weights based on available data
|
||||
/// Redistributes weights when some data is not available
|
||||
/// </summary>
|
||||
private static WeightSet CalculateAdaptiveWeights(
|
||||
bool hasMcl,
|
||||
bool hasCovariance,
|
||||
int constraintCount)
|
||||
{
|
||||
// Start with base weights
|
||||
var weights = new WeightSet
|
||||
{
|
||||
Covariance = CovarianceWeight,
|
||||
ConstraintCount = ConstraintCountWeight,
|
||||
ConstraintQuality = ConstraintQualityWeight,
|
||||
MclReliability = MclReliabilityWeight,
|
||||
MclMae = MclMaeWeight
|
||||
};
|
||||
|
||||
// If MCL not available, redistribute its weight
|
||||
if (!hasMcl)
|
||||
{
|
||||
double mclTotalWeight = MclReliabilityWeight + MclMaeWeight;
|
||||
weights.MclReliability = 0.0;
|
||||
weights.MclMae = 0.0;
|
||||
|
||||
// Give more weight to covariance and constraint quality
|
||||
weights.Covariance += mclTotalWeight * 0.5;
|
||||
weights.ConstraintQuality += mclTotalWeight * 0.5;
|
||||
}
|
||||
|
||||
// If few constraints, reduce constraint score weight
|
||||
if (constraintCount < MinConstraintsForGoodScore)
|
||||
{
|
||||
double reduction = weights.ConstraintCount * 0.5;
|
||||
weights.ConstraintCount *= 0.5;
|
||||
|
||||
// Redistribute to covariance and quality
|
||||
weights.Covariance += reduction * 0.5;
|
||||
weights.ConstraintQuality += reduction * 0.5;
|
||||
}
|
||||
|
||||
// Normalize weights to sum to 1.0
|
||||
double total = weights.Covariance + weights.ConstraintCount +
|
||||
weights.ConstraintQuality + weights.MclReliability + weights.MclMae;
|
||||
|
||||
if (total > 0)
|
||||
{
|
||||
weights.Covariance /= total;
|
||||
weights.ConstraintCount /= total;
|
||||
weights.ConstraintQuality /= total;
|
||||
weights.MclReliability /= total;
|
||||
weights.MclMae /= total;
|
||||
}
|
||||
|
||||
return weights;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Weight set for adaptive weighting (legacy - without scan match)
|
||||
/// </summary>
|
||||
private record struct WeightSet
|
||||
{
|
||||
public double Covariance { get; set; }
|
||||
public double ConstraintCount { get; set; }
|
||||
public double ConstraintQuality { get; set; }
|
||||
public double MclReliability { get; set; }
|
||||
public double MclMae { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate adaptive weights V2 including scan match score.
|
||||
/// Redistributes weights when some data is not available.
|
||||
/// Prioritizes scan match score as it's the most direct drift indicator.
|
||||
/// </summary>
|
||||
private static WeightSetV2 CalculateAdaptiveWeightsV2(
|
||||
bool hasMcl,
|
||||
bool hasCovariance,
|
||||
bool hasScanMatch,
|
||||
int constraintCount)
|
||||
{
|
||||
// Start with base weights
|
||||
var weights = new WeightSetV2
|
||||
{
|
||||
ScanMatch = ScanMatchWeight,
|
||||
Covariance = CovarianceWeight,
|
||||
ConstraintCount = ConstraintCountWeight,
|
||||
ConstraintQuality = ConstraintQualityWeight,
|
||||
MclReliability = MclReliabilityWeight,
|
||||
MclMae = MclMaeWeight
|
||||
};
|
||||
|
||||
// If scan match not available, redistribute to MCL and covariance
|
||||
if (!hasScanMatch)
|
||||
{
|
||||
double scanMatchTotal = weights.ScanMatch;
|
||||
weights.ScanMatch = 0.0;
|
||||
|
||||
if (hasMcl)
|
||||
{
|
||||
// Give more weight to MCL (it provides similar information)
|
||||
weights.MclReliability += scanMatchTotal * 0.5;
|
||||
weights.MclMae += scanMatchTotal * 0.3;
|
||||
weights.Covariance += scanMatchTotal * 0.2;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Redistribute to covariance and constraints
|
||||
weights.Covariance += scanMatchTotal * 0.4;
|
||||
weights.ConstraintQuality += scanMatchTotal * 0.4;
|
||||
weights.ConstraintCount += scanMatchTotal * 0.2;
|
||||
}
|
||||
}
|
||||
|
||||
// If MCL not available, redistribute its weight
|
||||
if (!hasMcl)
|
||||
{
|
||||
double mclTotalWeight = weights.MclReliability + weights.MclMae;
|
||||
weights.MclReliability = 0.0;
|
||||
weights.MclMae = 0.0;
|
||||
|
||||
if (hasScanMatch)
|
||||
{
|
||||
// Scan match is available, give it more weight
|
||||
weights.ScanMatch += mclTotalWeight * 0.5;
|
||||
weights.Covariance += mclTotalWeight * 0.3;
|
||||
weights.ConstraintQuality += mclTotalWeight * 0.2;
|
||||
}
|
||||
else
|
||||
{
|
||||
// No direct quality indicators, rely on constraints
|
||||
weights.Covariance += mclTotalWeight * 0.5;
|
||||
weights.ConstraintQuality += mclTotalWeight * 0.5;
|
||||
}
|
||||
}
|
||||
|
||||
// If few constraints, reduce constraint score weight
|
||||
if (constraintCount < MinConstraintsForGoodScore)
|
||||
{
|
||||
double reduction = weights.ConstraintCount * 0.5;
|
||||
weights.ConstraintCount *= 0.5;
|
||||
|
||||
// Redistribute to scan match (if available) or covariance
|
||||
if (hasScanMatch)
|
||||
{
|
||||
weights.ScanMatch += reduction * 0.6;
|
||||
weights.Covariance += reduction * 0.4;
|
||||
}
|
||||
else
|
||||
{
|
||||
weights.Covariance += reduction * 0.5;
|
||||
weights.ConstraintQuality += reduction * 0.5;
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize weights to sum to 1.0
|
||||
double total = weights.ScanMatch + weights.Covariance + weights.ConstraintCount +
|
||||
weights.ConstraintQuality + weights.MclReliability + weights.MclMae;
|
||||
|
||||
if (total > 0)
|
||||
{
|
||||
weights.ScanMatch /= total;
|
||||
weights.Covariance /= total;
|
||||
weights.ConstraintCount /= total;
|
||||
weights.ConstraintQuality /= total;
|
||||
weights.MclReliability /= total;
|
||||
weights.MclMae /= total;
|
||||
}
|
||||
|
||||
return weights;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Weight set V2 for adaptive weighting (includes scan match)
|
||||
/// </summary>
|
||||
private record struct WeightSetV2
|
||||
{
|
||||
public double ScanMatch { get; set; }
|
||||
public double Covariance { get; set; }
|
||||
public double ConstraintCount { get; set; }
|
||||
public double ConstraintQuality { get; set; }
|
||||
public double MclReliability { get; set; }
|
||||
public double MclMae { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,232 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using CartographerSharp.Models.Transform;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using System.Reflection;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Extension methods for IMapBuilder
|
||||
/// Provides trajectory management, loading, and serialization operations
|
||||
/// </summary>
|
||||
public static class MapBuilderExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Add trajectory builder to MapBuilder with sensor IDs and optional callback.
|
||||
/// </summary>
|
||||
/// <param name="gridTypeOverride">Override grid type for submap creation. Use ProbabilityGrid for ScanMapping, Tsdf for Localizing.</param>
|
||||
public static (int trajectoryId, ITrajectoryBuilder? trajectoryBuilder) AddTrajectoryBuilder(
|
||||
this IMapBuilder mapBuilder,
|
||||
CartographerConfiguration config,
|
||||
TrajectoryBuilderOptions? trajectoryOptionsOverride = null,
|
||||
GridOptions2D.GridType? gridTypeOverride = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
ArgumentNullException.ThrowIfNull(config);
|
||||
|
||||
var sensorIds = TrajectoryHelper.BuildSensorIds(config);
|
||||
var trajectoryOptions = trajectoryOptionsOverride ?? CreateTrajectoryBuilderOptions(config, gridTypeOverride);
|
||||
|
||||
var trajectoryId = mapBuilder.AddTrajectoryBuilder(sensorIds, trajectoryOptions);
|
||||
var trajectoryBuilder = mapBuilder.GetTrajectoryBuilder(trajectoryId);
|
||||
|
||||
return (trajectoryId, trajectoryBuilder);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Wait for trajectory to finish with async support
|
||||
/// </summary>
|
||||
public static async Task WaitForTrajectoryFinishedAsync(
|
||||
this IMapBuilder mapBuilder,
|
||||
int trajectoryId,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
|
||||
var checkInterval = TimeSpan.FromMilliseconds(200);
|
||||
|
||||
while (true)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
if (mapBuilder.PoseGraph.IsTrajectoryFinished(trajectoryId))
|
||||
return;
|
||||
|
||||
await Task.Delay(checkInterval, cancellationToken);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finish all active trajectories before final optimization
|
||||
/// </summary>
|
||||
public static async Task FinishAllActiveTrajectoriesAsync(
|
||||
this IMapBuilder mapBuilder,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
|
||||
var trajectoryStates = mapBuilder.PoseGraph.GetTrajectoryStates();
|
||||
var activeTrajectories = trajectoryStates
|
||||
.Where(kvp => kvp.Value == IPoseGraph.TrajectoryState.Active)
|
||||
.Select(kvp => kvp.Key)
|
||||
.ToList();
|
||||
|
||||
if (activeTrajectories.Count == 0)
|
||||
return;
|
||||
|
||||
foreach (var trajectoryId in activeTrajectories)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
mapBuilder.FinishTrajectory(trajectoryId);
|
||||
await mapBuilder.WaitForTrajectoryFinishedAsync(trajectoryId, cancellationToken);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Find first frozen trajectory in a loaded map
|
||||
/// </summary>
|
||||
public static int? FindFrozenTrajectory(this IMapBuilder mapBuilder)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
|
||||
var trajectoryStates = mapBuilder.PoseGraph.GetTrajectoryStates();
|
||||
if (trajectoryStates == null || trajectoryStates.Count == 0)
|
||||
return null;
|
||||
|
||||
foreach (var kvp in trajectoryStates)
|
||||
{
|
||||
if (kvp.Value == IPoseGraph.TrajectoryState.Frozen)
|
||||
{
|
||||
var trajectoryId = kvp.Key;
|
||||
|
||||
// Validate frozen trajectory has nodes
|
||||
var trajectoryNodePoses = mapBuilder.PoseGraph.GetTrajectoryNodePoses();
|
||||
if (trajectoryNodePoses != null && !trajectoryNodePoses.IsEmpty)
|
||||
{
|
||||
var trajectoryNodes = trajectoryNodePoses.BeginOfTrajectory(trajectoryId).ToList();
|
||||
if (trajectoryNodes.Count > 0)
|
||||
return trajectoryId;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if MapBuilder has trajectories and dispose if so
|
||||
/// </summary>
|
||||
public static bool DisposeIfHasTrajectories(this IMapBuilder mapBuilder)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
|
||||
var trajectoryStates = mapBuilder.PoseGraph.GetTrajectoryStates();
|
||||
if (trajectoryStates != null && trajectoryStates.Count > 0)
|
||||
{
|
||||
(mapBuilder as IDisposable)?.Dispose();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finish trajectory if active, then dispose MapBuilder
|
||||
/// </summary>
|
||||
public static void FinishTrajectoryAndDispose(this IMapBuilder mapBuilder, int trajectoryId)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
|
||||
if (trajectoryId >= 0)
|
||||
{
|
||||
var poseGraph = mapBuilder.PoseGraph;
|
||||
if (!poseGraph.IsTrajectoryFinished(trajectoryId))
|
||||
{
|
||||
mapBuilder.FinishTrajectory(trajectoryId);
|
||||
}
|
||||
}
|
||||
|
||||
(mapBuilder as IDisposable)?.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Add localization trajectory builder with optional initial pose.
|
||||
/// </summary>
|
||||
/// <param name="gridTypeOverride">Override grid type for submap creation. Use Tsdf for Localizing.</param>
|
||||
public static (int trajectoryId, ITrajectoryBuilder? trajectoryBuilder, Rigid3d? initialPoseInMapFrame) AddLocalizationTrajectoryBuilder(
|
||||
this IMapBuilder mapBuilder,
|
||||
CartographerConfiguration config,
|
||||
Pose? initialPose,
|
||||
GridOptions2D.GridType? gridTypeOverride = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
ArgumentNullException.ThrowIfNull(config);
|
||||
|
||||
var frozenTrajectoryId = mapBuilder.FindFrozenTrajectory();
|
||||
var trajectoryOptions = CreateTrajectoryBuilderOptions(config, gridTypeOverride);
|
||||
CartographerSharp.Transform.Rigid3d? initialPoseInMapFrame = null;
|
||||
|
||||
if (frozenTrajectoryId.HasValue)
|
||||
{
|
||||
CartographerSharp.Models.Transform.Rigid3dProto relativePoseProto;
|
||||
|
||||
if (initialPose.HasValue)
|
||||
{
|
||||
var poseInMapFrame = mapBuilder.PoseGraph.GetTransformToMap() * PoseConverter.ToRigid3d(initialPose.Value);
|
||||
initialPoseInMapFrame = poseInMapFrame;
|
||||
relativePoseProto = (CartographerSharp.Models.Transform.Rigid3dProto)poseInMapFrame;
|
||||
}
|
||||
else
|
||||
{
|
||||
relativePoseProto = new CartographerSharp.Models.Transform.Rigid3dProto(
|
||||
new CartographerSharp.Models.Transform.Vector3d(0, 0, 0),
|
||||
new CartographerSharp.Models.Transform.Quaterniond(0, 0, 0, 1));
|
||||
}
|
||||
|
||||
var initialTrajectoryPose = new CartographerSharp.Models.Mapping.InitialTrajectoryPose(
|
||||
relativePoseProto,
|
||||
frozenTrajectoryId.Value,
|
||||
DateTimeOffset.UtcNow.ToUnixTimeSeconds() * 1000000000L);
|
||||
|
||||
trajectoryOptions = new TrajectoryBuilderOptions(
|
||||
trajectoryBuilder2DOptions: trajectoryOptions.TrajectoryBuilder2DOptions,
|
||||
trajectoryBuilder3DOptions: trajectoryOptions.TrajectoryBuilder3DOptions,
|
||||
initialTrajectoryPose: initialTrajectoryPose,
|
||||
pureLocalizationTrimmer: trajectoryOptions.PureLocalizationTrimmer,
|
||||
collateFixedFrame: trajectoryOptions.CollateFixedFrame,
|
||||
collateLandmarks: trajectoryOptions.CollateLandmarks,
|
||||
poseGraphOdometryMotionFilter: trajectoryOptions.PoseGraphOdometryMotionFilter);
|
||||
}
|
||||
|
||||
var (trajectoryId, trajectoryBuilder) = mapBuilder.AddTrajectoryBuilder(config, trajectoryOptions);
|
||||
return (trajectoryId, trajectoryBuilder, initialPoseInMapFrame);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create trajectory builder options from config.
|
||||
/// </summary>
|
||||
/// <param name="gridTypeOverride">Override grid type. ProbabilityGrid for ScanMapping, Tsdf for Localizing.</param>
|
||||
public static TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
|
||||
CartographerConfiguration config,
|
||||
GridOptions2D.GridType? gridTypeOverride = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(config);
|
||||
|
||||
if (config.TrajectoryBuilder.Use2D)
|
||||
{
|
||||
return TrajectoryBuilderOptionsFactory.Create2D(
|
||||
config.TrajectoryBuilder,
|
||||
config.Sensors.Imu.Enabled,
|
||||
gridTypeOverride);
|
||||
}
|
||||
else
|
||||
{
|
||||
return TrajectoryBuilderOptionsFactory.Create3D(
|
||||
config.TrajectoryBuilder,
|
||||
config.Sensors.Imu.Enabled);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,214 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for creating and configuring MapBuilder instances
|
||||
/// Factory methods for MapBuilder and related configurations
|
||||
/// Extension methods are now in MapBuilderExtensions class
|
||||
/// </summary>
|
||||
public static class MapBuilderHelper
|
||||
{
|
||||
/// <summary>
|
||||
/// Create a new MapBuilder with configuration
|
||||
/// </summary>
|
||||
public static MapBuilder CreateMapBuilder(CartographerConfiguration config)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(config);
|
||||
|
||||
var use2D = config.MapBuilder.UseTrajectoryBuilder2D ?? config.TrajectoryBuilder.Use2D;
|
||||
var use3D = config.MapBuilder.UseTrajectoryBuilder3D ?? !config.TrajectoryBuilder.Use2D;
|
||||
|
||||
var poseGraphOptions = TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(config.MapBuilder);
|
||||
|
||||
var mapBuilderOptions = new MapBuilderOptions(
|
||||
useTrajectoryBuilder2D: use2D,
|
||||
useTrajectoryBuilder3D: use3D,
|
||||
numBackgroundThreads: config.MapBuilder.NumBackgroundThreads,
|
||||
poseGraphOptions: poseGraphOptions,
|
||||
collateByTrajectory: config.MapBuilder.CollateByTrajectory);
|
||||
|
||||
return new MapBuilder(mapBuilderOptions);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create MapBuilder from MapLoadResult (pbstream path and saved config).
|
||||
/// Uses saved config if available, otherwise uses current config.
|
||||
/// Then loads state from pbstream file with frozen trajectories for localization.
|
||||
/// </summary>
|
||||
public static MapBuilder CreateMapBuilderFromLoadResult(
|
||||
MapCartographerLoadResult loadResult,
|
||||
CartographerConfiguration currentConfig,
|
||||
ILogger? logger = null,
|
||||
bool loadFrozenState = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(loadResult);
|
||||
ArgumentNullException.ThrowIfNull(currentConfig);
|
||||
|
||||
if (string.IsNullOrEmpty(loadResult.PbstreamPath))
|
||||
{
|
||||
throw new ArgumentException("PbstreamPath cannot be null or empty", nameof(loadResult));
|
||||
}
|
||||
|
||||
if (!File.Exists(loadResult.PbstreamPath))
|
||||
{
|
||||
throw new FileNotFoundException("Pbstream file not found", loadResult.PbstreamPath);
|
||||
}
|
||||
|
||||
// Get MapBuilder options from saved config or current config
|
||||
bool use2D;
|
||||
bool use3D;
|
||||
int numBackgroundThreads;
|
||||
PoseGraphOptions poseGraphOptions;
|
||||
|
||||
if (loadResult.SavedConfig != null)
|
||||
{
|
||||
// Use saved config
|
||||
use2D = loadResult.SavedConfig.MapBuilder.UseTrajectoryBuilder2D ?? loadResult.SavedConfig.Use2D;
|
||||
use3D = loadResult.SavedConfig.MapBuilder.UseTrajectoryBuilder3D ?? !loadResult.SavedConfig.Use2D;
|
||||
numBackgroundThreads = loadResult.SavedConfig.MapBuilder.NumBackgroundThreads;
|
||||
poseGraphOptions = CreatePoseGraphOptionsFromSnapshot(loadResult.SavedConfig);
|
||||
|
||||
// Warn if config mismatch
|
||||
var currentUse2D = currentConfig.MapBuilder.UseTrajectoryBuilder2D ?? currentConfig.TrajectoryBuilder.Use2D;
|
||||
if (currentUse2D != use2D)
|
||||
{
|
||||
logger?.LogWarning(
|
||||
"MapBuilderHelper: Map config mismatch detected. Using saved config (2D={Saved2D}) instead of current config (2D={Current2D}).",
|
||||
use2D, currentUse2D);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Use current config
|
||||
use2D = currentConfig.MapBuilder.UseTrajectoryBuilder2D ?? currentConfig.TrajectoryBuilder.Use2D;
|
||||
use3D = currentConfig.MapBuilder.UseTrajectoryBuilder3D ?? !currentConfig.TrajectoryBuilder.Use2D;
|
||||
numBackgroundThreads = currentConfig.MapBuilder.NumBackgroundThreads;
|
||||
poseGraphOptions = TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(currentConfig.MapBuilder);
|
||||
}
|
||||
|
||||
var mapBuilderOptions = new MapBuilderOptions(
|
||||
useTrajectoryBuilder2D: use2D,
|
||||
useTrajectoryBuilder3D: use3D,
|
||||
numBackgroundThreads: numBackgroundThreads,
|
||||
poseGraphOptions: poseGraphOptions
|
||||
);
|
||||
|
||||
var mapBuilder = new MapBuilder(mapBuilderOptions);
|
||||
|
||||
// Load state from pbstream file
|
||||
// loadFrozenState: true freezes trajectories in the loaded map for localization
|
||||
// This prevents modifying the map during localization (as per xloc reference implementation)
|
||||
_ = mapBuilder.LoadStateFromFile(loadResult.PbstreamPath, loadFrozenState: loadFrozenState);
|
||||
|
||||
logger?.LogInformation(
|
||||
"MapBuilderHelper: Created MapBuilder from pbstream file: {PbstreamPath}, loadFrozenState={LoadFrozenState}",
|
||||
loadResult.PbstreamPath, loadFrozenState);
|
||||
|
||||
return mapBuilder;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates PoseGraphOptions from saved config snapshot
|
||||
/// </summary>
|
||||
private static PoseGraphOptions CreatePoseGraphOptionsFromSnapshot(MapCartographerConfigSnapshot savedConfig)
|
||||
{
|
||||
// Create temporary MapBuilderConfiguration from snapshot
|
||||
var tempMapBuilderConfig = new MapBuilderConfiguration
|
||||
{
|
||||
UseTrajectoryBuilder2D = savedConfig.MapBuilder.UseTrajectoryBuilder2D,
|
||||
UseTrajectoryBuilder3D = savedConfig.MapBuilder.UseTrajectoryBuilder3D,
|
||||
NumBackgroundThreads = savedConfig.MapBuilder.NumBackgroundThreads,
|
||||
OptimizeEveryNNodes = savedConfig.MapBuilder.OptimizeEveryNNodes,
|
||||
MatcherTranslationWeight = savedConfig.MapBuilder.MatcherTranslationWeight,
|
||||
MatcherRotationWeight = savedConfig.MapBuilder.MatcherRotationWeight,
|
||||
MaxNumFinalIterations = savedConfig.MapBuilder.MaxNumFinalIterations,
|
||||
GlobalSamplingRatio = savedConfig.MapBuilder.GlobalSamplingRatio,
|
||||
LogResidualHistograms = savedConfig.MapBuilder.LogResidualHistograms,
|
||||
GlobalConstraintSearchAfterNSeconds = savedConfig.MapBuilder.GlobalConstraintSearchAfterNSeconds,
|
||||
EnableSingleTrajectoryLoopClosure = savedConfig.MapBuilder.EnableSingleTrajectoryLoopClosure,
|
||||
SingleTrajectoryLoopClosureDistanceThreshold = savedConfig.MapBuilder.SingleTrajectoryLoopClosureDistanceThreshold,
|
||||
PoseGraphOptimizationProblemOptions = new OptimizationProblemOptions
|
||||
{
|
||||
HuberScale = savedConfig.MapBuilder.OptimizationProblemOptions.HuberScale,
|
||||
AccelerationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.AccelerationWeight,
|
||||
RotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.RotationWeight,
|
||||
LocalSlamPoseTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.LocalSlamPoseTranslationWeight,
|
||||
LocalSlamPoseRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.LocalSlamPoseRotationWeight,
|
||||
OdometryTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.OdometryTranslationWeight,
|
||||
OdometryRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.OdometryRotationWeight,
|
||||
FixedFramePoseTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTranslationWeight,
|
||||
FixedFramePoseRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseRotationWeight,
|
||||
FixedFramePoseUseTolerantLoss = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
|
||||
FixedFramePoseTolerantLossParamA = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
|
||||
FixedFramePoseTolerantLossParamB = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
|
||||
LogSolverSummary = savedConfig.MapBuilder.OptimizationProblemOptions.LogSolverSummary,
|
||||
MaxNumIterations = savedConfig.MapBuilder.OptimizationProblemOptions.MaxNumIterations
|
||||
}
|
||||
};
|
||||
|
||||
return TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(tempMapBuilderConfig);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tạo config snapshot từ configuration hiện tại (để lưu vào map.json khi save).
|
||||
/// </summary>
|
||||
public static MapCartographerConfigSnapshot CreateConfigSnapshot(CartographerConfiguration config)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(config);
|
||||
var use2D = config.MapBuilder.UseTrajectoryBuilder2D ?? config.TrajectoryBuilder.Use2D;
|
||||
|
||||
return new MapCartographerConfigSnapshot
|
||||
{
|
||||
Use2D = use2D,
|
||||
MapBuilder = new MapCarographerBuilderConfigSnapshot
|
||||
{
|
||||
UseTrajectoryBuilder2D = config.MapBuilder.UseTrajectoryBuilder2D,
|
||||
UseTrajectoryBuilder3D = config.MapBuilder.UseTrajectoryBuilder3D,
|
||||
NumBackgroundThreads = config.MapBuilder.NumBackgroundThreads,
|
||||
OptimizeEveryNNodes = config.MapBuilder.OptimizeEveryNNodes,
|
||||
MatcherTranslationWeight = config.MapBuilder.MatcherTranslationWeight,
|
||||
MatcherRotationWeight = config.MapBuilder.MatcherRotationWeight,
|
||||
MaxNumFinalIterations = config.MapBuilder.MaxNumFinalIterations,
|
||||
GlobalSamplingRatio = config.MapBuilder.GlobalSamplingRatio,
|
||||
LogResidualHistograms = config.MapBuilder.LogResidualHistograms,
|
||||
GlobalConstraintSearchAfterNSeconds = config.MapBuilder.GlobalConstraintSearchAfterNSeconds,
|
||||
EnableSingleTrajectoryLoopClosure = config.MapBuilder.EnableSingleTrajectoryLoopClosure,
|
||||
SingleTrajectoryLoopClosureDistanceThreshold = config.MapBuilder.SingleTrajectoryLoopClosureDistanceThreshold,
|
||||
OptimizationProblemOptions = new OptimizationProblemOptionsSnapshot
|
||||
{
|
||||
HuberScale = config.MapBuilder.PoseGraphOptimizationProblemOptions.HuberScale,
|
||||
AccelerationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.AccelerationWeight,
|
||||
RotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.RotationWeight,
|
||||
LocalSlamPoseTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.LocalSlamPoseTranslationWeight,
|
||||
LocalSlamPoseRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.LocalSlamPoseRotationWeight,
|
||||
OdometryTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.OdometryTranslationWeight,
|
||||
OdometryRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.OdometryRotationWeight,
|
||||
FixedFramePoseTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTranslationWeight,
|
||||
FixedFramePoseRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseRotationWeight,
|
||||
FixedFramePoseUseTolerantLoss = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
|
||||
FixedFramePoseTolerantLossParamA = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
|
||||
FixedFramePoseTolerantLossParamB = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
|
||||
LogSolverSummary = config.MapBuilder.PoseGraphOptimizationProblemOptions.LogSolverSummary,
|
||||
MaxNumIterations = config.MapBuilder.PoseGraphOptimizationProblemOptions.MaxNumIterations
|
||||
}
|
||||
},
|
||||
TrajectoryBuilder = new TrajectoryBuilderConfigSnapshot
|
||||
{
|
||||
Use2D = config.TrajectoryBuilder.Use2D,
|
||||
MinRange = config.TrajectoryBuilder.MinRange,
|
||||
MaxRange = config.TrajectoryBuilder.MaxRange,
|
||||
MinZ = config.TrajectoryBuilder.MinZ,
|
||||
MaxZ = config.TrajectoryBuilder.MaxZ,
|
||||
MissingDataRayLength = config.TrajectoryBuilder.MissingDataRayLength,
|
||||
VoxelFilterSize = config.TrajectoryBuilder.VoxelFilterSize,
|
||||
NumAccumulatedRangeData = config.TrajectoryBuilder.NumAccumulatedRangeData,
|
||||
UseImuData = config.TrajectoryBuilder.UseImuData,
|
||||
UseOnlineCorrelativeScanMatching = config.TrajectoryBuilder.UseOnlineCorrelativeScanMatching
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
using RobotNet10.RobotApp.Shared;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Shared helper for map name sanitization and legacy metadata migration.
|
||||
/// Used by CartographerService and MapSaveProcessor.
|
||||
/// </summary>
|
||||
public static class MapNameHelper
|
||||
{
|
||||
/// <summary>
|
||||
/// Sanitize map name for use as directory name.
|
||||
/// Removes invalid filename characters and trims spaces/dots.
|
||||
/// </summary>
|
||||
public static string Sanitize(string mapName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(mapName))
|
||||
throw new ArgumentException("Map name cannot be null or empty", nameof(mapName));
|
||||
|
||||
var invalidChars = Path.GetInvalidFileNameChars();
|
||||
var sanitized = mapName;
|
||||
|
||||
foreach (var c in invalidChars)
|
||||
{
|
||||
sanitized = sanitized.Replace(c, '_');
|
||||
}
|
||||
|
||||
sanitized = sanitized.Trim(' ', '.');
|
||||
|
||||
if (string.IsNullOrWhiteSpace(sanitized))
|
||||
throw new ArgumentException("Map name is invalid after sanitization", nameof(mapName));
|
||||
|
||||
return sanitized;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ensures MapInfo has Size calculated from Bounds for legacy map.json files
|
||||
/// that don't have Size field populated.
|
||||
/// </summary>
|
||||
public static MapInfo EnsureMapSize(MapInfo metadata)
|
||||
{
|
||||
if (metadata.Size.Width == 0 && metadata.Size.Height == 0)
|
||||
{
|
||||
var width = metadata.Bounds.MaxX - metadata.Bounds.MinX;
|
||||
var height = metadata.Bounds.MaxY - metadata.Bounds.MinY;
|
||||
return new MapInfo
|
||||
{
|
||||
Name = metadata.Name,
|
||||
FolderPath = metadata.FolderPath,
|
||||
CreatedDate = metadata.CreatedDate,
|
||||
Resolution = metadata.Resolution,
|
||||
Size = new MapSize(width, height),
|
||||
Origin = metadata.Origin,
|
||||
Bounds = metadata.Bounds,
|
||||
TrajectoryNodeCount = metadata.TrajectoryNodeCount
|
||||
};
|
||||
}
|
||||
return metadata;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Centralizes map directory path construction and legacy file name resolution.
|
||||
/// Eliminates duplicated Path.Combine + legacy fallback patterns across CartographerService.
|
||||
/// </summary>
|
||||
public static class MapPathHelper
|
||||
{
|
||||
#region Path Construction
|
||||
|
||||
/// <summary>
|
||||
/// Get the full path to a map directory, sanitizing the map name.
|
||||
/// </summary>
|
||||
public static string GetMapPath(string mapsDirectory, string mapName)
|
||||
{
|
||||
var sanitizedMapName = MapNameHelper.Sanitize(mapName);
|
||||
return Path.Combine(Path.GetFullPath(mapsDirectory), sanitizedMapName);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region File Resolution
|
||||
|
||||
/// <summary>
|
||||
/// Resolve metadata JSON path, trying map.json first then legacy metadata.json.
|
||||
/// Returns null if neither exists.
|
||||
/// </summary>
|
||||
public static string? ResolveMetadataPath(string mapPath)
|
||||
{
|
||||
var metadataPath = Path.Combine(mapPath, "map.json");
|
||||
if (File.Exists(metadataPath))
|
||||
return metadataPath;
|
||||
|
||||
var legacyPath = Path.Combine(mapPath, "metadata.json");
|
||||
if (File.Exists(legacyPath))
|
||||
return legacyPath;
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Resolve pbstream path, trying map.pbstream first then any .pbstream file in the directory.
|
||||
/// Returns null if none found.
|
||||
/// </summary>
|
||||
public static string? ResolvePbstreamPath(string mapPath)
|
||||
{
|
||||
var pbstreamPath = Path.Combine(mapPath, "map.pbstream");
|
||||
if (File.Exists(pbstreamPath))
|
||||
return pbstreamPath;
|
||||
|
||||
var pbstreamFiles = Directory.GetFiles(mapPath, "*.pbstream");
|
||||
return pbstreamFiles.Length > 0 ? pbstreamFiles[0] : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Resolve map image path (PNG or JPG).
|
||||
/// Returns null if no image found.
|
||||
/// </summary>
|
||||
public static string? ResolveImagePath(string mapPath)
|
||||
{
|
||||
var pngPath = Path.Combine(mapPath, "map.png");
|
||||
if (File.Exists(pngPath))
|
||||
return pngPath;
|
||||
|
||||
var jpgPath = Path.Combine(mapPath, "map.jpg");
|
||||
if (File.Exists(jpgPath))
|
||||
return jpgPath;
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,636 @@
|
||||
using CartographerSharp.IO;
|
||||
using CartographerSharp.Mapping;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.RobotApp.Shared;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Localization;
|
||||
using System.Text.Json;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Handles map saving workflow including scan matching, optimization, and file I/O
|
||||
/// </summary>
|
||||
public class MapSaveProcessor
|
||||
{
|
||||
#region Fields and Constructor
|
||||
|
||||
private readonly CartographerConfiguration _config;
|
||||
private readonly ILogger<MapSaveProcessor> _logger;
|
||||
private readonly string _mapsDirectory;
|
||||
private static readonly JsonSerializerOptions SerializerOptions = new() { WriteIndented = true };
|
||||
|
||||
public MapSaveProcessor(
|
||||
CartographerConfiguration config,
|
||||
ILogger<MapSaveProcessor> logger)
|
||||
{
|
||||
_config = config ?? throw new ArgumentNullException(nameof(config));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
// Get maps directory from configuration
|
||||
_mapsDirectory = Path.GetFullPath(_config.MapStorage.Directory);
|
||||
Directory.CreateDirectory(_mapsDirectory);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Save Workflow
|
||||
|
||||
/// <summary>
|
||||
/// Execute full save map workflow with optimization and file I/O
|
||||
/// </summary>
|
||||
/// <param name="mapName">Name of the map to save</param>
|
||||
/// <param name="mapBuilder">MapBuilder containing map data</param>
|
||||
/// <param name="trajectoryId">Active trajectory ID to finish (-1 if none)</param>
|
||||
/// <param name="progressCallback">Progress callback (total, current, percent)</param>
|
||||
/// <param name="newOrigin">Optional new origin to transform the map before saving (e.g., wall-aligned pose)</param>
|
||||
/// <param name="cancellationToken">Cancellation token</param>
|
||||
public async Task<string> SaveMapAsync(
|
||||
string mapName,
|
||||
IMapBuilder mapBuilder,
|
||||
int trajectoryId,
|
||||
Func<int, int, int, Task> progressCallback,
|
||||
Pose? newOrigin,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(mapBuilder);
|
||||
ArgumentNullException.ThrowIfNull(mapName);
|
||||
|
||||
try
|
||||
{
|
||||
_logger.LogInformation("MapSaveProcessor: Starting map save process for: {MapName}", mapName);
|
||||
|
||||
// Wait for active scan matching to complete
|
||||
await WaitForScanMatchingAsync(cancellationToken);
|
||||
|
||||
// Track total progress across all phases
|
||||
// Phase 1: Initial work queue drain (0-20%)
|
||||
// Phase 2: Finish trajectory (20-40%)
|
||||
// Phase 3: RunFinalOptimization - work queue + constraint builder (40-90%)
|
||||
// Phase 4: Final scan matching + file saving (90-100%)
|
||||
|
||||
// Finish trajectory if active
|
||||
if (trajectoryId >= 0)
|
||||
{
|
||||
// Phase 1: Drain work queue before finishing trajectory (0-20%)
|
||||
_logger.LogDebug("MapSaveProcessor: Phase 1 - Draining work queue before finishing trajectory");
|
||||
await DrainWorkQueueWithProgressAsync(mapBuilder, progressCallback, 0, 20, cancellationToken);
|
||||
|
||||
// Phase 2: Finish trajectory (20-40%)
|
||||
_logger.LogDebug("MapSaveProcessor: Phase 2 - Finishing trajectory ID: {TrajectoryId}", trajectoryId);
|
||||
await progressCallback(100, 20, 20);
|
||||
await FinishTrajectoryAsync(mapBuilder, trajectoryId, cancellationToken);
|
||||
await progressCallback(100, 40, 40);
|
||||
}
|
||||
else
|
||||
{
|
||||
// No trajectory to finish, skip to phase 3
|
||||
await progressCallback(100, 40, 40);
|
||||
}
|
||||
|
||||
_logger.LogDebug("MapSaveProcessor: Finishing all active trajectories");
|
||||
// Finish all other active trajectories
|
||||
await mapBuilder.FinishAllActiveTrajectoriesAsync(cancellationToken);
|
||||
|
||||
// Phase 3: Run final optimization (40-90%)
|
||||
// RunFinalOptimization internally calls WaitForAllComputations which:
|
||||
// - Drains work queue
|
||||
// - Waits for constraint builder to finish
|
||||
// - Runs optimization
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
_logger.LogDebug("MapSaveProcessor: Phase 3 - Running final pose graph optimization");
|
||||
|
||||
// Start optimization in background task and track progress
|
||||
var optimizationTask = Task.Run(() => mapBuilder.PoseGraph.RunFinalOptimization(), cancellationToken);
|
||||
|
||||
// Track progress while optimization is running
|
||||
await TrackOptimizationProgressAsync(mapBuilder, progressCallback, 40, 90, optimizationTask, cancellationToken);
|
||||
|
||||
// Phase 4: Final scan matching + file saving (90-100%)
|
||||
_logger.LogDebug("MapSaveProcessor: Phase 4 - Final scan matching and file saving");
|
||||
await progressCallback(100, 90, 90);
|
||||
|
||||
// Final wait for scan matching
|
||||
await WaitForScanMatchingAsync(cancellationToken);
|
||||
|
||||
// Get map path
|
||||
var mapPath = GetMapPath(mapName);
|
||||
|
||||
// Validate map before saving
|
||||
var (isValid, errorMessage) = ValidateMapBeforeSave(mapBuilder, mapName, mapPath);
|
||||
if (!isValid)
|
||||
{
|
||||
throw new InvalidOperationException($"Map validation failed: {errorMessage}");
|
||||
}
|
||||
|
||||
_logger.LogDebug("MapSaveProcessor: Saving pbstream and metadata to: {MapPath}", mapPath);
|
||||
// Save pbstream file
|
||||
SavePbstream(mapBuilder, mapPath);
|
||||
|
||||
// Transform origin if newOrigin is provided (e.g., wall alignment)
|
||||
IMapBuilder mapBuilderForMetadata = mapBuilder;
|
||||
if (newOrigin.HasValue && !PbstreamTransformHelper.IsIdentityPose(newOrigin.Value))
|
||||
{
|
||||
_logger.LogDebug("MapSaveProcessor: Transforming map origin with wall-aligned pose");
|
||||
var pbstreamPath = Path.Combine(mapPath, "map.pbstream");
|
||||
|
||||
// For wall alignment, we need to apply the INVERSE of the compensation pose.
|
||||
// The compensation angle represents "how much the robot should rotate to align the wall",
|
||||
// but TransformToMap uses the inverse when rendering (via TransformToMapInverse).
|
||||
// So we invert the pose here to get the correct rotation direction in the final map.
|
||||
var poseRigid = PbstreamTransformHelper.PoseToRigid3d(newOrigin.Value);
|
||||
var invertedRigid = poseRigid.Inverse();
|
||||
var invertedPose = new Pose
|
||||
{
|
||||
Position = new RobotNet10.Shared.Numbers.Vector3(
|
||||
invertedRigid.Translation.X,
|
||||
invertedRigid.Translation.Y,
|
||||
invertedRigid.Translation.Z),
|
||||
Orientation = new RobotNet10.Shared.Numbers.Quaternion(
|
||||
invertedRigid.Rotation.X,
|
||||
invertedRigid.Rotation.Y,
|
||||
invertedRigid.Rotation.Z,
|
||||
invertedRigid.Rotation.W)
|
||||
};
|
||||
|
||||
PbstreamTransformHelper.TransformPbstreamOrigin(pbstreamPath, invertedPose, _logger);
|
||||
|
||||
// Reload MapBuilder from transformed pbstream
|
||||
var loadResult = new MapCartographerLoadResult
|
||||
{
|
||||
PbstreamPath = pbstreamPath,
|
||||
MapPath = mapPath,
|
||||
SavedConfig = null // Will use current config
|
||||
};
|
||||
mapBuilderForMetadata = MapBuilderHelper.CreateMapBuilderFromLoadResult(
|
||||
loadResult, _config, _logger, loadFrozenState: false);
|
||||
}
|
||||
|
||||
_logger.LogDebug("MapSaveProcessor: Generating occupancy grid and saving metadata");
|
||||
// Generate occupancy grid and save metadata
|
||||
var saveResult = await SaveMapMetadataAsync(mapName, mapBuilderForMetadata, mapPath, cancellationToken);
|
||||
|
||||
// Dispose reloaded MapBuilder if it was created
|
||||
if (mapBuilderForMetadata != mapBuilder && mapBuilderForMetadata is IDisposable disposable)
|
||||
{
|
||||
disposable.Dispose();
|
||||
}
|
||||
|
||||
// Final progress update (100%)
|
||||
await progressCallback(100, 100, 100);
|
||||
|
||||
_logger.LogInformation("MapSaveProcessor: Map saved successfully: {MapName}", mapName);
|
||||
return saveResult;
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
_logger.LogWarning("MapSaveProcessor: Map save cancelled for: {MapName}", mapName);
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "MapSaveProcessor: Failed to save map: {MapName}", mapName);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Progress Tracking
|
||||
|
||||
/// <summary>
|
||||
/// Wait for active scan matching operations to complete
|
||||
/// </summary>
|
||||
private static async Task WaitForScanMatchingAsync(CancellationToken cancellationToken, int checkIntervalMs = 100)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var activeCount = CartographerSharp.Mapping.Internal.D2D.ScanMatching.CeresScanMatcher2D.GetActiveScanMatchingCount();
|
||||
if (activeCount == 0)
|
||||
break;
|
||||
|
||||
await Task.Delay(checkIntervalMs, cancellationToken);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finish active trajectory and wait for completion
|
||||
/// </summary>
|
||||
private static async Task FinishTrajectoryAsync(IMapBuilder mapBuilder, int trajectoryId, CancellationToken cancellationToken)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
// Wait for scan matching to complete before finishing trajectory
|
||||
// Wait for scan matching to complete
|
||||
await WaitForScanMatchingAsync(cancellationToken);
|
||||
|
||||
// Finish trajectory
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
// FinishTrajectory is called synchronously
|
||||
mapBuilder.FinishTrajectory(trajectoryId);
|
||||
|
||||
// Wait for trajectory to finish asynchronously
|
||||
// Wait for trajectory to finish
|
||||
await mapBuilder.WaitForTrajectoryFinishedAsync(trajectoryId, cancellationToken);
|
||||
|
||||
// Trajectory finished successfully
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Drain work queue with progress updates mapped to a percentage range.
|
||||
/// </summary>
|
||||
private static async Task DrainWorkQueueWithProgressAsync(
|
||||
IMapBuilder mapBuilder,
|
||||
Func<int, int, int, Task> progressCallback,
|
||||
int progressStart,
|
||||
int progressEnd,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var poseGraph = mapBuilder.PoseGraph;
|
||||
var initialQueueCount = poseGraph.WorkQueueCount;
|
||||
|
||||
if (initialQueueCount == 0)
|
||||
{
|
||||
await progressCallback(100, progressEnd, progressEnd);
|
||||
return;
|
||||
}
|
||||
|
||||
var progressRange = progressEnd - progressStart;
|
||||
|
||||
while (poseGraph.WorkQueueCount > 0)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var currentQueueCount = poseGraph.WorkQueueCount;
|
||||
var processed = initialQueueCount - currentQueueCount;
|
||||
|
||||
// Calculate progress within the range
|
||||
int percentComplete;
|
||||
if (initialQueueCount > 0)
|
||||
{
|
||||
var phaseProgress = (double)processed / initialQueueCount;
|
||||
percentComplete = progressStart + (int)(phaseProgress * progressRange);
|
||||
}
|
||||
else
|
||||
{
|
||||
percentComplete = progressEnd;
|
||||
}
|
||||
percentComplete = Math.Clamp(percentComplete, progressStart, progressEnd);
|
||||
|
||||
await progressCallback(initialQueueCount, processed, percentComplete);
|
||||
|
||||
await Task.Delay(500, cancellationToken);
|
||||
}
|
||||
|
||||
await progressCallback(initialQueueCount, initialQueueCount, progressEnd);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Track optimization progress (work queue + constraint builder) while optimization task is running.
|
||||
/// </summary>
|
||||
private static async Task TrackOptimizationProgressAsync(
|
||||
IMapBuilder mapBuilder,
|
||||
Func<int, int, int, Task> progressCallback,
|
||||
int progressStart,
|
||||
int progressEnd,
|
||||
Task optimizationTask,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var poseGraph = mapBuilder.PoseGraph;
|
||||
var progressRange = progressEnd - progressStart;
|
||||
|
||||
// Capture the remaining work at the start of tracking.
|
||||
// Progress is calculated based on how much of this remaining work has been completed,
|
||||
// so the remaining portion at call time = 100%.
|
||||
var initialWorkQueueCount = poseGraph.WorkQueueCount;
|
||||
var initialConstraintTasksFinished = poseGraph.ConstraintTasksFinished;
|
||||
var initialConstraintTasksTotal = poseGraph.ConstraintTasksTotal;
|
||||
var remainingConstraintTasks = initialConstraintTasksTotal - initialConstraintTasksFinished;
|
||||
|
||||
// Tracking optimization progress
|
||||
|
||||
var lastLogTime = DateTime.UtcNow;
|
||||
|
||||
while (!optimizationTask.IsCompleted)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var workQueueCount = poseGraph.WorkQueueCount;
|
||||
var constraintTasksTotal = poseGraph.ConstraintTasksTotal;
|
||||
var constraintTasksFinished = poseGraph.ConstraintTasksFinished;
|
||||
|
||||
// Recalculate remaining tasks as total may grow during processing
|
||||
var currentRemainingTotal = constraintTasksTotal - initialConstraintTasksFinished;
|
||||
var effectiveRemaining = Math.Max(remainingConstraintTasks, currentRemainingTotal);
|
||||
|
||||
// Calculate progress based on remaining work from the start of tracking:
|
||||
// 1. Work queue drain progress (30% of phase)
|
||||
double workQueueProgress;
|
||||
if (initialWorkQueueCount > 0)
|
||||
{
|
||||
var workQueueProcessed = initialWorkQueueCount - workQueueCount;
|
||||
workQueueProgress = Math.Min(1.0, (double)workQueueProcessed / initialWorkQueueCount);
|
||||
}
|
||||
else
|
||||
{
|
||||
workQueueProgress = 1.0; // No work queue items at start
|
||||
}
|
||||
|
||||
// 2. Constraint task progress (70% of phase)
|
||||
double constraintProgress;
|
||||
if (effectiveRemaining > 0)
|
||||
{
|
||||
var newlyFinished = constraintTasksFinished - initialConstraintTasksFinished;
|
||||
constraintProgress = Math.Min(1.0, (double)newlyFinished / effectiveRemaining);
|
||||
}
|
||||
else
|
||||
{
|
||||
constraintProgress = 1.0; // No constraint tasks to process
|
||||
}
|
||||
|
||||
// Combined progress weighted by actual work amounts
|
||||
var totalWork = initialWorkQueueCount + effectiveRemaining;
|
||||
var combinedProgress = totalWork > 0
|
||||
? (workQueueProgress * initialWorkQueueCount + constraintProgress * effectiveRemaining) / totalWork
|
||||
: 1.0;
|
||||
var percentComplete = progressStart + (int)(combinedProgress * progressRange);
|
||||
percentComplete = Math.Clamp(percentComplete, progressStart, progressEnd);
|
||||
|
||||
await progressCallback(constraintTasksTotal, constraintTasksFinished, percentComplete);
|
||||
|
||||
// Update last log time periodically (logging removed to reduce noise)
|
||||
if ((DateTime.UtcNow - lastLogTime).TotalSeconds >= 10)
|
||||
{
|
||||
lastLogTime = DateTime.UtcNow;
|
||||
}
|
||||
|
||||
// Check more frequently for quicker updates
|
||||
await Task.Delay(1000, cancellationToken);
|
||||
}
|
||||
|
||||
// Wait for optimization task to complete (may throw if cancelled)
|
||||
await optimizationTask;
|
||||
|
||||
await progressCallback(poseGraph.ConstraintTasksTotal, poseGraph.ConstraintTasksFinished, progressEnd);
|
||||
// Optimization completed successfully
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region File I/O
|
||||
|
||||
/// <summary>
|
||||
/// Save pbstream file from MapBuilder state
|
||||
/// </summary>
|
||||
private void SavePbstream(IMapBuilder mapBuilder, string mapPath)
|
||||
{
|
||||
Directory.CreateDirectory(mapPath);
|
||||
var pbstreamPath = Path.Combine(mapPath, "map.pbstream");
|
||||
var includeUnfinished = _config.MapStorage.IncludeUnfinishedSubmaps;
|
||||
|
||||
try
|
||||
{
|
||||
using var writer = new ProtoStreamWriter(pbstreamPath);
|
||||
mapBuilder.SerializeState(includeUnfinishedSubmaps: includeUnfinished, writer);
|
||||
if (!writer.Close())
|
||||
{
|
||||
throw new InvalidOperationException($"ProtoStreamWriter.Close() failed for {pbstreamPath}");
|
||||
}
|
||||
|
||||
_logger.LogInformation("MapSaveProcessor: Pbstream saved to: {Path}", pbstreamPath);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "MapSaveProcessor: Failed to save pbstream to: {Path}", pbstreamPath);
|
||||
throw new InvalidOperationException($"Failed to save pbstream: {ex.Message}", ex);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Generate occupancy grid and save metadata files (PGM, PNG, JSON).
|
||||
/// Can be used for both new map creation and map update (e.g., after transform).
|
||||
/// </summary>
|
||||
/// <param name="mapName">Map name for sanitization</param>
|
||||
/// <param name="mapBuilder">MapBuilder containing the map data</param>
|
||||
/// <param name="mapPath">Directory path to save files</param>
|
||||
/// <param name="cancellationToken">Cancellation token</param>
|
||||
/// <returns>The map path</returns>
|
||||
public async Task<string> SaveMapMetadataAsync(
|
||||
string mapName,
|
||||
IMapBuilder mapBuilder,
|
||||
string mapPath,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
// Generate occupancy grid
|
||||
var (occupancyGrid, submapCount) = GenerateOccupancyGridForSave(mapBuilder, _config.MapStorage.OccupancyGridResolution);
|
||||
|
||||
var poseGraph = mapBuilder.PoseGraph;
|
||||
var trajectoryNodePoses = poseGraph.GetTrajectoryNodePoses();
|
||||
var trajectoryNodeCount = trajectoryNodePoses?.Count ?? 0;
|
||||
|
||||
var finalSubmapCount = submapCount;
|
||||
if (finalSubmapCount == 0)
|
||||
{
|
||||
var allSubmapData = poseGraph.GetAllSubmapData();
|
||||
finalSubmapCount = allSubmapData?.Count ?? 0;
|
||||
}
|
||||
|
||||
// Save occupancy grid files if available
|
||||
if (occupancyGrid != null)
|
||||
{
|
||||
await OccupancyGridFileHelper.SaveAllFormatsAsync(occupancyGrid, mapPath, cancellationToken, _logger);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.LogWarning("MapSaveProcessor: No occupancy grid generated, skipping PGM/PNG/JPG files");
|
||||
}
|
||||
|
||||
// Calculate bounds and size (in meters)
|
||||
var bounds = new BoundingBox();
|
||||
var size = new MapSize();
|
||||
if (occupancyGrid != null)
|
||||
{
|
||||
var originX = occupancyGrid.Origin.Position.X;
|
||||
var originY = occupancyGrid.Origin.Position.Y;
|
||||
var resolution = occupancyGrid.Resolution;
|
||||
// Size in meters = pixels * resolution
|
||||
var widthMeters = occupancyGrid.Width * resolution;
|
||||
var heightMeters = occupancyGrid.Height * resolution;
|
||||
size = new MapSize(widthMeters, heightMeters);
|
||||
bounds = new BoundingBox
|
||||
{
|
||||
MinX = originX,
|
||||
MinY = originY,
|
||||
MaxX = originX + widthMeters,
|
||||
MaxY = originY + heightMeters
|
||||
};
|
||||
}
|
||||
|
||||
// Create metadata
|
||||
var sanitizedMapName = MapNameHelper.Sanitize(mapName);
|
||||
var metadata = new MapCartographerInfo
|
||||
{
|
||||
Name = sanitizedMapName,
|
||||
FolderPath = mapPath,
|
||||
CreatedDate = DateTime.UtcNow,
|
||||
Resolution = occupancyGrid?.Resolution ?? _config.MapStorage.OccupancyGridResolution,
|
||||
Size = size,
|
||||
Origin = occupancyGrid?.Origin ?? new Pose
|
||||
{
|
||||
Position = new RobotNet10.Shared.Numbers.Vector3(0, 0, 0),
|
||||
Orientation = new RobotNet10.Shared.Numbers.Quaternion(0, 0, 0, 1)
|
||||
},
|
||||
Bounds = bounds,
|
||||
TrajectoryNodeCount = trajectoryNodeCount,
|
||||
MapConfig = MapBuilderHelper.CreateConfigSnapshot(_config)
|
||||
};
|
||||
|
||||
// Save metadata JSON
|
||||
var metadataPath = Path.Combine(mapPath, "map.json");
|
||||
await File.WriteAllTextAsync(metadataPath, JsonSerializer.Serialize(metadata, SerializerOptions), cancellationToken);
|
||||
|
||||
return mapPath;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Validation
|
||||
|
||||
/// <summary>
|
||||
/// Get map path from map name
|
||||
/// </summary>
|
||||
private string GetMapPath(string mapName)
|
||||
{
|
||||
var sanitized = MapNameHelper.Sanitize(mapName);
|
||||
return Path.Combine(_mapsDirectory, sanitized);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Validate map before saving
|
||||
/// </summary>
|
||||
private (bool IsValid, string? ErrorMessage) ValidateMapBeforeSave(IMapBuilder mapBuilder, string mapName, string mapPath)
|
||||
{
|
||||
try
|
||||
{
|
||||
var poseGraph = mapBuilder.PoseGraph;
|
||||
var trajectoryStates = poseGraph.GetTrajectoryStates();
|
||||
var trajectoryNodePoses = poseGraph.GetTrajectoryNodePoses();
|
||||
var allSubmapData = poseGraph.GetAllSubmapData();
|
||||
|
||||
// Check at least 1 trajectory
|
||||
if (trajectoryStates.Count == 0)
|
||||
{
|
||||
return (false, "No trajectories found. Cannot save empty map.");
|
||||
}
|
||||
|
||||
// Check at least 1 node
|
||||
if (trajectoryNodePoses.Count == 0)
|
||||
{
|
||||
return (false, "No trajectory nodes found. Cannot save map without nodes.");
|
||||
}
|
||||
|
||||
// Check at least 1 submap
|
||||
if (allSubmapData.Count == 0)
|
||||
{
|
||||
return (false, "No submaps found. Cannot save map without submaps.");
|
||||
}
|
||||
|
||||
// Validate map name
|
||||
var sanitizedMapName = MapNameHelper.Sanitize(mapName);
|
||||
if (string.IsNullOrWhiteSpace(sanitizedMapName))
|
||||
{
|
||||
return (false, $"Invalid map name: '{mapName}' (sanitized to empty string)");
|
||||
}
|
||||
|
||||
// Check poses for NaN/Infinity
|
||||
int invalidPoseCount = 0;
|
||||
foreach (var nodePose in trajectoryNodePoses)
|
||||
{
|
||||
var globalPose = nodePose.Data.GlobalPose;
|
||||
if (!globalPose.IsValid())
|
||||
{
|
||||
invalidPoseCount++;
|
||||
}
|
||||
}
|
||||
|
||||
if (invalidPoseCount > 0)
|
||||
{
|
||||
return (false, $"Found {invalidPoseCount} trajectory node(s) with invalid poses (NaN/Infinity or invalid quaternion).");
|
||||
}
|
||||
|
||||
// Check submap poses for NaN/Infinity
|
||||
int invalidSubmapPoseCount = 0;
|
||||
foreach (var submapPose in poseGraph.GetAllSubmapPoses())
|
||||
{
|
||||
var pose = submapPose.Data.Pose;
|
||||
if (!pose.IsValid())
|
||||
{
|
||||
invalidSubmapPoseCount++;
|
||||
}
|
||||
}
|
||||
|
||||
if (invalidSubmapPoseCount > 0)
|
||||
{
|
||||
return (false, $"Found {invalidSubmapPoseCount} submap(s) with invalid poses (NaN/Infinity or invalid quaternion).");
|
||||
}
|
||||
|
||||
// Validate occupancy grid resolution
|
||||
if (_config.MapStorage.OccupancyGridResolution <= 0)
|
||||
{
|
||||
return (false, $"Invalid occupancy grid resolution: {_config.MapStorage.OccupancyGridResolution}. Must be > 0.");
|
||||
}
|
||||
|
||||
// All validations passed
|
||||
return (true, string.Empty);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return (false, $"Validation error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Grid Generation
|
||||
|
||||
/// <summary>
|
||||
/// Generate occupancy grid from MapBuilder for save
|
||||
/// </summary>
|
||||
private (OccupancyGrid? Grid, int SubmapCount) GenerateOccupancyGridForSave(IMapBuilder mapBuilder, double targetResolution)
|
||||
{
|
||||
try
|
||||
{
|
||||
var padding = _config.MapStorage.MapPadding;
|
||||
|
||||
// Use the same Generate() dispatch as OccupancyGridManager (UI display)
|
||||
// to ensure saved files match what the user sees on the UI.
|
||||
// Generate() respects OccupancyGridConfiguration.MergeStrategy (default: LogOddsSum).
|
||||
var occupancyGrid = OccupancyGridGenerator.Generate(
|
||||
mapBuilder, targetResolution, padding, _logger, _config.OccupancyGrid);
|
||||
|
||||
if (occupancyGrid == null)
|
||||
{
|
||||
_logger.LogWarning("MapSaveProcessor: Occupancy grid generation returned null");
|
||||
return (null, 0);
|
||||
}
|
||||
|
||||
// Get submap count for result
|
||||
var submapCount = mapBuilder.PoseGraph.GetAllSubmapData()?.Count ?? 0;
|
||||
|
||||
return (occupancyGrid, submapCount);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "MapSaveProcessor: Failed to generate occupancy grid from MapBuilder");
|
||||
return (null, 0);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,269 @@
|
||||
using CartographerSharp.Sensor;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Localization;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Reason for MCL convergence
|
||||
/// </summary>
|
||||
public enum MclConvergenceReason
|
||||
{
|
||||
/// <summary>MCL has not converged yet</summary>
|
||||
NotConverged,
|
||||
/// <summary>Pose is stable and quality criteria met</summary>
|
||||
PoseStable,
|
||||
/// <summary>Timeout reached without convergence</summary>
|
||||
Timeout,
|
||||
/// <summary>Max iterations reached without convergence</summary>
|
||||
MaxIterations
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Processes MCL (Monte Carlo Localization) convergence and pose tracking
|
||||
/// </summary>
|
||||
public class MclProcessor
|
||||
{
|
||||
private readonly ILogger<MclProcessor> _logger;
|
||||
private readonly CartographerConfiguration _config;
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
private bool _running;
|
||||
private string? _primaryLidarId;
|
||||
private DateTime? _stableSinceUtc;
|
||||
private DateTime _runningSinceUtc = DateTime.MinValue;
|
||||
private int _iterationCount;
|
||||
private Pose? _lastPose;
|
||||
private Pose _initialPose; // Initial pose for fallback on timeout
|
||||
private OdometryData? _lastOdometryData;
|
||||
private double _lastOdometryTimestampSec;
|
||||
|
||||
public MclProcessor(CartographerConfiguration config, ILogger<MclProcessor> logger)
|
||||
{
|
||||
_config = config ?? throw new ArgumentNullException(nameof(config));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
}
|
||||
|
||||
public bool IsRunning
|
||||
{
|
||||
get { lock (_lock) { return _running; } }
|
||||
}
|
||||
|
||||
public string? PrimaryLidarId
|
||||
{
|
||||
get { lock (_lock) { return _primaryLidarId; } }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the initial pose used when MCL started (for fallback on timeout)
|
||||
/// </summary>
|
||||
public Pose InitialPose
|
||||
{
|
||||
get { lock (_lock) { return _initialPose; } }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Start MCL with initial seed pose
|
||||
/// </summary>
|
||||
public void Start(Pose seedPose, string effectivePrimaryLiderId, OccupancyGrid? grid)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_running = true;
|
||||
_primaryLidarId = effectivePrimaryLiderId;
|
||||
_iterationCount = 0;
|
||||
_lastPose = seedPose;
|
||||
_initialPose = seedPose; // Store initial pose for timeout fallback
|
||||
_stableSinceUtc = null;
|
||||
_runningSinceUtc = DateTime.UtcNow;
|
||||
_lastOdometryData = null;
|
||||
_lastOdometryTimestampSec = 0;
|
||||
}
|
||||
|
||||
_logger.LogInformation("MclProcessor: MCL started with primary lidar: {LidarId}", effectivePrimaryLiderId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Stop MCL
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_running = false;
|
||||
_primaryLidarId = null;
|
||||
_stableSinceUtc = null;
|
||||
_iterationCount = 0;
|
||||
_lastPose = null;
|
||||
_lastOdometryData = null;
|
||||
_lastOdometryTimestampSec = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update MCL seed pose during running state
|
||||
/// </summary>
|
||||
public void UpdateSeedPose(Pose newPose)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_running)
|
||||
return;
|
||||
|
||||
_iterationCount = 0;
|
||||
_lastPose = null;
|
||||
_stableSinceUtc = null;
|
||||
_lastOdometryData = null;
|
||||
_lastOdometryTimestampSec = 0;
|
||||
}
|
||||
|
||||
_logger.LogInformation("MclProcessor: Seed pose updated");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check MCL convergence based on pose stability and quality criteria
|
||||
/// </summary>
|
||||
/// <returns>True if converged (for any reason), false otherwise</returns>
|
||||
public bool CheckConvergence(Pose currentPose, double reliability, double? mae, out int iterationCount)
|
||||
{
|
||||
return CheckConvergenceWithReason(currentPose, reliability, mae, out iterationCount) != MclConvergenceReason.NotConverged;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check MCL convergence and return the reason
|
||||
/// </summary>
|
||||
/// <returns>Convergence reason indicating why MCL stopped or NotConverged if still running</returns>
|
||||
public MclConvergenceReason CheckConvergenceWithReason(Pose currentPose, double reliability, double? mae, out int iterationCount)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
iterationCount = ++_iterationCount;
|
||||
|
||||
if (_iterationCount < _config.Mcl.ConvergenceMinIterations)
|
||||
{
|
||||
_lastPose = currentPose;
|
||||
return MclConvergenceReason.NotConverged;
|
||||
}
|
||||
|
||||
// Calculate pose change
|
||||
double poseDist = double.MaxValue;
|
||||
double yawDiff = double.MaxValue;
|
||||
|
||||
if (_lastPose.HasValue)
|
||||
{
|
||||
double dx = currentPose.Position.X - _lastPose.Value.Position.X;
|
||||
double dy = currentPose.Position.Y - _lastPose.Value.Position.Y;
|
||||
poseDist = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
double yawCur = GetYawFromPose(currentPose);
|
||||
double yawLast = GetYawFromPose(_lastPose.Value);
|
||||
double dyaw = yawCur - yawLast;
|
||||
while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
|
||||
while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
|
||||
yawDiff = Math.Abs(dyaw);
|
||||
}
|
||||
|
||||
_lastPose = currentPose;
|
||||
|
||||
// Check stability criteria
|
||||
bool poseStable = poseDist < _config.Mcl.ConvergencePoseChangeThresholdMeters &&
|
||||
yawDiff < _config.Mcl.ConvergenceYawChangeThresholdRad;
|
||||
|
||||
bool qualityGood = !_config.Mcl.EstimateReliability ||
|
||||
(reliability >= _config.Mcl.ConvergenceReliabilityMin &&
|
||||
(!mae.HasValue || mae.Value <= _config.Mcl.ConvergenceMaeMaxMeters));
|
||||
|
||||
var now = DateTime.UtcNow;
|
||||
double runningSec = (now - _runningSinceUtc).TotalSeconds;
|
||||
double stableDurationSec = _config.Mcl.ConvergenceStableDurationSeconds;
|
||||
double timeoutSec = _config.Mcl.ConvergenceTimeoutSeconds;
|
||||
|
||||
if (qualityGood)
|
||||
{
|
||||
if (poseStable)
|
||||
{
|
||||
if (!_stableSinceUtc.HasValue)
|
||||
_stableSinceUtc = now;
|
||||
else if ((now - _stableSinceUtc.Value).TotalSeconds >= stableDurationSec)
|
||||
return MclConvergenceReason.PoseStable;
|
||||
}
|
||||
else
|
||||
{
|
||||
_stableSinceUtc = null;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_stableSinceUtc = null;
|
||||
if (runningSec >= timeoutSec)
|
||||
return MclConvergenceReason.Timeout;
|
||||
}
|
||||
|
||||
if (_iterationCount >= _config.Mcl.ConvergenceMaxIterations)
|
||||
return MclConvergenceReason.MaxIterations;
|
||||
|
||||
return MclConvergenceReason.NotConverged;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Process odometry data for MCL motion model
|
||||
/// </summary>
|
||||
public void ProcessOdometry(OdometryData odomData)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_running || !_lastOdometryData.HasValue)
|
||||
{
|
||||
_lastOdometryData = odomData;
|
||||
_lastOdometryTimestampSec = odomData.Time / 10_000_000.0;
|
||||
return;
|
||||
}
|
||||
|
||||
double currentTimeSec = odomData.Time / 10_000_000.0;
|
||||
double deltaTimeSec = currentTimeSec - _lastOdometryTimestampSec;
|
||||
|
||||
if (deltaTimeSec > 0 && deltaTimeSec < 1.0)
|
||||
{
|
||||
var lastPose = _lastOdometryData.Value.Pose;
|
||||
var currentOdomPose = odomData.Pose;
|
||||
|
||||
double dx = currentOdomPose.Translation.X - lastPose.Translation.X;
|
||||
double dy = currentOdomPose.Translation.Y - lastPose.Translation.Y;
|
||||
double linearX = dx / deltaTimeSec;
|
||||
double linearY = dy / deltaTimeSec;
|
||||
|
||||
double yawLast = ((RobotNet10.Shared.Numbers.Quaternion)lastPose.Rotation).ToYawRadian();
|
||||
double yawCurrent = ((RobotNet10.Shared.Numbers.Quaternion)currentOdomPose.Rotation).ToYawRadian();
|
||||
double dyaw = yawCurrent - yawLast;
|
||||
while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
|
||||
while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
|
||||
double angularZ = dyaw / deltaTimeSec;
|
||||
|
||||
// Store for use by MCL (would be passed to mcl.OnOdom in actual implementation)
|
||||
OnOdometryProcessed?.Invoke(deltaTimeSec, linearX, linearY, angularZ);
|
||||
}
|
||||
|
||||
_lastOdometryData = odomData;
|
||||
_lastOdometryTimestampSec = currentTimeSec;
|
||||
}
|
||||
}
|
||||
|
||||
// Event to signal odometry processing
|
||||
public event Action<double, double, double, double>? OnOdometryProcessed;
|
||||
|
||||
private static double GetYawFromPose(Pose p)
|
||||
{
|
||||
var q = p.Orientation;
|
||||
return Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z));
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
Stop();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,243 @@
|
||||
using RobotNet10.Shared.Localization;
|
||||
using SkiaSharp;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for saving OccupancyGrid to various file formats.
|
||||
/// Consolidates file saving logic used by both CartographerService and MapSaveProcessor.
|
||||
/// </summary>
|
||||
public static class OccupancyGridFileHelper
|
||||
{
|
||||
#region Public API
|
||||
|
||||
/// <summary>
|
||||
/// Save all map file formats (PGM, YAML, PNG, JPG) to the specified directory.
|
||||
/// </summary>
|
||||
public static async Task SaveAllFormatsAsync(
|
||||
OccupancyGrid occupancyGrid,
|
||||
string mapPath,
|
||||
CancellationToken cancellationToken = default,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
var pgmPath = Path.Combine(mapPath, "map.pgm");
|
||||
var yamlPath = Path.Combine(mapPath, "map.yaml");
|
||||
var pngPath = Path.Combine(mapPath, "map.png");
|
||||
var jpgPath = Path.Combine(mapPath, "map.jpg");
|
||||
|
||||
SaveAsPgm(occupancyGrid, pgmPath, logger);
|
||||
SaveAsYaml(occupancyGrid, yamlPath, "map.png", logger);
|
||||
await Task.Run(() => SaveAsPng(occupancyGrid, pngPath, cancellationToken, logger), cancellationToken);
|
||||
await Task.Run(() => SaveAsJpg(occupancyGrid, jpgPath, cancellationToken, logger), cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Save occupancy grid as PGM file (binary format P5)
|
||||
/// </summary>
|
||||
public static void SaveAsPgm(OccupancyGrid occupancyGrid, string pgmPath, ILogger? logger = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
using var fileStream = new FileStream(pgmPath, FileMode.Create, FileAccess.Write);
|
||||
using var writer = new StreamWriter(fileStream);
|
||||
|
||||
// Write PGM header
|
||||
writer.WriteLine("P5"); // Binary format
|
||||
writer.WriteLine($"{occupancyGrid.Width} {occupancyGrid.Height}");
|
||||
writer.WriteLine("255"); // Max value
|
||||
|
||||
// Flush header before writing binary data
|
||||
writer.Flush();
|
||||
|
||||
// Write pixel data directly without Y-flip.
|
||||
// Both OccupancyGrid and PGM file use same convention for consistency with PgmLoader.
|
||||
var buffer = new byte[occupancyGrid.Width * occupancyGrid.Height];
|
||||
for (int y = 0; y < occupancyGrid.Height; y++)
|
||||
{
|
||||
for (int x = 0; x < occupancyGrid.Width; x++)
|
||||
{
|
||||
int index = y * occupancyGrid.Width + x;
|
||||
var occupancyValue = occupancyGrid.Data[index];
|
||||
|
||||
if (occupancyValue == -1)
|
||||
{
|
||||
buffer[index] = 205; // Unknown (gray)
|
||||
}
|
||||
else
|
||||
{
|
||||
// Convert occupancy (0-100) to PGM (0-255)
|
||||
// occupancy 0 (free) -> PGM 254 (white)
|
||||
// occupancy 100 (occupied) -> PGM 0 (black)
|
||||
buffer[index] = (byte)(254 - (occupancyValue * 254 / 100));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fileStream.Write(buffer, 0, buffer.Length);
|
||||
fileStream.Flush();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save PGM file: {Path}", pgmPath);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Save occupancy grid as YAML file (ROS map format)
|
||||
/// </summary>
|
||||
public static void SaveAsYaml(OccupancyGrid occupancyGrid, string yamlPath, string imageFilename, ILogger? logger = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var originX = occupancyGrid.Origin.Position.X;
|
||||
var originY = occupancyGrid.Origin.Position.Y;
|
||||
|
||||
// Write YAML file (ROS map format)
|
||||
var yamlContent = $"image: {imageFilename}\n" +
|
||||
$"resolution: {occupancyGrid.Resolution:F10}\n" +
|
||||
$"origin: [{originX:F10}, {originY:F10}, 0.0]\n" +
|
||||
$"negate: 0\n" +
|
||||
$"occupied_thresh: 0.65\n" +
|
||||
$"free_thresh: 0.196\n";
|
||||
|
||||
File.WriteAllText(yamlPath, yamlContent);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save YAML file: {Path}", yamlPath);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Save occupancy grid as PNG file
|
||||
/// </summary>
|
||||
public static void SaveAsPng(OccupancyGrid occupancyGrid, string pngPath, CancellationToken cancellationToken = default, ILogger? logger = null)
|
||||
{
|
||||
RenderAndSaveImage(occupancyGrid, pngPath, SKEncodedImageFormat.Png, 100, cancellationToken, logger);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Save occupancy grid as JPG file
|
||||
/// </summary>
|
||||
public static void SaveAsJpg(OccupancyGrid occupancyGrid, string jpgPath, CancellationToken cancellationToken = default, ILogger? logger = null)
|
||||
{
|
||||
RenderAndSaveImage(occupancyGrid, jpgPath, SKEncodedImageFormat.Jpeg, 95, cancellationToken, logger);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Image Rendering
|
||||
|
||||
/// <summary>
|
||||
/// Render occupancy grid to an image and save in the specified format.
|
||||
/// Shared implementation for both PNG and JPG output.
|
||||
/// </summary>
|
||||
private static void RenderAndSaveImage(
|
||||
OccupancyGrid occupancyGrid,
|
||||
string outputPath,
|
||||
SKEncodedImageFormat format,
|
||||
int quality,
|
||||
CancellationToken cancellationToken,
|
||||
ILogger? logger)
|
||||
{
|
||||
var formatName = format == SKEncodedImageFormat.Png ? "PNG" : "JPG";
|
||||
try
|
||||
{
|
||||
if (occupancyGrid == null || occupancyGrid.Width <= 0 || occupancyGrid.Height <= 0)
|
||||
{
|
||||
logger?.LogWarning("OccupancyGridFileHelper: Cannot save {Format} - invalid occupancy grid", formatName);
|
||||
return;
|
||||
}
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var width = occupancyGrid.Width;
|
||||
var height = occupancyGrid.Height;
|
||||
|
||||
// Create SKBitmap with RGBA_8888 format
|
||||
using var bitmap = new SKBitmap(width, height, SKColorType.Rgba8888, SKAlphaType.Opaque);
|
||||
|
||||
// Get pixel buffer pointer for direct memory access
|
||||
var pixelsPtr = bitmap.GetPixels();
|
||||
if (pixelsPtr == IntPtr.Zero)
|
||||
{
|
||||
throw new InvalidOperationException("Failed to get pixel buffer from bitmap");
|
||||
}
|
||||
|
||||
// Convert occupancy grid data to image pixels
|
||||
// Flip Y: OccupancyGrid uses ROS convention (row 0 = world bottom, Y up)
|
||||
// but PNG/JPG image convention is row 0 = top, Y down.
|
||||
unsafe
|
||||
{
|
||||
var pixels = (byte*)pixelsPtr.ToPointer();
|
||||
|
||||
for (int y = 0; y < height; y++)
|
||||
{
|
||||
for (int x = 0; x < width; x++)
|
||||
{
|
||||
// Read from flipped Y in grid (bottom-up) to write top-down in image
|
||||
var srcIndex = (height - 1 - y) * width + x;
|
||||
var occupancyValue = occupancyGrid.Data[srcIndex];
|
||||
|
||||
byte intensity;
|
||||
if (occupancyValue == -1)
|
||||
{
|
||||
intensity = 205; // Unknown (gray)
|
||||
}
|
||||
else if (occupancyValue == 0)
|
||||
{
|
||||
intensity = 254; // Free space (white)
|
||||
}
|
||||
else
|
||||
{
|
||||
// Occupied space: Convert occupancy (0-100) to pixel intensity (0-255)
|
||||
var intensityValue = 254.0 - (occupancyValue * 254.0 / 100.0);
|
||||
intensity = (byte)Math.Clamp((int)Math.Round(intensityValue), 0, 254);
|
||||
}
|
||||
|
||||
// Write RGBA bytes directly (destination index uses image row y)
|
||||
var dstIndex = y * width + x;
|
||||
var pixelOffset = dstIndex * 4;
|
||||
pixels[pixelOffset] = intensity; // R
|
||||
pixels[pixelOffset + 1] = intensity; // G
|
||||
pixels[pixelOffset + 2] = intensity; // B
|
||||
pixels[pixelOffset + 3] = 255; // A (opaque)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Encode and save
|
||||
using var image = SKImage.FromBitmap(bitmap) ?? throw new InvalidOperationException("Failed to create SKImage from bitmap");
|
||||
using var data = image.Encode(format, quality) ?? throw new InvalidOperationException($"Failed to encode {formatName} image");
|
||||
|
||||
var directory = Path.GetDirectoryName(outputPath);
|
||||
if (!string.IsNullOrEmpty(directory))
|
||||
{
|
||||
Directory.CreateDirectory(directory);
|
||||
}
|
||||
|
||||
if (File.Exists(outputPath))
|
||||
{
|
||||
File.Delete(outputPath);
|
||||
}
|
||||
|
||||
using var stream = File.Create(outputPath);
|
||||
data.SaveTo(stream);
|
||||
stream.Flush();
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
logger?.LogWarning("OccupancyGridFileHelper: {Format} save cancelled: {Path}", formatName, outputPath);
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save {Format} file: {Path}", formatName, outputPath);
|
||||
// Don't throw - image files are optional
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,297 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using CartographerSharp.Mapping.D2D;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Localization;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
//using Pose = RobotNet10.Shared.Geometry.Pose;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for occupancy grid generation and manipulation.
|
||||
/// Contains pure functions extracted from OccupancyGridProvider.
|
||||
/// </summary>
|
||||
public static class OccupancyGridHelper
|
||||
{
|
||||
#region Constants
|
||||
|
||||
/// <summary>
|
||||
/// Initial size for empty occupancy grid (cells)
|
||||
/// </summary>
|
||||
public const int InitialEmptyGridSize = 100;
|
||||
|
||||
/// <summary>
|
||||
/// Probability threshold for free space
|
||||
/// </summary>
|
||||
public const double FreeSpaceProbabilityThreshold = 0.01;
|
||||
|
||||
/// <summary>
|
||||
/// Probability threshold for occupied space
|
||||
/// </summary>
|
||||
public const double OccupiedSpaceProbabilityThreshold = 0.99;
|
||||
|
||||
/// <summary>
|
||||
/// Lower bound for unknown space probability range
|
||||
/// </summary>
|
||||
public const double UnknownSpaceLowerBound = 0.4;
|
||||
|
||||
/// <summary>
|
||||
/// Upper bound for unknown space probability range
|
||||
/// </summary>
|
||||
public const double UnknownSpaceUpperBound = 0.6;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Bounds Calculation
|
||||
|
||||
/// <summary>
|
||||
/// Calculate global bounds of a submap by transforming its corners to global frame
|
||||
/// </summary>
|
||||
public static (double minX, double minY, double maxX, double maxY) CalculateSubmapGlobalBounds(
|
||||
Submap2D submap2D,
|
||||
Rigid3d globalPose)
|
||||
{
|
||||
var grid = submap2D.Grid;
|
||||
if (grid == null)
|
||||
{
|
||||
return (double.MaxValue, double.MaxValue, double.MinValue, double.MinValue);
|
||||
}
|
||||
|
||||
var limits = grid.Limits;
|
||||
|
||||
// Use cropped limits if available (only known cells) for more accurate bounds
|
||||
grid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||||
|
||||
// If cropped limits are valid (has known cells), use them; otherwise use full limits
|
||||
var hasCroppedLimits = croppedLimits.NumXCells > 0 && croppedLimits.NumYCells > 0;
|
||||
|
||||
// Get bounds of cropped (known) cells in submap local frame using GetCellCenter
|
||||
var cornerIndices = new[]
|
||||
{
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
|
||||
hasCroppedLimits ? croppedOffset.Y : 0),
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
|
||||
hasCroppedLimits ? croppedOffset.Y : 0),
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
|
||||
hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1),
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
|
||||
hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1)
|
||||
};
|
||||
|
||||
double croppedMinX = double.MaxValue, croppedMinY = double.MaxValue;
|
||||
double croppedMaxX = double.MinValue, croppedMaxY = double.MinValue;
|
||||
|
||||
foreach (var cornerIndex in cornerIndices)
|
||||
{
|
||||
var cellCenter = limits.GetCellCenter(cornerIndex);
|
||||
croppedMinX = Math.Min(croppedMinX, cellCenter.X);
|
||||
croppedMinY = Math.Min(croppedMinY, cellCenter.Y);
|
||||
croppedMaxX = Math.Max(croppedMaxX, cellCenter.X);
|
||||
croppedMaxY = Math.Max(croppedMaxY, cellCenter.Y);
|
||||
}
|
||||
|
||||
// Transform corners to global frame
|
||||
var corners = new[]
|
||||
{
|
||||
new Vector2(croppedMinX, croppedMinY),
|
||||
new Vector2(croppedMaxX, croppedMinY),
|
||||
new Vector2(croppedMaxX, croppedMaxY),
|
||||
new Vector2(croppedMinX, croppedMaxY)
|
||||
};
|
||||
|
||||
double submapGlobalMinX = double.MaxValue, submapGlobalMinY = double.MaxValue;
|
||||
double submapGlobalMaxX = double.MinValue, submapGlobalMaxY = double.MinValue;
|
||||
|
||||
foreach (var corner in corners)
|
||||
{
|
||||
var corner3D = new Vector3(corner.X, corner.Y, 0.0);
|
||||
var globalCorner = globalPose.TransformPoint(corner3D);
|
||||
|
||||
submapGlobalMinX = Math.Min((float)submapGlobalMinX, (float)globalCorner.X);
|
||||
submapGlobalMinY = Math.Min((float)submapGlobalMinY, (float)globalCorner.Y);
|
||||
submapGlobalMaxX = Math.Max((float)submapGlobalMaxX, (float)globalCorner.X);
|
||||
submapGlobalMaxY = Math.Max((float)submapGlobalMaxY, (float)globalCorner.Y);
|
||||
}
|
||||
|
||||
return (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get current grid bounds from origin and dimensions
|
||||
/// </summary>
|
||||
public static (double minX, double minY, double maxX, double maxY) GetGridBounds(OccupancyGrid grid)
|
||||
{
|
||||
var gridMinX = grid.Origin.Position.X;
|
||||
var gridMinY = grid.Origin.Position.Y;
|
||||
var gridMaxX = gridMinX + (grid.Width * grid.Resolution);
|
||||
var gridMaxY = gridMinY + (grid.Height * grid.Resolution);
|
||||
return (gridMinX, gridMinY, gridMaxX, gridMaxY);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate bounds from a list of submaps with padding
|
||||
/// </summary>
|
||||
public static (double minX, double minY, double maxX, double maxY) CalculateBounds(
|
||||
List<(Submap2D Submap, Rigid3d GlobalPose)> submapList,
|
||||
double mapPadding)
|
||||
{
|
||||
double minX = double.MaxValue, minY = double.MaxValue;
|
||||
double maxX = double.MinValue, maxY = double.MinValue;
|
||||
|
||||
foreach (var (submap, globalPose) in submapList)
|
||||
{
|
||||
var (submapMinX, submapMinY, submapMaxX, submapMaxY) =
|
||||
CalculateSubmapGlobalBounds(submap, globalPose);
|
||||
|
||||
minX = Math.Min(minX, submapMinX);
|
||||
minY = Math.Min(minY, submapMinY);
|
||||
maxX = Math.Max(maxX, submapMaxX);
|
||||
maxY = Math.Max(maxY, submapMaxY);
|
||||
}
|
||||
|
||||
return (minX - mapPadding, minY - mapPadding, maxX + mapPadding, maxY + mapPadding);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Probability Conversion
|
||||
|
||||
/// <summary>
|
||||
/// Convert probability value to occupancy grid value
|
||||
/// </summary>
|
||||
public static sbyte ConvertProbabilityToOccupancyValue(double probability)
|
||||
{
|
||||
if (probability < FreeSpaceProbabilityThreshold)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if (probability > OccupiedSpaceProbabilityThreshold)
|
||||
{
|
||||
return 100;
|
||||
}
|
||||
if (probability < UnknownSpaceLowerBound || probability > UnknownSpaceUpperBound)
|
||||
{
|
||||
var occupancyValue = (sbyte)Math.Round(probability * 100.0);
|
||||
return (sbyte)Math.Clamp(occupancyValue, (sbyte)0, (sbyte)100);
|
||||
}
|
||||
return -1; // Unknown space
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Grid Creation
|
||||
|
||||
/// <summary>
|
||||
/// Create an empty occupancy grid with default size
|
||||
/// </summary>
|
||||
public static OccupancyGrid CreateEmptyOccupancyGrid(double resolution)
|
||||
{
|
||||
var origin = new Pose
|
||||
{
|
||||
Position = new Vector3(0, 0, 0),
|
||||
Orientation = new Quaternion(0, 0, 0, 1)
|
||||
};
|
||||
|
||||
return new OccupancyGrid(resolution, InitialEmptyGridSize, InitialEmptyGridSize, origin);
|
||||
}
|
||||
|
||||
// NOTE: GenerateOccupancyGridFromSubmapList and MergeSubmapIntoOccupancyGrid
|
||||
// have been moved to OccupancyGridGenerator for unified grid generation.
|
||||
// Use OccupancyGridGenerator.GenerateFromMapBuilder instead.
|
||||
|
||||
#endregion
|
||||
|
||||
#region Grid Expansion
|
||||
|
||||
/// <summary>
|
||||
/// Expand grid if needed to fit multiple submaps (optimized version)
|
||||
/// </summary>
|
||||
public static OccupancyGrid? ExpandGridToFitSubmaps(
|
||||
OccupancyGrid currentGrid,
|
||||
List<(Submap2D Submap, Rigid3d GlobalPose, SubmapId SubmapId)> submaps,
|
||||
double resolution,
|
||||
double mapPadding)
|
||||
{
|
||||
if (currentGrid == null || submaps == null || submaps.Count == 0)
|
||||
return null;
|
||||
|
||||
// Calculate combined bounds of all submaps
|
||||
double combinedMinX = double.MaxValue, combinedMinY = double.MaxValue;
|
||||
double combinedMaxX = double.MinValue, combinedMaxY = double.MinValue;
|
||||
bool hasValidBounds = false;
|
||||
|
||||
foreach (var (submap2D, globalPose, _) in submaps)
|
||||
{
|
||||
var grid = submap2D.Grid;
|
||||
if (grid == null)
|
||||
continue;
|
||||
|
||||
var (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY) =
|
||||
CalculateSubmapGlobalBounds(submap2D, globalPose);
|
||||
|
||||
if (submapGlobalMinX < submapGlobalMaxX && submapGlobalMinY < submapGlobalMaxY &&
|
||||
submapGlobalMinX != double.MaxValue && submapGlobalMinY != double.MaxValue &&
|
||||
submapGlobalMaxX != double.MinValue && submapGlobalMaxY != double.MinValue)
|
||||
{
|
||||
combinedMinX = Math.Min(combinedMinX, submapGlobalMinX);
|
||||
combinedMinY = Math.Min(combinedMinY, submapGlobalMinY);
|
||||
combinedMaxX = Math.Max(combinedMaxX, submapGlobalMaxX);
|
||||
combinedMaxY = Math.Max(combinedMaxY, submapGlobalMaxY);
|
||||
hasValidBounds = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!hasValidBounds)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var (gridMinX, gridMinY, gridMaxX, gridMaxY) = GetGridBounds(currentGrid);
|
||||
|
||||
var needsExpansion = combinedMinX < gridMinX || combinedMinY < gridMinY ||
|
||||
combinedMaxX > gridMaxX || combinedMaxY > gridMaxY;
|
||||
|
||||
if (!needsExpansion)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var newMinX = Math.Min(gridMinX, combinedMinX) - mapPadding;
|
||||
var newMinY = Math.Min(gridMinY, combinedMinY) - mapPadding;
|
||||
var newMaxX = Math.Max(gridMaxX, combinedMaxX) + mapPadding;
|
||||
var newMaxY = Math.Max(gridMaxY, combinedMaxY) + mapPadding;
|
||||
|
||||
var newWidth = (int)Math.Ceiling((newMaxX - newMinX) / resolution);
|
||||
var newHeight = (int)Math.Ceiling((newMaxY - newMinY) / resolution);
|
||||
|
||||
var newOrigin = new Pose
|
||||
{
|
||||
Position = new Vector3(newMinX, newMinY, 0),
|
||||
Orientation = new Quaternion(0, 0, 0, 1)
|
||||
};
|
||||
|
||||
var expandedGrid = new OccupancyGrid(resolution, newWidth, newHeight, newOrigin);
|
||||
|
||||
// Copy existing grid data to expanded grid
|
||||
for (int y = 0; y < currentGrid.Height; y++)
|
||||
{
|
||||
for (int x = 0; x < currentGrid.Width; x++)
|
||||
{
|
||||
var (worldX, worldY) = currentGrid.GridToWorld(x, y);
|
||||
var (newGridX, newGridY) = expandedGrid.WorldToGrid(worldX, worldY);
|
||||
|
||||
if (newGridX >= 0 && newGridX < expandedGrid.Width &&
|
||||
newGridY >= 0 && newGridY < expandedGrid.Height)
|
||||
{
|
||||
var cellValue = currentGrid.GetCell(x, y);
|
||||
expandedGrid.SetCell(newGridX, newGridY, cellValue);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return expandedGrid;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,306 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using RobotNet10.RobotApp.Shared;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
using RobotNet10.Shared.Localization;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Manages occupancy grid loading, updating, and caching
|
||||
/// Handles throttled updates during ScanMapping using counter-based logic
|
||||
/// </summary>
|
||||
public class OccupancyGridManager(CartographerConfiguration _config, ILogger<OccupancyGridManager> _logger)
|
||||
{
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
private OccupancyGrid? _occupancyGrid;
|
||||
private OccupancyGrid? _occupancyGridMcl;
|
||||
private DateTime _lastUpdatedOccupancyGrid = DateTime.MinValue;
|
||||
private volatile int _generationInProgress = 0; // 1 = generation running
|
||||
|
||||
// Version tracking: PoseGraph node insertion version at last successful generation.
|
||||
// Compared against the live PoseGraph version to skip redundant regenerations.
|
||||
private int _lastGeneratedVersion = -1;
|
||||
|
||||
// Configuration: time-based throttling (~3 seconds between updates)
|
||||
private readonly System.Diagnostics.Stopwatch _updateStopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
private const double UpdateIntervalSeconds = 3.0;
|
||||
|
||||
// Insertion tracking: flag to track if at least one scan was inserted into submap since last update
|
||||
// Used in ScanMapping mode to ensure we only update when there's new data
|
||||
private volatile bool _hasInsertionSinceLastUpdate = false;
|
||||
|
||||
public OccupancyGrid? OccupancyGrid
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock) { return _occupancyGrid; }
|
||||
}
|
||||
}
|
||||
|
||||
public OccupancyGrid? OccupancyGridMcl
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock) { return _occupancyGridMcl; }
|
||||
}
|
||||
}
|
||||
|
||||
public DateTime LastUpdated
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock) { return _lastUpdatedOccupancyGrid; }
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get occupancy grid, optionally checking if updated since a given time
|
||||
/// </summary>
|
||||
public OccupancyGrid? GetGrid(DateTime since = default)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (since == DateTime.MinValue)
|
||||
return _occupancyGrid;
|
||||
return _lastUpdatedOccupancyGrid > since ? _occupancyGrid : null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Signal that a scan was inserted into a submap (InsertionResult.HasValue = true).
|
||||
/// Called from ProcessWithTrajectoryBuilder when AddSensorData returns insertion result.
|
||||
/// This flag is required for ShouldUpdateGrid to return true.
|
||||
/// </summary>
|
||||
public void SignalInsertion()
|
||||
{
|
||||
_hasInsertionSinceLastUpdate = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if grid should be updated based on two conditions:
|
||||
/// 1. Enough time has elapsed since last update (3 seconds) OR grid is null
|
||||
/// 2. At least one scan was inserted into submap since last update (SignalInsertion was called)
|
||||
/// Both conditions must be met for update to proceed.
|
||||
/// Thread-safe: prevents concurrent generation via _generationInProgress flag.
|
||||
/// </summary>
|
||||
public bool ShouldUpdateGrid()
|
||||
{
|
||||
// Fast check: skip if generation is already in progress
|
||||
if (Interlocked.CompareExchange(ref _generationInProgress, 0, 0) == 1)
|
||||
return false;
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
bool gridIsNull = _occupancyGrid == null;
|
||||
bool timeElapsed = _updateStopwatch.Elapsed.TotalSeconds >= UpdateIntervalSeconds;
|
||||
bool hasInsertion = _hasInsertionSinceLastUpdate;
|
||||
|
||||
// Require BOTH: (time elapsed OR grid null) AND has insertion
|
||||
if ((gridIsNull || timeElapsed) && hasInsertion)
|
||||
{
|
||||
_logger.LogDebug(
|
||||
"OccupancyGridManager: ShouldUpdateGrid=true, gridIsNull={GridIsNull}, elapsed={Elapsed:F1}s, hasInsertion={HasInsertion}",
|
||||
gridIsNull, _updateStopwatch.Elapsed.TotalSeconds, hasInsertion);
|
||||
_updateStopwatch.Restart();
|
||||
_hasInsertionSinceLastUpdate = false; // Reset insertion flag after update
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update occupancy grid from MapBuilder submaps during ScanMapping
|
||||
/// Only generates Display grid for visualization (MCL not used during mapping)
|
||||
/// </summary>
|
||||
public void UpdateFromMapBuilder(IMapBuilder mapBuilder)
|
||||
{
|
||||
// Prevent concurrent generation
|
||||
if (Interlocked.CompareExchange(ref _generationInProgress, 1, 0) != 0)
|
||||
{
|
||||
_logger.LogDebug("OccupancyGridManager: Skipping - generation already in progress");
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (mapBuilder == null)
|
||||
{
|
||||
_logger.LogWarning("OccupancyGridManager: UpdateFromMapBuilder called with null mapBuilder");
|
||||
return;
|
||||
}
|
||||
|
||||
var resolution = _config.MapStorage.OccupancyGridResolution;
|
||||
var padding = _config.MapStorage.MapPadding;
|
||||
var strategy = _config.OccupancyGrid?.MergeStrategy ?? SubmapMergeStrategy.LogOddsSum;
|
||||
|
||||
var sw = System.Diagnostics.Stopwatch.StartNew();
|
||||
|
||||
// Generate display grid using configured merge strategy.
|
||||
// Pass the last generated version so the generator can skip if no new nodes
|
||||
// have been inserted (avoids redundant regeneration of identical data).
|
||||
var displayGrid = OccupancyGridGenerator.Generate(
|
||||
mapBuilder, resolution, padding,
|
||||
Volatile.Read(ref _lastGeneratedVersion),
|
||||
out var snapshotVersion,
|
||||
_logger, _config.OccupancyGrid);
|
||||
|
||||
sw.Stop();
|
||||
|
||||
if (displayGrid == null)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_occupancyGrid = null;
|
||||
_occupancyGridMcl = null;
|
||||
_lastUpdatedOccupancyGrid = DateTime.MinValue;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Count cell statistics for debugging
|
||||
int freeCells = 0, occupiedCells = 0, unknownCells = 0;
|
||||
for (int i = 0; i < displayGrid.Data.Length; i++)
|
||||
{
|
||||
if (displayGrid.Data[i] < 0) unknownCells++;
|
||||
else if (displayGrid.Data[i] == 0) freeCells++;
|
||||
else occupiedCells++;
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_occupancyGrid = displayGrid;
|
||||
// Keep existing mclGrid (if any) - don't regenerate during mapping
|
||||
_lastUpdatedOccupancyGrid = DateTime.UtcNow;
|
||||
}
|
||||
|
||||
// Cache the version so the next cycle can skip if no new data was added.
|
||||
Volatile.Write(ref _lastGeneratedVersion, snapshotVersion);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "OccupancyGridManager: Failed to update grid from MapBuilder");
|
||||
}
|
||||
finally
|
||||
{
|
||||
Interlocked.Exchange(ref _generationInProgress, 0);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Load occupancy grid from PGM file for localization
|
||||
/// </summary>
|
||||
public void LoadFromPgm(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = Path.Combine(_config.MapStorage.Directory, mapName);
|
||||
var pgmPath = Path.Combine(mapPath, "map.pgm");
|
||||
|
||||
if (!File.Exists(pgmPath))
|
||||
{
|
||||
var pgmFiles = Directory.GetFiles(mapPath, "*.pgm", SearchOption.TopDirectoryOnly);
|
||||
if (pgmFiles.Length == 0)
|
||||
{
|
||||
_logger.LogWarning("OccupancyGridManager: No PGM file found for map: {MapName}", mapName);
|
||||
Clear();
|
||||
return;
|
||||
}
|
||||
pgmPath = pgmFiles[0];
|
||||
}
|
||||
|
||||
// Load grid once and clone for MCL (Fix 2: avoid loading PGM file twice)
|
||||
// UNIFIED CONVENTION: Both use ROS convention (row 0 = world BOTTOM, Y-axis pointing UP)
|
||||
var displayGrid = PgmLoader.LoadFromPgm(pgmPath, logger: _logger);
|
||||
OccupancyGrid? mclGrid = null;
|
||||
if (displayGrid != null)
|
||||
{
|
||||
mclGrid = new OccupancyGrid
|
||||
{
|
||||
Resolution = displayGrid.Resolution,
|
||||
Width = displayGrid.Width,
|
||||
Height = displayGrid.Height,
|
||||
Origin = displayGrid.Origin,
|
||||
Data = (sbyte[])displayGrid.Data.Clone()
|
||||
};
|
||||
}
|
||||
|
||||
// Load origin from map.json if available
|
||||
if (displayGrid != null && mclGrid != null)
|
||||
{
|
||||
var mapJsonPath = Path.Combine(mapPath, "map.json");
|
||||
if (File.Exists(mapJsonPath))
|
||||
{
|
||||
try
|
||||
{
|
||||
var jsonContent = File.ReadAllText(mapJsonPath);
|
||||
var metadata = JsonSerializer.Deserialize<MapInfo>(jsonContent);
|
||||
if (metadata?.Origin != null)
|
||||
{
|
||||
displayGrid.Origin = metadata.Origin;
|
||||
mclGrid.Origin = metadata.Origin;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogWarning(ex, "OccupancyGridManager: Could not read origin from map.json");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_occupancyGrid = displayGrid;
|
||||
_occupancyGridMcl = mclGrid;
|
||||
_lastUpdatedOccupancyGrid = DateTime.UtcNow;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "OccupancyGridManager: Failed to load PGM for map: {MapName}", mapName);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clear grids for new scanning session
|
||||
/// </summary>
|
||||
public void Clear()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_occupancyGrid = null;
|
||||
_occupancyGridMcl = null;
|
||||
_lastUpdatedOccupancyGrid = DateTime.MinValue;
|
||||
_updateStopwatch.Restart();
|
||||
}
|
||||
_hasInsertionSinceLastUpdate = false;
|
||||
Volatile.Write(ref _lastGeneratedVersion, -1);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reset timer and insertion flag for new state (ScanMapping/Localizing)
|
||||
/// </summary>
|
||||
public void ResetCounter()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_updateStopwatch.Restart();
|
||||
}
|
||||
_hasInsertionSinceLastUpdate = false;
|
||||
Volatile.Write(ref _lastGeneratedVersion, -1);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dispose resources
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_occupancyGrid = null;
|
||||
_occupancyGridMcl = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,163 @@
|
||||
using CartographerSharp.IO;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for transforming pbstream files.
|
||||
/// Centralizes the logic for updating TransformToMap in PoseGraph proto.
|
||||
/// </summary>
|
||||
public static class PbstreamTransformHelper
|
||||
{
|
||||
/// <summary>
|
||||
/// Transform pbstream file by updating TransformToMap in PoseGraph proto.
|
||||
/// This applies a coordinate transform to shift the map origin.
|
||||
/// </summary>
|
||||
/// <param name="pbstreamPath">Path to the pbstream file</param>
|
||||
/// <param name="newOrigin">New origin pose to apply</param>
|
||||
/// <param name="logger">Optional logger for logging</param>
|
||||
/// <exception cref="InvalidOperationException">Thrown when pbstream read/write fails</exception>
|
||||
/// <exception cref="ArgumentException">Thrown when newOrigin contains invalid values (NaN/Infinity)</exception>
|
||||
public static void TransformPbstreamOrigin(string pbstreamPath, Pose newOrigin, ILogger? logger = null)
|
||||
{
|
||||
// Validate newOrigin for NaN/Infinity
|
||||
ValidatePose(newOrigin);
|
||||
|
||||
var tempPbstreamPath = pbstreamPath + ".tmp";
|
||||
try
|
||||
{
|
||||
using (var reader = new ProtoStreamReader(pbstreamPath))
|
||||
using (var writer = new ProtoStreamWriter(tempPbstreamPath))
|
||||
{
|
||||
// Read and write header
|
||||
if (!reader.ReadProto<CartographerSharp.Models.Mapping.SerializedData>(out var headerData))
|
||||
{
|
||||
throw new InvalidOperationException("Failed to read serialization header");
|
||||
}
|
||||
if (!headerData.SerializationHeader.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException("Invalid serialization header");
|
||||
}
|
||||
writer.WriteProto(headerData);
|
||||
|
||||
// Read PoseGraph, update TransformToMap, and write
|
||||
if (!reader.ReadProto<CartographerSharp.Models.Mapping.SerializedData>(out var poseGraphData))
|
||||
{
|
||||
throw new InvalidOperationException("Failed to read pose graph");
|
||||
}
|
||||
if (!poseGraphData.PoseGraph.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException("Invalid pose graph data");
|
||||
}
|
||||
|
||||
var poseGraphProto = poseGraphData.PoseGraph.Value;
|
||||
|
||||
// Convert Pose to Rigid3d and compose with current transform
|
||||
// Key logic from xloc.cc ChangeMapOrigin:
|
||||
// SetTransformToMap(GetTransformToMap() * Rigid3d(position, orientation));
|
||||
//
|
||||
// In Cartographer:
|
||||
// - TransformToMap: converts map frame -> internal frame
|
||||
// - TransformToMapInverse: converts internal frame -> map frame (used in OccupancyGridGenerator)
|
||||
//
|
||||
// We want: newOrigin.Position (T) in old map becomes (0,0) in new map
|
||||
// Formula: newMapPoint = R^-1 * (oldMapPoint - T) where T = newOrigin.Position, R = newOrigin.Orientation
|
||||
var newOriginRigid = new Rigid3d(
|
||||
new Vector3(newOrigin.Position.X, newOrigin.Position.Y, newOrigin.Position.Z),
|
||||
new Quaternion(newOrigin.Orientation.X, newOrigin.Orientation.Y, newOrigin.Orientation.Z, newOrigin.Orientation.W));
|
||||
|
||||
var currentTransform = poseGraphProto.TransformToMap.HasValue
|
||||
? (Rigid3d)poseGraphProto.TransformToMap.Value
|
||||
: Rigid3d.Identity;
|
||||
var newTransform = currentTransform * newOriginRigid;
|
||||
|
||||
// Update TransformToMap in the proto
|
||||
poseGraphProto.TransformToMap = (CartographerSharp.Models.Transform.Rigid3dProto)newTransform;
|
||||
|
||||
// Write updated pose graph
|
||||
var updatedPoseGraphData = new CartographerSharp.Models.Mapping.SerializedData { PoseGraph = poseGraphProto };
|
||||
writer.WriteProto(updatedPoseGraphData);
|
||||
|
||||
// Copy all remaining data unchanged (AllTrajectoryBuilderOptions, submaps, nodes, etc.)
|
||||
while (!reader.Eof)
|
||||
{
|
||||
if (!reader.ReadProto<CartographerSharp.Models.Mapping.SerializedData>(out var data))
|
||||
{
|
||||
break;
|
||||
}
|
||||
writer.WriteProto(data);
|
||||
}
|
||||
}
|
||||
|
||||
// Replace original file with updated file
|
||||
File.Move(tempPbstreamPath, pbstreamPath, overwrite: true);
|
||||
logger?.LogInformation("PbstreamTransformHelper: Transformed pbstream origin successfully");
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// Clean up temp file on error
|
||||
if (File.Exists(tempPbstreamPath))
|
||||
{
|
||||
try { File.Delete(tempPbstreamPath); } catch { }
|
||||
}
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if pose is identity (no transform needed)
|
||||
/// </summary>
|
||||
public static bool IsIdentityPose(Pose pose)
|
||||
{
|
||||
const double tolerance = 1e-6;
|
||||
|
||||
// Check position is (0, 0, 0)
|
||||
var posIsZero = Math.Abs(pose.Position.X) < tolerance &&
|
||||
Math.Abs(pose.Position.Y) < tolerance &&
|
||||
Math.Abs(pose.Position.Z) < tolerance;
|
||||
|
||||
// Check orientation is identity quaternion (0, 0, 0, 1)
|
||||
var quatIsIdentity = Math.Abs(pose.Orientation.X) < tolerance &&
|
||||
Math.Abs(pose.Orientation.Y) < tolerance &&
|
||||
Math.Abs(pose.Orientation.Z) < tolerance &&
|
||||
Math.Abs(pose.Orientation.W - 1.0) < tolerance;
|
||||
|
||||
return posIsZero && quatIsIdentity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Validate pose for NaN/Infinity values
|
||||
/// </summary>
|
||||
/// <exception cref="ArgumentException">Thrown when pose contains invalid values</exception>
|
||||
public static void ValidatePose(Pose pose)
|
||||
{
|
||||
// Check position
|
||||
if (double.IsNaN(pose.Position.X) || double.IsInfinity(pose.Position.X) ||
|
||||
double.IsNaN(pose.Position.Y) || double.IsInfinity(pose.Position.Y) ||
|
||||
double.IsNaN(pose.Position.Z) || double.IsInfinity(pose.Position.Z))
|
||||
{
|
||||
throw new ArgumentException($"Pose position contains invalid values (NaN/Infinity): ({pose.Position.X}, {pose.Position.Y}, {pose.Position.Z})", nameof(pose));
|
||||
}
|
||||
|
||||
// Check orientation
|
||||
if (double.IsNaN(pose.Orientation.X) || double.IsInfinity(pose.Orientation.X) ||
|
||||
double.IsNaN(pose.Orientation.Y) || double.IsInfinity(pose.Orientation.Y) ||
|
||||
double.IsNaN(pose.Orientation.Z) || double.IsInfinity(pose.Orientation.Z) ||
|
||||
double.IsNaN(pose.Orientation.W) || double.IsInfinity(pose.Orientation.W))
|
||||
{
|
||||
throw new ArgumentException($"Pose orientation contains invalid values (NaN/Infinity): ({pose.Orientation.X}, {pose.Orientation.Y}, {pose.Orientation.Z}, {pose.Orientation.W})", nameof(pose));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert Pose to Rigid3d
|
||||
/// </summary>
|
||||
public static Rigid3d PoseToRigid3d(Pose pose)
|
||||
{
|
||||
var translation = new Vector3(pose.Position.X, pose.Position.Y, pose.Position.Z);
|
||||
var rotation = new Quaternion(pose.Orientation.X, pose.Orientation.Y, pose.Orientation.Z, pose.Orientation.W);
|
||||
return new Rigid3d(translation, rotation);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for converting between Cartographer Rigid3d and RobotNet10 Pose
|
||||
/// </summary>
|
||||
public static class PoseConverter
|
||||
{
|
||||
/// <summary>
|
||||
/// Convert Rigid3d to Pose
|
||||
/// </summary>
|
||||
public static Pose ToPose(Rigid3d rigid3d)
|
||||
{
|
||||
return new Pose
|
||||
{
|
||||
Position = new Vector3
|
||||
{
|
||||
X = rigid3d.Translation.X,
|
||||
Y = rigid3d.Translation.Y,
|
||||
Z = rigid3d.Translation.Z
|
||||
},
|
||||
Orientation = new Quaternion
|
||||
{
|
||||
X = rigid3d.Rotation.X,
|
||||
Y = rigid3d.Rotation.Y,
|
||||
Z = rigid3d.Rotation.Z,
|
||||
W = rigid3d.Rotation.W
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert Pose to Rigid3d
|
||||
/// </summary>
|
||||
public static Rigid3d ToRigid3d(Pose pose)
|
||||
{
|
||||
var translation = new Vector3(
|
||||
(float)pose.Position.X,
|
||||
(float)pose.Position.Y,
|
||||
(float)pose.Position.Z);
|
||||
|
||||
var rotation = new Quaternion(
|
||||
(float)pose.Orientation.X,
|
||||
(float)pose.Orientation.Y,
|
||||
(float)pose.Orientation.Z,
|
||||
(float)pose.Orientation.W);
|
||||
|
||||
return new Rigid3d(translation, rotation);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
using CartographerSharp.Sensor;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Point cloud data in both base_link and sensor (lidar) frame.
|
||||
/// Pipeline produces both so Cartographer gets base_link for trajectory and MCL gets sensor_frame without re-transform.
|
||||
/// </summary>
|
||||
public sealed class RangeDataPayload
|
||||
{
|
||||
/// <summary>Point cloud in base_link (robot) frame — for Cartographer ITrajectoryBuilder.AddSensorData.</summary>
|
||||
public TimedPointCloudData BaseLink { get; set; }
|
||||
|
||||
/// <summary>Point cloud in sensor (lidar) frame — for MCL OnScan (beam angles relative to sensor). SensorPipeline always produces this.</summary>
|
||||
public TimedPointCloudData SensorFrame { get; set; }
|
||||
|
||||
/// <summary>Actual minimum angle (radians) of filtered point cloud in sensor frame — for MCL scan conversion.</summary>
|
||||
public double ActualAngleMin { get; set; }
|
||||
|
||||
/// <summary>Actual maximum angle (radians) of filtered point cloud in sensor frame — for MCL scan conversion.</summary>
|
||||
public double ActualAngleMax { get; set; }
|
||||
|
||||
/// <summary>Actual angle increment (radians) from original scan — for MCL scan conversion.</summary>
|
||||
public double ActualAngleIncrement { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,414 @@
|
||||
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);
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for converting sensor data (Lidar, IMU, Odometry) to CartographerSharp types.
|
||||
/// Used by SensorPipeline when feeding data into ITrajectoryBuilder.AddSensorData.
|
||||
/// </summary>
|
||||
public static class SensorDataTransformHelper
|
||||
{
|
||||
/// <summary>
|
||||
/// Transforms one Lidar scan into both base_link and sensor-frame point clouds in a single pass.
|
||||
/// Filters by range and optional angle limits; applies sensor transform for base_link. Use for RangeDataPayload.
|
||||
/// </summary>
|
||||
/// <param name="timestamp">Scan timestamp</param>
|
||||
/// <param name="scan">LaserScan from ILidar</param>
|
||||
/// <param name="sensorTransform">Transform from lidar frame to base_link (Rigid3f)</param>
|
||||
/// <param name="angleMinDeg">Optional minimum angle in degrees (points below filtered out)</param>
|
||||
/// <param name="angleMaxDeg">Optional maximum angle in degrees (points above filtered out)</param>
|
||||
/// <returns>(BaseLink, SensorFrame, actualAngleMin, actualAngleMax, angleIncrement) for Cartographer and MCL</returns>
|
||||
public static (TimedPointCloudData BaseLink, TimedPointCloudData SensorFrame, double ActualAngleMin, double ActualAngleMax, double AngleIncrement) ToTimedPointCloudDataBaseAndSensorFrame(
|
||||
DateTime timestamp,
|
||||
LaserScan scan,
|
||||
Transform sensorTransform,
|
||||
double? angleMinDeg = null,
|
||||
double? angleMaxDeg = null)
|
||||
{
|
||||
var rotation = sensorTransform.Rotation;
|
||||
var translation = sensorTransform.Translation;
|
||||
|
||||
double r00 = 1.0 - 2.0 * (rotation.Y * rotation.Y + rotation.Z * rotation.Z);
|
||||
double r01 = 2.0 * (rotation.X * rotation.Y - rotation.Z * rotation.W);
|
||||
double r02 = 2.0 * (rotation.X * rotation.Z + rotation.Y * rotation.W);
|
||||
double r10 = 2.0 * (rotation.X * rotation.Y + rotation.Z * rotation.W);
|
||||
double r11 = 1.0 - 2.0 * (rotation.X * rotation.X + rotation.Z * rotation.Z);
|
||||
double r12 = 2.0 * (rotation.Y * rotation.Z - rotation.X * rotation.W);
|
||||
double r20 = 2.0 * (rotation.X * rotation.Z - rotation.Y * rotation.W);
|
||||
double r21 = 2.0 * (rotation.Y * rotation.Z + rotation.X * rotation.W);
|
||||
double r22 = 1.0 - 2.0 * (rotation.X * rotation.X + rotation.Y * rotation.Y);
|
||||
|
||||
int estimatedCount = (int)(scan.Ranges.Length * 0.8);
|
||||
var basePoints = new TimedPointCloud(estimatedCount);
|
||||
var sensorPoints = new TimedPointCloud(estimatedCount);
|
||||
|
||||
var ranges = scan.Ranges;
|
||||
var rangeMin = scan.RangeMin;
|
||||
var rangeMax = scan.RangeMax;
|
||||
var angleMin = scan.AngleMin;
|
||||
var angleIncrement = scan.AngleIncrement;
|
||||
var useTimeIncrement = scan.TimeIncrement > 0 && !double.IsNaN(scan.TimeIncrement) && !double.IsInfinity(scan.TimeIncrement);
|
||||
var timeIncrement = scan.TimeIncrement;
|
||||
|
||||
double? angleMinRad = angleMinDeg.HasValue ? (angleMinDeg.Value * Math.PI / 180.0) : null;
|
||||
double? angleMaxRad = angleMaxDeg.HasValue ? (angleMaxDeg.Value * Math.PI / 180.0) : null;
|
||||
|
||||
double angle = angleMin;
|
||||
double? firstValidAngle = null;
|
||||
double? lastValidAngle = null;
|
||||
|
||||
for (int i = 0; i < ranges.Length; i++)
|
||||
{
|
||||
var range = ranges[i];
|
||||
|
||||
if (double.IsNaN(range) || double.IsInfinity(range) ||
|
||||
range < rangeMin || range > rangeMax)
|
||||
{
|
||||
angle += angleIncrement;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (angleMinRad.HasValue && angle < angleMinRad.Value)
|
||||
{
|
||||
angle += angleIncrement;
|
||||
continue;
|
||||
}
|
||||
if (angleMaxRad.HasValue && angle > angleMaxRad.Value)
|
||||
{
|
||||
angle += angleIncrement;
|
||||
continue;
|
||||
}
|
||||
|
||||
var cosAngle = Math.Cos(angle);
|
||||
var sinAngle = Math.Sin(angle);
|
||||
var x = range * cosAngle;
|
||||
var y = range * sinAngle;
|
||||
var z = 0.0;
|
||||
|
||||
double time = useTimeIncrement ? i * timeIncrement : 0.0;
|
||||
|
||||
sensorPoints.Add(new TimedRangefinderPoint(new Vector3(x, y, z), time));
|
||||
|
||||
var tx = r00 * x + r01 * y + r02 * z + translation.X;
|
||||
var ty = r10 * x + r11 * y + r12 * z + translation.Y;
|
||||
var tz = r20 * x + r21 * y + r22 * z + translation.Z;
|
||||
basePoints.Add(new TimedRangefinderPoint(new Vector3(tx, ty, tz), time));
|
||||
|
||||
// Track actual angle range of filtered points
|
||||
if (!firstValidAngle.HasValue)
|
||||
firstValidAngle = angle;
|
||||
lastValidAngle = angle;
|
||||
|
||||
angle += angleIncrement;
|
||||
}
|
||||
|
||||
if (basePoints.Count == 0)
|
||||
throw new InvalidOperationException("No valid points in scan after filtering");
|
||||
|
||||
long adjustedTimestamp = timestamp.Ticks;
|
||||
if (useTimeIncrement)
|
||||
{
|
||||
double duration = basePoints[^1].Time;
|
||||
adjustedTimestamp = timestamp.Ticks + (long)(duration * TimeSpan.TicksPerSecond);
|
||||
for (int i = 0; i < basePoints.Count; i++)
|
||||
{
|
||||
var bp = basePoints[i];
|
||||
var sp = sensorPoints[i];
|
||||
basePoints[i] = new TimedRangefinderPoint(bp.Position, bp.Time - duration);
|
||||
sensorPoints[i] = new TimedRangefinderPoint(sp.Position, sp.Time - duration);
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate actual angle metadata from filtered points
|
||||
double actualAngleMin = firstValidAngle ?? angleMin;
|
||||
double actualAngleMax = lastValidAngle ?? angleMin;
|
||||
|
||||
return (
|
||||
new TimedPointCloudData(adjustedTimestamp, translation, basePoints),
|
||||
new TimedPointCloudData(adjustedTimestamp, Vector3.Zero, sensorPoints),
|
||||
actualAngleMin,
|
||||
actualAngleMax,
|
||||
angleIncrement);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transform IMU data from primitive values to Cartographer ImuData.
|
||||
/// Applies sensor transform (rotation only) to express in base_link frame.
|
||||
/// </summary>
|
||||
public static ImuData ToImuData(
|
||||
Vector3 linearAcceleration,
|
||||
Vector3 angularVelocity,
|
||||
DateTime timestamp,
|
||||
Transform sensorTransform)
|
||||
{
|
||||
return new ImuData(timestamp.Ticks,
|
||||
Vector3.Transform(linearAcceleration, sensorTransform.Rotation),
|
||||
Vector3.Transform(angularVelocity, sensorTransform.Rotation));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transform odometry Pose to Cartographer OdometryData.
|
||||
/// </summary>
|
||||
/// <param name="pose">Pose in base_link frame</param>
|
||||
/// <param name="timestamp">Timestamp in ticks</param>
|
||||
/// <returns>OdometryData for AddSensorData</returns>
|
||||
public static OdometryData ToOdometryData(Pose pose, long timestamp)
|
||||
{
|
||||
const double QuaternionEpsilon = 1e-6;
|
||||
if (pose.Orientation.LengthSquared() < QuaternionEpsilon)
|
||||
{
|
||||
throw new ArgumentException($"Quaternion is invalid (near zero length: {pose.Orientation.LengthSquared()})", nameof(pose));
|
||||
}
|
||||
|
||||
pose.Orientation = pose.Orientation.Normalize();
|
||||
var rigid3d = new Rigid3d(pose.Position, pose.Orientation);
|
||||
|
||||
return new OdometryData(timestamp, rigid3d);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,674 @@
|
||||
using CartographerSharp.Sensor;
|
||||
using Microsoft.Extensions.Options;
|
||||
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.RobotApp.Motion;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Diagnostics;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Sensor pipeline: subscribes to Lidar/IMU/Odometry, queues data, and forwards to caller via actions.
|
||||
/// Does not reference ITrajectoryBuilder or sample point cloud; CartographerService provides AddRangeData, AddImuData, AddOdometryData.
|
||||
/// </summary>
|
||||
internal sealed class SensorPipeline(
|
||||
IDeviceProvider _deviceProvider,
|
||||
IOdometryEstimator _odometryEstimator,
|
||||
IOptions<CartographerConfiguration> configuration,
|
||||
ILogger _logger,
|
||||
Action<string, RangeDataPayload> _addRangeData,
|
||||
Action<string, ImuData> _addImuData,
|
||||
Action<string, OdometryData> _addOdometryData) : IDisposable
|
||||
{
|
||||
#region Fields
|
||||
|
||||
private readonly CartographerConfiguration _config = configuration.Value;
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
private readonly List<ILidar> _subscribedLidars = [];
|
||||
private IInertialMeasurementUnit? _subscribedImu;
|
||||
private bool _odometrySubscribed;
|
||||
|
||||
private volatile bool _active;
|
||||
|
||||
private Dictionary<string, LidarSensorConfiguration> _lidarConfigs = [];
|
||||
private Dictionary<string, Transform> _lidarTransforms = [];
|
||||
private Transform? _imuTransform;
|
||||
|
||||
private const int NotProcessing = 0;
|
||||
private const int Processing = 1;
|
||||
|
||||
// Per-device processing flags: each lidar has its own flag
|
||||
private readonly ConcurrentDictionary<string, int> _isProcessingLidar = new();
|
||||
private long _lastValidEnqueueTimeTicks; // Use Volatile.Read/Write for thread-safe access
|
||||
private const int LidarRoundTimeoutMs = 300;
|
||||
|
||||
private volatile List<string> _lidarDeviceOrder = [];
|
||||
|
||||
private struct LidarScanQueueItem
|
||||
{
|
||||
public string DeviceId { get; set; }
|
||||
public LidarScanDataEventArgs ScanData { get; set; }
|
||||
}
|
||||
|
||||
private readonly ConcurrentQueue<LidarScanQueueItem> _scanDataQueue = new();
|
||||
|
||||
// Transformed lidar data queue item (output of transform thread, input to processing thread)
|
||||
private struct TransformedLidarQueueItem
|
||||
{
|
||||
public string DeviceId { get; set; }
|
||||
public RangeDataPayload Payload { get; set; }
|
||||
}
|
||||
|
||||
private readonly ConcurrentQueue<TransformedLidarQueueItem> _transformedQueue = new();
|
||||
|
||||
private struct ImuDataQueueItem
|
||||
{
|
||||
public string DeviceId { get; set; }
|
||||
public Vector3 LinearAcceleration { get; set; }
|
||||
public Vector3 AngularVelocity { get; set; }
|
||||
public DateTime Timestamp { get; set; }
|
||||
}
|
||||
|
||||
private readonly ConcurrentQueue<ImuDataQueueItem> _imuDataQueue = new();
|
||||
|
||||
private Thread? _lidarTransformThread;
|
||||
private volatile bool _lidarTransformThreadRunning;
|
||||
|
||||
private Thread? _lidarProcessingThread;
|
||||
private volatile bool _lidarProcessingThreadRunning;
|
||||
|
||||
private Thread? _imuProcessingThread;
|
||||
private volatile bool _imuThreadRunning;
|
||||
|
||||
private Thread? _odometryProcessingThread;
|
||||
private volatile bool _odometryThreadRunning;
|
||||
|
||||
private volatile bool _disposed;
|
||||
|
||||
// ManualResetEventSlim for efficient blocking when queues are empty
|
||||
private readonly ManualResetEventSlim _lidarDataAvailable = new(false);
|
||||
private readonly ManualResetEventSlim _transformedDataAvailable = new(false);
|
||||
private readonly ManualResetEventSlim _imuDataAvailable = new(false);
|
||||
|
||||
// Queue capacity limits (Fix 9)
|
||||
private const int MaxScanQueueSize = 5;
|
||||
private const int MaxImuQueueSize = 50;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Initialization
|
||||
|
||||
public async Task InitializeAsync()
|
||||
{
|
||||
await DiscoverAndSubscribeLidarsAsync();
|
||||
await SubscribeToImuAsync();
|
||||
SubscribeToOdometry();
|
||||
}
|
||||
|
||||
public void ResumeSubscriptions()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (_active)
|
||||
{
|
||||
_logger.LogDebug("SensorPipeline.ResumeSubscriptions: Already active, skipping");
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("SensorPipeline.ResumeSubscriptions: Resuming {LidarCount} lidars", _subscribedLidars.Count);
|
||||
|
||||
foreach (var lidar in _subscribedLidars)
|
||||
{
|
||||
lidar.ScanDataReceived += OnLidarScanDataReceived;
|
||||
}
|
||||
_subscribedImu?.ImuDataChanged += OnImuDataChanged;
|
||||
|
||||
if (_config.UseOdometry && _odometrySubscribed)
|
||||
StartOdometryProcessingThread();
|
||||
StartLidarProcessingThread();
|
||||
if (_subscribedImu != null)
|
||||
StartImuProcessingThread();
|
||||
|
||||
_active = true;
|
||||
_logger.LogInformation("SensorPipeline.ResumeSubscriptions: Sensors resumed, active=true");
|
||||
}
|
||||
}
|
||||
|
||||
public void PauseSubscriptions()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_active)
|
||||
{
|
||||
_logger.LogDebug("SensorPipeline.PauseSubscriptions: Already inactive, skipping");
|
||||
return;
|
||||
}
|
||||
|
||||
_logger.LogInformation("SensorPipeline.PauseSubscriptions: Pausing sensors");
|
||||
|
||||
foreach (var lidar in _subscribedLidars)
|
||||
{
|
||||
lidar.ScanDataReceived -= OnLidarScanDataReceived;
|
||||
}
|
||||
|
||||
_subscribedImu?.ImuDataChanged -= OnImuDataChanged;
|
||||
|
||||
if (_odometrySubscribed)
|
||||
StopOdometryProcessingThread();
|
||||
StopLidarProcessingThread();
|
||||
if (_subscribedImu != null)
|
||||
StopImuProcessingThread();
|
||||
|
||||
_active = false;
|
||||
_logger.LogInformation("SensorPipeline.PauseSubscriptions: Sensors paused, active=false");
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Subscriptions
|
||||
|
||||
private async Task DiscoverAndSubscribeLidarsAsync()
|
||||
{
|
||||
var allLidars = await _deviceProvider.GetDevicesByTypeAsync(DeviceType.Lidar);
|
||||
var lidarList = allLidars.OfType<ILidar>().ToList();
|
||||
if (lidarList.Count == 0)
|
||||
throw new InvalidOperationException("No Lidars found in device provider");
|
||||
|
||||
var lidarConfigsDict = new Dictionary<string, LidarSensorConfiguration>();
|
||||
var lidarTransformsDict = new Dictionary<string, Transform>();
|
||||
foreach (var lidar in lidarList)
|
||||
{
|
||||
var deviceId = (lidar as DeviceBase)?.DeviceId;
|
||||
if (string.IsNullOrEmpty(deviceId))
|
||||
continue;
|
||||
var sensorConfig = _config.Sensors.Lidars.FirstOrDefault(cfg => cfg.DeviceId == deviceId && cfg.Enabled);
|
||||
if (sensorConfig != null)
|
||||
{
|
||||
lidarConfigsDict[deviceId] = sensorConfig;
|
||||
lidarTransformsDict[deviceId] = sensorConfig.Transform;
|
||||
_subscribedLidars.Add(lidar);
|
||||
}
|
||||
else
|
||||
{
|
||||
// FIXED: Skip lidars not configured in Cartographer.Sensors.Lidars
|
||||
// This prevents unintended lidars (e.g., those used only for Detection)
|
||||
// from being subscribed to CartographerSharp SLAM pipeline
|
||||
_logger.LogDebug("SensorPipeline: Lidar {DeviceId} found but not enabled in Cartographer.Sensors.Lidars config, skipping", deviceId);
|
||||
}
|
||||
}
|
||||
if (_subscribedLidars.Count == 0)
|
||||
throw new InvalidOperationException("No Lidars to subscribe to");
|
||||
_lidarConfigs = lidarConfigsDict;
|
||||
_lidarTransforms = lidarTransformsDict;
|
||||
|
||||
// Initialize lidar device order and per-device processing flags
|
||||
_lidarDeviceOrder = [.. _subscribedLidars
|
||||
.Select(lidar => (lidar as DeviceBase)?.DeviceId)
|
||||
.Where(id => !string.IsNullOrEmpty(id))
|
||||
.Cast<string>()];
|
||||
|
||||
foreach (var lidarId in _lidarDeviceOrder)
|
||||
{
|
||||
_isProcessingLidar[lidarId] = NotProcessing;
|
||||
}
|
||||
}
|
||||
|
||||
private async Task SubscribeToImuAsync()
|
||||
{
|
||||
if (!_config.Sensors.Imu.Enabled || string.IsNullOrEmpty(_config.Sensors.Imu.DeviceId))
|
||||
return;
|
||||
_imuTransform = _config.Sensors.Imu.Transform;
|
||||
var imu = await _deviceProvider.GetDeviceAsync(_config.Sensors.Imu.DeviceId);
|
||||
if (imu is IInertialMeasurementUnit imuDevice)
|
||||
_subscribedImu = imuDevice;
|
||||
else
|
||||
_logger.LogWarning("SensorPipeline: IMU device {DeviceId} not found or not an IMU", _config.Sensors.Imu.DeviceId);
|
||||
}
|
||||
|
||||
private void SubscribeToOdometry()
|
||||
{
|
||||
if (!_config.UseOdometry || _odometrySubscribed)
|
||||
return;
|
||||
_odometrySubscribed = true;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Lidar Processing
|
||||
|
||||
private void StartLidarProcessingThread()
|
||||
{
|
||||
if (_lidarTransformThreadRunning || _lidarProcessingThreadRunning)
|
||||
return;
|
||||
|
||||
// Start transform thread
|
||||
_lidarTransformThreadRunning = true;
|
||||
_lidarTransformThread = new Thread(LidarTransformThreadProc) { Name = "LidarTransform", Priority = ThreadPriority.Highest, IsBackground = true };
|
||||
_lidarTransformThread.Start();
|
||||
|
||||
// Start processing thread
|
||||
_lidarProcessingThreadRunning = true;
|
||||
_lidarProcessingThread = new Thread(LidarProcessingThreadProc) { Name = "LidarProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
|
||||
_lidarProcessingThread.Start();
|
||||
}
|
||||
|
||||
private void StopLidarProcessingThread()
|
||||
{
|
||||
// Stop transform thread
|
||||
if (_lidarTransformThreadRunning)
|
||||
{
|
||||
_lidarTransformThreadRunning = false;
|
||||
_lidarDataAvailable.Set(); // Wake thread to check flag
|
||||
if (_lidarTransformThread != null)
|
||||
{
|
||||
_lidarTransformThread.Join(2000);
|
||||
if (_lidarTransformThread.IsAlive)
|
||||
_logger.LogWarning("SensorPipeline: Lidar transform thread did not stop in time");
|
||||
_lidarTransformThread = null;
|
||||
}
|
||||
}
|
||||
|
||||
// Stop processing thread
|
||||
if (_lidarProcessingThreadRunning)
|
||||
{
|
||||
_lidarProcessingThreadRunning = false;
|
||||
_transformedDataAvailable.Set(); // Wake thread to check flag
|
||||
if (_lidarProcessingThread != null)
|
||||
{
|
||||
_lidarProcessingThread.Join(2000);
|
||||
if (_lidarProcessingThread.IsAlive)
|
||||
_logger.LogWarning("SensorPipeline: Lidar processing thread did not stop in time");
|
||||
_lidarProcessingThread = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transform thread: dequeues raw scan data, transforms to RangeDataPayload, enqueues to transformed queue.
|
||||
/// Does NOT check _isProcessingLidar flags - just processes everything in _scanDataQueue.
|
||||
/// </summary>
|
||||
private void LidarTransformThreadProc()
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
while (_lidarTransformThreadRunning)
|
||||
{
|
||||
_lidarDataAvailable.Wait(50); // timeout to check _lidarTransformThreadRunning
|
||||
_lidarDataAvailable.Reset();
|
||||
|
||||
if (_scanDataQueue.IsEmpty)
|
||||
continue;
|
||||
|
||||
while (_scanDataQueue.TryDequeue(out var queueItem))
|
||||
{
|
||||
if (_disposed || !_active ||
|
||||
!_lidarConfigs.TryGetValue(queueItem.DeviceId, out var lidarConfig) ||
|
||||
!_lidarTransforms.TryGetValue(queueItem.DeviceId, out var lidarTransform))
|
||||
continue;
|
||||
|
||||
try
|
||||
{
|
||||
var (baseLink, sensorFrame, actualAngleMin, actualAngleMax, angleIncrement) = SensorDataTransformHelper.ToTimedPointCloudDataBaseAndSensorFrame(
|
||||
queueItem.ScanData.Timestamp, queueItem.ScanData.MeasurementData,
|
||||
lidarTransform, lidarConfig.AngleMin, lidarConfig.AngleMax);
|
||||
|
||||
_transformedQueue.Enqueue(new TransformedLidarQueueItem
|
||||
{
|
||||
DeviceId = queueItem.DeviceId,
|
||||
Payload = new RangeDataPayload
|
||||
{
|
||||
BaseLink = baseLink,
|
||||
SensorFrame = sensorFrame,
|
||||
ActualAngleMin = actualAngleMin,
|
||||
ActualAngleMax = actualAngleMax,
|
||||
ActualAngleIncrement = angleIncrement
|
||||
}
|
||||
});
|
||||
_transformedDataAvailable.Set();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "SensorPipeline: Error transforming Lidar from {DeviceId}", queueItem.DeviceId);
|
||||
}
|
||||
}
|
||||
}
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Processing thread: dequeues transformed data, calls _addRangeData, handles round tracking and timeout.
|
||||
/// </summary>
|
||||
private void LidarProcessingThreadProc()
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
var processedLidarsInRound = new HashSet<string>();
|
||||
var expectedLidarCount = _lidarDeviceOrder.Count;
|
||||
|
||||
while (_lidarProcessingThreadRunning)
|
||||
{
|
||||
_transformedDataAvailable.Wait(50); // timeout to check _lidarProcessingThreadRunning
|
||||
_transformedDataAvailable.Reset();
|
||||
|
||||
if (_transformedQueue.IsEmpty)
|
||||
{
|
||||
// Check timeout if we're in the middle of a round (at least one lidar processed)
|
||||
if (processedLidarsInRound.Count > 0)
|
||||
{
|
||||
var lastEnqueueTicks = Volatile.Read(ref _lastValidEnqueueTimeTicks);
|
||||
var elapsedMs = (DateTime.UtcNow.Ticks - lastEnqueueTicks) / TimeSpan.TicksPerMillisecond;
|
||||
if (elapsedMs >= LidarRoundTimeoutMs)
|
||||
{
|
||||
var missingLidars = _lidarDeviceOrder.Except(processedLidarsInRound).ToList();
|
||||
_logger.LogError(
|
||||
"SensorPipeline: Lidar round timeout after {ElapsedMs}ms. Processed {ProcessedCount}/{ExpectedCount} lidars. Processed: [{ProcessedList}]. Missing: [{MissingList}]",
|
||||
elapsedMs, processedLidarsInRound.Count, expectedLidarCount,
|
||||
string.Join(", ", processedLidarsInRound),
|
||||
string.Join(", ", missingLidars));
|
||||
throw new TimeoutException($"SensorPipeline: Lidar round timeout - processed {processedLidarsInRound.Count}/{expectedLidarCount} lidars, missing: [{string.Join(", ", missingLidars)}]");
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
while (_transformedQueue.TryDequeue(out var queueItem))
|
||||
{
|
||||
if (_disposed || !_active)
|
||||
continue;
|
||||
|
||||
try
|
||||
{
|
||||
_addRangeData(queueItem.DeviceId, queueItem.Payload);
|
||||
|
||||
// Track processed lidar in this round
|
||||
processedLidarsInRound.Add(queueItem.DeviceId);
|
||||
|
||||
// Check if all lidars have been processed in this round
|
||||
if (processedLidarsInRound.Count >= expectedLidarCount)
|
||||
{
|
||||
// Reset all per-device flags to allow new round
|
||||
foreach (var lidarId in _lidarDeviceOrder)
|
||||
{
|
||||
_isProcessingLidar[lidarId] = NotProcessing;
|
||||
}
|
||||
processedLidarsInRound.Clear();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "SensorPipeline: Error processing Lidar from {DeviceId}", queueItem.DeviceId);
|
||||
}
|
||||
}
|
||||
}
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region IMU Processing
|
||||
|
||||
private void StartImuProcessingThread()
|
||||
{
|
||||
if (_imuThreadRunning)
|
||||
return;
|
||||
_imuThreadRunning = true;
|
||||
_imuProcessingThread = new Thread(ImuProcessingThreadProc) { Name = "ImuProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
|
||||
_imuProcessingThread.Start();
|
||||
}
|
||||
|
||||
private void StopImuProcessingThread()
|
||||
{
|
||||
if (!_imuThreadRunning)
|
||||
return;
|
||||
_imuThreadRunning = false;
|
||||
_imuDataAvailable.Set(); // Wake thread to check flag
|
||||
if (_imuProcessingThread != null)
|
||||
{
|
||||
_imuProcessingThread.Join(2000);
|
||||
if (_imuProcessingThread.IsAlive)
|
||||
_logger.LogWarning("SensorPipeline: IMU processing thread did not stop in time");
|
||||
_imuProcessingThread = null;
|
||||
}
|
||||
}
|
||||
|
||||
private void ImuProcessingThreadProc()
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
var imuUpdateIntervalMs = _config.Sensors.ImuUpdateIntervalMs;
|
||||
var imuMinIntervalTicks = imuUpdateIntervalMs * TimeSpan.TicksPerMillisecond;
|
||||
var lastProcessTimeTicks = 0L;
|
||||
while (_imuThreadRunning)
|
||||
{
|
||||
_imuDataAvailable.Wait(50); // timeout to check _imuThreadRunning
|
||||
_imuDataAvailable.Reset();
|
||||
|
||||
while (_imuDataQueue.TryDequeue(out var queueItem))
|
||||
{
|
||||
if (_disposed || !_active || _imuTransform == null)
|
||||
continue;
|
||||
|
||||
// Rate limiting: skip if too soon since last process (when interval > 0)
|
||||
var currentTicks = queueItem.Timestamp.Ticks;
|
||||
if (imuMinIntervalTicks > 0 && lastProcessTimeTicks > 0 &&
|
||||
currentTicks - lastProcessTimeTicks < imuMinIntervalTicks)
|
||||
continue;
|
||||
|
||||
try
|
||||
{
|
||||
var imuData = SensorDataTransformHelper.ToImuData(
|
||||
queueItem.LinearAcceleration, queueItem.AngularVelocity, queueItem.Timestamp, _imuTransform.Value);
|
||||
_addImuData(queueItem.DeviceId, imuData);
|
||||
lastProcessTimeTicks = currentTicks;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "SensorPipeline: Error processing IMU from {DeviceId}", queueItem.DeviceId);
|
||||
}
|
||||
}
|
||||
}
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Odometry Processing
|
||||
|
||||
private void StartOdometryProcessingThread()
|
||||
{
|
||||
if (_odometryThreadRunning)
|
||||
return;
|
||||
_odometryThreadRunning = true;
|
||||
_odometryProcessingThread = new Thread(OdometryProcessingThreadProc) { Name = "OdometryProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
|
||||
_odometryProcessingThread.Start();
|
||||
}
|
||||
|
||||
private void StopOdometryProcessingThread()
|
||||
{
|
||||
if (!_odometryThreadRunning)
|
||||
return;
|
||||
_odometryThreadRunning = false;
|
||||
if (_odometryProcessingThread != null)
|
||||
{
|
||||
_odometryProcessingThread.Join(1000);
|
||||
if (_odometryProcessingThread.IsAlive)
|
||||
_logger.LogWarning("SensorPipeline: Odometry processing thread did not stop in time");
|
||||
_odometryProcessingThread = null;
|
||||
}
|
||||
}
|
||||
|
||||
private void OdometryProcessingThreadProc()
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
try
|
||||
{
|
||||
var spinWait = new SpinWait();
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
var odometryUpdateIntervalMs = _config.Sensors.OdometryUpdateIntervalMs;
|
||||
var targetTicksPerInterval = odometryUpdateIntervalMs * Stopwatch.Frequency / 1000;
|
||||
var lastProcessTimeTicks = stopwatch.ElapsedTicks;
|
||||
while (_odometryThreadRunning)
|
||||
{
|
||||
var currentTicks = stopwatch.ElapsedTicks;
|
||||
var remaining = targetTicksPerInterval - (currentTicks - lastProcessTimeTicks);
|
||||
if (remaining > Stopwatch.Frequency / 100) // > 10ms remaining
|
||||
Thread.Sleep(1);
|
||||
else
|
||||
spinWait.SpinOnce();
|
||||
|
||||
currentTicks = stopwatch.ElapsedTicks;
|
||||
if (currentTicks - lastProcessTimeTicks >= targetTicksPerInterval)
|
||||
{
|
||||
if (!_disposed && _active)
|
||||
{
|
||||
try
|
||||
{
|
||||
var currentPose = _odometryEstimator.CurrentPose;
|
||||
var currentTimeTicks = DateTime.UtcNow.Ticks;
|
||||
|
||||
// Extract theta from quaternion
|
||||
var currentTheta = currentPose.Orientation.ToYawRadian();
|
||||
var currentX = currentPose.Position.X;
|
||||
var currentY = currentPose.Position.Y;
|
||||
|
||||
var odometryData = SensorDataTransformHelper.ToOdometryData(currentPose, currentTimeTicks);
|
||||
_addOdometryData("odometry", odometryData);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "SensorPipeline: Error processing odometry at {Ticks}", DateTime.UtcNow.Ticks);
|
||||
}
|
||||
}
|
||||
lastProcessTimeTicks = currentTicks;
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Event Handlers
|
||||
|
||||
private void OnLidarScanDataReceived(object? sender, LidarScanDataEventArgs e)
|
||||
{
|
||||
if (_disposed || !_active || sender is not DeviceBase device)
|
||||
return;
|
||||
if (_scanDataQueue.Count >= MaxScanQueueSize)
|
||||
return; // Drop: processing can't keep up
|
||||
|
||||
var deviceId = device.DeviceId;
|
||||
|
||||
// Check if this lidar already has data in the current round
|
||||
if (_isProcessingLidar.TryGetValue(deviceId, out var status) && status == Processing)
|
||||
return; // Drop: this lidar already has data in current round
|
||||
|
||||
// Set flag to indicate this lidar has data in current round
|
||||
_isProcessingLidar[deviceId] = Processing;
|
||||
|
||||
try
|
||||
{
|
||||
_scanDataQueue.Enqueue(new LidarScanQueueItem { DeviceId = deviceId, ScanData = e });
|
||||
Volatile.Write(ref _lastValidEnqueueTimeTicks, DateTime.UtcNow.Ticks);
|
||||
_lidarDataAvailable.Set();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// Reset flag on error so this lidar can try again
|
||||
_isProcessingLidar[deviceId] = NotProcessing;
|
||||
_logger.LogError(ex, "SensorPipeline: Error enqueueing Lidar from {DeviceId}", deviceId);
|
||||
}
|
||||
}
|
||||
|
||||
private void OnImuDataChanged(object? sender, ImuDataChangedEventArgs e)
|
||||
{
|
||||
if (sender is not DeviceBase device || _disposed || string.IsNullOrEmpty(device.DeviceId) || _imuTransform == null || !_active)
|
||||
return;
|
||||
if (_imuDataQueue.Count >= MaxImuQueueSize)
|
||||
return; // Drop: processing can't keep up
|
||||
try
|
||||
{
|
||||
_imuDataQueue.Enqueue(new ImuDataQueueItem
|
||||
{
|
||||
DeviceId = device.DeviceId,
|
||||
LinearAcceleration = e.Acceleration.Accel.Linear,
|
||||
AngularVelocity = e.AngularVelocity.Vector,
|
||||
Timestamp = e.Timestamp
|
||||
});
|
||||
_imuDataAvailable.Set();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "SensorPipeline: Error enqueueing IMU from {DeviceId}", device.DeviceId);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Processing Status
|
||||
|
||||
/// <summary>
|
||||
/// Wait for all queued data to be processed, with timeout.
|
||||
/// Returns true if all queues are drained within the timeout.
|
||||
/// </summary>
|
||||
public bool WaitForProcessingComplete(int timeoutMs = 200)
|
||||
{
|
||||
var sw = Stopwatch.StartNew();
|
||||
var spin = new SpinWait();
|
||||
while (sw.ElapsedMilliseconds < timeoutMs)
|
||||
{
|
||||
// Check if all queues are empty and no lidar is being processed
|
||||
var allLidarsNotProcessing = _isProcessingLidar.Values.All(status => status == NotProcessing);
|
||||
if (_scanDataQueue.IsEmpty && _transformedQueue.IsEmpty && _imuDataQueue.IsEmpty && allLidarsNotProcessing)
|
||||
return true;
|
||||
spin.SpinOnce();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region IDisposable
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
_disposed = true;
|
||||
try
|
||||
{
|
||||
PauseSubscriptions();
|
||||
foreach (var lidar in _subscribedLidars)
|
||||
lidar.ScanDataReceived -= OnLidarScanDataReceived;
|
||||
_subscribedLidars.Clear();
|
||||
_subscribedImu?.ImuDataChanged -= OnImuDataChanged;
|
||||
_subscribedImu = null;
|
||||
if (_odometrySubscribed)
|
||||
{
|
||||
StopOdometryProcessingThread();
|
||||
_odometrySubscribed = false;
|
||||
}
|
||||
StopLidarProcessingThread();
|
||||
StopImuProcessingThread();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "SensorPipeline: Error during Dispose");
|
||||
}
|
||||
finally
|
||||
{
|
||||
_lidarDataAvailable.Dispose();
|
||||
_transformedDataAvailable.Dispose();
|
||||
_imuDataAvailable.Dispose();
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,153 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using RobotNet10.RobotApp.Shared.Enums;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Processes SLAM results from sensor data
|
||||
/// Handles both ScanMapping and Localization modes with appropriate pose/covariance updates
|
||||
/// </summary>
|
||||
public class SlamResultProcessor(ILogger<SlamResultProcessor> _logger)
|
||||
{
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
private int _totalSubmapsCreated;
|
||||
private int _scanMappingTrajectoryNodeCount;
|
||||
private Matrix3x3? _poseCovariance;
|
||||
private int _constraintCount;
|
||||
private double _averageConstraintQuality;
|
||||
|
||||
public int TotalSubmapsCreated
|
||||
{
|
||||
get { lock (_lock) { return _totalSubmapsCreated; } }
|
||||
}
|
||||
|
||||
public int TrajectoryNodeCount
|
||||
{
|
||||
get { lock (_lock) { return _scanMappingTrajectoryNodeCount; } }
|
||||
}
|
||||
|
||||
public Matrix3x3? PoseCovariance
|
||||
{
|
||||
get { lock (_lock) { return _poseCovariance; } }
|
||||
}
|
||||
|
||||
public int ConstraintCount
|
||||
{
|
||||
get { lock (_lock) { return _constraintCount; } }
|
||||
}
|
||||
|
||||
public double AverageConstraintQuality
|
||||
{
|
||||
get { lock (_lock) { return _averageConstraintQuality; } }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Process SLAM result for ScanMapping mode
|
||||
/// Tracks submap creation and trajectory nodes
|
||||
/// </summary>
|
||||
public bool ProcessScanMappingResult(
|
||||
ITrajectoryBuilder.MatchingResult result,
|
||||
SLAMState currentState,
|
||||
out IReadOnlyList<CartographerSharp.Mapping.Submap>? newSubmaps)
|
||||
{
|
||||
newSubmaps = null;
|
||||
|
||||
if (result.InsertionResult?.InsertionSubmaps == null || result.InsertionResult.Value.InsertionSubmaps.Count == 0)
|
||||
return false;
|
||||
|
||||
var insertionSubmaps = result.InsertionResult.Value.InsertionSubmaps;
|
||||
bool shouldFireSubmapsUpdated = false;
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_totalSubmapsCreated += insertionSubmaps.Count;
|
||||
|
||||
if (currentState == SLAMState.ScanMapping)
|
||||
{
|
||||
shouldFireSubmapsUpdated = true;
|
||||
newSubmaps = insertionSubmaps;
|
||||
|
||||
// Track nodes
|
||||
if (result.InsertionResult.Value.ConstantData != null)
|
||||
{
|
||||
_scanMappingTrajectoryNodeCount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (shouldFireSubmapsUpdated)
|
||||
{
|
||||
_logger.LogDebug("SlamResultProcessor: {Count} new submaps created (Total: {Total})",
|
||||
insertionSubmaps.Count, _totalSubmapsCreated);
|
||||
}
|
||||
|
||||
return shouldFireSubmapsUpdated;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Process SLAM result for Localization mode
|
||||
/// Updates covariance and constraint tracking
|
||||
/// </summary>
|
||||
public void ProcessLocalizationResult(
|
||||
Pose currentPose,
|
||||
Matrix3x3? cachedCovariance,
|
||||
int cachedConstraintCount,
|
||||
double cachedAverageQuality)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_poseCovariance = cachedCovariance;
|
||||
_constraintCount = cachedConstraintCount;
|
||||
_averageConstraintQuality = cachedAverageQuality;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update covariance from constraint calculation
|
||||
/// </summary>
|
||||
public void UpdateCovariance(Matrix3x3? covariance, int constraintCount, double averageQuality)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_poseCovariance = covariance;
|
||||
_constraintCount = constraintCount;
|
||||
_averageConstraintQuality = averageQuality;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reset counters for new ScanMapping session
|
||||
/// </summary>
|
||||
public void ResetForNewSession()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_totalSubmapsCreated = 0;
|
||||
_scanMappingTrajectoryNodeCount = 0;
|
||||
_poseCovariance = null;
|
||||
_constraintCount = 0;
|
||||
_averageConstraintQuality = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clear localization data
|
||||
/// </summary>
|
||||
public void ClearLocalizationData()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_poseCovariance = null;
|
||||
_constraintCount = 0;
|
||||
_averageConstraintQuality = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
// Nothing to dispose currently
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Cache for pose graph constraints of a single trajectory.
|
||||
/// Used by CartographerService to avoid repeatedly querying constraints when calculating covariance.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a constraint cache with the given refresh interval.
|
||||
/// </remarks>
|
||||
public sealed class TrajectoryConstraintCache(TimeSpan _updateInterval)
|
||||
{
|
||||
private readonly Lock _lock = new();
|
||||
private List<IPoseGraph.Constraint>? _cachedConstraints;
|
||||
private DateTime _lastUpdate = DateTime.MinValue;
|
||||
private IMapBuilder? _lastMapBuilder;
|
||||
private int _lastTrajectoryId = -1;
|
||||
|
||||
/// <summary>
|
||||
/// Gets constraints for the given trajectory, refreshing from the pose graph when the cache is stale
|
||||
/// or when map builder / trajectory id change.
|
||||
/// </summary>
|
||||
/// <param name="mapBuilder">Current map builder (may be null)</param>
|
||||
/// <param name="trajectoryId">Trajectory id to filter constraints</param>
|
||||
/// <param name="logger">Optional logger for warnings</param>
|
||||
/// <returns>Cached or freshly fetched constraints, or null if mapBuilder is null or fetch failed</returns>
|
||||
public List<IPoseGraph.Constraint>? GetOrUpdate(
|
||||
IMapBuilder? mapBuilder,
|
||||
int trajectoryId,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
var now = DateTime.UtcNow;
|
||||
bool needsUpdate = false;
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
if (mapBuilder != _lastMapBuilder || trajectoryId != _lastTrajectoryId)
|
||||
{
|
||||
needsUpdate = true;
|
||||
}
|
||||
else if (_cachedConstraints == null || (now - _lastUpdate) > _updateInterval)
|
||||
{
|
||||
needsUpdate = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!needsUpdate)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _cachedConstraints;
|
||||
}
|
||||
}
|
||||
|
||||
if (mapBuilder == null)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var constraints = mapBuilder.PoseGraph.Constraints().Where(c => c.NodeId.TrajectoryId == trajectoryId).ToList();
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_cachedConstraints = constraints;
|
||||
_lastUpdate = now;
|
||||
_lastMapBuilder = mapBuilder;
|
||||
_lastTrajectoryId = trajectoryId;
|
||||
}
|
||||
|
||||
return constraints;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
logger?.LogWarning(ex, "TrajectoryConstraintCache: Failed to update constraint cache");
|
||||
lock (_lock)
|
||||
{
|
||||
return _cachedConstraints;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clears the cache (e.g. when switching map or trajectory).
|
||||
/// </summary>
|
||||
public void Invalidate()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_cachedConstraints = null;
|
||||
_lastUpdate = DateTime.MinValue;
|
||||
_lastMapBuilder = null;
|
||||
_lastTrajectoryId = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for trajectory-related operations
|
||||
/// </summary>
|
||||
public static class TrajectoryHelper
|
||||
{
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Build sensor IDs from configuration
|
||||
/// </summary>
|
||||
/// <param name="config">Cartographer configuration</param>
|
||||
/// <returns>HashSet of sensor IDs</returns>
|
||||
public static HashSet<ITrajectoryBuilder.SensorId> BuildSensorIds(CartographerConfiguration config)
|
||||
{
|
||||
var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>();
|
||||
|
||||
// Add Lidar sensor IDs
|
||||
foreach (var lidarConfig in config.Sensors.Lidars.Where(l => l.Enabled))
|
||||
{
|
||||
sensorIds.Add(new ITrajectoryBuilder.SensorId(
|
||||
ITrajectoryBuilder.SensorId.SensorType.Range,
|
||||
lidarConfig.DeviceId));
|
||||
}
|
||||
|
||||
// Add IMU sensor ID
|
||||
if (config.Sensors.Imu.Enabled && !string.IsNullOrEmpty(config.Sensors.Imu.DeviceId))
|
||||
{
|
||||
sensorIds.Add(new ITrajectoryBuilder.SensorId(
|
||||
ITrajectoryBuilder.SensorId.SensorType.Imu,
|
||||
config.Sensors.Imu.DeviceId));
|
||||
}
|
||||
|
||||
// Add Odometry sensor ID
|
||||
if (config.UseOdometry)
|
||||
{
|
||||
sensorIds.Add(new ITrajectoryBuilder.SensorId(
|
||||
ITrajectoryBuilder.SensorId.SensorType.Odometry,
|
||||
"odometry"));
|
||||
}
|
||||
|
||||
return sensorIds;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,359 @@
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for detecting walls from point cloud data and calculating alignment angles
|
||||
/// </summary>
|
||||
public static class WallAlignmentHelper
|
||||
{
|
||||
private const double MIN_WALL_LENGTH = 1.0; // Minimum wall length in meters
|
||||
private const double RANSAC_INLIER_THRESHOLD = 0.05; // 5cm tolerance for RANSAC
|
||||
private const int RANSAC_ITERATIONS = 100;
|
||||
private const int MIN_INLIERS = 20; // Minimum points to consider a valid wall
|
||||
|
||||
#region Internal Types
|
||||
|
||||
/// <summary>
|
||||
/// Line representation: ax + by + c = 0 (normalized: a^2 + b^2 = 1)
|
||||
/// </summary>
|
||||
private struct Line
|
||||
{
|
||||
public double A { get; set; }
|
||||
public double B { get; set; }
|
||||
public double C { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get angle of line relative to X-axis in radians
|
||||
/// Line equation: ax + by + c = 0
|
||||
/// Direction vector: (-b, a)
|
||||
/// Angle = atan2(a, -b)
|
||||
/// </summary>
|
||||
public double GetAngle() => Math.Atan2(A, -B);
|
||||
|
||||
/// <summary>
|
||||
/// Get perpendicular distance from a point to this line
|
||||
/// </summary>
|
||||
public double DistanceToPoint(Vector3 point)
|
||||
{
|
||||
return Math.Abs(A * point.X + B * point.Y + C);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Detected wall information
|
||||
/// </summary>
|
||||
private struct Wall
|
||||
{
|
||||
public Line Line { get; set; }
|
||||
public int InlierCount { get; set; }
|
||||
public double Length { get; set; }
|
||||
public Vector3 StartPoint { get; set; }
|
||||
public Vector3 EndPoint { get; set; }
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public API
|
||||
|
||||
/// <summary>
|
||||
/// Detect the longest wall from a collection of 2D points and calculate the minimum
|
||||
/// rotation angle needed to align it with either the X or Y axis
|
||||
/// </summary>
|
||||
/// <param name="points">Point cloud in base_link frame</param>
|
||||
/// <param name="logger">Logger for debug information</param>
|
||||
/// <returns>
|
||||
/// Compensation angle in radians, or null if no valid wall found.
|
||||
/// This angle should be applied to robot orientation to make the wall parallel to X or Y axis.
|
||||
/// </returns>
|
||||
public static double? DetectWallAndCalculateCompensation(
|
||||
IReadOnlyList<Vector3> points,
|
||||
ILogger logger)
|
||||
{
|
||||
if (points == null || points.Count < MIN_INLIERS)
|
||||
{
|
||||
logger.LogWarning("WallAlignment: Insufficient points for wall detection (count: {Count})", points?.Count ?? 0);
|
||||
return null;
|
||||
}
|
||||
|
||||
logger.LogInformation("WallAlignment: Processing {Count} points for wall detection", points.Count);
|
||||
|
||||
// Detect all walls using RANSAC
|
||||
var walls = DetectWallsRANSAC(points, logger);
|
||||
|
||||
if (walls.Count == 0)
|
||||
{
|
||||
logger.LogWarning("WallAlignment: No walls detected");
|
||||
return null;
|
||||
}
|
||||
|
||||
// Find the longest wall
|
||||
var longestWall = walls.OrderByDescending(w => w.Length).First();
|
||||
|
||||
logger.LogInformation(
|
||||
"WallAlignment: Longest wall found - Length: {Length:F2}m, Inliers: {Inliers}, Angle: {Angle:F2}rad ({AngleDeg:F2}°)",
|
||||
longestWall.Length,
|
||||
longestWall.InlierCount,
|
||||
longestWall.Line.GetAngle(),
|
||||
longestWall.Line.GetAngle() * 180.0 / Math.PI);
|
||||
|
||||
// Calculate compensation angle
|
||||
var wallAngle = longestWall.Line.GetAngle();
|
||||
var compensationAngle = CalculateMinimumRotationToAxis(wallAngle);
|
||||
|
||||
logger.LogInformation(
|
||||
"WallAlignment: Compensation angle: {Angle:F2}rad ({AngleDeg:F2}°)",
|
||||
compensationAngle,
|
||||
compensationAngle * 180.0 / Math.PI);
|
||||
|
||||
return compensationAngle;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region RANSAC Wall Detection
|
||||
|
||||
/// <summary>
|
||||
/// Detect walls using RANSAC line fitting algorithm
|
||||
/// </summary>
|
||||
private static List<Wall> DetectWallsRANSAC(IReadOnlyList<Vector3> points, ILogger logger)
|
||||
{
|
||||
var walls = new List<Wall>();
|
||||
var unusedPoints = points.ToList();
|
||||
var random = new Random(DateTime.Now.Millisecond);
|
||||
|
||||
// Iteratively find walls until not enough points remain
|
||||
while (unusedPoints.Count >= MIN_INLIERS)
|
||||
{
|
||||
Line bestLine = default;
|
||||
int bestInlierCount = 0;
|
||||
List<Vector3> bestInliers = [];
|
||||
|
||||
// RANSAC iterations
|
||||
for (int iter = 0; iter < RANSAC_ITERATIONS; iter++)
|
||||
{
|
||||
// Randomly select 2 points
|
||||
if (unusedPoints.Count < 2) break;
|
||||
|
||||
var idx1 = random.Next(unusedPoints.Count);
|
||||
var idx2 = random.Next(unusedPoints.Count);
|
||||
|
||||
if (idx1 == idx2) continue;
|
||||
|
||||
var p1 = unusedPoints[idx1];
|
||||
var p2 = unusedPoints[idx2];
|
||||
|
||||
// Skip if points are too close
|
||||
var dx = p2.X - p1.X;
|
||||
var dy = p2.Y - p1.Y;
|
||||
var dist = Math.Sqrt(dx * dx + dy * dy);
|
||||
if (dist < 0.1) continue; // Minimum 10cm distance
|
||||
|
||||
// Fit line through these 2 points
|
||||
var line = FitLineThroughPoints(p1, p2);
|
||||
|
||||
// Count inliers
|
||||
var inliers = new List<Vector3>();
|
||||
foreach (var point in unusedPoints)
|
||||
{
|
||||
if (line.DistanceToPoint(point) < RANSAC_INLIER_THRESHOLD)
|
||||
{
|
||||
inliers.Add(point);
|
||||
}
|
||||
}
|
||||
|
||||
// Update best model
|
||||
if (inliers.Count > bestInlierCount)
|
||||
{
|
||||
bestInlierCount = inliers.Count;
|
||||
bestInliers = inliers;
|
||||
bestLine = line;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we found a valid wall
|
||||
if (bestInlierCount < MIN_INLIERS)
|
||||
{
|
||||
break; // No more walls to find
|
||||
}
|
||||
|
||||
// Calculate wall length (distance between furthest inlier points)
|
||||
var (startPoint, endPoint, length) = CalculateWallExtent(bestInliers);
|
||||
|
||||
if (length < MIN_WALL_LENGTH)
|
||||
{
|
||||
// Wall too short, remove inliers and continue
|
||||
foreach (var inlier in bestInliers)
|
||||
{
|
||||
unusedPoints.Remove(inlier);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// Valid wall found
|
||||
walls.Add(new Wall
|
||||
{
|
||||
Line = bestLine,
|
||||
InlierCount = bestInlierCount,
|
||||
Length = length,
|
||||
StartPoint = startPoint,
|
||||
EndPoint = endPoint
|
||||
});
|
||||
|
||||
logger.LogDebug(
|
||||
"WallAlignment: Wall detected - Length: {Length:F2}m, Inliers: {Inliers}",
|
||||
length, bestInlierCount);
|
||||
|
||||
// Remove inliers from unused points
|
||||
foreach (var inlier in bestInliers)
|
||||
{
|
||||
unusedPoints.Remove(inlier);
|
||||
}
|
||||
}
|
||||
|
||||
return walls;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fit a line through two points using line equation: ax + by + c = 0
|
||||
/// where a^2 + b^2 = 1 (normalized)
|
||||
/// </summary>
|
||||
private static Line FitLineThroughPoints(Vector3 p1, Vector3 p2)
|
||||
{
|
||||
var dx = p2.X - p1.X;
|
||||
var dy = p2.Y - p1.Y;
|
||||
var length = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (length < 1e-6)
|
||||
{
|
||||
// Points are identical, return arbitrary line
|
||||
return new Line { A = 1, B = 0, C = -p1.X };
|
||||
}
|
||||
|
||||
// Normal to line: (dy, -dx) / length (perpendicular to direction vector)
|
||||
var a = dy / length;
|
||||
var b = -dx / length;
|
||||
var c = -(a * p1.X + b * p1.Y);
|
||||
|
||||
return new Line { A = a, B = b, C = c };
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate wall extent (start point, end point, and length)
|
||||
/// </summary>
|
||||
private static (Vector3 StartPoint, Vector3 EndPoint, double Length) CalculateWallExtent(
|
||||
List<Vector3> inliers)
|
||||
{
|
||||
if (inliers.Count < 2)
|
||||
{
|
||||
return (Vector3.Zero, Vector3.Zero, 0);
|
||||
}
|
||||
|
||||
// Find two points that are furthest apart
|
||||
var maxDist = 0.0;
|
||||
var startIdx = 0;
|
||||
var endIdx = 0;
|
||||
|
||||
for (int i = 0; i < inliers.Count; i++)
|
||||
{
|
||||
for (int j = i + 1; j < inliers.Count; j++)
|
||||
{
|
||||
var dx = inliers[j].X - inliers[i].X;
|
||||
var dy = inliers[j].Y - inliers[i].Y;
|
||||
var dist = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (dist > maxDist)
|
||||
{
|
||||
maxDist = dist;
|
||||
startIdx = i;
|
||||
endIdx = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (inliers[startIdx], inliers[endIdx], maxDist);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Angle Compensation
|
||||
|
||||
/// <summary>
|
||||
/// Calculate minimum rotation angle to align the wall with X or Y axis
|
||||
/// </summary>
|
||||
/// <param name="wallAngle">Wall angle in radians (relative to X-axis)</param>
|
||||
/// <returns>Compensation angle in radians</returns>
|
||||
private static double CalculateMinimumRotationToAxis(double wallAngle)
|
||||
{
|
||||
// Normalize angle to [-pi, pi]
|
||||
while (wallAngle > Math.PI) wallAngle -= 2 * Math.PI;
|
||||
while (wallAngle < -Math.PI) wallAngle += 2 * Math.PI;
|
||||
|
||||
// Calculate rotation needed for each axis
|
||||
// For X-axis: wall should be at 0° or ±180°
|
||||
// For Y-axis: wall should be at ±90°
|
||||
|
||||
var rotations = new[]
|
||||
{
|
||||
-wallAngle, // Align with X-axis (0°)
|
||||
Math.PI - wallAngle, // Align with X-axis (180°)
|
||||
-Math.PI - wallAngle, // Align with X-axis (-180°)
|
||||
Math.PI / 2 - wallAngle, // Align with Y-axis (90°)
|
||||
-Math.PI / 2 - wallAngle // Align with Y-axis (-90°)
|
||||
};
|
||||
|
||||
// Find the smallest absolute rotation
|
||||
var minRotation = rotations.OrderBy(Math.Abs).First();
|
||||
|
||||
// Normalize result to [-pi, pi]
|
||||
while (minRotation > Math.PI) minRotation -= 2 * Math.PI;
|
||||
while (minRotation < -Math.PI) minRotation += 2 * Math.PI;
|
||||
|
||||
return minRotation;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Quaternion Helpers
|
||||
|
||||
/// <summary>
|
||||
/// Create a quaternion from a yaw angle (rotation around Z-axis)
|
||||
/// </summary>
|
||||
public static Quaternion CreateQuaternionFromYaw(double yawRadians)
|
||||
{
|
||||
// Quaternion for rotation around Z-axis:
|
||||
// q = [0, 0, sin(yaw/2), cos(yaw/2)]
|
||||
var halfYaw = yawRadians / 2.0;
|
||||
return new Quaternion(
|
||||
x: 0,
|
||||
y: 0,
|
||||
z: Math.Sin(halfYaw),
|
||||
w: Math.Cos(halfYaw)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Extract yaw angle from a quaternion
|
||||
/// </summary>
|
||||
public static double GetYawFromQuaternion(Quaternion q)
|
||||
{
|
||||
// Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
|
||||
return Math.Atan2(
|
||||
2.0 * (q.W * q.Z + q.X * q.Y),
|
||||
1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Combine current robot yaw with compensation angle
|
||||
/// </summary>
|
||||
public static Quaternion ApplyCompensation(Quaternion currentOrientation, double compensationAngle)
|
||||
{
|
||||
var currentYaw = GetYawFromQuaternion(currentOrientation);
|
||||
var newYaw = currentYaw + compensationAngle;
|
||||
return CreateQuaternionFromYaw(newYaw);
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
Reference in New Issue
Block a user