Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Configuration/MclConfiguration.cs

@@ -0,0 +1,127 @@
+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;
+}