Initial commit
This commit is contained in:
@@ -0,0 +1,214 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for creating and configuring MapBuilder instances
|
||||
/// Factory methods for MapBuilder and related configurations
|
||||
/// Extension methods are now in MapBuilderExtensions class
|
||||
/// </summary>
|
||||
public static class MapBuilderHelper
|
||||
{
|
||||
/// <summary>
|
||||
/// Create a new MapBuilder with configuration
|
||||
/// </summary>
|
||||
public static MapBuilder CreateMapBuilder(CartographerConfiguration config)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(config);
|
||||
|
||||
var use2D = config.MapBuilder.UseTrajectoryBuilder2D ?? config.TrajectoryBuilder.Use2D;
|
||||
var use3D = config.MapBuilder.UseTrajectoryBuilder3D ?? !config.TrajectoryBuilder.Use2D;
|
||||
|
||||
var poseGraphOptions = TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(config.MapBuilder);
|
||||
|
||||
var mapBuilderOptions = new MapBuilderOptions(
|
||||
useTrajectoryBuilder2D: use2D,
|
||||
useTrajectoryBuilder3D: use3D,
|
||||
numBackgroundThreads: config.MapBuilder.NumBackgroundThreads,
|
||||
poseGraphOptions: poseGraphOptions,
|
||||
collateByTrajectory: config.MapBuilder.CollateByTrajectory);
|
||||
|
||||
return new MapBuilder(mapBuilderOptions);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create MapBuilder from MapLoadResult (pbstream path and saved config).
|
||||
/// Uses saved config if available, otherwise uses current config.
|
||||
/// Then loads state from pbstream file with frozen trajectories for localization.
|
||||
/// </summary>
|
||||
public static MapBuilder CreateMapBuilderFromLoadResult(
|
||||
MapCartographerLoadResult loadResult,
|
||||
CartographerConfiguration currentConfig,
|
||||
ILogger? logger = null,
|
||||
bool loadFrozenState = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(loadResult);
|
||||
ArgumentNullException.ThrowIfNull(currentConfig);
|
||||
|
||||
if (string.IsNullOrEmpty(loadResult.PbstreamPath))
|
||||
{
|
||||
throw new ArgumentException("PbstreamPath cannot be null or empty", nameof(loadResult));
|
||||
}
|
||||
|
||||
if (!File.Exists(loadResult.PbstreamPath))
|
||||
{
|
||||
throw new FileNotFoundException("Pbstream file not found", loadResult.PbstreamPath);
|
||||
}
|
||||
|
||||
// Get MapBuilder options from saved config or current config
|
||||
bool use2D;
|
||||
bool use3D;
|
||||
int numBackgroundThreads;
|
||||
PoseGraphOptions poseGraphOptions;
|
||||
|
||||
if (loadResult.SavedConfig != null)
|
||||
{
|
||||
// Use saved config
|
||||
use2D = loadResult.SavedConfig.MapBuilder.UseTrajectoryBuilder2D ?? loadResult.SavedConfig.Use2D;
|
||||
use3D = loadResult.SavedConfig.MapBuilder.UseTrajectoryBuilder3D ?? !loadResult.SavedConfig.Use2D;
|
||||
numBackgroundThreads = loadResult.SavedConfig.MapBuilder.NumBackgroundThreads;
|
||||
poseGraphOptions = CreatePoseGraphOptionsFromSnapshot(loadResult.SavedConfig);
|
||||
|
||||
// Warn if config mismatch
|
||||
var currentUse2D = currentConfig.MapBuilder.UseTrajectoryBuilder2D ?? currentConfig.TrajectoryBuilder.Use2D;
|
||||
if (currentUse2D != use2D)
|
||||
{
|
||||
logger?.LogWarning(
|
||||
"MapBuilderHelper: Map config mismatch detected. Using saved config (2D={Saved2D}) instead of current config (2D={Current2D}).",
|
||||
use2D, currentUse2D);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Use current config
|
||||
use2D = currentConfig.MapBuilder.UseTrajectoryBuilder2D ?? currentConfig.TrajectoryBuilder.Use2D;
|
||||
use3D = currentConfig.MapBuilder.UseTrajectoryBuilder3D ?? !currentConfig.TrajectoryBuilder.Use2D;
|
||||
numBackgroundThreads = currentConfig.MapBuilder.NumBackgroundThreads;
|
||||
poseGraphOptions = TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(currentConfig.MapBuilder);
|
||||
}
|
||||
|
||||
var mapBuilderOptions = new MapBuilderOptions(
|
||||
useTrajectoryBuilder2D: use2D,
|
||||
useTrajectoryBuilder3D: use3D,
|
||||
numBackgroundThreads: numBackgroundThreads,
|
||||
poseGraphOptions: poseGraphOptions
|
||||
);
|
||||
|
||||
var mapBuilder = new MapBuilder(mapBuilderOptions);
|
||||
|
||||
// Load state from pbstream file
|
||||
// loadFrozenState: true freezes trajectories in the loaded map for localization
|
||||
// This prevents modifying the map during localization (as per xloc reference implementation)
|
||||
_ = mapBuilder.LoadStateFromFile(loadResult.PbstreamPath, loadFrozenState: loadFrozenState);
|
||||
|
||||
logger?.LogInformation(
|
||||
"MapBuilderHelper: Created MapBuilder from pbstream file: {PbstreamPath}, loadFrozenState={LoadFrozenState}",
|
||||
loadResult.PbstreamPath, loadFrozenState);
|
||||
|
||||
return mapBuilder;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates PoseGraphOptions from saved config snapshot
|
||||
/// </summary>
|
||||
private static PoseGraphOptions CreatePoseGraphOptionsFromSnapshot(MapCartographerConfigSnapshot savedConfig)
|
||||
{
|
||||
// Create temporary MapBuilderConfiguration from snapshot
|
||||
var tempMapBuilderConfig = new MapBuilderConfiguration
|
||||
{
|
||||
UseTrajectoryBuilder2D = savedConfig.MapBuilder.UseTrajectoryBuilder2D,
|
||||
UseTrajectoryBuilder3D = savedConfig.MapBuilder.UseTrajectoryBuilder3D,
|
||||
NumBackgroundThreads = savedConfig.MapBuilder.NumBackgroundThreads,
|
||||
OptimizeEveryNNodes = savedConfig.MapBuilder.OptimizeEveryNNodes,
|
||||
MatcherTranslationWeight = savedConfig.MapBuilder.MatcherTranslationWeight,
|
||||
MatcherRotationWeight = savedConfig.MapBuilder.MatcherRotationWeight,
|
||||
MaxNumFinalIterations = savedConfig.MapBuilder.MaxNumFinalIterations,
|
||||
GlobalSamplingRatio = savedConfig.MapBuilder.GlobalSamplingRatio,
|
||||
LogResidualHistograms = savedConfig.MapBuilder.LogResidualHistograms,
|
||||
GlobalConstraintSearchAfterNSeconds = savedConfig.MapBuilder.GlobalConstraintSearchAfterNSeconds,
|
||||
EnableSingleTrajectoryLoopClosure = savedConfig.MapBuilder.EnableSingleTrajectoryLoopClosure,
|
||||
SingleTrajectoryLoopClosureDistanceThreshold = savedConfig.MapBuilder.SingleTrajectoryLoopClosureDistanceThreshold,
|
||||
PoseGraphOptimizationProblemOptions = new OptimizationProblemOptions
|
||||
{
|
||||
HuberScale = savedConfig.MapBuilder.OptimizationProblemOptions.HuberScale,
|
||||
AccelerationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.AccelerationWeight,
|
||||
RotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.RotationWeight,
|
||||
LocalSlamPoseTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.LocalSlamPoseTranslationWeight,
|
||||
LocalSlamPoseRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.LocalSlamPoseRotationWeight,
|
||||
OdometryTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.OdometryTranslationWeight,
|
||||
OdometryRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.OdometryRotationWeight,
|
||||
FixedFramePoseTranslationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTranslationWeight,
|
||||
FixedFramePoseRotationWeight = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseRotationWeight,
|
||||
FixedFramePoseUseTolerantLoss = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
|
||||
FixedFramePoseTolerantLossParamA = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
|
||||
FixedFramePoseTolerantLossParamB = savedConfig.MapBuilder.OptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
|
||||
LogSolverSummary = savedConfig.MapBuilder.OptimizationProblemOptions.LogSolverSummary,
|
||||
MaxNumIterations = savedConfig.MapBuilder.OptimizationProblemOptions.MaxNumIterations
|
||||
}
|
||||
};
|
||||
|
||||
return TrajectoryBuilderOptionsFactory.CreatePoseGraphOptions(tempMapBuilderConfig);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tạo config snapshot từ configuration hiện tại (để lưu vào map.json khi save).
|
||||
/// </summary>
|
||||
public static MapCartographerConfigSnapshot CreateConfigSnapshot(CartographerConfiguration config)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(config);
|
||||
var use2D = config.MapBuilder.UseTrajectoryBuilder2D ?? config.TrajectoryBuilder.Use2D;
|
||||
|
||||
return new MapCartographerConfigSnapshot
|
||||
{
|
||||
Use2D = use2D,
|
||||
MapBuilder = new MapCarographerBuilderConfigSnapshot
|
||||
{
|
||||
UseTrajectoryBuilder2D = config.MapBuilder.UseTrajectoryBuilder2D,
|
||||
UseTrajectoryBuilder3D = config.MapBuilder.UseTrajectoryBuilder3D,
|
||||
NumBackgroundThreads = config.MapBuilder.NumBackgroundThreads,
|
||||
OptimizeEveryNNodes = config.MapBuilder.OptimizeEveryNNodes,
|
||||
MatcherTranslationWeight = config.MapBuilder.MatcherTranslationWeight,
|
||||
MatcherRotationWeight = config.MapBuilder.MatcherRotationWeight,
|
||||
MaxNumFinalIterations = config.MapBuilder.MaxNumFinalIterations,
|
||||
GlobalSamplingRatio = config.MapBuilder.GlobalSamplingRatio,
|
||||
LogResidualHistograms = config.MapBuilder.LogResidualHistograms,
|
||||
GlobalConstraintSearchAfterNSeconds = config.MapBuilder.GlobalConstraintSearchAfterNSeconds,
|
||||
EnableSingleTrajectoryLoopClosure = config.MapBuilder.EnableSingleTrajectoryLoopClosure,
|
||||
SingleTrajectoryLoopClosureDistanceThreshold = config.MapBuilder.SingleTrajectoryLoopClosureDistanceThreshold,
|
||||
OptimizationProblemOptions = new OptimizationProblemOptionsSnapshot
|
||||
{
|
||||
HuberScale = config.MapBuilder.PoseGraphOptimizationProblemOptions.HuberScale,
|
||||
AccelerationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.AccelerationWeight,
|
||||
RotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.RotationWeight,
|
||||
LocalSlamPoseTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.LocalSlamPoseTranslationWeight,
|
||||
LocalSlamPoseRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.LocalSlamPoseRotationWeight,
|
||||
OdometryTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.OdometryTranslationWeight,
|
||||
OdometryRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.OdometryRotationWeight,
|
||||
FixedFramePoseTranslationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTranslationWeight,
|
||||
FixedFramePoseRotationWeight = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseRotationWeight,
|
||||
FixedFramePoseUseTolerantLoss = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseUseTolerantLoss,
|
||||
FixedFramePoseTolerantLossParamA = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamA,
|
||||
FixedFramePoseTolerantLossParamB = config.MapBuilder.PoseGraphOptimizationProblemOptions.FixedFramePoseTolerantLossParamB,
|
||||
LogSolverSummary = config.MapBuilder.PoseGraphOptimizationProblemOptions.LogSolverSummary,
|
||||
MaxNumIterations = config.MapBuilder.PoseGraphOptimizationProblemOptions.MaxNumIterations
|
||||
}
|
||||
},
|
||||
TrajectoryBuilder = new TrajectoryBuilderConfigSnapshot
|
||||
{
|
||||
Use2D = config.TrajectoryBuilder.Use2D,
|
||||
MinRange = config.TrajectoryBuilder.MinRange,
|
||||
MaxRange = config.TrajectoryBuilder.MaxRange,
|
||||
MinZ = config.TrajectoryBuilder.MinZ,
|
||||
MaxZ = config.TrajectoryBuilder.MaxZ,
|
||||
MissingDataRayLength = config.TrajectoryBuilder.MissingDataRayLength,
|
||||
VoxelFilterSize = config.TrajectoryBuilder.VoxelFilterSize,
|
||||
NumAccumulatedRangeData = config.TrajectoryBuilder.NumAccumulatedRangeData,
|
||||
UseImuData = config.TrajectoryBuilder.UseImuData,
|
||||
UseOnlineCorrelativeScanMatching = config.TrajectoryBuilder.UseOnlineCorrelativeScanMatching
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user