1494 lines
54 KiB
C#
1494 lines
54 KiB
C#
/*
|
|
* Copyright 2016 The Cartographer Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
using CartographerSharp.Common.Threading;
|
|
using CartographerSharp.Mapping.Internal.Constraints;
|
|
using CartographerSharp.Mapping.Internal.D3D.Optimization;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Models.Transform;
|
|
using CartographerSharp.Sensor;
|
|
using CartographerSharp.Transform;
|
|
using System;
|
|
using RobotNet10.Shared.Numbers;
|
|
using PoseGraphOptions = CartographerSharp.Models.Mapping.PoseGraphOptions;
|
|
|
|
namespace CartographerSharp.Mapping.Internal.D3D;
|
|
|
|
/// <summary>
|
|
/// Internal submap data structure for PoseGraph3D.
|
|
/// </summary>
|
|
internal struct InternalSubmapData3D
|
|
{
|
|
public Mapping.D3D.Submap3D? Submap { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
|
|
/// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
|
|
/// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
|
|
/// on (pp. 22--29). IEEE, 2010.
|
|
///
|
|
/// It is extended for submapping in 3D:
|
|
/// Each node has been matched against one or more submaps (adding a constraint
|
|
/// for each match), both poses of nodes and of submaps are to be optimized.
|
|
/// All constraints are between a submap i and a node j.
|
|
/// </summary>
|
|
public class PoseGraph3D : PoseGraph, IDisposable
|
|
{
|
|
private readonly PoseGraphOptions _options;
|
|
private readonly ThreadPoolInterface? _threadPool;
|
|
private GlobalSlamOptimizationCallback? _globalSlamOptimizationCallback;
|
|
|
|
// Data structures
|
|
private readonly MapById<SubmapId, IPoseGraph.SubmapData> _submapData = new();
|
|
private readonly MapById<NodeId, TrajectoryNode> _trajectoryNodes = new();
|
|
private readonly MapById<NodeId, TrajectoryNodePose> _trajectoryNodePoses = new();
|
|
private readonly List<IPoseGraph.Constraint> _constraints = [];
|
|
private readonly Dictionary<int, IPoseGraph.TrajectoryState> _trajectoryStates = [];
|
|
private readonly Dictionary<int, IPoseGraph.TrajectoryData> _trajectoryData = [];
|
|
private readonly Dictionary<string, Rigid3d> _landmarkPoses = [];
|
|
private readonly Dictionary<string, IPoseGraph.LandmarkNode> _landmarkNodes = [];
|
|
private readonly Dictionary<int, Rigid3d> _localToGlobalTransforms = [];
|
|
private readonly Dictionary<int, InitialTrajectoryPose> _initialTrajectoryPoses = [];
|
|
|
|
// Sensor data storage (simplified - full implementation would use MapByTime)
|
|
private readonly Dictionary<int, List<ImuData>> _imuData = [];
|
|
private readonly Dictionary<int, List<OdometryData>> _odometryData = [];
|
|
private readonly Dictionary<int, List<FixedFramePoseData>> _fixedFramePoseData = [];
|
|
|
|
// ConstraintBuilder3D for inter-submap constraint computation
|
|
private readonly ConstraintBuilder3D? _constraintBuilder;
|
|
|
|
// Periodic optimization tracking
|
|
private readonly Dictionary<int, int> _numNodesSinceLastOptimization = [];
|
|
private readonly Lock _optimizationLock = new();
|
|
private Thread? _optimizationThread;
|
|
private bool _optimizationInProgress = false;
|
|
|
|
// Track node-to-submap insertions for constraint builder
|
|
private readonly Dictionary<SubmapId, HashSet<NodeId>> _submapNodeInsertions = [];
|
|
|
|
// Trimmer support
|
|
private readonly List<PoseGraphTrimmer> _trimmers = [];
|
|
|
|
private OptimizationProblem3D? _optimizationProblem;
|
|
|
|
// CRITICAL FIX: Match C++ - Work queue for serializing operations (similar to PoseGraph2D)
|
|
// Without work queue, constraint computation blocks AddNode caller and can cause lock contention.
|
|
private readonly WorkQueue _workQueue;
|
|
|
|
// Mutex for data structures (only accessed from work queue thread)
|
|
private readonly object _dataLock = new();
|
|
|
|
// Track work items for status reporting
|
|
private int _workItemsAdded = 0;
|
|
private int _workItemsCompleted = 0;
|
|
|
|
/// <summary>
|
|
/// Gets the total number of work items added to the work queue.
|
|
/// </summary>
|
|
public override int WorkItemsAdded => _workItemsAdded;
|
|
|
|
/// <summary>
|
|
/// Gets the total number of work items completed by the work queue.
|
|
/// </summary>
|
|
public override int WorkItemsCompleted => _workItemsCompleted;
|
|
|
|
/// <summary>
|
|
/// Gets the number of work items currently pending in the work queue.
|
|
/// </summary>
|
|
public override int WorkItemsPending => _workQueue.Count;
|
|
|
|
/// <summary>
|
|
/// Gets the current number of items in the work queue.
|
|
/// </summary>
|
|
public override int WorkQueueCount => _workQueue.Count;
|
|
|
|
/// <summary>
|
|
/// Gets the number of nodes started in the constraint builder.
|
|
/// </summary>
|
|
public override int ConstraintBuilderNodesStarted => _constraintBuilder?.GetNumStartedNodes() ?? 0;
|
|
|
|
/// <summary>
|
|
/// Gets the number of nodes finished in the constraint builder.
|
|
/// </summary>
|
|
public override int ConstraintBuilderNodesFinished => _constraintBuilder?.GetNumFinishedNodes() ?? 0;
|
|
|
|
/// <summary>
|
|
/// Gets the total number of trajectory nodes in the pose graph.
|
|
/// Used for progress tracking during optimization.
|
|
/// </summary>
|
|
public override int TrajectoryNodesCount
|
|
{
|
|
get
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return _trajectoryNodes.Count;
|
|
}
|
|
}
|
|
}
|
|
|
|
public override int ConstraintTasksTotal => 0;
|
|
public override int ConstraintTasksFinished => 0;
|
|
|
|
// Constructor
|
|
public PoseGraph3D(PoseGraphOptions options, OptimizationProblem3D? optimizationProblem = null, ThreadPoolInterface? threadPool = null) : base()
|
|
{
|
|
_options = options;
|
|
_threadPool = threadPool;
|
|
|
|
// CRITICAL FIX: Initialize work queue for async constraint computation (match PoseGraph2D)
|
|
_workQueue = new WorkQueue();
|
|
_workQueue.OptimizationNeeded += OnOptimizationNeeded;
|
|
|
|
// Get OptimizationProblemOptions from PoseGraphOptions or use defaults
|
|
var optimizationProblemOptions = _options.OptimizationProblemOptions ?? new OptimizationProblemOptions();
|
|
_optimizationProblem = optimizationProblem ?? new OptimizationProblem3D(optimizationProblemOptions);
|
|
|
|
// Setup ConstraintBuilder3D if options are provided and threadPool is available
|
|
// Match C++ (pose_graph_3d.cc): Pass thread_pool to ConstraintBuilder3D
|
|
if (_options.ConstraintBuilderOptions.HasValue && _threadPool != null)
|
|
{
|
|
var constraintBuilderOptions = _options.ConstraintBuilderOptions.Value;
|
|
_constraintBuilder = new ConstraintBuilder3D(constraintBuilderOptions, _threadPool);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Handler for when work queue signals optimization is needed.
|
|
/// Match C++ HandleWorkQueue: Run optimization SYNCHRONOUSLY and then notify work queue to continue.
|
|
/// </summary>
|
|
private void OnOptimizationNeeded(object? sender, EventArgs e)
|
|
{
|
|
try
|
|
{
|
|
// Run optimization SYNCHRONOUSLY (matches C++ HandleWorkQueue behavior)
|
|
// This blocks the work queue thread until optimization completes,
|
|
// preventing lock contention between optimization and work items.
|
|
RunOptimizationSync();
|
|
}
|
|
finally
|
|
{
|
|
// CRITICAL: Always notify work queue that optimization is done
|
|
// This allows work queue to continue processing items
|
|
_workQueue.NotifyOptimizationDone();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a work item to the queue. Non-blocking.
|
|
/// </summary>
|
|
private void AddWorkItem(string name, Func<WorkItemResult> workItem)
|
|
{
|
|
Interlocked.Increment(ref _workItemsAdded);
|
|
|
|
Func<WorkItemResult> wrappedWorkItem = () =>
|
|
{
|
|
try
|
|
{
|
|
return workItem();
|
|
}
|
|
finally
|
|
{
|
|
Interlocked.Increment(ref _workItemsCompleted);
|
|
}
|
|
};
|
|
|
|
_workQueue.AddWorkItem(new WorkItem(wrappedWorkItem));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
|
|
/// determined by scan matching against 'insertion_submaps.front()' and the
|
|
/// node data was inserted into the 'insertion_submaps'.
|
|
///
|
|
/// CRITICAL FIX: Match C++ implementation - AppendNode is SYNCHRONOUS,
|
|
/// constraint computation is QUEUED to work queue for async processing.
|
|
/// </summary>
|
|
public NodeId AddNode(
|
|
TrajectoryNode.Data constantData,
|
|
int trajectoryId,
|
|
List<Mapping.D3D.Submap3D> insertionSubmaps)
|
|
{
|
|
var optimizedPose = GetLocalToGlobalTransformInternal(trajectoryId) * constantData.LocalPose;
|
|
|
|
// Call AppendNodeUnsafe SYNCHRONOUSLY with lock (matches C++ AppendNode behavior)
|
|
// This is fast and blocking, but necessary to return nodeId immediately
|
|
NodeId nodeId;
|
|
bool newlyFinishedSubmap;
|
|
List<SubmapId> submapIds;
|
|
|
|
lock (_dataLock)
|
|
{
|
|
nodeId = AppendNodeUnsafe(constantData, trajectoryId, insertionSubmaps, optimizedPose, out submapIds);
|
|
// Check if first submap is newly finished (must check here before queuing)
|
|
newlyFinishedSubmap = insertionSubmaps.Count > 0 && insertionSubmaps[0].InsertionFinished;
|
|
}
|
|
|
|
// Queue constraint computation to work queue (matches C++ ComputeConstraintsForNode)
|
|
// This is done asynchronously so AddNode can return quickly
|
|
var capturedNodeId = nodeId;
|
|
var capturedConstantData = constantData;
|
|
var capturedTrajectoryId = trajectoryId;
|
|
var capturedInsertionSubmaps = insertionSubmaps;
|
|
var capturedSubmapIds = submapIds;
|
|
var capturedOptimizedPose = optimizedPose;
|
|
var capturedNewlyFinishedSubmap = newlyFinishedSubmap;
|
|
|
|
AddWorkItem($"AddNode trajectoryId={trajectoryId} nodeIndex={nodeId.NodeIndex}", () =>
|
|
{
|
|
WorkItemResult result;
|
|
|
|
lock (_dataLock)
|
|
{
|
|
result = ComputeConstraintsForNodeUnsafe(
|
|
capturedNodeId,
|
|
capturedConstantData,
|
|
capturedTrajectoryId,
|
|
capturedInsertionSubmaps,
|
|
capturedSubmapIds,
|
|
capturedOptimizedPose,
|
|
capturedNewlyFinishedSubmap);
|
|
}
|
|
|
|
return result;
|
|
});
|
|
|
|
return nodeId;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Appends a node to the trajectory (matches C++ AppendNode).
|
|
/// Only appends node and submap to data structures with intra-submap constraints - fast and synchronous.
|
|
/// Constraint building is done separately in ComputeConstraintsForNodeUnsafe.
|
|
/// Assumes lock (_dataLock) held by caller.
|
|
/// </summary>
|
|
private NodeId AppendNodeUnsafe(
|
|
TrajectoryNode.Data constantData,
|
|
int trajectoryId,
|
|
List<Mapping.D3D.Submap3D> insertionSubmaps,
|
|
Rigid3d optimizedPose,
|
|
out List<SubmapId> submapIds)
|
|
{
|
|
AddTrajectoryIfNeeded(trajectoryId);
|
|
|
|
var nodeId = _trajectoryNodes.Append(
|
|
trajectoryId,
|
|
new TrajectoryNode
|
|
{
|
|
ConstantData = constantData,
|
|
GlobalPose = optimizedPose
|
|
});
|
|
|
|
// Add trajectory node pose
|
|
_trajectoryNodePoses.Insert(nodeId, new TrajectoryNodePose
|
|
{
|
|
GlobalPose = optimizedPose,
|
|
ConstantPoseData = new ConstantPoseData
|
|
{
|
|
Time = constantData.Time,
|
|
LocalPose = constantData.LocalPose
|
|
}
|
|
});
|
|
|
|
// Test if the 'insertion_submaps.back()' is one we never saw before
|
|
var lastSubmap = insertionSubmaps[^1];
|
|
var lastSubmapFound = false;
|
|
if (_submapData.SizeOfTrajectoryOrZero(trajectoryId) > 0)
|
|
{
|
|
// Check if last submap in trajectory matches
|
|
var trajectorySubmaps = _submapData.BeginOfTrajectory(trajectoryId).ToList();
|
|
if (trajectorySubmaps.Count > 0)
|
|
{
|
|
var lastTrajectorySubmap = trajectorySubmaps[^1];
|
|
if (lastTrajectorySubmap.Data.Submap == lastSubmap)
|
|
{
|
|
lastSubmapFound = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!lastSubmapFound)
|
|
{
|
|
// We grow 'data_.submap_data' as needed
|
|
_submapData.Append(trajectoryId, new IPoseGraph.SubmapData(lastSubmap, optimizedPose));
|
|
}
|
|
|
|
// Add intra-submap constraints for all insertion submaps
|
|
// Use matcher weights from options for constraint weights
|
|
var translationWeight = _options.MatcherTranslationWeight;
|
|
var rotationWeight = _options.MatcherRotationWeight;
|
|
|
|
// Find or create submap IDs for insertion submaps
|
|
submapIds = [];
|
|
foreach (var submap in insertionSubmaps)
|
|
{
|
|
SubmapId? foundId = null;
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId && kvp.Data.Submap == submap)
|
|
{
|
|
foundId = kvp.Id;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (foundId.HasValue)
|
|
{
|
|
submapIds.Add(foundId.Value);
|
|
}
|
|
}
|
|
|
|
// Add intra-submap constraints
|
|
// C++ line 337-338: constraint_transform = insertion_submaps[i]->local_pose().inverse() * local_pose
|
|
// Constraint transform must be computed from local poses, not global poses
|
|
foreach (var submapId in submapIds)
|
|
{
|
|
// Find the corresponding insertion submap for this submapId
|
|
Mapping.D3D.Submap3D? insertionSubmap = null;
|
|
foreach (var submap in insertionSubmaps)
|
|
{
|
|
if (_submapData.Contains(submapId) && _submapData[submapId].Submap == submap)
|
|
{
|
|
insertionSubmap = submap;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (insertionSubmap == null)
|
|
continue;
|
|
|
|
// Compute constraint transform from local poses (C++ line 338)
|
|
var localPose = constantData.LocalPose;
|
|
var constraintTransform = insertionSubmap.LocalPose.Inverse() * localPose;
|
|
|
|
// Track node-to-submap insertion for constraint builder
|
|
if (!_submapNodeInsertions.TryGetValue(submapId, out var nodes))
|
|
{
|
|
nodes = [];
|
|
_submapNodeInsertions[submapId] = nodes;
|
|
}
|
|
nodes.Add(nodeId);
|
|
|
|
// Check if constraint already exists
|
|
var constraintExists = _constraints.Any(c =>
|
|
c.NodeId.Equals(nodeId) && c.SubmapId.Equals(submapId));
|
|
|
|
if (!constraintExists)
|
|
{
|
|
_constraints.Add(new IPoseGraph.Constraint(
|
|
submapId,
|
|
nodeId,
|
|
new IPoseGraph.Constraint.Pose(
|
|
constraintTransform,
|
|
translationWeight,
|
|
rotationWeight
|
|
),
|
|
IPoseGraph.Constraint.Tag.IntraSubmap
|
|
));
|
|
}
|
|
}
|
|
|
|
return nodeId;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes inter-submap constraints for a node.
|
|
/// Called from work queue after AppendNodeUnsafe.
|
|
/// Assumes lock (_dataLock) held by caller.
|
|
/// </summary>
|
|
private WorkItemResult ComputeConstraintsForNodeUnsafe(
|
|
NodeId nodeId,
|
|
TrajectoryNode.Data constantData,
|
|
int trajectoryId,
|
|
List<Mapping.D3D.Submap3D> insertionSubmaps,
|
|
List<SubmapId> submapIds,
|
|
Rigid3d optimizedPose,
|
|
bool newlyFinishedSubmap)
|
|
{
|
|
// Schedule inter-submap constraint search with ConstraintBuilder3D if available
|
|
if (_constraintBuilder != null && insertionSubmaps.Count > 0 && submapIds.Count > 0)
|
|
{
|
|
var firstSubmap = insertionSubmaps[0];
|
|
var firstSubmapId = submapIds[0];
|
|
|
|
if (firstSubmap.InsertionFinished)
|
|
{
|
|
bool constraintsAdded = false;
|
|
|
|
// Find finished submaps to match against (inter-submap constraints)
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId &&
|
|
kvp.Data.Submap is Mapping.D3D.Submap3D finishedSubmap3D &&
|
|
finishedSubmap3D.InsertionFinished &&
|
|
kvp.Id != firstSubmapId) // Don't match against itself
|
|
{
|
|
var node = _trajectoryNodes[nodeId];
|
|
var finishedSubmapPose = kvp.Data.Pose;
|
|
|
|
// Schedule constraint computation (inter-submap constraint)
|
|
_constraintBuilder.MaybeAddConstraint(
|
|
kvp.Id,
|
|
nodeId,
|
|
finishedSubmap3D,
|
|
node,
|
|
optimizedPose,
|
|
finishedSubmapPose);
|
|
constraintsAdded = true;
|
|
}
|
|
}
|
|
|
|
// Also try global constraint search if configured
|
|
if (_options.GlobalSamplingRatio > 0)
|
|
{
|
|
// Sample based on global_sampling_ratio
|
|
var random = new Random((int)nodeId.NodeIndex);
|
|
if (random.NextDouble() < _options.GlobalSamplingRatio)
|
|
{
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
if (kvp.Data.Submap is Mapping.D3D.Submap3D globalSubmap3D &&
|
|
globalSubmap3D.InsertionFinished)
|
|
{
|
|
var node = _trajectoryNodes[nodeId];
|
|
var globalSubmapPose = kvp.Data.Pose;
|
|
|
|
// Get rotations for global matching
|
|
// MaybeAddGlobalConstraint takes Quaternion, which we can extract from Rigid3d
|
|
var globalNodeRotation = optimizedPose.Rotation;
|
|
var globalSubmapRotation = globalSubmapPose.Rotation;
|
|
|
|
_constraintBuilder.MaybeAddGlobalConstraint(
|
|
kvp.Id,
|
|
nodeId,
|
|
globalSubmap3D,
|
|
node,
|
|
globalNodeRotation,
|
|
globalSubmapRotation);
|
|
constraintsAdded = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Call WhenDone once after all constraint computations are scheduled
|
|
if (constraintsAdded)
|
|
{
|
|
_constraintBuilder.WhenDone(OnConstraintBuilderResult);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Check if periodic optimization should be triggered
|
|
// FIXED: Use GLOBAL counter instead of per-trajectory (match C++ behavior)
|
|
if (!_numNodesSinceLastOptimization.TryGetValue(trajectoryId, out int value))
|
|
{
|
|
value = 0;
|
|
_numNodesSinceLastOptimization[trajectoryId] = value;
|
|
}
|
|
_numNodesSinceLastOptimization[trajectoryId] = ++value;
|
|
|
|
if (_options.OptimizeEveryNNodes > 0 && value >= _options.OptimizeEveryNNodes)
|
|
{
|
|
// Reset counter and signal optimization needed
|
|
_numNodesSinceLastOptimization[trajectoryId] = 0;
|
|
return WorkItemResult.RunOptimization;
|
|
}
|
|
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Callback invoked when ConstraintBuilder3D finishes computing constraints.
|
|
/// Match C++ HandleWorkQueue behavior: Add constraints then run optimization.
|
|
/// </summary>
|
|
private void OnConstraintBuilderResult(ConstraintBuilder3DResult result)
|
|
{
|
|
if (result.Constraints == null || result.Constraints.Count == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Match C++ HandleWorkQueue: Add constraints with dataLock
|
|
// FIXED: Use _dataLock instead of _optimizationLock for data access
|
|
lock (_dataLock)
|
|
{
|
|
foreach (var constraint in result.Constraints)
|
|
{
|
|
_constraints.Add(constraint);
|
|
}
|
|
}
|
|
|
|
// Match C++ HandleWorkQueue: Run optimization SYNCHRONOUSLY after adding constraints
|
|
RunOptimizationSync();
|
|
|
|
// Invoke global SLAM optimization callback
|
|
InvokeGlobalSlamOptimizationCallback();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds connectivity and sampler for a trajectory if it does not exist.
|
|
/// </summary>
|
|
private void AddTrajectoryIfNeeded(int trajectoryId)
|
|
{
|
|
if (!_trajectoryStates.TryGetValue(trajectoryId, out IPoseGraph.TrajectoryState state))
|
|
{
|
|
state = IPoseGraph.TrajectoryState.Active;
|
|
_trajectoryStates[trajectoryId] = state;
|
|
}
|
|
|
|
if (state == IPoseGraph.TrajectoryState.Finished || state == IPoseGraph.TrajectoryState.Deleted)
|
|
{
|
|
throw new InvalidOperationException($"Cannot add to finished or deleted trajectory {trajectoryId}");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the local to global transform for a trajectory (private helper).
|
|
/// </summary>
|
|
private Rigid3d GetLocalToGlobalTransformInternal(int trajectoryId)
|
|
{
|
|
if (_localToGlobalTransforms.TryGetValue(trajectoryId, out var transform))
|
|
{
|
|
return transform;
|
|
}
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the local to global map frame transform based on the given global submap poses.
|
|
/// </summary>
|
|
private Rigid3d ComputeLocalToGlobalTransform(
|
|
MapById<SubmapId, SubmapSpec3D> globalSubmapPoses,
|
|
int trajectoryId)
|
|
{
|
|
// Find first submap for this trajectory
|
|
var firstSubmapEnum = globalSubmapPoses.BeginOfTrajectory(trajectoryId);
|
|
var firstSubmapList = firstSubmapEnum.ToList();
|
|
if (firstSubmapList.Count == 0)
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
var firstSubmap = firstSubmapList[0];
|
|
|
|
// Get the submap from our data
|
|
var submapId = firstSubmap.Id;
|
|
if (!_submapData.Contains(submapId))
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
if (_submapData[submapId].Submap is not Mapping.D3D.Submap3D submap)
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
// Compute transform: global_submap_pose * local_submap_pose^-1
|
|
var globalSubmapPose = firstSubmap.Data.GlobalPose;
|
|
var localSubmapPose = submap.LocalPose;
|
|
return globalSubmapPose * localSubmapPose.Inverse();
|
|
}
|
|
|
|
// PoseGraphInterface implementation
|
|
public override void AddImuData(int trajectoryId, ImuData imuData)
|
|
{
|
|
if (!_imuData.TryGetValue(trajectoryId, out List<ImuData>? value))
|
|
{
|
|
value = [];
|
|
_imuData[trajectoryId] = value;
|
|
}
|
|
|
|
value.Add(imuData);
|
|
|
|
_optimizationProblem?.AddImuData(trajectoryId, imuData);
|
|
}
|
|
|
|
public override void AddOdometryData(int trajectoryId, OdometryData odometryData)
|
|
{
|
|
if (!_odometryData.TryGetValue(trajectoryId, out List<OdometryData>? value))
|
|
{
|
|
value = [];
|
|
_odometryData[trajectoryId] = value;
|
|
}
|
|
|
|
value.Add(odometryData);
|
|
|
|
_optimizationProblem?.AddOdometryData(trajectoryId, odometryData);
|
|
}
|
|
|
|
public override void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData)
|
|
{
|
|
if (!_fixedFramePoseData.TryGetValue(trajectoryId, out List<FixedFramePoseData>? value))
|
|
{
|
|
value = [];
|
|
_fixedFramePoseData[trajectoryId] = value;
|
|
}
|
|
|
|
value.Add(fixedFramePoseData);
|
|
|
|
_optimizationProblem?.AddFixedFramePoseData(trajectoryId, fixedFramePoseData);
|
|
}
|
|
|
|
public override void AddLandmarkData(int trajectoryId, LandmarkData landmarkData)
|
|
{
|
|
foreach (var observation in landmarkData.LandmarkObservations)
|
|
{
|
|
if (!_landmarkNodes.TryGetValue(observation.Id, out IPoseGraph.LandmarkNode node))
|
|
{
|
|
node = new IPoseGraph.LandmarkNode();
|
|
_landmarkNodes[observation.Id] = node;
|
|
}
|
|
|
|
node.LandmarkObservations.Add(new IPoseGraph.LandmarkNode.LandmarkObservation(
|
|
trajectoryId,
|
|
landmarkData.Time,
|
|
observation.LandmarkToTrackingTransform,
|
|
observation.TranslationWeight,
|
|
observation.RotationWeight
|
|
));
|
|
_landmarkNodes[observation.Id] = node;
|
|
}
|
|
}
|
|
|
|
public override void FinishTrajectory(int trajectoryId)
|
|
{
|
|
if (_trajectoryStates.ContainsKey(trajectoryId))
|
|
{
|
|
_trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Finished;
|
|
}
|
|
}
|
|
|
|
public override void FreezeTrajectory(int trajectoryId)
|
|
{
|
|
if (_trajectoryStates.ContainsKey(trajectoryId))
|
|
{
|
|
_trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Frozen;
|
|
}
|
|
}
|
|
|
|
public override bool IsTrajectoryFinished(int trajectoryId)
|
|
{
|
|
return _trajectoryStates.TryGetValue(trajectoryId, out var state) &&
|
|
state == IPoseGraph.TrajectoryState.Finished;
|
|
}
|
|
|
|
public override bool IsTrajectoryFrozen(int trajectoryId)
|
|
{
|
|
return _trajectoryStates.TryGetValue(trajectoryId, out var state) &&
|
|
state == IPoseGraph.TrajectoryState.Frozen;
|
|
}
|
|
|
|
public override void DeleteTrajectory(int trajectoryId)
|
|
{
|
|
_trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Deleted;
|
|
|
|
// Remove trajectory data
|
|
_trajectoryData.Remove(trajectoryId);
|
|
_imuData.Remove(trajectoryId);
|
|
_odometryData.Remove(trajectoryId);
|
|
_fixedFramePoseData.Remove(trajectoryId);
|
|
_localToGlobalTransforms.Remove(trajectoryId);
|
|
|
|
// Remove nodes and submaps for this trajectory
|
|
var nodesToRemove = new List<NodeId>();
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
nodesToRemove.Add(kvp.Id);
|
|
}
|
|
}
|
|
foreach (var nodeId in nodesToRemove)
|
|
{
|
|
_trajectoryNodes.Trim(nodeId);
|
|
_trajectoryNodePoses.Trim(nodeId);
|
|
}
|
|
|
|
var submapsToRemove = new List<SubmapId>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
submapsToRemove.Add(kvp.Id);
|
|
}
|
|
}
|
|
foreach (var submapId in submapsToRemove)
|
|
{
|
|
_submapData.Trim(submapId);
|
|
}
|
|
|
|
// Remove constraints involving this trajectory
|
|
_constraints.RemoveAll(c =>
|
|
c.SubmapId.TrajectoryId == trajectoryId ||
|
|
c.NodeId.TrajectoryId == trajectoryId);
|
|
}
|
|
|
|
public override void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap)
|
|
{
|
|
if (!submap.Submap3D.HasValue)
|
|
{
|
|
throw new ArgumentException("Submap must contain Submap3D", nameof(submap));
|
|
}
|
|
|
|
var submap3D = new Mapping.D3D.Submap3D(submap.Submap3D.Value);
|
|
var trajectoryId = submap.SubmapId.TrajectoryId;
|
|
var submapIndex = submap.SubmapId.SubmapIndex;
|
|
var submapId = new SubmapId(trajectoryId, submapIndex);
|
|
|
|
_submapData.Insert(submapId, new IPoseGraph.SubmapData(submap3D, globalPose));
|
|
|
|
_optimizationProblem?.InsertSubmap(submapId, globalPose);
|
|
}
|
|
|
|
public override void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node)
|
|
{
|
|
var nodeData = TrajectoryNodeOperations.FromProto(node.NodeData);
|
|
var trajectoryId = node.NodeId.TrajectoryId;
|
|
var nodeIndex = node.NodeId.NodeIndex;
|
|
var nodeId = new NodeId(trajectoryId, nodeIndex);
|
|
|
|
_trajectoryNodes.Insert(nodeId, new TrajectoryNode
|
|
{
|
|
ConstantData = nodeData,
|
|
GlobalPose = globalPose
|
|
});
|
|
|
|
_trajectoryNodePoses.Insert(nodeId, new TrajectoryNodePose
|
|
{
|
|
GlobalPose = globalPose,
|
|
ConstantPoseData = new ConstantPoseData
|
|
{
|
|
Time = nodeData.Time,
|
|
LocalPose = nodeData.LocalPose
|
|
}
|
|
});
|
|
|
|
_optimizationProblem?.InsertTrajectoryNode(nodeId, new NodeSpec3D(
|
|
nodeData.Time,
|
|
nodeData.LocalPose,
|
|
globalPose
|
|
));
|
|
}
|
|
|
|
public override void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data)
|
|
{
|
|
var trajectoryData = new IPoseGraph.TrajectoryData(
|
|
data.GravityConstant,
|
|
data.ImuCalibration.HasValue ? (Quaternion)data.ImuCalibration.Value : Quaternion.Identity,
|
|
data.FixedFrameOriginInMap.HasValue ? (Rigid3d)data.FixedFrameOriginInMap.Value : null
|
|
);
|
|
|
|
// Store trajectory data - trajectory ID should be in the proto
|
|
var trajectoryId = data.TrajectoryId;
|
|
_trajectoryData[trajectoryId] = trajectoryData;
|
|
|
|
// Set trajectory data in optimization problem if available
|
|
if (_optimizationProblem != null)
|
|
{
|
|
// Note: OptimizationProblem3D may have SetTrajectoryData method
|
|
// For now, trajectory data is stored in _trajectoryData dictionary
|
|
// and will be used during optimization if needed
|
|
}
|
|
}
|
|
|
|
public override void AddNodeToSubmap(NodeId nodeId, SubmapId submapId)
|
|
{
|
|
// C++ line 763-772: AddNodeToSubmap only inserts node_id into submap_data.node_ids
|
|
// It does NOT compute constraints here. Constraints are computed in ComputeConstraintsForNode
|
|
// Track node-to-submap insertion for constraint builder
|
|
if (!_submapNodeInsertions.TryGetValue(submapId, out var nodes))
|
|
{
|
|
nodes = [];
|
|
_submapNodeInsertions[submapId] = nodes;
|
|
}
|
|
nodes.Add(nodeId);
|
|
|
|
// Note: Constraints are computed in AppendNode/ComputeConstraintsForNode, not here
|
|
// This method only tracks the node-to-submap relationship
|
|
}
|
|
|
|
public override void AddSerializedConstraints(List<IPoseGraph.Constraint> constraints)
|
|
{
|
|
_constraints.AddRange(constraints);
|
|
}
|
|
|
|
public override void RunFinalOptimization()
|
|
{
|
|
var optimizationProblemOptions = _options.OptimizationProblemOptions ?? new OptimizationProblemOptions();
|
|
_optimizationProblem ??= new OptimizationProblem3D(optimizationProblemOptions);
|
|
|
|
// Sync submaps and nodes to optimization problem
|
|
// Add all submaps
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
if (!_optimizationProblem.SubmapData().Contains(kvp.Id))
|
|
{
|
|
_optimizationProblem.InsertSubmap(kvp.Id, kvp.Data.Pose);
|
|
}
|
|
}
|
|
|
|
// Add all nodes
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
if (!_optimizationProblem.NodeData().Contains(kvp.Id))
|
|
{
|
|
var nodeData = kvp.Data.ConstantData;
|
|
if (nodeData != null)
|
|
{
|
|
_optimizationProblem.InsertTrajectoryNode(kvp.Id, new NodeSpec3D(
|
|
nodeData.Time,
|
|
nodeData.LocalPose,
|
|
kvp.Data.GlobalPose
|
|
));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Get trajectory states
|
|
var trajectoryStates = GetTrajectoryStates();
|
|
|
|
// Get landmark nodes
|
|
var landmarkNodes = GetLandmarkNodes();
|
|
|
|
// Use MaxNumFinalIterations for final optimization if available
|
|
// This allows more iterations for better convergence in final optimization
|
|
if (_options.MaxNumFinalIterations > 0)
|
|
{
|
|
_optimizationProblem.SetMaxNumIterations(_options.MaxNumFinalIterations);
|
|
}
|
|
|
|
// Solve optimization problem
|
|
_optimizationProblem.Solve(Constraints(), trajectoryStates, landmarkNodes);
|
|
|
|
// Invoke global SLAM optimization callback if set
|
|
InvokeGlobalSlamOptimizationCallback();
|
|
|
|
// Update poses from optimization results
|
|
// Update submap poses
|
|
var optimizedSubmapData = _optimizationProblem.SubmapData();
|
|
foreach (var kvp in optimizedSubmapData)
|
|
{
|
|
if (_submapData.Contains(kvp.Id))
|
|
{
|
|
var submapData = _submapData[kvp.Id];
|
|
_submapData[kvp.Id] = new IPoseGraph.SubmapData(submapData.Submap, kvp.Data.GlobalPose);
|
|
}
|
|
}
|
|
|
|
// Update node poses
|
|
var optimizedNodeData = _optimizationProblem.NodeData();
|
|
foreach (var kvp in optimizedNodeData)
|
|
{
|
|
if (_trajectoryNodes.Contains(kvp.Id))
|
|
{
|
|
var optimizedPose3D = kvp.Data.GlobalPose;
|
|
|
|
// Update trajectory node pose
|
|
if (_trajectoryNodePoses.Contains(kvp.Id))
|
|
{
|
|
var nodePose = _trajectoryNodePoses[kvp.Id];
|
|
nodePose.GlobalPose = optimizedPose3D;
|
|
_trajectoryNodePoses[kvp.Id] = nodePose;
|
|
}
|
|
|
|
// Update trajectory node
|
|
var node = _trajectoryNodes[kvp.Id];
|
|
node.GlobalPose = optimizedPose3D;
|
|
_trajectoryNodes[kvp.Id] = node;
|
|
}
|
|
}
|
|
|
|
// Update local to global transforms
|
|
foreach (var trajectoryId in _trajectoryStates.Keys)
|
|
{
|
|
var transform = ComputeLocalToGlobalTransform(_optimizationProblem.SubmapData(), trajectoryId);
|
|
_localToGlobalTransforms[trajectoryId] = transform;
|
|
}
|
|
|
|
// Run trimmers after optimization
|
|
RunTrimmers();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Runs optimization (can be called periodically or as final optimization).
|
|
/// Thread-safe: only one optimization runs at a time.
|
|
/// </summary>
|
|
private void RunOptimization()
|
|
{
|
|
lock (_optimizationLock)
|
|
{
|
|
// If optimization is already in progress, skip
|
|
if (_optimizationInProgress)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_optimizationInProgress = true;
|
|
}
|
|
|
|
// Run optimization on dedicated high-priority thread to avoid blocking
|
|
_optimizationThread = new Thread(() =>
|
|
{
|
|
Thread.BeginThreadAffinity();
|
|
try
|
|
{
|
|
RunFinalOptimization();
|
|
}
|
|
finally
|
|
{
|
|
Thread.EndThreadAffinity();
|
|
lock (_optimizationLock)
|
|
{
|
|
_optimizationInProgress = false;
|
|
}
|
|
}
|
|
})
|
|
{
|
|
Priority = ThreadPriority.Highest,
|
|
IsBackground = false,
|
|
Name = "CartographerOptimization3D"
|
|
};
|
|
_optimizationThread.Start();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Runs optimization synchronously in the calling thread.
|
|
/// Match C++ RunOptimization() implementation and PoseGraph2D.RunOptimizationSync().
|
|
/// Thread-safe: only one optimization runs at a time.
|
|
/// If optimization is already running, skips (does not queue).
|
|
/// </summary>
|
|
private void RunOptimizationSync()
|
|
{
|
|
lock (_optimizationLock)
|
|
{
|
|
// If optimization is already in progress, skip
|
|
if (_optimizationInProgress)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_optimizationInProgress = true;
|
|
}
|
|
|
|
try
|
|
{
|
|
// Match C++: Check if optimization problem is empty
|
|
if (_optimizationProblem == null || _optimizationProblem.SubmapData().IsEmpty)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Get trajectory states
|
|
var trajectoryStates = GetTrajectoryStates();
|
|
|
|
// Get landmark nodes
|
|
var landmarkNodes = GetLandmarkNodes();
|
|
|
|
// Solve optimization problem with lock for data access
|
|
lock (_dataLock)
|
|
{
|
|
_optimizationProblem.Solve(Constraints(), trajectoryStates, landmarkNodes);
|
|
|
|
// Update poses from optimization results
|
|
var optimizedSubmapData = _optimizationProblem.SubmapData();
|
|
foreach (var kvp in optimizedSubmapData)
|
|
{
|
|
if (_submapData.Contains(kvp.Id))
|
|
{
|
|
var submapData = _submapData[kvp.Id];
|
|
_submapData[kvp.Id] = new IPoseGraph.SubmapData(submapData.Submap, kvp.Data.GlobalPose);
|
|
}
|
|
}
|
|
|
|
// Update node poses
|
|
var optimizedNodeData = _optimizationProblem.NodeData();
|
|
foreach (var kvp in optimizedNodeData)
|
|
{
|
|
if (_trajectoryNodes.Contains(kvp.Id))
|
|
{
|
|
var optimizedPose3D = kvp.Data.GlobalPose;
|
|
|
|
// Update trajectory node pose
|
|
if (_trajectoryNodePoses.Contains(kvp.Id))
|
|
{
|
|
var nodePose = _trajectoryNodePoses[kvp.Id];
|
|
nodePose.GlobalPose = optimizedPose3D;
|
|
_trajectoryNodePoses[kvp.Id] = nodePose;
|
|
}
|
|
|
|
// Update trajectory node
|
|
var node = _trajectoryNodes[kvp.Id];
|
|
node.GlobalPose = optimizedPose3D;
|
|
_trajectoryNodes[kvp.Id] = node;
|
|
}
|
|
}
|
|
|
|
// Update local to global transforms
|
|
foreach (var trajectoryId in _trajectoryStates.Keys)
|
|
{
|
|
var transform = ComputeLocalToGlobalTransform(_optimizationProblem.SubmapData(), trajectoryId);
|
|
_localToGlobalTransforms[trajectoryId] = transform;
|
|
}
|
|
}
|
|
|
|
// Invoke global SLAM optimization callback if set
|
|
InvokeGlobalSlamOptimizationCallback();
|
|
|
|
// Run trimmers after optimization
|
|
RunTrimmers();
|
|
}
|
|
finally
|
|
{
|
|
lock (_optimizationLock)
|
|
{
|
|
_optimizationInProgress = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Invokes the global SLAM optimization callback with current submap and node IDs.
|
|
/// </summary>
|
|
private void InvokeGlobalSlamOptimizationCallback()
|
|
{
|
|
if (_globalSlamOptimizationCallback == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Collect optimized submap IDs per trajectory
|
|
var submapIds = new Dictionary<int, SubmapId>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
submapIds[kvp.Id.TrajectoryId] = kvp.Id;
|
|
}
|
|
|
|
// Collect optimized node IDs per trajectory (use the most recent node)
|
|
var nodeIds = new Dictionary<int, NodeId>();
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
// Keep the latest node for each trajectory
|
|
if (!nodeIds.TryGetValue(kvp.Id.TrajectoryId, out NodeId value) || kvp.Id.NodeIndex > value.NodeIndex)
|
|
{
|
|
value = kvp.Id;
|
|
nodeIds[kvp.Id.TrajectoryId] = value;
|
|
}
|
|
}
|
|
|
|
_globalSlamOptimizationCallback(submapIds, nodeIds);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Runs all registered trimmers.
|
|
/// </summary>
|
|
private void RunTrimmers()
|
|
{
|
|
foreach (var trimmer in _trimmers)
|
|
{
|
|
trimmer.Trim(new TrimmablePoseGraph3D(this));
|
|
}
|
|
}
|
|
|
|
public override MapById<SubmapId, IPoseGraph.SubmapData> GetAllSubmapData()
|
|
{
|
|
return _submapData;
|
|
}
|
|
|
|
public override IPoseGraph.SubmapData GetSubmapData(SubmapId submapId)
|
|
{
|
|
if (_submapData.Contains(submapId))
|
|
{
|
|
return _submapData[submapId];
|
|
}
|
|
return new IPoseGraph.SubmapData(null, Rigid3d.Identity);
|
|
}
|
|
|
|
public override MapById<SubmapId, IPoseGraph.SubmapPose> GetAllSubmapPoses()
|
|
{
|
|
var result = new MapById<SubmapId, IPoseGraph.SubmapPose>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
var version = 0;
|
|
if (kvp.Data.Submap is Mapping.D3D.Submap3D submap3D)
|
|
{
|
|
version = submap3D.NumRangeData;
|
|
}
|
|
result.Insert(kvp.Id, new IPoseGraph.SubmapPose(version, kvp.Data.Pose));
|
|
}
|
|
return result;
|
|
}
|
|
|
|
public override Rigid3d GetLocalToGlobalTransform(int trajectoryId)
|
|
{
|
|
if (_localToGlobalTransforms.TryGetValue(trajectoryId, out var transform))
|
|
{
|
|
return transform;
|
|
}
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
public override MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
|
|
{
|
|
return _trajectoryNodes;
|
|
}
|
|
|
|
public override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
|
|
{
|
|
return _trajectoryNodePoses;
|
|
}
|
|
|
|
public override Dictionary<int, IPoseGraph.TrajectoryState> GetTrajectoryStates()
|
|
{
|
|
return new Dictionary<int, IPoseGraph.TrajectoryState>(_trajectoryStates);
|
|
}
|
|
|
|
public override Dictionary<string, Rigid3d> GetLandmarkPoses()
|
|
{
|
|
return new Dictionary<string, Rigid3d>(_landmarkPoses);
|
|
}
|
|
|
|
public override void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false)
|
|
{
|
|
_landmarkPoses[landmarkId] = globalPose;
|
|
if (!_landmarkNodes.TryGetValue(landmarkId, out var node))
|
|
{
|
|
node = new IPoseGraph.LandmarkNode();
|
|
}
|
|
node.GlobalLandmarkPose = globalPose;
|
|
node.Frozen = frozen;
|
|
_landmarkNodes[landmarkId] = node;
|
|
}
|
|
|
|
public override Dictionary<string, IPoseGraph.LandmarkNode> GetLandmarkNodes()
|
|
{
|
|
return new Dictionary<string, IPoseGraph.LandmarkNode>(_landmarkNodes);
|
|
}
|
|
|
|
public override Dictionary<int, IPoseGraph.TrajectoryData> GetTrajectoryData()
|
|
{
|
|
return new Dictionary<int, IPoseGraph.TrajectoryData>(_trajectoryData);
|
|
}
|
|
|
|
public override List<IPoseGraph.Constraint> Constraints()
|
|
{
|
|
return [.. _constraints];
|
|
}
|
|
|
|
public override void SetInitialTrajectoryPose(int fromTrajectoryId, int toTrajectoryId, Rigid3d pose, long time)
|
|
{
|
|
_initialTrajectoryPoses[fromTrajectoryId] = new InitialTrajectoryPose(toTrajectoryId, pose, time);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets localization initial poses for relocalizing against the map.
|
|
/// Match C++: SetLocalizationInitialPoses (pose_graph_3d.h:166) - empty implementation in 3D.
|
|
/// </summary>
|
|
public override void SetLocalizationInitialPoses(IReadOnlyList<Rigid3d> localizationInitialPoses)
|
|
{
|
|
// Empty implementation - 3D pose graph does not support relocalization
|
|
}
|
|
|
|
public override void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback)
|
|
{
|
|
_globalSlamOptimizationCallback = callback;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 3D pose graph does not use transform_to_map (C++ override is empty).
|
|
/// </summary>
|
|
public override void SetTransformToMap(Rigid3d transform) { }
|
|
|
|
public override Rigid3d GetTransformToMap() => Rigid3d.Identity;
|
|
|
|
public override List<List<int>> GetConnectedTrajectories()
|
|
{
|
|
// Simplified implementation - full implementation would use trajectory connectivity state
|
|
var result = new List<List<int>>();
|
|
var processed = new HashSet<int>();
|
|
|
|
foreach (var trajectoryId in _trajectoryStates.Keys)
|
|
{
|
|
if (processed.Contains(trajectoryId))
|
|
continue;
|
|
|
|
var connected = new List<int> { trajectoryId };
|
|
processed.Add(trajectoryId);
|
|
|
|
// Find connected trajectories via constraints
|
|
foreach (var constraint in _constraints)
|
|
{
|
|
var submapTrajId = constraint.SubmapId.TrajectoryId;
|
|
var nodeTrajId = constraint.NodeId.TrajectoryId;
|
|
|
|
if (submapTrajId == trajectoryId && !processed.Contains(nodeTrajId))
|
|
{
|
|
connected.Add(nodeTrajId);
|
|
processed.Add(nodeTrajId);
|
|
}
|
|
else if (nodeTrajId == trajectoryId && !processed.Contains(submapTrajId))
|
|
{
|
|
connected.Add(submapTrajId);
|
|
processed.Add(submapTrajId);
|
|
}
|
|
}
|
|
|
|
result.Add(connected);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
public override Dictionary<int, List<ImuData>> GetImuData()
|
|
{
|
|
return new Dictionary<int, List<ImuData>>(_imuData);
|
|
}
|
|
|
|
public override Dictionary<int, List<OdometryData>> GetOdometryData()
|
|
{
|
|
return new Dictionary<int, List<OdometryData>>(_odometryData);
|
|
}
|
|
|
|
public override Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData()
|
|
{
|
|
return new Dictionary<int, List<FixedFramePoseData>>(_fixedFramePoseData);
|
|
}
|
|
|
|
public override void AddTrimmer(PoseGraphTrimmer trimmer)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(trimmer);
|
|
|
|
_trimmers.Add(trimmer);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trimmable interface implementation for PoseGraph3D.
|
|
/// </summary>
|
|
private class TrimmablePoseGraph3D(PoseGraph3D poseGraph) : ITrimmable
|
|
{
|
|
public int NumSubmaps(int trajectoryId)
|
|
{
|
|
int count = 0;
|
|
foreach (var kvp in poseGraph._submapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
count++;
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
public List<SubmapId> GetSubmapIds(int trajectoryId)
|
|
{
|
|
var result = new List<SubmapId>();
|
|
foreach (var kvp in poseGraph._submapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
result.Add(kvp.Id);
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
public MapById<SubmapId, IPoseGraph.SubmapData> GetOptimizedSubmapData()
|
|
{
|
|
return poseGraph._submapData;
|
|
}
|
|
|
|
public MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
|
|
{
|
|
return poseGraph._trajectoryNodes;
|
|
}
|
|
|
|
public List<IPoseGraph.Constraint> GetConstraints()
|
|
{
|
|
return [.. poseGraph._constraints];
|
|
}
|
|
|
|
public void TrimSubmap(SubmapId submapId)
|
|
{
|
|
poseGraph._submapData.Trim(submapId);
|
|
}
|
|
|
|
public bool IsFinished(int trajectoryId)
|
|
{
|
|
return poseGraph.IsTrajectoryFinished(trajectoryId);
|
|
}
|
|
|
|
public void SetTrajectoryState(int trajectoryId, IPoseGraph.TrajectoryState state)
|
|
{
|
|
poseGraph._trajectoryStates[trajectoryId] = state;
|
|
}
|
|
}
|
|
|
|
public override Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps)
|
|
{
|
|
var constraints = new List<Models.Mapping.PoseGraph.Constraint>();
|
|
foreach (var constraint in _constraints)
|
|
{
|
|
constraints.Add(ConstraintOperations.ToProto(constraint));
|
|
}
|
|
|
|
var proto = new Models.Mapping.PoseGraph
|
|
{
|
|
Constraints = constraints,
|
|
Trajectories = []
|
|
};
|
|
|
|
// Add trajectories
|
|
var trajectoryIds = new HashSet<int>();
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
trajectoryIds.Add(kvp.Id.TrajectoryId);
|
|
}
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
trajectoryIds.Add(kvp.Id.TrajectoryId);
|
|
}
|
|
|
|
foreach (var trajectoryId in trajectoryIds)
|
|
{
|
|
var trajectory = new Models.Mapping.Trajectory
|
|
{
|
|
TrajectoryId = trajectoryId,
|
|
Nodes = [],
|
|
Submaps = []
|
|
};
|
|
|
|
// Add nodes
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
var nodePose = _trajectoryNodePoses.Contains(kvp.Id)
|
|
? _trajectoryNodePoses[kvp.Id].GlobalPose
|
|
: Rigid3d.Identity;
|
|
trajectory.Nodes.Add(new Models.Mapping.Trajectory.Node(
|
|
kvp.Id.NodeIndex,
|
|
kvp.Data.ConstantData?.Time ?? 0,
|
|
(Rigid3dProto)nodePose
|
|
));
|
|
}
|
|
}
|
|
|
|
// Add submaps
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
if (kvp.Data.Submap is Mapping.D3D.Submap3D submap3D)
|
|
{
|
|
if (includeUnfinishedSubmaps || submap3D.InsertionFinished)
|
|
{
|
|
trajectory.Submaps.Add(new Models.Mapping.Trajectory.Submap(
|
|
kvp.Id.SubmapIndex,
|
|
(Rigid3dProto)kvp.Data.Pose
|
|
));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
proto.Trajectories.Add(trajectory);
|
|
}
|
|
|
|
return proto;
|
|
}
|
|
|
|
// Manual compute methods (not implemented for 3D pose graph - these are 2D-only features)
|
|
|
|
/// <summary>
|
|
/// Manual constraint computation is not implemented for 3D pose graphs.
|
|
/// This feature is only available in 2D SLAM (PoseGraph2D).
|
|
/// </summary>
|
|
public override (double Score, IPoseGraph.Constraint? Constraint) ManualComputeConstraint(
|
|
NodeId nodeId, SubmapId submapId)
|
|
{
|
|
throw new NotImplementedException("Manual compute methods are not implemented for 3D pose graph. Use PoseGraph2D for these features.");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manual constraint score computation is not implemented for 3D pose graphs.
|
|
/// This feature is only available in 2D SLAM (PoseGraph2D).
|
|
/// </summary>
|
|
public override double ManualComputeConstraintScore(
|
|
NodeId nodeId, SubmapId submapId, Rigid3d initialPose)
|
|
{
|
|
throw new NotImplementedException("Manual compute methods are not implemented for 3D pose graph. Use PoseGraph2D for these features.");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manual scan matcher is not implemented for 3D pose graphs.
|
|
/// This feature is only available in 2D SLAM (PoseGraph2D).
|
|
/// </summary>
|
|
public override double ManualComputeScanMatcher(
|
|
NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate)
|
|
{
|
|
poseManualEstimate = Rigid3d.Identity;
|
|
throw new NotImplementedException("Manual compute methods are not implemented for 3D pose graph. Use PoseGraph2D for these features.");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manual relocalization is not implemented for 3D pose graphs.
|
|
/// This feature is only available in 2D SLAM (PoseGraph2D).
|
|
/// </summary>
|
|
public override bool ManualRelocalization(
|
|
int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback)
|
|
{
|
|
score = 0.0;
|
|
throw new NotImplementedException("Manual relocalization is not implemented for 3D pose graph. Use PoseGraph2D for this feature.");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Disposes resources, including work queue and waiting for optimization thread to complete.
|
|
/// </summary>
|
|
public void Dispose()
|
|
{
|
|
// Unsubscribe from work queue events to prevent memory leaks
|
|
_workQueue.OptimizationNeeded -= OnOptimizationNeeded;
|
|
|
|
// Dispose work queue first (this will drain remaining items and stop background thread)
|
|
_workQueue.Dispose();
|
|
|
|
// Wait for optimization thread to complete (with timeout)
|
|
if (_optimizationThread != null)
|
|
{
|
|
if (!_optimizationThread.Join(TimeSpan.FromSeconds(30)))
|
|
{
|
|
// Thread didn't finish in time, but continue cleanup
|
|
// Optimization thread is foreground thread, so it will complete eventually
|
|
}
|
|
}
|
|
|
|
// Clear large collections to free memory
|
|
// Note: After optimization thread is joined, it's safe to clear without lock
|
|
// but we use lock for extra safety in case of concurrent access
|
|
lock (_optimizationLock)
|
|
{
|
|
_imuData.Clear();
|
|
_odometryData.Clear();
|
|
_fixedFramePoseData.Clear();
|
|
_submapNodeInsertions.Clear();
|
|
}
|
|
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
}
|