Initial commit
This commit is contained in:
@@ -0,0 +1,89 @@
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
/// <summary>
|
||||
/// Map builder configuration
|
||||
/// </summary>
|
||||
public class MapBuilderConfiguration
|
||||
{
|
||||
public bool? UseTrajectoryBuilder2D { get; set; }
|
||||
public bool? UseTrajectoryBuilder3D { get; set; }
|
||||
public int NumBackgroundThreads { get; set; } = 4;
|
||||
public int OptimizeEveryNNodes { get; set; } = 90;
|
||||
public double MatcherTranslationWeight { get; set; } = 5e2;
|
||||
public double MatcherRotationWeight { get; set; } = 1.6e3;
|
||||
public int? MaxNumFinalIterations { get; set; }
|
||||
public double? GlobalSamplingRatio { get; set; }
|
||||
public bool? LogResidualHistograms { get; set; }
|
||||
public double? GlobalConstraintSearchAfterNSeconds { get; set; }
|
||||
public bool EnableSingleTrajectoryLoopClosure { get; set; } = true;
|
||||
public double SingleTrajectoryLoopClosureDistanceThreshold { get; set; } = 3.0;
|
||||
public OptimizationProblemOptions PoseGraphOptimizationProblemOptions { get; set; } = new();
|
||||
public ConstraintBuilderOptionsConfiguration? ConstraintBuilderOptions { get; set; }
|
||||
public OverlappingSubmapsTrimmerOptions2DConfiguration? OverlappingSubmapsTrimmer2D { get; set; }
|
||||
public bool CollateByTrajectory { get; set; } = false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Optimization problem options
|
||||
/// </summary>
|
||||
public class OptimizationProblemOptions
|
||||
{
|
||||
public double HuberScale { get; set; } = 1e1;
|
||||
public double OdometryHuberScale { get; set; } = 1e1;
|
||||
public double LocalSlamPoseHuberScale { get; set; } = 1e1;
|
||||
public double AccelerationWeight { get; set; } = 1.1e2;
|
||||
public double RotationWeight { get; set; } = 1.6e4;
|
||||
public double LocalSlamPoseTranslationWeight { get; set; } = 1e5;
|
||||
public double LocalSlamPoseRotationWeight { get; set; } = 1e5;
|
||||
public double OdometryTranslationWeight { get; set; } = 1e5;
|
||||
public double OdometryRotationWeight { get; set; } = 1e5;
|
||||
public double FixedFramePoseTranslationWeight { get; set; } = 1e1;
|
||||
public double FixedFramePoseRotationWeight { get; set; } = 1e2;
|
||||
public bool FixedFramePoseUseTolerantLoss { get; set; } = false;
|
||||
public double FixedFramePoseTolerantLossParamA { get; set; } = 1.0;
|
||||
public double FixedFramePoseTolerantLossParamB { get; set; } = 1.0;
|
||||
public bool LogSolverSummary { get; set; } = false;
|
||||
public int MaxNumIterations { get; set; } = 50;
|
||||
public CeresSolverOptionsConfiguration? CeresSolverOptions { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Map storage configuration
|
||||
/// </summary>
|
||||
public class MapStorageConfiguration
|
||||
{
|
||||
public string Directory { get; set; } = "./maps";
|
||||
public double OccupancyGridResolution { get; set; } = 0.05;
|
||||
public double MapPadding { get; set; } = 1.0;
|
||||
public bool IncludeUnfinishedSubmaps { get; set; } = false;
|
||||
public string LocalizingStateDirectory { get; set; } = "./localizing_state";
|
||||
public double PoseSyncIntervalSeconds { get; set; } = 5.0;
|
||||
public PngVisualizationOptions PngVisualizationOptions { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PNG visualization options
|
||||
/// </summary>
|
||||
public class PngVisualizationOptions
|
||||
{
|
||||
public bool ShowGrid { get; set; } = true;
|
||||
public bool ShowTrajectory { get; set; } = true;
|
||||
public string GridColor { get; set; } = "#CCCCCC";
|
||||
public string TrajectoryColor { get; set; } = "#FF0000";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constraint builder options configuration
|
||||
/// </summary>
|
||||
public class ConstraintBuilderOptionsConfiguration
|
||||
{
|
||||
public double SamplingRatio { get; set; } = 0.3;
|
||||
public double MaxConstraintDistance { get; set; } = 15.0;
|
||||
public double MinScore { get; set; } = 0.55;
|
||||
public double GlobalLocalizationMinScore { get; set; } = 0.6;
|
||||
public double LoopClosureTranslationWeight { get; set; } = 1.1e4;
|
||||
public double LoopClosureRotationWeight { get; set; } = 1.0;
|
||||
public bool LogMatches { get; set; } = true;
|
||||
public FastCorrelativeScanMatcherOptions2DConfiguration? FastCorrelativeScanMatcherOptions { get; set; }
|
||||
public CeresScanMatcherOptions2DConfiguration? CeresScanMatcherOptions { get; set; }
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -0,0 +1,206 @@
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
/// <summary>
|
||||
/// Strategy for merging multiple submaps into a single occupancy grid.
|
||||
/// </summary>
|
||||
public enum SubmapMergeStrategy
|
||||
{
|
||||
/// <summary>
|
||||
/// Porter-Duff Source-Over compositing (Cairo-style).
|
||||
/// Matches original Cartographer C++ behavior.
|
||||
/// </summary>
|
||||
PorterDuff,
|
||||
|
||||
/// <summary>
|
||||
/// Sum log-odds from all submaps (Bayesian approach).
|
||||
/// Provides clearer free/occupied distinction with multiple observations.
|
||||
/// LogOdds = Σ log(p_i / (1 - p_i)), then convert back to probability.
|
||||
/// </summary>
|
||||
LogOddsSum,
|
||||
|
||||
/// <summary>
|
||||
/// Take maximum probability (most pessimistic/conservative).
|
||||
/// Good for navigation safety - any occupied observation dominates.
|
||||
/// </summary>
|
||||
MaxProbability
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Configuration for occupancy grid generation and filtering.
|
||||
/// Controls how probability values are converted to occupancy values (0=free, 100=occupied, -1=unknown).
|
||||
/// </summary>
|
||||
public class OccupancyGridConfiguration
|
||||
{
|
||||
#region Merge Strategy
|
||||
|
||||
/// <summary>
|
||||
/// Strategy for merging overlapping cells from multiple submaps.
|
||||
/// Default: LogOddsSum (clearer free/occupied distinction)
|
||||
/// </summary>
|
||||
public SubmapMergeStrategy MergeStrategy { get; set; } = SubmapMergeStrategy.LogOddsSum;
|
||||
|
||||
/// <summary>
|
||||
/// Clamp log-odds to prevent extreme values from dominating.
|
||||
/// Range: [1, 20], Default: 10 (corresponds to probability ~0.00005 to ~0.99995)
|
||||
/// </summary>
|
||||
public double LogOddsClamp { get; set; } = 10.0;
|
||||
|
||||
/// <summary>
|
||||
/// When true, use average log-odds instead of sum.
|
||||
/// This prevents amplification when a cell is observed by many submaps.
|
||||
/// Default: true (average is more stable for visualization)
|
||||
/// </summary>
|
||||
public bool UseLogOddsAverage { get; set; } = true;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Threshold Configuration
|
||||
|
||||
/// <summary>
|
||||
/// Threshold for classifying a cell as FREE (white, occupancy=0).
|
||||
/// In texture-based conversion: textureValue >= FreeSpaceThreshold → FREE
|
||||
/// Higher value = stricter (fewer free cells), Lower value = more permissive (more free cells).
|
||||
/// Range: [0, 255], Default: 100
|
||||
///
|
||||
/// Technical detail:
|
||||
/// - textureValue = max(0, 128 - logOddsInteger)
|
||||
/// - logOddsInteger maps probability [0.1, 0.9] to [1, 255]
|
||||
/// - FreeSpaceThreshold=100 requires probability ≤ ~0.15 (very confident free)
|
||||
/// - FreeSpaceThreshold=50 requires probability ≤ ~0.30 (moderately confident free)
|
||||
/// </summary>
|
||||
public int FreeSpaceThreshold { get; set; } = 100;
|
||||
|
||||
/// <summary>
|
||||
/// Threshold for classifying a cell as OCCUPIED (black, occupancy=100).
|
||||
/// In texture-based conversion: textureAlpha > OccupiedSpaceThreshold → OCCUPIED
|
||||
/// Higher value = stricter (fewer occupied cells, thinner walls), Lower value = more permissive.
|
||||
/// Range: [0, 255], Default: 0
|
||||
///
|
||||
/// Technical detail:
|
||||
/// - textureAlpha = max(0, logOddsInteger - 128)
|
||||
/// - OccupiedSpaceThreshold=0 requires probability > 0.5 (any occupied tendency)
|
||||
/// - OccupiedSpaceThreshold=50 requires probability > ~0.70 (confident occupied)
|
||||
/// - OccupiedSpaceThreshold=100 requires probability > ~0.85 (very confident occupied)
|
||||
/// </summary>
|
||||
public int OccupiedSpaceThreshold { get; set; } = 0;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Output Mode
|
||||
|
||||
/// <summary>
|
||||
/// When true, output only binary values (0=free, 100=occupied, -1=unknown).
|
||||
/// When false, output gradient values (0-100) based on probability.
|
||||
/// Default: true (binary output for compatibility with most navigation systems).
|
||||
/// </summary>
|
||||
public bool UseBinaryOutput { get; set; } = true;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Wall Thinning (Post-processing)
|
||||
|
||||
/// <summary>
|
||||
/// Enable morphological erosion to thin walls in the occupancy grid.
|
||||
/// Useful for reducing wall thickness caused by sensor noise or multiple observations.
|
||||
/// Default: false
|
||||
/// </summary>
|
||||
public bool EnableWallThinning { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Number of erosion iterations for wall thinning.
|
||||
/// Each iteration removes one pixel layer from occupied regions.
|
||||
/// Higher value = thinner walls, but may disconnect thin walls.
|
||||
/// Range: [1, 5], Default: 1
|
||||
/// </summary>
|
||||
public int WallThinningIterations { get; set; } = 1;
|
||||
|
||||
/// <summary>
|
||||
/// Minimum wall thickness to preserve (in pixels) during wall thinning.
|
||||
/// Walls thinner than this will not be eroded further.
|
||||
/// Range: [1, 10], Default: 1
|
||||
/// </summary>
|
||||
public int MinWallThicknessPixels { get; set; } = 1;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Ambiguous Cell Handling
|
||||
|
||||
/// <summary>
|
||||
/// How to handle ambiguous cells (probability ~0.5, neither clearly free nor occupied).
|
||||
/// Values: -1 = Unknown, 0 = Free, 100 = Occupied
|
||||
/// Default: -1 (mark as unknown)
|
||||
///
|
||||
/// Note: This applies when UseBinaryOutput=true and the cell doesn't meet
|
||||
/// either FreeSpaceThreshold or OccupiedSpaceThreshold.
|
||||
/// </summary>
|
||||
public sbyte AmbiguousCellValue { get; set; } = -1;
|
||||
|
||||
/// <summary>
|
||||
/// Lower bound of the ambiguous range (probability).
|
||||
/// Cells with probability between AmbiguousRangeLower and AmbiguousRangeUpper
|
||||
/// are considered ambiguous and handled according to AmbiguousCellValue.
|
||||
/// Default: 0.35 (corresponding to ~neither free nor occupied)
|
||||
/// </summary>
|
||||
public double AmbiguousRangeLower { get; set; } = 0.35;
|
||||
|
||||
/// <summary>
|
||||
/// Upper bound of the ambiguous range (probability).
|
||||
/// Default: 0.65
|
||||
/// </summary>
|
||||
public double AmbiguousRangeUpper { get; set; } = 0.65;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Advanced Options
|
||||
|
||||
/// <summary>
|
||||
/// Apply median filter to reduce noise in the occupancy grid.
|
||||
/// Useful for removing salt-and-pepper noise.
|
||||
/// Default: false
|
||||
/// </summary>
|
||||
public bool EnableMedianFilter { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Kernel size for median filter (must be odd number).
|
||||
/// Range: [3, 7], Default: 3
|
||||
/// </summary>
|
||||
public int MedianFilterKernelSize { get; set; } = 3;
|
||||
|
||||
#endregion
|
||||
|
||||
#region TSDF-Specific Options
|
||||
|
||||
/// <summary>
|
||||
/// TSD threshold for classifying a cell as FREE space (meters).
|
||||
/// Cells with TSD > TsdfFreeThreshold are considered free.
|
||||
/// Lower value = more aggressive free space detection (closer to walls).
|
||||
/// Range: [0.01, 0.3], Default: 0.05 (5cm from surface)
|
||||
/// </summary>
|
||||
public double TsdfFreeThreshold { get; set; } = 0.05;
|
||||
|
||||
/// <summary>
|
||||
/// TSD threshold for classifying a cell as OCCUPIED (meters, negative value).
|
||||
/// Cells with TSD < TsdfOccupiedThreshold are considered occupied.
|
||||
/// Higher value (closer to 0) = more aggressive obstacle detection.
|
||||
/// Range: [-0.3, 0], Default: -0.02 (2cm inside surface)
|
||||
/// </summary>
|
||||
public double TsdfOccupiedThreshold { get; set; } = -0.02;
|
||||
|
||||
/// <summary>
|
||||
/// Minimum weight required for a TSDF cell to be considered valid.
|
||||
/// Cells with weight < TsdfMinWeight are skipped (treated as unknown).
|
||||
/// Lower value = include more cells but with less confidence.
|
||||
/// Range: [0.01, 5.0], Default: 0.1
|
||||
/// </summary>
|
||||
public double TsdfMinWeight { get; set; } = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum TSD value for normalization (meters).
|
||||
/// Should match the TruncationDistance in TsdfRangeDataInserterOptions.
|
||||
/// Used to convert TSD to probability: probability = 0.5 * (1 - tsd/TsdfMaxTsd)
|
||||
/// Range: [0.1, 1.0], Default: 0.3
|
||||
/// </summary>
|
||||
public double TsdfMaxTsd { get; set; } = 0.3;
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,115 @@
|
||||
using RobotNet10.Shared.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
/// <summary>
|
||||
/// Sensor configuration
|
||||
/// </summary>
|
||||
public class SensorConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Lidar sensor configurations (multiple lidars supported)
|
||||
/// </summary>
|
||||
public List<LidarSensorConfiguration> Lidars { get; set; } = [];
|
||||
|
||||
/// <summary>
|
||||
/// IMU sensor configuration (only one IMU supported by CartographerSharp)
|
||||
/// </summary>
|
||||
public ImuSensorConfiguration Imu { get; set; } = new();
|
||||
|
||||
/// <summary>
|
||||
/// Sampling ratio for sensor data (0.0 to 1.0)
|
||||
/// - 1.0 = process all messages (default)
|
||||
/// - 0.5 = process 50% of messages
|
||||
/// - 0.0 = process no messages
|
||||
/// Based on xloc flow: sampling ratio to reduce message overload
|
||||
/// </summary>
|
||||
public double? SamplingRatio { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Sampling ratio for Lidar data (overrides global SamplingRatio if set)
|
||||
/// </summary>
|
||||
public double? LidarSamplingRatio { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Sampling ratio for IMU data (overrides global SamplingRatio if set)
|
||||
/// </summary>
|
||||
public double? ImuSamplingRatio { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Sampling ratio for Odometry data (overrides global SamplingRatio if set)
|
||||
/// </summary>
|
||||
public double? OdometrySamplingRatio { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Odometry update interval in milliseconds.
|
||||
/// Odometry is polled at this interval from the odometry estimator.
|
||||
/// Default: 5ms (200Hz). Lower values = higher frequency, more CPU usage.
|
||||
/// Recommended: 5-20ms (50-200Hz)
|
||||
/// </summary>
|
||||
public int OdometryUpdateIntervalMs { get; set; } = 5;
|
||||
|
||||
/// <summary>
|
||||
/// IMU update interval in milliseconds.
|
||||
/// IMU data is processed at this minimum interval (rate limiting).
|
||||
/// Default: 5ms (200Hz). Set to 0 to process all IMU events without rate limiting.
|
||||
/// Recommended: 0-10ms (100Hz-unlimited)
|
||||
/// </summary>
|
||||
public int ImuUpdateIntervalMs { get; set; } = 5;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Lidar sensor configuration
|
||||
/// </summary>
|
||||
public class LidarSensorConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Device ID from IDeviceProvider
|
||||
/// </summary>
|
||||
public string DeviceId { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// Enable or disable this lidar
|
||||
/// </summary>
|
||||
public bool Enabled { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// Transform from lidar frame to base_link frame
|
||||
/// </summary>
|
||||
public Transform Transform { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Minimum angle for filtering point cloud (degrees).
|
||||
/// Points with angle less than this will be filtered out.
|
||||
/// If null, no minimum angle filtering is applied.
|
||||
/// </summary>
|
||||
public double? AngleMin { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Maximum angle for filtering point cloud (degrees).
|
||||
/// Points with angle greater than this will be filtered out.
|
||||
/// If null, no maximum angle filtering is applied.
|
||||
/// </summary>
|
||||
public double? AngleMax { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// IMU sensor configuration
|
||||
/// </summary>
|
||||
public class ImuSensorConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Device ID from IDeviceProvider
|
||||
/// </summary>
|
||||
public string DeviceId { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// Enable or disable this IMU
|
||||
/// </summary>
|
||||
public bool Enabled { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Transform from IMU frame to base_link frame
|
||||
/// </summary>
|
||||
public Transform Transform { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,398 @@
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
/// <summary>
|
||||
/// Trajectory builder configuration
|
||||
/// </summary>
|
||||
public class TrajectoryBuilderConfiguration
|
||||
{
|
||||
/// <summary>
|
||||
/// Use 2D SLAM (true) or 3D SLAM (false)
|
||||
/// </summary>
|
||||
public bool Use2D { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// Minimum range for lidar points (meters)
|
||||
/// </summary>
|
||||
public double MinRange { get; set; } = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum range for lidar points (meters)
|
||||
/// </summary>
|
||||
public double MaxRange { get; set; } = 30.0;
|
||||
|
||||
/// <summary>
|
||||
/// Minimum Z coordinate for lidar points (meters). Points below this will be filtered out.
|
||||
/// </summary>
|
||||
public double? MinZ { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Maximum Z coordinate for lidar points (meters). Points above this will be filtered out.
|
||||
/// </summary>
|
||||
public double? MaxZ { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Points beyond 'max_range' will be inserted with this length as empty space (meters).
|
||||
/// </summary>
|
||||
public double? MissingDataRayLength { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Voxel filter size (meters)
|
||||
/// </summary>
|
||||
public double VoxelFilterSize { get; set; } = 0.025;
|
||||
|
||||
/// <summary>
|
||||
/// Number of accumulated range data before processing
|
||||
/// </summary>
|
||||
public int NumAccumulatedRangeData { get; set; } = 1;
|
||||
|
||||
/// <summary>
|
||||
/// Use IMU data if available. If null, will be auto-detected from sensor configuration.
|
||||
/// </summary>
|
||||
public bool? UseImuData { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Use online correlative scan matching
|
||||
/// </summary>
|
||||
public bool UseOnlineCorrelativeScanMatching { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Real-time correlative scan matcher options
|
||||
/// </summary>
|
||||
public RealTimeCorrelativeScanMatcherOptionsConfiguration? RealTimeCorrelativeScanMatcherOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Ceres scan matcher options for 2D
|
||||
/// </summary>
|
||||
public CeresScanMatcherOptions2DConfiguration? CeresScanMatcherOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Motion filter options
|
||||
/// </summary>
|
||||
public MotionFilterOptionsConfiguration? MotionFilterOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Adaptive voxel filter options (optional, if not provided uses fixed voxel_filter_size)
|
||||
/// </summary>
|
||||
public AdaptiveVoxelFilterOptionsConfiguration? AdaptiveVoxelFilterOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Loop closure adaptive voxel filter options (optional)
|
||||
/// Used to compute a sparser point cloud for finding loop closures.
|
||||
/// If not provided, uses the same filter as AdaptiveVoxelFilterOptions or fixed voxel_filter_size.
|
||||
/// </summary>
|
||||
public AdaptiveVoxelFilterOptionsConfiguration? LoopClosureAdaptiveVoxelFilterOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Pose extrapolator options
|
||||
/// </summary>
|
||||
public PoseExtrapolatorOptionsConfiguration? PoseExtrapolatorOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Debug options for scan matching convergence
|
||||
/// </summary>
|
||||
public ScanMatchingDebugOptionsConfiguration? ScanMatchingDebugOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Submaps options for 2D
|
||||
/// </summary>
|
||||
public SubmapsOptions2DConfiguration? SubmapsOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// High resolution grid options for 2D (optional)
|
||||
/// Used for higher precision scan matching
|
||||
/// </summary>
|
||||
public GridOptions2DConfiguration? HighResGridOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// High resolution adaptive voxel filter options (optional)
|
||||
/// Used to filter point cloud for high resolution grid
|
||||
/// </summary>
|
||||
public AdaptiveVoxelFilterOptionsConfiguration? HighResAdaptiveVoxelFilterOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// High resolution voxel filter size (meters)
|
||||
/// Used when HighResAdaptiveVoxelFilterOptions is not provided
|
||||
/// </summary>
|
||||
public double? HighResVoxelFilterSize { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Landmark threshold options (optional)
|
||||
/// Used for landmark-based localization
|
||||
/// </summary>
|
||||
public LandmarkThresholdOptionsConfiguration? LandmarkThreshold { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Number of duplicate inserts to accurate submap
|
||||
/// </summary>
|
||||
public int? NumDuplicateInsertToAccurateSubmap { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// True score cell threshold
|
||||
/// Threshold for determining if a cell has a true score
|
||||
/// </summary>
|
||||
public double? TrueScoreCellThreshold { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// If true, provides a confidence score for the pose estimate based on
|
||||
/// real-time correlative scan matching.
|
||||
/// </summary>
|
||||
public bool ProvideConfidenceScore { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Soft limit for Ceres scan match cost. When exceeded, pose is trusted but
|
||||
/// scan is NOT inserted into submap (prevents map corruption).
|
||||
/// Set to 0 to disable. Default: 0 (disabled).
|
||||
/// </summary>
|
||||
public double CeresScoreSoftLimit { get; set; } = 0;
|
||||
|
||||
/// <summary>
|
||||
/// Hard limit for Ceres scan match cost. When exceeded, the pose is considered
|
||||
/// unreliable and odometry prediction is used instead.
|
||||
/// Set to 0 to disable. Default: 0 (disabled).
|
||||
/// </summary>
|
||||
public double CeresScoreHardLimit { get; set; } = 0;
|
||||
|
||||
/// <summary>
|
||||
/// After this many consecutive hard-limit failures, a new submap is forced
|
||||
/// to break the map-growth deadlock.
|
||||
/// Set to 0 to disable. Default: 5.
|
||||
/// </summary>
|
||||
public int MaxConsecutiveHighCostBeforeNewSubmap { get; set; } = 5;
|
||||
|
||||
/// <summary>
|
||||
/// Real-time correlative scan matcher options for initial pose (SetInitialPose).
|
||||
/// Uses wider search window for relocalization scenarios.
|
||||
/// </summary>
|
||||
public RealTimeCorrelativeScanMatcherOptionsConfiguration? InitialPoseRealTimeCorrelativeScanMatcherOptions { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Ceres scan matcher options for initial pose (SetInitialPose).
|
||||
/// Uses stricter weights for better accuracy in relocalization scenarios.
|
||||
/// </summary>
|
||||
public CeresScanMatcherOptions2DConfiguration? InitialPoseCeresScanMatcherOptions { get; set; }
|
||||
}
|
||||
|
||||
#region Scan Matching Options
|
||||
|
||||
/// <summary>
|
||||
/// Ceres scan matcher options for 2D
|
||||
/// </summary>
|
||||
public class CeresScanMatcherOptions2DConfiguration
|
||||
{
|
||||
public double OccupiedSpaceWeight { get; set; } = 1.0;
|
||||
public double TranslationWeight { get; set; } = 10.0;
|
||||
public double RotationWeight { get; set; } = 40.0;
|
||||
public double? HighResOccupiedSpaceWeight { get; set; }
|
||||
public double? HighResTranslationWeight { get; set; }
|
||||
public double? HighResRotationWeight { get; set; }
|
||||
public double? LandmarkWeight { get; set; }
|
||||
public CeresSolverOptionsConfiguration? CeresSolverOptions { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Real-time correlative scan matcher options
|
||||
/// </summary>
|
||||
public class RealTimeCorrelativeScanMatcherOptionsConfiguration
|
||||
{
|
||||
public double LinearSearchWindow { get; set; } = 0.1;
|
||||
public double AngularSearchWindow { get; set; } = 0.349066; // ~20 degrees
|
||||
public double TranslationDeltaCostWeight { get; set; } = 0.1;
|
||||
public double RotationDeltaCostWeight { get; set; } = 0.1;
|
||||
public int NumThreads { get; set; } = 1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fast correlative scan matcher options for 2D
|
||||
/// </summary>
|
||||
public class FastCorrelativeScanMatcherOptions2DConfiguration
|
||||
{
|
||||
public double LinearSearchWindow { get; set; } = 7.0;
|
||||
public double AngularSearchWindow { get; set; } = 0.523599; // ~30 degrees
|
||||
public int BranchAndBoundDepth { get; set; } = 7;
|
||||
public double? LocalizationLinearSearchWindow { get; set; }
|
||||
public double? LocalizationAngularSearchWindow { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ceres solver options
|
||||
/// </summary>
|
||||
public class CeresSolverOptionsConfiguration
|
||||
{
|
||||
public bool UseNonmonotonicSteps { get; set; } = false;
|
||||
public int MaxNumIterations { get; set; } = 20;
|
||||
public int NumThreads { get; set; } = 1;
|
||||
public double? FunctionTolerance { get; set; }
|
||||
public double? GradientTolerance { get; set; }
|
||||
public double? ParameterTolerance { get; set; }
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Motion and Filter Options
|
||||
|
||||
/// <summary>
|
||||
/// Motion filter options
|
||||
/// </summary>
|
||||
public class MotionFilterOptionsConfiguration
|
||||
{
|
||||
public double MaxTimeSeconds { get; set; } = 5.0;
|
||||
public double MaxDistanceMeters { get; set; } = 0.2;
|
||||
public double MaxAngleRadians { get; set; } = 0.0174533; // ~1 degree
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adaptive voxel filter options
|
||||
/// </summary>
|
||||
public class AdaptiveVoxelFilterOptionsConfiguration
|
||||
{
|
||||
public double MaxLength { get; set; } = 0.5;
|
||||
public double MinNumPoints { get; set; } = 200;
|
||||
public double MaxRange { get; set; } = 50.0;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Submap Options
|
||||
|
||||
/// <summary>
|
||||
/// Submaps options for 2D
|
||||
/// </summary>
|
||||
public class SubmapsOptions2DConfiguration
|
||||
{
|
||||
public int NumRangeData { get; set; } = 90;
|
||||
public GridOptions2DConfiguration GridOptions { get; set; } = new();
|
||||
public RangeDataInserterOptionsConfiguration RangeDataInserterOptions { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Grid options for 2D
|
||||
/// </summary>
|
||||
public class GridOptions2DConfiguration
|
||||
{
|
||||
public int GridType { get; set; } = 1; // ProbabilityGrid
|
||||
public double Resolution { get; set; } = 0.05;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Range data inserter options
|
||||
/// </summary>
|
||||
public class RangeDataInserterOptionsConfiguration
|
||||
{
|
||||
public int RangeDataInserterType { get; set; } = 1; // ProbabilityGridInserter2D
|
||||
public ProbabilityGridRangeDataInserterOptions2DConfiguration? ProbabilityGridRangeDataInserterOptions { get; set; }
|
||||
public TsdfRangeDataInserterOptions2DConfiguration? TsdfRangeDataInserterOptions { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// TSDF range data inserter options for 2D
|
||||
/// </summary>
|
||||
public class TsdfRangeDataInserterOptions2DConfiguration
|
||||
{
|
||||
public double TruncationDistance { get; set; } = 0.3;
|
||||
public double MaximumWeight { get; set; } = 10.0;
|
||||
public bool UpdateFreeSpace { get; set; } = false;
|
||||
public NormalEstimationOptions2DConfiguration NormalEstimationOptions { get; set; } = new();
|
||||
public bool ProjectSdfDistanceToScanNormal { get; set; } = true;
|
||||
public int UpdateWeightRangeExponent { get; set; } = 0;
|
||||
public double UpdateWeightAngleScanNormalToRayKernelBandwidth { get; set; } = 0.5;
|
||||
public double UpdateWeightDistanceCellToHitKernelBandwidth { get; set; } = 0.5;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Normal estimation options for 2D TSDF
|
||||
/// </summary>
|
||||
public class NormalEstimationOptions2DConfiguration
|
||||
{
|
||||
public int NumNormalSamples { get; set; } = 4;
|
||||
public double SampleRadius { get; set; } = 0.5;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Probability grid range data inserter options for 2D
|
||||
/// </summary>
|
||||
public class ProbabilityGridRangeDataInserterOptions2DConfiguration
|
||||
{
|
||||
public double HitProbability { get; set; } = 0.55;
|
||||
public double MissProbability { get; set; } = 0.49;
|
||||
public bool InsertFreeSpace { get; set; } = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Overlapping submaps trimmer options for 2D
|
||||
/// </summary>
|
||||
public class OverlappingSubmapsTrimmerOptions2DConfiguration
|
||||
{
|
||||
public int FreshSubmapsCount { get; set; } = 2;
|
||||
public double MinCoveredArea { get; set; } = 1.0;
|
||||
public int MinAddedSubmapsCount { get; set; } = 5;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Pose Extrapolator Options
|
||||
|
||||
/// <summary>
|
||||
/// Pose extrapolator options
|
||||
/// </summary>
|
||||
public class PoseExtrapolatorOptionsConfiguration
|
||||
{
|
||||
public bool UseImuBased { get; set; } = false;
|
||||
public ConstantVelocityPoseExtrapolatorOptionsConfiguration ConstantVelocity { get; set; } = new();
|
||||
public ImuBasedPoseExtrapolatorOptionsConfiguration? ImuBased { get; set; }
|
||||
public double? VelocityThreshold { get; set; }
|
||||
public double? AccelerationThreshold { get; set; }
|
||||
public double? GravityDeviationThreshold { get; set; }
|
||||
public bool? UseOdometryDirectly { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Constant velocity pose extrapolator options
|
||||
/// </summary>
|
||||
public class ConstantVelocityPoseExtrapolatorOptionsConfiguration
|
||||
{
|
||||
public double ImuGravityTimeConstant { get; set; } = 10.0;
|
||||
public double PoseQueueDuration { get; set; } = 0.001;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// IMU-based pose extrapolator options
|
||||
/// </summary>
|
||||
public class ImuBasedPoseExtrapolatorOptionsConfiguration
|
||||
{
|
||||
public double PoseQueueDuration { get; set; } = 5.0;
|
||||
public double GravityConstant { get; set; } = 9.806;
|
||||
public double PoseTranslationWeight { get; set; } = 1.0;
|
||||
public double PoseRotationWeight { get; set; } = 1.0;
|
||||
public double ImuAccelerationWeight { get; set; } = 1.0;
|
||||
public double ImuRotationWeight { get; set; } = 1.0;
|
||||
public double OdometryTranslationWeight { get; set; } = 1.0;
|
||||
public double OdometryRotationWeight { get; set; } = 1.0;
|
||||
public CeresSolverOptionsConfiguration? SolverOptions { get; set; }
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Debug Options
|
||||
|
||||
/// <summary>
|
||||
/// Debug options for scan matching convergence
|
||||
/// </summary>
|
||||
public class ScanMatchingDebugOptionsConfiguration
|
||||
{
|
||||
public bool EnableDebugMode { get; set; } = false;
|
||||
public double MinCostReductionPercent { get; set; } = 80.0;
|
||||
public double MaxPoseChangeDistanceMeters { get; set; } = 0.5;
|
||||
public double MaxPoseChangeRotationDegrees { get; set; } = 5.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Landmark threshold options
|
||||
/// </summary>
|
||||
public class LandmarkThresholdOptionsConfiguration
|
||||
{
|
||||
public double MirrorLandmarkMatchingDistance { get; set; } = 0.5;
|
||||
public double NewLandmarkDistance { get; set; } = 2.0;
|
||||
public int LocalLandmarksHistory { get; set; } = 80;
|
||||
}
|
||||
|
||||
#endregion
|
||||
Reference in New Issue
Block a user