Initial commit
This commit is contained in:
@@ -0,0 +1,474 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using static CartographerSharp.Mapping.PoseGraph;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
/// <summary>
|
||||
/// Factory for creating TrajectoryBuilderOptions to avoid code duplication
|
||||
/// </summary>
|
||||
public static class TrajectoryBuilderOptionsFactory
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates 2D trajectory builder options from configuration.
|
||||
/// </summary>
|
||||
/// <param name="config">Trajectory builder configuration</param>
|
||||
/// <param name="useImuData">Whether to use IMU data</param>
|
||||
/// <param name="gridTypeOverride">Override grid type (null = use config value). ProbabilityGrid for ScanMapping, Tsdf for Localizing.</param>
|
||||
public static TrajectoryBuilderOptions Create2D(
|
||||
TrajectoryBuilderConfiguration config,
|
||||
bool useImuData,
|
||||
GridOptions2D.GridType? gridTypeOverride = null)
|
||||
{
|
||||
if (!config.Use2D)
|
||||
{
|
||||
throw new ArgumentException("Configuration must have Use2D = true", nameof(config));
|
||||
}
|
||||
|
||||
// Use explicit UseImuData from config if provided, otherwise use parameter
|
||||
var finalUseImuData = config.UseImuData ?? useImuData;
|
||||
|
||||
// Create RealTimeCorrelativeScanMatcherOptions
|
||||
RealTimeCorrelativeScanMatcherOptions? realTimeCorrelativeScanMatcherOptions = null;
|
||||
if (config.RealTimeCorrelativeScanMatcherOptions != null)
|
||||
{
|
||||
var rtcConfig = config.RealTimeCorrelativeScanMatcherOptions;
|
||||
realTimeCorrelativeScanMatcherOptions = new RealTimeCorrelativeScanMatcherOptions(
|
||||
linearSearchWindow: rtcConfig.LinearSearchWindow,
|
||||
angularSearchWindow: rtcConfig.AngularSearchWindow,
|
||||
translationDeltaCostWeight: rtcConfig.TranslationDeltaCostWeight,
|
||||
rotationDeltaCostWeight: rtcConfig.RotationDeltaCostWeight,
|
||||
numThreads: rtcConfig.NumThreads);
|
||||
}
|
||||
|
||||
// Create CeresScanMatcherOptions2D
|
||||
CeresScanMatcherOptions2D? ceresScanMatcherOptions = null;
|
||||
if (config.CeresScanMatcherOptions != null)
|
||||
{
|
||||
var csConfig = config.CeresScanMatcherOptions;
|
||||
CartographerSharp.Models.Common.CeresSolverOptions? ceresSolverOptions = null;
|
||||
if (csConfig.CeresSolverOptions != null)
|
||||
{
|
||||
var solverConfig = csConfig.CeresSolverOptions;
|
||||
ceresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
|
||||
useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
|
||||
maxNumIterations: solverConfig.MaxNumIterations,
|
||||
numThreads: solverConfig.NumThreads,
|
||||
functionTolerance: solverConfig.FunctionTolerance,
|
||||
gradientTolerance: solverConfig.GradientTolerance,
|
||||
parameterTolerance: solverConfig.ParameterTolerance);
|
||||
}
|
||||
|
||||
ceresScanMatcherOptions = new CeresScanMatcherOptions2D(
|
||||
occupiedSpaceWeight: csConfig.OccupiedSpaceWeight,
|
||||
translationWeight: csConfig.TranslationWeight,
|
||||
rotationWeight: csConfig.RotationWeight,
|
||||
ceresSolverOptions: ceresSolverOptions,
|
||||
highResOccupiedSpaceWeight: csConfig.HighResOccupiedSpaceWeight,
|
||||
highResTranslationWeight: csConfig.HighResTranslationWeight,
|
||||
highResRotationWeight: csConfig.HighResRotationWeight,
|
||||
landmarkWeight: csConfig.LandmarkWeight);
|
||||
}
|
||||
|
||||
// Create MotionFilterOptions
|
||||
MotionFilterOptions? motionFilterOptions = null;
|
||||
if (config.MotionFilterOptions != null)
|
||||
{
|
||||
var mfConfig = config.MotionFilterOptions;
|
||||
motionFilterOptions = new MotionFilterOptions(
|
||||
maxTimeSeconds: mfConfig.MaxTimeSeconds,
|
||||
maxDistanceMeters: mfConfig.MaxDistanceMeters,
|
||||
maxAngleRadians: mfConfig.MaxAngleRadians);
|
||||
}
|
||||
|
||||
// Create SubmapsOptions2D
|
||||
SubmapsOptions2D? submapsOptions = null;
|
||||
if (config.SubmapsOptions != null)
|
||||
{
|
||||
var subConfig = config.SubmapsOptions;
|
||||
|
||||
// Determine effective grid type (override takes priority over config)
|
||||
var configGridType = (GridOptions2D.GridType)subConfig.GridOptions.GridType;
|
||||
var effectiveGridType = gridTypeOverride ?? configGridType;
|
||||
|
||||
var gridOptions = new GridOptions2D(
|
||||
gridType: effectiveGridType,
|
||||
resolution: subConfig.GridOptions.Resolution);
|
||||
|
||||
// Create RangeDataInserterOptions
|
||||
RangeDataInserterOptions? rangeDataInserterOptions = null;
|
||||
if (subConfig.RangeDataInserterOptions != null)
|
||||
{
|
||||
var rdiConfig = subConfig.RangeDataInserterOptions;
|
||||
|
||||
// Determine inserter type to match effective grid type
|
||||
var effectiveInserterType = effectiveGridType switch
|
||||
{
|
||||
GridOptions2D.GridType.ProbabilityGrid => RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D,
|
||||
GridOptions2D.GridType.Tsdf => RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D,
|
||||
_ => (RangeDataInserterOptions.RangeDataInserterType)rdiConfig.RangeDataInserterType
|
||||
};
|
||||
|
||||
// Read ProbabilityGrid options (use configured or C++ defaults)
|
||||
ProbabilityGridRangeDataInserterOptions2D? probGridOptions = null;
|
||||
if (rdiConfig.ProbabilityGridRangeDataInserterOptions != null)
|
||||
{
|
||||
var pgConfig = rdiConfig.ProbabilityGridRangeDataInserterOptions;
|
||||
probGridOptions = new ProbabilityGridRangeDataInserterOptions2D(
|
||||
hitProbability: pgConfig.HitProbability,
|
||||
missProbability: pgConfig.MissProbability,
|
||||
insertFreeSpace: pgConfig.InsertFreeSpace);
|
||||
}
|
||||
else if (effectiveGridType == GridOptions2D.GridType.ProbabilityGrid)
|
||||
{
|
||||
// C++ Cartographer defaults
|
||||
probGridOptions = new ProbabilityGridRangeDataInserterOptions2D(0.55, 0.49, true);
|
||||
}
|
||||
|
||||
// Read TSDF options (use configured or C++ defaults)
|
||||
TSDFRangeDataInserterOptions2D? tsdfOptions = null;
|
||||
if (rdiConfig.TsdfRangeDataInserterOptions != null)
|
||||
{
|
||||
var tsdfConfig = rdiConfig.TsdfRangeDataInserterOptions;
|
||||
var normalEstOptions = new NormalEstimationOptions2D(
|
||||
tsdfConfig.NormalEstimationOptions.NumNormalSamples,
|
||||
tsdfConfig.NormalEstimationOptions.SampleRadius);
|
||||
tsdfOptions = new TSDFRangeDataInserterOptions2D(
|
||||
truncationDistance: tsdfConfig.TruncationDistance,
|
||||
maximumWeight: tsdfConfig.MaximumWeight,
|
||||
updateFreeSpace: tsdfConfig.UpdateFreeSpace,
|
||||
normalEstimationOptions: normalEstOptions,
|
||||
projectSdfDistanceToScanNormal: tsdfConfig.ProjectSdfDistanceToScanNormal,
|
||||
updateWeightRangeExponent: tsdfConfig.UpdateWeightRangeExponent,
|
||||
updateWeightAngleScanNormalToRayKernelBandwidth: tsdfConfig.UpdateWeightAngleScanNormalToRayKernelBandwidth,
|
||||
updateWeightDistanceCellToHitKernelBandwidth: tsdfConfig.UpdateWeightDistanceCellToHitKernelBandwidth);
|
||||
}
|
||||
else if (effectiveGridType == GridOptions2D.GridType.Tsdf)
|
||||
{
|
||||
// C++ Cartographer defaults for TSDF
|
||||
tsdfOptions = new TSDFRangeDataInserterOptions2D(
|
||||
truncationDistance: 0.3,
|
||||
maximumWeight: 10.0);
|
||||
}
|
||||
|
||||
rangeDataInserterOptions = new RangeDataInserterOptions(
|
||||
rangeDataInserterType: effectiveInserterType,
|
||||
probabilityGridRangeDataInserterOptions2D: probGridOptions,
|
||||
tsdfRangeDataInserterOptions2D: tsdfOptions);
|
||||
}
|
||||
|
||||
// Create HighResGridOptions2D if configured
|
||||
// Apply same gridTypeOverride so HighResGridOptions matches the main grid type
|
||||
GridOptions2D? highResGridOptions = null;
|
||||
if (config.HighResGridOptions != null)
|
||||
{
|
||||
highResGridOptions = new GridOptions2D(
|
||||
gridType: gridTypeOverride ?? (GridOptions2D.GridType)config.HighResGridOptions.GridType,
|
||||
resolution: config.HighResGridOptions.Resolution);
|
||||
}
|
||||
|
||||
submapsOptions = new SubmapsOptions2D(
|
||||
numRangeData: subConfig.NumRangeData,
|
||||
gridOptions2D: gridOptions,
|
||||
rangeDataInserterOptions: rangeDataInserterOptions ?? new RangeDataInserterOptions(
|
||||
RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D),
|
||||
highResGridOptions2D: highResGridOptions);
|
||||
}
|
||||
|
||||
// Create AdaptiveVoxelFilterOptions
|
||||
CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions? adaptiveVoxelFilterOptions = null;
|
||||
if (config.AdaptiveVoxelFilterOptions != null)
|
||||
{
|
||||
var avfConfig = config.AdaptiveVoxelFilterOptions;
|
||||
adaptiveVoxelFilterOptions = new CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions(
|
||||
maxLength: avfConfig.MaxLength,
|
||||
minNumPoints: avfConfig.MinNumPoints,
|
||||
maxRange: avfConfig.MaxRange);
|
||||
}
|
||||
|
||||
// Create LoopClosureAdaptiveVoxelFilterOptions
|
||||
CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions? loopClosureAdaptiveVoxelFilterOptions = null;
|
||||
if (config.LoopClosureAdaptiveVoxelFilterOptions != null)
|
||||
{
|
||||
var lcavfConfig = config.LoopClosureAdaptiveVoxelFilterOptions;
|
||||
loopClosureAdaptiveVoxelFilterOptions = new CartographerSharp.Models.Sensor.AdaptiveVoxelFilterOptions(
|
||||
maxLength: lcavfConfig.MaxLength,
|
||||
minNumPoints: lcavfConfig.MinNumPoints,
|
||||
maxRange: lcavfConfig.MaxRange);
|
||||
}
|
||||
|
||||
// Create PoseExtrapolatorOptions
|
||||
PoseExtrapolatorOptions? poseExtrapolatorOptions = null;
|
||||
if (config.PoseExtrapolatorOptions != null)
|
||||
{
|
||||
var peConfig = config.PoseExtrapolatorOptions;
|
||||
|
||||
// Create ConstantVelocityPoseExtrapolatorOptions
|
||||
var constantVelocityOptions = new ConstantVelocityPoseExtrapolatorOptions(
|
||||
imuGravityTimeConstant: peConfig.ConstantVelocity.ImuGravityTimeConstant,
|
||||
poseQueueDuration: peConfig.ConstantVelocity.PoseQueueDuration);
|
||||
|
||||
// Create ImuBasedPoseExtrapolatorOptions
|
||||
ImuBasedPoseExtrapolatorOptions? imuBasedOptions = null;
|
||||
if (peConfig.ImuBased != null)
|
||||
{
|
||||
var imuConfig = peConfig.ImuBased;
|
||||
CartographerSharp.Models.Common.CeresSolverOptions? solverOptions = null;
|
||||
if (imuConfig.SolverOptions != null)
|
||||
{
|
||||
var solverConfig = imuConfig.SolverOptions;
|
||||
solverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
|
||||
useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
|
||||
maxNumIterations: solverConfig.MaxNumIterations,
|
||||
numThreads: solverConfig.NumThreads,
|
||||
functionTolerance: solverConfig.FunctionTolerance,
|
||||
gradientTolerance: solverConfig.GradientTolerance,
|
||||
parameterTolerance: solverConfig.ParameterTolerance);
|
||||
}
|
||||
|
||||
imuBasedOptions = new ImuBasedPoseExtrapolatorOptions(
|
||||
poseQueueDuration: imuConfig.PoseQueueDuration,
|
||||
gravityConstant: imuConfig.GravityConstant,
|
||||
poseTranslationWeight: imuConfig.PoseTranslationWeight,
|
||||
poseRotationWeight: imuConfig.PoseRotationWeight,
|
||||
imuAccelerationWeight: imuConfig.ImuAccelerationWeight,
|
||||
imuRotationWeight: imuConfig.ImuRotationWeight,
|
||||
solverOptions: solverOptions,
|
||||
odometryTranslationWeight: imuConfig.OdometryTranslationWeight,
|
||||
odometryRotationWeight: imuConfig.OdometryRotationWeight);
|
||||
}
|
||||
|
||||
poseExtrapolatorOptions = new PoseExtrapolatorOptions(
|
||||
useImuBased: peConfig.UseImuBased,
|
||||
constantVelocity: constantVelocityOptions,
|
||||
imuBased: imuBasedOptions,
|
||||
velocityThreshold: peConfig.VelocityThreshold,
|
||||
accelerationThreshold: peConfig.AccelerationThreshold,
|
||||
gravityDeviationThreshold: peConfig.GravityDeviationThreshold,
|
||||
useOdometryDirectly: peConfig.UseOdometryDirectly ?? false);
|
||||
}
|
||||
|
||||
// Create InitialPoseRealTimeCorrelativeScanMatcherOptions
|
||||
RealTimeCorrelativeScanMatcherOptions? initialPoseRealTimeCorrelativeScanMatcherOptions = null;
|
||||
if (config.InitialPoseRealTimeCorrelativeScanMatcherOptions != null)
|
||||
{
|
||||
var iprtcConfig = config.InitialPoseRealTimeCorrelativeScanMatcherOptions;
|
||||
initialPoseRealTimeCorrelativeScanMatcherOptions = new RealTimeCorrelativeScanMatcherOptions(
|
||||
linearSearchWindow: iprtcConfig.LinearSearchWindow,
|
||||
angularSearchWindow: iprtcConfig.AngularSearchWindow,
|
||||
translationDeltaCostWeight: iprtcConfig.TranslationDeltaCostWeight,
|
||||
rotationDeltaCostWeight: iprtcConfig.RotationDeltaCostWeight,
|
||||
numThreads: iprtcConfig.NumThreads);
|
||||
}
|
||||
|
||||
// Create InitialPoseCeresScanMatcherOptions
|
||||
CeresScanMatcherOptions2D? initialPoseCeresScanMatcherOptions = null;
|
||||
if (config.InitialPoseCeresScanMatcherOptions != null)
|
||||
{
|
||||
var ipcsConfig = config.InitialPoseCeresScanMatcherOptions;
|
||||
CartographerSharp.Models.Common.CeresSolverOptions? ipCeresSolverOptions = null;
|
||||
if (ipcsConfig.CeresSolverOptions != null)
|
||||
{
|
||||
var solverConfig = ipcsConfig.CeresSolverOptions;
|
||||
ipCeresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
|
||||
useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
|
||||
maxNumIterations: solverConfig.MaxNumIterations,
|
||||
numThreads: solverConfig.NumThreads,
|
||||
functionTolerance: solverConfig.FunctionTolerance,
|
||||
gradientTolerance: solverConfig.GradientTolerance,
|
||||
parameterTolerance: solverConfig.ParameterTolerance);
|
||||
}
|
||||
|
||||
initialPoseCeresScanMatcherOptions = new CeresScanMatcherOptions2D(
|
||||
occupiedSpaceWeight: ipcsConfig.OccupiedSpaceWeight,
|
||||
translationWeight: ipcsConfig.TranslationWeight,
|
||||
rotationWeight: ipcsConfig.RotationWeight,
|
||||
ceresSolverOptions: ipCeresSolverOptions,
|
||||
highResOccupiedSpaceWeight: ipcsConfig.HighResOccupiedSpaceWeight,
|
||||
highResTranslationWeight: ipcsConfig.HighResTranslationWeight,
|
||||
highResRotationWeight: ipcsConfig.HighResRotationWeight,
|
||||
landmarkWeight: ipcsConfig.LandmarkWeight);
|
||||
}
|
||||
|
||||
var options2D = new LocalTrajectoryBuilderOptions2D(
|
||||
minRange: config.MinRange,
|
||||
maxRange: config.MaxRange,
|
||||
minZ: (config.MinZ ?? -0.8f), // Default from Cartographer C++
|
||||
maxZ: (config.MaxZ ?? 2.0), // Default from Cartographer C++
|
||||
missingDataRayLength: (config.MissingDataRayLength ?? 5.0), // Default from Cartographer C++
|
||||
numAccumulatedRangeData: config.NumAccumulatedRangeData,
|
||||
voxelFilterSize: config.VoxelFilterSize,
|
||||
useOnlineCorrelativeScanMatching: config.UseOnlineCorrelativeScanMatching,
|
||||
realTimeCorrelativeScanMatcherOptions: realTimeCorrelativeScanMatcherOptions ?? default,
|
||||
ceresScanMatcherOptions: ceresScanMatcherOptions ?? default,
|
||||
motionFilterOptions: motionFilterOptions ?? default,
|
||||
submapsOptions: submapsOptions ?? default,
|
||||
useImuData: finalUseImuData,
|
||||
adaptiveVoxelFilterOptions: adaptiveVoxelFilterOptions ?? default,
|
||||
loopClosureAdaptiveVoxelFilterOptions: loopClosureAdaptiveVoxelFilterOptions ?? default,
|
||||
poseExtrapolatorOptions: poseExtrapolatorOptions ?? default,
|
||||
initialPoseRealTimeCorrelativeScanMatcherOptions: initialPoseRealTimeCorrelativeScanMatcherOptions ?? default,
|
||||
initialPoseCeresScanMatcherOptions: initialPoseCeresScanMatcherOptions ?? default,
|
||||
provideConfidenceScore: config.ProvideConfidenceScore,
|
||||
ceresScoreSoftLimit: config.CeresScoreSoftLimit,
|
||||
ceresScoreHardLimit: config.CeresScoreHardLimit,
|
||||
maxConsecutiveHighCostBeforeNewSubmap: config.MaxConsecutiveHighCostBeforeNewSubmap);
|
||||
|
||||
var jsonOptions = System.Text.Json.JsonSerializer.Serialize(options2D);
|
||||
var jsonElement = System.Text.Json.JsonDocument.Parse(jsonOptions).RootElement;
|
||||
|
||||
return new TrajectoryBuilderOptions(trajectoryBuilder2DOptions: jsonElement);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates 3D trajectory builder options from configuration
|
||||
/// </summary>
|
||||
public static TrajectoryBuilderOptions Create3D(
|
||||
TrajectoryBuilderConfiguration config,
|
||||
bool useImuData)
|
||||
{
|
||||
if (config.Use2D)
|
||||
{
|
||||
throw new ArgumentException("Configuration must have Use2D = false", nameof(config));
|
||||
}
|
||||
|
||||
// TODO: Implement 3D options when needed
|
||||
throw new NotSupportedException("3D trajectory builder not yet implemented");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates PoseGraphOptions from configuration
|
||||
/// </summary>
|
||||
public static PoseGraphOptions CreatePoseGraphOptions(MapBuilderConfiguration config)
|
||||
{
|
||||
// Create OptimizationProblemOptions from configuration
|
||||
// Note: OptimizationProblemOptions proto only supports MaxNumIterations directly,
|
||||
// not the full CeresSolverOptions (UseNonmonotonicSteps, NumThreads).
|
||||
// If CeresSolverOptions.MaxNumIterations is provided, it takes precedence over MaxNumIterations.
|
||||
var maxNumIterations = config.PoseGraphOptimizationProblemOptions.MaxNumIterations;
|
||||
if (config.PoseGraphOptimizationProblemOptions.CeresSolverOptions != null)
|
||||
{
|
||||
// Use MaxNumIterations from CeresSolverOptions if provided, otherwise use direct MaxNumIterations
|
||||
maxNumIterations = config.PoseGraphOptimizationProblemOptions.CeresSolverOptions.MaxNumIterations;
|
||||
}
|
||||
|
||||
var optimizationOptions = new CartographerSharp.Models.Mapping.OptimizationProblemOptions
|
||||
{
|
||||
HuberScale = config.PoseGraphOptimizationProblemOptions.HuberScale,
|
||||
OdometryHuberScale = config.PoseGraphOptimizationProblemOptions.OdometryHuberScale,
|
||||
LocalSlamPoseHuberScale = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseHuberScale,
|
||||
AccelerationWeight = config.PoseGraphOptimizationProblemOptions.AccelerationWeight,
|
||||
RotationWeight = config.PoseGraphOptimizationProblemOptions.RotationWeight,
|
||||
LocalSlamPoseTranslationWeight = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseTranslationWeight,
|
||||
LocalSlamPoseRotationWeight = config.PoseGraphOptimizationProblemOptions.LocalSlamPoseRotationWeight,
|
||||
OdometryTranslationWeight = config.PoseGraphOptimizationProblemOptions.OdometryTranslationWeight,
|
||||
OdometryRotationWeight = config.PoseGraphOptimizationProblemOptions.OdometryRotationWeight,
|
||||
FixedFramePoseTranslationWeight = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTranslationWeight,
|
||||
FixedFramePoseRotationWeight = config.PoseGraphOptimizationProblemOptions.FixedFramePoseRotationWeight,
|
||||
FixedFramePoseUseTolerantLoss = config.PoseGraphOptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
|
||||
FixedFramePoseTolerantLossParamA = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
|
||||
FixedFramePoseTolerantLossParamB = config.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
|
||||
LogSolverSummary = config.PoseGraphOptimizationProblemOptions.LogSolverSummary,
|
||||
MaxNumIterations = maxNumIterations
|
||||
};
|
||||
|
||||
// Log mapped values to verify correct assignment
|
||||
|
||||
// Note: Other CeresSolverOptions properties (UseNonmonotonicSteps, NumThreads) are not supported
|
||||
// by OptimizationProblemOptions proto. These options are hardcoded in OptimizationProblem2D/3D classes.
|
||||
|
||||
// Create ConstraintBuilderOptions from configuration
|
||||
CartographerSharp.Models.Mapping.ConstraintBuilderOptions? constraintBuilderOptions = null;
|
||||
if (config.ConstraintBuilderOptions != null)
|
||||
{
|
||||
var cbConfig = config.ConstraintBuilderOptions;
|
||||
|
||||
// Create FastCorrelativeScanMatcherOptions2D
|
||||
FastCorrelativeScanMatcherOptions2D? fastCorrelativeScanMatcherOptions = null;
|
||||
if (cbConfig.FastCorrelativeScanMatcherOptions != null)
|
||||
{
|
||||
var fcsConfig = cbConfig.FastCorrelativeScanMatcherOptions;
|
||||
fastCorrelativeScanMatcherOptions = new FastCorrelativeScanMatcherOptions2D(
|
||||
linearSearchWindow: fcsConfig.LinearSearchWindow,
|
||||
angularSearchWindow: fcsConfig.AngularSearchWindow,
|
||||
branchAndBoundDepth: fcsConfig.BranchAndBoundDepth,
|
||||
localizationLinearSearchWindow: fcsConfig.LocalizationLinearSearchWindow,
|
||||
localizationAngularSearchWindow: fcsConfig.LocalizationAngularSearchWindow);
|
||||
}
|
||||
|
||||
// Create CeresScanMatcherOptions2D for constraint builder
|
||||
CeresScanMatcherOptions2D? ceresScanMatcherOptions = null;
|
||||
if (cbConfig.CeresScanMatcherOptions != null)
|
||||
{
|
||||
var csConfig = cbConfig.CeresScanMatcherOptions;
|
||||
CartographerSharp.Models.Common.CeresSolverOptions? ceresSolverOptions = null;
|
||||
if (csConfig.CeresSolverOptions != null)
|
||||
{
|
||||
var solverConfig = csConfig.CeresSolverOptions;
|
||||
ceresSolverOptions = new CartographerSharp.Models.Common.CeresSolverOptions(
|
||||
useNonmonotonicSteps: solverConfig.UseNonmonotonicSteps,
|
||||
maxNumIterations: solverConfig.MaxNumIterations,
|
||||
numThreads: solverConfig.NumThreads,
|
||||
functionTolerance: solverConfig.FunctionTolerance,
|
||||
gradientTolerance: solverConfig.GradientTolerance,
|
||||
parameterTolerance: solverConfig.ParameterTolerance);
|
||||
}
|
||||
|
||||
ceresScanMatcherOptions = new CeresScanMatcherOptions2D(
|
||||
occupiedSpaceWeight: csConfig.OccupiedSpaceWeight,
|
||||
translationWeight: csConfig.TranslationWeight,
|
||||
rotationWeight: csConfig.RotationWeight,
|
||||
ceresSolverOptions: ceresSolverOptions,
|
||||
highResOccupiedSpaceWeight: csConfig.HighResOccupiedSpaceWeight,
|
||||
highResTranslationWeight: csConfig.HighResTranslationWeight,
|
||||
highResRotationWeight: csConfig.HighResRotationWeight,
|
||||
landmarkWeight: csConfig.LandmarkWeight);
|
||||
}
|
||||
|
||||
constraintBuilderOptions = new CartographerSharp.Models.Mapping.ConstraintBuilderOptions
|
||||
{
|
||||
SamplingRatio = cbConfig.SamplingRatio,
|
||||
MaxConstraintDistance = cbConfig.MaxConstraintDistance,
|
||||
MinScore = cbConfig.MinScore,
|
||||
GlobalLocalizationMinScore = cbConfig.GlobalLocalizationMinScore,
|
||||
LoopClosureTranslationWeight = cbConfig.LoopClosureTranslationWeight,
|
||||
LoopClosureRotationWeight = cbConfig.LoopClosureRotationWeight,
|
||||
LogMatches = cbConfig.LogMatches,
|
||||
FastCorrelativeScanMatcherOptions = fastCorrelativeScanMatcherOptions,
|
||||
CeresScanMatcherOptions = ceresScanMatcherOptions
|
||||
};
|
||||
}
|
||||
|
||||
// Create OverlappingSubmapsTrimmerOptions2D from configuration
|
||||
PoseGraphOptions.OverlappingSubmapsTrimmerOptions2D? overlappingSubmapsTrimmer2D = null;
|
||||
if (config.OverlappingSubmapsTrimmer2D != null)
|
||||
{
|
||||
var trimmerConfig = config.OverlappingSubmapsTrimmer2D;
|
||||
overlappingSubmapsTrimmer2D = new PoseGraphOptions.OverlappingSubmapsTrimmerOptions2D(
|
||||
freshSubmapsCount: trimmerConfig.FreshSubmapsCount,
|
||||
minCoveredArea: trimmerConfig.MinCoveredArea,
|
||||
minAddedSubmapsCount: trimmerConfig.MinAddedSubmapsCount
|
||||
);
|
||||
}
|
||||
|
||||
// Create PoseGraphOptions with optimization options from configuration
|
||||
return new PoseGraphOptions(
|
||||
optimizeEveryNNodes: config.OptimizeEveryNNodes,
|
||||
matcherTranslationWeight: config.MatcherTranslationWeight,
|
||||
matcherRotationWeight: config.MatcherRotationWeight,
|
||||
maxNumFinalIterations: config.MaxNumFinalIterations ?? 200, // Default from Cartographer C++
|
||||
globalSamplingRatio: config.GlobalSamplingRatio ?? 0.003, // Default from Cartographer C++
|
||||
logResidualHistograms: config.LogResidualHistograms ?? false, // Default
|
||||
globalConstraintSearchAfterNSeconds: config.GlobalConstraintSearchAfterNSeconds ?? 0.0, // Default (disabled)
|
||||
overlappingSubmapsTrimmer2D: overlappingSubmapsTrimmer2D,
|
||||
constraintBuilderOptions: constraintBuilderOptions,
|
||||
optimizationProblemOptions: optimizationOptions
|
||||
)
|
||||
{
|
||||
// Single-trajectory loop closure extension (not in C++ Cartographer)
|
||||
EnableSingleTrajectoryLoopClosure = config.EnableSingleTrajectoryLoopClosure,
|
||||
SingleTrajectoryLoopClosureDistanceThreshold = config.SingleTrajectoryLoopClosureDistanceThreshold
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user