567 lines
26 KiB
C#
567 lines
26 KiB
C#
using System.Diagnostics;
|
|
using CartographerSharp.Mapping;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Sensor;
|
|
using RobotNet10.RobotApp.Shared;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Localization;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
|
|
|
public partial class CartographerService
|
|
{
|
|
#region Sensor Data Callbacks
|
|
|
|
private void OnAddRangeData(string deviceId, RangeDataPayload payload)
|
|
{
|
|
// Get current state and relevant objects
|
|
var currentState = State;
|
|
var tb = _trajectoryBuilder;
|
|
bool mclRunning = _mclProcessor?.IsRunning ?? false;
|
|
MclService? mcl = _mcl;
|
|
|
|
// Handle data based on current state
|
|
switch (currentState)
|
|
{
|
|
case SLAMState.Relocalizing:
|
|
// InitializingLocalizing: MCL is computing initial pose
|
|
// - If MCL enabled and running: Process with MCL (no trajectory yet)
|
|
// - If MCL disabled: Trajectory already added, process with CartographerSharp
|
|
if (mclRunning && mcl != null && _mclProcessor != null)
|
|
{
|
|
// MCL path: Feed scan to MCL from primary lidar only
|
|
string? primaryLidarId = _mclProcessor.PrimaryLidarId;
|
|
if (string.IsNullOrEmpty(primaryLidarId) || deviceId == primaryLidarId)
|
|
{
|
|
try
|
|
{
|
|
RunMclOnScan(payload, mcl);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerService: OnAddRangeData MCL failed for {DeviceId}", deviceId);
|
|
}
|
|
}
|
|
}
|
|
else if (tb != null)
|
|
{
|
|
// Non-MCL path: Trajectory already added in StartLocalizationAsync, process with CartographerSharp
|
|
ProcessWithTrajectoryBuilder(deviceId, payload, tb);
|
|
}
|
|
break;
|
|
|
|
case SLAMState.Localizing:
|
|
// Localizing: Use CartographerSharp for localization
|
|
if (tb != null)
|
|
{
|
|
ProcessWithTrajectoryBuilder(deviceId, payload, tb);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("CartographerService.OnAddRangeData: Localizing but _trajectoryBuilder is null!");
|
|
}
|
|
break;
|
|
|
|
case SLAMState.ScanMapping:
|
|
// ScanMapping: Use CartographerSharp for SLAM
|
|
if (tb != null)
|
|
{
|
|
ProcessWithTrajectoryBuilder(deviceId, payload, tb);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
// Other states (Idle, Ready, SavingMap, Error): Ignore sensor data
|
|
break;
|
|
}
|
|
}
|
|
|
|
private void OnAddImuData(string deviceId, ImuData data) => _trajectoryBuilder?.AddSensorData(deviceId, data);
|
|
|
|
private void OnAddOdometryData(string sensorId, OdometryData data)
|
|
{
|
|
// Feed to trajectory builder (existing)
|
|
_trajectoryBuilder?.AddSensorData(sensorId, data);
|
|
|
|
// Feed to MCL when running (xloc flow: mcl_->CallOdomCallback continuously)
|
|
if (_mclProcessor?.IsRunning ?? false)
|
|
{
|
|
_mclProcessor.ProcessOdometry(data);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Trajectory Builder Processing
|
|
|
|
/// <summary>
|
|
/// Process range data with trajectory builder (CartographerSharp)
|
|
/// Used for both Localizing and ScanMapping states
|
|
/// </summary>
|
|
private void ProcessWithTrajectoryBuilder(string deviceId, RangeDataPayload payload, ITrajectoryBuilder tb)
|
|
{
|
|
try
|
|
{
|
|
var result = tb.AddSensorData(deviceId, payload.BaseLink);
|
|
|
|
// Skip if disposed to prevent issues during shutdown
|
|
if (_disposed)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Get current state and pose for operations that don't depend on result.HasValue
|
|
var currentState = State;
|
|
var currentPose = Volatile.Read(ref _poseSnapshot).Pose;
|
|
|
|
// === SAMPLE POINT CLOUD (does NOT depend on hasValue) ===
|
|
// Process sample point cloud even when result.HasValue is false
|
|
// Use SamplePointCloudGlobal from result if available, otherwise transform raw payload to global frame
|
|
if (_pointCloudUpdateStopwatch.ElapsedMilliseconds >= 1000)
|
|
{
|
|
// Capture variables for Task.Run closure
|
|
var capturedPayload = payload;
|
|
var capturedPose = currentPose;
|
|
var capturedResult = result;
|
|
var logger = _logger;
|
|
|
|
// Move ALL calculation into Task.Run to avoid blocking sensor thread
|
|
_ = Task.Run(() =>
|
|
{
|
|
List<Vector3>? pointsToStore = null;
|
|
|
|
// Check if result has SamplePointCloudGlobal (preferred)
|
|
if (capturedResult.HasValue && capturedResult.Value.SamplePointCloudGlobal is { } pcFromResult)
|
|
{
|
|
// Use CartographerSharp's sample point cloud (already in global frame)
|
|
pointsToStore = new List<Vector3>(pcFromResult.Count);
|
|
foreach (var p in pcFromResult.Points)
|
|
pointsToStore.Add(new Vector3(p.Position.X, p.Position.Y, p.Position.Z));
|
|
|
|
if (pointsToStore != null && pointsToStore.Count > 0)
|
|
{
|
|
lock (_pointCloudLock)
|
|
{
|
|
_completedAccumulatedSamplePointClouds = pointsToStore;
|
|
}
|
|
}
|
|
|
|
_pointCloudUpdateStopwatch.Restart();
|
|
}
|
|
});
|
|
}
|
|
|
|
// === POSE SYNC (does NOT depend on hasValue) ===
|
|
// Periodic pose sync to file (for auto-resume on next startup)
|
|
var syncInterval = _config.MapStorage.PoseSyncIntervalSeconds;
|
|
if (syncInterval > 0 && _poseSyncStopwatch.Elapsed.TotalSeconds >= syncInterval)
|
|
{
|
|
var mapName = _currentMapName;
|
|
if (!string.IsNullOrEmpty(mapName) && (currentState == SLAMState.Localizing || currentState == SLAMState.ScanMapping))
|
|
{
|
|
// Use cached global-frame pose from snapshot
|
|
SavePoseToFile(mapName, currentPose);
|
|
_poseSyncStopwatch.Restart();
|
|
}
|
|
}
|
|
|
|
// === CARTOGRAPHER RESULT PROCESSING (DEPENDS on hasValue) ===
|
|
// Skip result processing if no valid result
|
|
if (!result.HasValue)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var r = result.Value;
|
|
|
|
// Process based on mode
|
|
if (currentState == SLAMState.ScanMapping)
|
|
{
|
|
// Store matching score for pose thread (volatile write, no lock needed)
|
|
Volatile.Write(ref _lastMatchingScore, r.PoseConfidence);
|
|
|
|
// Process scan mapping result (only if processor is available)
|
|
_slamResultProcessor?.ProcessScanMappingResult(r, currentState, out _);
|
|
|
|
// Signal insertion to OccupancyGridManager when a scan was inserted into submap.
|
|
// This sets a flag that ShouldUpdateGrid() will check.
|
|
if (r.InsertionResult.HasValue)
|
|
{
|
|
_occupancyGridManager.SignalInsertion();
|
|
}
|
|
|
|
// Update occupancy grid when BOTH conditions are met:
|
|
// 1. At least 3 seconds have elapsed since last update
|
|
// 2. At least one scan was inserted since last update (SignalInsertion was called)
|
|
if (_mapBuilder != null && _occupancyGridManager.ShouldUpdateGrid())
|
|
{
|
|
var mapBuilder = _mapBuilder;
|
|
_ = Task.Run(() => _occupancyGridManager.UpdateFromMapBuilder(mapBuilder));
|
|
}
|
|
}
|
|
else if (currentState == SLAMState.Localizing)
|
|
{
|
|
// Store scan match score for drift detection (same as ScanMapping but also used in LocalizationScore)
|
|
// This is critical for detecting map drift during localization
|
|
Volatile.Write(ref _lastMatchingScore, r.PoseConfidence);
|
|
|
|
// Get cached covariance data
|
|
var cachedCovariance = _slamResultProcessor?.PoseCovariance;
|
|
var cachedConstraintCount = _slamResultProcessor?.ConstraintCount ?? 0;
|
|
var cachedAverageQuality = _slamResultProcessor?.AverageConstraintQuality ?? 0.0;
|
|
|
|
// Update confidence with scan match score (pose is handled by background thread at 100Hz)
|
|
UpdateConfidenceMetrics(cachedCovariance, cachedConstraintCount, cachedAverageQuality, r.PoseConfidence);
|
|
|
|
// Calculate covariance asynchronously (throttled: skip if already computing)
|
|
IMapBuilder? mbSnapshot;
|
|
int trajIdSnapshot;
|
|
lock (_lock)
|
|
{
|
|
mbSnapshot = _mapBuilder;
|
|
trajIdSnapshot = _trajectoryId;
|
|
}
|
|
if (mbSnapshot != null && Interlocked.CompareExchange(ref _covarianceComputing, 1, 0) == 0)
|
|
{
|
|
_ = Task.Run(() =>
|
|
{
|
|
try
|
|
{
|
|
var constraints = _constraintCache.GetOrUpdate(mbSnapshot, trajIdSnapshot, _logger);
|
|
|
|
CovarianceCalculator.Result covResult;
|
|
try
|
|
{
|
|
covResult = constraints != null
|
|
? CovarianceCalculator.Calculate(constraints)
|
|
: new CovarianceCalculator.Result(null, 0, 0.0);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "CartographerService: Failed to calculate covariance from constraints");
|
|
covResult = new CovarianceCalculator.Result(null, 0, 0.0);
|
|
}
|
|
|
|
_slamResultProcessor?.UpdateCovariance(covResult.Covariance, covResult.ConstraintCount, covResult.AverageConstraintQuality);
|
|
|
|
lock (_lock)
|
|
{
|
|
_poseCovariance = covResult.Covariance;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "CartographerService: Error calculating covariance asynchronously");
|
|
}
|
|
finally
|
|
{
|
|
Interlocked.Exchange(ref _covarianceComputing, 0);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogDebug(ex, "CartographerService: ProcessWithTrajectoryBuilder failed for {DeviceId}", deviceId);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region MCL Scan Processing
|
|
|
|
/// <summary>Runs MCL with one scan from the primary lidar. Convergence logic matches xloc MCLThread: pose stable (dist+yaw) + stable_duration + reliability/MAE, or timeout.</summary>
|
|
private void RunMclOnScan(RangeDataPayload payload, MclService mcl)
|
|
{
|
|
// Base_link to laser was set at MCL start (SetMclBaseLinkToLaser). SensorPipeline always provides SensorFrame; use it for MCL (beam angles in sensor frame).
|
|
var ranges = payload.SensorFrame.Ranges;
|
|
_mclScanBuffer ??= new List<Vector2>(ranges.Count);
|
|
_mclScanBuffer.Clear();
|
|
foreach (var r in ranges)
|
|
_mclScanBuffer.Add(new Vector2(r.Position.X, r.Position.Y));
|
|
var pointsForScan = _mclScanBuffer;
|
|
|
|
// Use actual angle information from filtered point cloud (SensorPipeline provides accurate angles after filtering)
|
|
// Convert from radians to degrees for MclScanHelper.ConvertToScan
|
|
double angleMinDeg = payload.ActualAngleMin * (180f / Math.PI);
|
|
double angleMaxDeg = payload.ActualAngleMax * (180f / Math.PI);
|
|
|
|
// Use actual number of points from scan data
|
|
int numBins = pointsForScan.Count;
|
|
|
|
double rangeMin = _config.TrajectoryBuilder.MinRange;
|
|
double rangeMax = _config.TrajectoryBuilder.MaxRange;
|
|
var (angleMin, angleMax, angleIncrement, _, _, mclRanges) = MclScanHelper.ConvertToScan(pointsForScan, angleMinDeg, angleMaxDeg, numBins, rangeMin, rangeMax);
|
|
|
|
mcl.OnScan(angleMin, angleMax, angleIncrement, rangeMin, rangeMax, mclRanges);
|
|
|
|
const int iterationsPerScan = 5;
|
|
for (int i = 0; i < iterationsPerScan; i++)
|
|
mcl.RunOneIteration();
|
|
|
|
Pose currentPose = mcl.GetPose();
|
|
double reliability = mcl.Reliability;
|
|
double? mae = mcl.MaeForBestParticle;
|
|
double totalLikelihood = mcl.TotalLikelihood;
|
|
|
|
// Use MclProcessor to check convergence with reason
|
|
var convergenceReason = _mclProcessor?.CheckConvergenceWithReason(currentPose, reliability, mae, out var _)
|
|
?? MclConvergenceReason.NotConverged;
|
|
|
|
// Note: OnPoseUpdated is called in all paths below (converged/not-converged),
|
|
// which publishes pose to _poseSnapshot with MCL reliability and MAE for proper score calculation.
|
|
|
|
if (convergenceReason != MclConvergenceReason.NotConverged)
|
|
{
|
|
_mclProcessor?.Stop();
|
|
|
|
// Disable global localization mode after convergence to switch to normal tracking mode
|
|
mcl.DisableGlobalLocalizationMode();
|
|
|
|
if (_mapBuilder == null)
|
|
{
|
|
_logger.LogWarning("CartographerService: MCL converged but MapBuilder is null; trajectory not added.");
|
|
return;
|
|
}
|
|
|
|
// Verify no active trajectory before adding new one (defensive check)
|
|
int currentTrajId;
|
|
lock (_lock)
|
|
{
|
|
currentTrajId = _trajectoryId;
|
|
}
|
|
|
|
if (currentTrajId != -1)
|
|
{
|
|
_logger.LogWarning("CartographerService: MCL converged but active trajectory {TrajectoryId} exists. " +
|
|
"Waiting for trajectory to finish before adding new one.", currentTrajId);
|
|
|
|
// Don't add trajectory yet, MCL will check again on next scan
|
|
// This should never happen if first defensive check is working, but being defensive
|
|
OnPoseUpdated(currentPose, null, mclReliability: reliability, mclMae: mae);
|
|
return;
|
|
}
|
|
|
|
// Determine which pose to use for trajectory:
|
|
// - PoseStable: MCL converged successfully, use MCL's current pose
|
|
// - Timeout/MaxIterations: MCL failed to converge, fall back to initial pose
|
|
Pose poseForTrajectory;
|
|
if (convergenceReason == MclConvergenceReason.PoseStable)
|
|
{
|
|
poseForTrajectory = currentPose;
|
|
_logger.LogInformation("CartographerService: MCL converged successfully, using MCL pose ({X:F3}, {Y:F3})",
|
|
currentPose.Position.X, currentPose.Position.Y);
|
|
}
|
|
else
|
|
{
|
|
// Timeout or MaxIterations - use initial pose (saved from auto-resume)
|
|
poseForTrajectory = _mclProcessor?.InitialPose ?? currentPose;
|
|
_logger.LogWarning("CartographerService: MCL did not converge (reason={Reason}), falling back to initial pose ({X:F3}, {Y:F3})",
|
|
convergenceReason, poseForTrajectory.Position.X, poseForTrajectory.Position.Y);
|
|
}
|
|
|
|
var (trajId, trajBuilder, poseInMapFrame) = _mapBuilder.AddLocalizationTrajectoryBuilder(_config, poseForTrajectory);
|
|
lock (_lock)
|
|
{
|
|
_trajectoryId = trajId;
|
|
_trajectoryBuilder = trajBuilder;
|
|
}
|
|
|
|
if (poseInMapFrame.HasValue)
|
|
{
|
|
var p = PoseConverter.ToPose(poseInMapFrame.Value);
|
|
_logger.LogDebug("CartographerService: AddLocalizationTrajectoryBuilder trajId: {TrajId}, poseInMapFrame: [{X}, {Y}, {Z}] yaw: {Yaw:F4} ({YawDeg:F2})",
|
|
trajId, poseInMapFrame.Value.Translation.X, poseInMapFrame.Value.Translation.Y, poseInMapFrame.Value.Translation.Z, GetYawFromPose(p), GetYawFromPose(p) * 180.0 / Math.PI);
|
|
// NOTE: Don't call StartRelocalization here - MCL already found the pose.
|
|
// StartRelocalization triggers constraint-based relocalization which is redundant
|
|
// and causes excessive optimization runs (sets _isRelocalized=false).
|
|
}
|
|
|
|
// Pass pose and MCL reliability/MAE for proper score calculation
|
|
// Use poseForTrajectory to be consistent with what we're using for the trajectory
|
|
OnPoseUpdated(poseForTrajectory, null, mclReliability: reliability, mclMae: mae);
|
|
|
|
// Use deferred trigger to avoid deadlock: FireStateMachine would call PauseSubscriptions
|
|
// which tries to stop the lidar thread, but we're currently running inside that thread.
|
|
// QueueStateMachineTrigger fires the trigger from a separate thread.
|
|
QueueStateMachineTrigger(CartographerTrigger.MclConverged);
|
|
}
|
|
else
|
|
{
|
|
// Pass MCL reliability and MAE for proper score calculation during convergence
|
|
OnPoseUpdated(currentPose, null, mclReliability: reliability, mclMae: mae);
|
|
}
|
|
}
|
|
|
|
/// <summary>Sets MCL base_link to laser transform from primary lidar config. Call once when starting MCL (after SetMap and SetInitialPose).</summary>
|
|
/// <remarks>Config Transform is "lidar frame to base_link frame": P_base = R*P_lidar + Translation, so Translation = lidar origin in base_link (sensor pose in base_link). MCL expects baseLink2Laser = sensor pose in base_link (same meaning), so we pass (tx, ty, yaw) as-is; no inverse needed.</remarks>
|
|
private void SetMclBaseLinkToLaser(string primaryLidarId, MclService mcl)
|
|
{
|
|
var lidarConfig = string.IsNullOrEmpty(primaryLidarId) ? null : _config.Sensors.Lidars.FirstOrDefault(l => l.DeviceId == primaryLidarId);
|
|
if (lidarConfig?.Transform == null)
|
|
return;
|
|
var lidarTransform = lidarConfig.Transform;
|
|
double tx = lidarTransform.Translation.X;
|
|
double ty = lidarTransform.Translation.Y;
|
|
double yawLidarInBase = lidarTransform.Rotation.ToYawRadian();
|
|
mcl.SetBaseLinkToLaser(tx, ty, yawLidarInBase);
|
|
}
|
|
|
|
/// <summary>Resolves effective primary lidar for MCL (PrimaryLidarId if present in config, else first lidar). Logs and validates; returns empty if no lidars.</summary>
|
|
private string GetEffectiveMclPrimaryLidarIdAndLog()
|
|
{
|
|
var lidars = _config.Sensors.Lidars;
|
|
string? configured = string.IsNullOrEmpty(_config.Mcl.PrimaryLidarId) ? null : _config.Mcl.PrimaryLidarId;
|
|
if (lidars.Count == 0)
|
|
{
|
|
if (!string.IsNullOrEmpty(configured))
|
|
_logger.LogWarning("CartographerService: MCL PrimaryLidarId is set to {PrimaryLidarId} but Sensors.Lidars is empty; MCL will accept any device.", configured);
|
|
return string.Empty;
|
|
}
|
|
if (!string.IsNullOrEmpty(configured))
|
|
{
|
|
var found = lidars.Any(l => l.DeviceId == configured);
|
|
if (found)
|
|
{
|
|
_logger.LogInformation("CartographerService: MCL using primary lidar: {PrimaryLidarId}", configured);
|
|
return configured;
|
|
}
|
|
_logger.LogWarning(
|
|
"CartographerService: MCL PrimaryLidarId '{PrimaryLidarId}' not found in Sensors.Lidars; using first lidar: {FirstId}",
|
|
configured, lidars[0].DeviceId);
|
|
}
|
|
var firstId = lidars[0].DeviceId ?? string.Empty;
|
|
if (string.IsNullOrEmpty(configured))
|
|
_logger.LogInformation("CartographerService: MCL using first lidar: {FirstId}", firstId);
|
|
return firstId;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Pose Update
|
|
|
|
/// <summary>
|
|
/// Updates pose snapshot and confidence metrics.
|
|
/// Used by MCL (Relocalizing) and state entry handlers when no background pose thread is running.
|
|
/// When background pose thread IS running (ScanMapping/Localizing), use UpdateConfidenceMetrics instead.
|
|
/// </summary>
|
|
/// <param name="pose">Current pose in global frame</param>
|
|
/// <param name="covariance">Pose covariance (optional)</param>
|
|
/// <param name="constraintCount">Number of constraints (default 0)</param>
|
|
/// <param name="constraintQuality">Average constraint quality 0-1 (default 0)</param>
|
|
/// <param name="mclReliability">MCL reliability 0-1 (optional, for Relocalizing state)</param>
|
|
/// <param name="mclMae">MCL mean absolute error in meters (optional, for Relocalizing state)</param>
|
|
private void OnPoseUpdated(
|
|
Pose pose,
|
|
Matrix3x3? covariance,
|
|
int constraintCount = 0,
|
|
double constraintQuality = 0.0,
|
|
double? mclReliability = null,
|
|
double? mclMae = null)
|
|
{
|
|
// Publish pose snapshot (atomic volatile write)
|
|
// For Relocalizing: use MCL reliability and MAE for score calculation
|
|
var score = LocalizationScoreCalculator.Calculate(
|
|
covariance,
|
|
constraintCount,
|
|
constraintQuality,
|
|
mclReliability: mclReliability,
|
|
mclMae: mclMae);
|
|
|
|
// Determine drift status based on MCL reliability
|
|
var driftStatus = DriftDetector.DriftStatus.Stable;
|
|
if (mclReliability.HasValue)
|
|
{
|
|
if (mclReliability.Value < 0.3)
|
|
driftStatus = DriftDetector.DriftStatus.Critical;
|
|
else if (mclReliability.Value < 0.5)
|
|
driftStatus = DriftDetector.DriftStatus.Warning;
|
|
}
|
|
|
|
Volatile.Write(ref _poseSnapshot, new PoseSnapshot(
|
|
pose,
|
|
covariance,
|
|
score,
|
|
scanMatchScore: null, // Not available during Relocalizing
|
|
driftStatus));
|
|
|
|
// Also update covariance fields for when pose thread takes over
|
|
lock (_lock)
|
|
{
|
|
_poseCovariance = covariance;
|
|
_constraintCount = constraintCount;
|
|
_averageConstraintQuality = constraintQuality;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Updates only confidence metrics (covariance, constraints) without touching pose.
|
|
/// Used by sensor callback during ScanMapping/Localizing when background pose thread handles pose.
|
|
/// </summary>
|
|
private void UpdateConfidenceMetrics(Matrix3x3? covariance, int constraintCount, double constraintQuality)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_poseCovariance = covariance;
|
|
_constraintCount = constraintCount;
|
|
_averageConstraintQuality = constraintQuality;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Updates confidence metrics including scan match score for Localizing state.
|
|
/// Scan match score is critical for detecting map drift.
|
|
/// </summary>
|
|
private void UpdateConfidenceMetrics(Matrix3x3? covariance, int constraintCount, double constraintQuality, double scanMatchScore)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_poseCovariance = covariance;
|
|
_constraintCount = constraintCount;
|
|
_averageConstraintQuality = constraintQuality;
|
|
}
|
|
// Scan match score is stored via volatile write (already done before calling this method)
|
|
// It will be read by background pose thread for LocalizationScore calculation
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform raw payload points to global frame using the given pose.
|
|
/// Used as fallback when CartographerSharp doesn't return SamplePointCloudGlobal.
|
|
/// </summary>
|
|
private static List<Vector3> TransformPayloadToGlobalFrame(RangeDataPayload payload, Pose pose)
|
|
{
|
|
var yaw = GetYawFromPose(pose);
|
|
var cosYaw = Math.Cos(yaw);
|
|
var sinYaw = Math.Sin(yaw);
|
|
var poseX = pose.Position.X;
|
|
var poseY = pose.Position.Y;
|
|
|
|
var result = new List<Vector3>(payload.BaseLink.Ranges.Count);
|
|
foreach (var range in payload.BaseLink.Ranges)
|
|
{
|
|
// Transform from base_link frame to global frame
|
|
var localX = range.Position.X;
|
|
var localY = range.Position.Y;
|
|
var globalX = poseX + localX * cosYaw - localY * sinYaw;
|
|
var globalY = poseY + localX * sinYaw + localY * cosYaw;
|
|
result.Add(new Vector3(globalX, globalY, 0));
|
|
}
|
|
return result;
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
#endregion
|
|
}
|