128 lines
7.8 KiB
C#
128 lines
7.8 KiB
C#
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
|
|
|
/// <summary>
|
|
/// MCL configuration (from mcl.yaml). Used when SetInitialPoseAsync runs MCL before adding localization trajectory.
|
|
/// </summary>
|
|
public class MclConfiguration
|
|
{
|
|
public bool Enabled { get; set; }
|
|
public double InitialNoiseX { get; set; } = 1.0;
|
|
public double InitialNoiseY { get; set; } = 1.0;
|
|
public double InitialNoiseYaw { get; set; } = 0.3;
|
|
/// <summary>When user provides initial pose (SetInitialPoseAsync), use these smaller noise values so particles stay near the pose and iteration 1 does not jump far. If zero, fall back to InitialNoiseX/Y/Yaw.</summary>
|
|
public double InitialNoiseWhenPoseGivenX { get; set; } = 0.15;
|
|
public double InitialNoiseWhenPoseGivenY { get; set; } = 0.15;
|
|
public double InitialNoiseWhenPoseGivenYaw { get; set; } = 0.05;
|
|
public int ParticlesNum { get; set; } = 1000;
|
|
public bool UseAugmentedMcl { get; set; }
|
|
public bool AddRandomParticlesInResampling { get; set; } = true;
|
|
public double RandomParticlesRate { get; set; } = 0.1;
|
|
public double[] RandomParticlesNoise { get; set; } = [0.05f, 0.05f, 0.1];
|
|
public double[] OdomNoiseDdm { get; set; } = [1.0, 0.5, 0.5, 1.5];
|
|
/// <summary>Odometry noise for omni-directional model (9 params: xx, xy, xyaw, yx, yy, yyaw, yawx, yawy, yawyaw). Used when UseOmniDirectionalModel=true.</summary>
|
|
public double[] OdomNoiseOdm { get; set; } = [4.0, 1.0, 1.0, 1.0, 4.0, 1.0, 1.0, 1.0, 8.0];
|
|
public bool UseOmniDirectionalModel { get; set; }
|
|
public int MeasurementModelType { get; set; } = 0;
|
|
/// <summary>Scan step for likelihood calculation (xloc: scanStep_ = 10). Use every Nth beam to avoid underflow with many beams. 1 = use all beams (slow, underflow risk), 10 = every 10th beam (recommended).</summary>
|
|
public int ScanStep { get; set; } = 10;
|
|
public double ZHit { get; set; } = 0.9;
|
|
public double ZShort { get; set; } = 0.2;
|
|
public double ZMax { get; set; } = 0.05;
|
|
public double ZRand { get; set; } = 0.05;
|
|
public double VarHit { get; set; } = 0.08;
|
|
public double LambdaShort { get; set; } = 1.0;
|
|
public double LambdaUnknown { get; set; } = 0.01;
|
|
/// <summary>Prior probability for known obstacles (class-conditional model). Default: 0.5. xloc: pKnownPrior_.</summary>
|
|
public double KnownClassPrior { get; set; } = 0.5;
|
|
/// <summary>Computed: Prior probability for known obstacles (same as KnownClassPrior, for C# code compatibility).</summary>
|
|
public double PKnownPrior => KnownClassPrior;
|
|
/// <summary>Computed: Prior probability for unknown obstacles (1 - KnownClassPrior).</summary>
|
|
public double PUnknownPrior => 1.0 - KnownClassPrior;
|
|
public double UnknownScanProbThreshold { get; set; } = 0.9;
|
|
public double AlphaSlow { get; set; } = 0.001;
|
|
public double AlphaFast { get; set; } = 0.99;
|
|
public bool RejectUnknownScan { get; set; } = true;
|
|
public double ResampleThresholdEss { get; set; } = 0.5;
|
|
public double[] ResampleThresholds { get; set; } = [0.2, 0.2, 0.2, 0.02f, -99999.0];
|
|
/// <summary>Reliability transition decay for differential drive (2 params: dist_coeff, yaw_coeff). Used when UseOmniDirectionalModel=false.</summary>
|
|
public double[] RelTransDdm { get; set; } = [0.0, 0.0];
|
|
/// <summary>Reliability transition decay for omni-directional (3 params: x_coeff, y_coeff, yaw_coeff). Used when UseOmniDirectionalModel=true.</summary>
|
|
public double[] RelTransOdm { get; set; } = [0.0, 0.0, 0.0];
|
|
public int ClassifierType { get; set; }
|
|
/// <summary>Estimate reliability per particle (MAE-based); used by decision model (xloc: estimateReliability_).</summary>
|
|
public bool EstimateReliability { get; set; }
|
|
/// <summary>MAE failure threshold in meters for simple decision model when classifier files not used (xloc: ~0.12).</summary>
|
|
public double FailureThreshold { get; set; } = 0.12;
|
|
/// <summary>Use global-localization pose sampler (merge external poses as extra particles).</summary>
|
|
public bool UseGLPoseSampler { get; set; }
|
|
/// <summary>Max time diff (sec) between scan and GL poses to fuse (xloc: glSampledPoseTimeTH_).</summary>
|
|
public double GLSampledPoseTimeTH { get; set; } = 0.5;
|
|
/// <summary>GMM positional variance for GL predictive likelihood (xloc: gmmPositionalVariance_).</summary>
|
|
public double GmmPositionalVariance { get; set; } = 0.1;
|
|
/// <summary>GMM angular variance for GL predictive likelihood (xloc: gmmAngularVariance_).</summary>
|
|
public double GmmAngularVariance { get; set; } = 0.1;
|
|
/// <summary>Uniform rate in predictive distribution for GL (xloc: predDistUnifRate_).</summary>
|
|
public double PredDistUnifRate { get; set; } = 0.05;
|
|
/// <summary>Pose change below this (meters) for convergence (xloc: pose_change_threshold 0.03).</summary>
|
|
public double ConvergencePoseChangeThresholdMeters { get; set; } = 0.03;
|
|
/// <summary>Yaw change below this (radians) for convergence (xloc: yaw_change_threshold 0.05).</summary>
|
|
public double ConvergenceYawChangeThresholdRad { get; set; } = 0.05;
|
|
/// <summary>Pose must stay stable for this duration (seconds) before convergence (xloc: stable_duration 0.3).</summary>
|
|
public double ConvergenceStableDurationSeconds { get; set; } = 0.3;
|
|
/// <summary>Min reliability [0,1] to allow pose-stable convergence (xloc: reliability_ >= 0.9).</summary>
|
|
public double ConvergenceReliabilityMin { get; set; } = 0.9;
|
|
/// <summary>Max MAE (meters) for best particle to allow pose-stable convergence (xloc: mae <= 0.12).</summary>
|
|
public double ConvergenceMaeMaxMeters { get; set; } = 0.12;
|
|
/// <summary>Timeout in seconds; after this, force convergence with initial pose (for Relocalizing state auto-resume fallback).</summary>
|
|
public double ConvergenceTimeoutSeconds { get; set; } = 30.0;
|
|
/// <summary>
|
|
/// Max MCL iterations before forcing convergence.
|
|
/// Note: C# does 5 iterations per scan. At 10Hz lidar = 50 iter/sec.
|
|
/// 1500 iterations = 30 seconds at 10Hz, 60 seconds at 5Hz (matches ConvergenceTimeoutSeconds).
|
|
/// </summary>
|
|
public int ConvergenceMaxIterations { get; set; } = 1500;
|
|
/// <summary>
|
|
/// Min MCL iterations before allowing convergence (at least 2 scans at 10Hz).
|
|
/// </summary>
|
|
public int ConvergenceMinIterations { get; set; } = 10;
|
|
/// <summary>
|
|
/// DeviceId of the lidar used for MCL scan (xloc uses a single "scan" topic).
|
|
/// If null or empty, the first lidar in Sensors.Lidars is used.
|
|
/// When multiple lidars exist, only this lidar's range data is passed to MCL; others are ignored during MCL phase.
|
|
/// </summary>
|
|
public string? PrimaryLidarId { get; set; }
|
|
|
|
/// <summary>
|
|
/// MCL reliability monitoring configuration for Localizing state
|
|
/// </summary>
|
|
public MclReliabilityMonitoringConfiguration ReliabilityMonitoring { get; set; } = new();
|
|
}
|
|
|
|
/// <summary>
|
|
/// MCL reliability monitoring configuration for Localizing state
|
|
/// Runs MCL periodically (every 2 seconds by default) to provide reliability and MAE metrics
|
|
/// </summary>
|
|
public class MclReliabilityMonitoringConfiguration
|
|
{
|
|
/// <summary>
|
|
/// Enable MCL reliability monitoring during Localizing state
|
|
/// </summary>
|
|
public bool EnableReliabilityMonitoring { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Interval between MCL updates (seconds)
|
|
/// </summary>
|
|
public double MonitoringIntervalSeconds { get; set; } = 2.0;
|
|
|
|
/// <summary>
|
|
/// Number of particles for monitoring (less than initialization for performance)
|
|
/// Recommended: 500 (vs 1000 for initialization)
|
|
/// </summary>
|
|
public int MonitoringParticlesNum { get; set; } = 500;
|
|
|
|
/// <summary>
|
|
/// Number of MCL iterations per monitoring cycle
|
|
/// </summary>
|
|
public int MonitoringIterationsPerCycle { get; set; } = 5;
|
|
}
|