475 lines
26 KiB
C#
475 lines
26 KiB
C#
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
|
|
};
|
|
}
|
|
}
|
|
|