3987 lines
174 KiB
C#
3987 lines
174 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;
|
|
using CartographerSharp.Mapping.Internal.Constraints;
|
|
using CartographerSharp.Mapping.Internal.Optimization;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Models.Transform;
|
|
using CartographerSharp.Sensor;
|
|
using CartographerSharp.Transform;
|
|
using System.Diagnostics;
|
|
using RobotNet10.Shared.Numbers;
|
|
using PoseGraphOptions = CartographerSharp.Models.Mapping.PoseGraphOptions;
|
|
|
|
namespace CartographerSharp.Mapping.Internal.D2D;
|
|
|
|
/// <summary>
|
|
/// Callback invoked when relocalization search reaches max nodes without success.
|
|
/// </summary>
|
|
public delegate void LocalizationSearchCallback(int trajectoryId, long time, Rigid3d pose);
|
|
|
|
/// <summary>
|
|
/// Memory estimation for Copy-on-Write optimization overhead.
|
|
/// </summary>
|
|
public struct CopyOnWriteMemoryEstimate
|
|
{
|
|
/// <summary>Number of trajectory nodes.</summary>
|
|
public int NodeCount { get; init; }
|
|
|
|
/// <summary>Number of submaps.</summary>
|
|
public int SubmapCount { get; init; }
|
|
|
|
/// <summary>Number of constraints.</summary>
|
|
public int ConstraintCount { get; init; }
|
|
|
|
/// <summary>Memory used by trajectory nodes in bytes.</summary>
|
|
public long TrajectoryNodesBytes { get; init; }
|
|
|
|
/// <summary>Memory used by trajectory node poses in bytes.</summary>
|
|
public long TrajectoryNodePosesBytes { get; init; }
|
|
|
|
/// <summary>Memory used by submap poses in bytes.</summary>
|
|
public long SubmapPosesBytes { get; init; }
|
|
|
|
/// <summary>Memory used by constraints in bytes.</summary>
|
|
public long ConstraintsBytes { get; init; }
|
|
|
|
/// <summary>Peak additional memory overhead during COW optimization in bytes.</summary>
|
|
public long PeakOverheadBytes { get; init; }
|
|
|
|
/// <summary>Peak additional memory overhead during COW optimization in MB.</summary>
|
|
public double PeakOverheadMB { get; init; }
|
|
|
|
/// <summary>
|
|
/// Returns a formatted string with memory breakdown.
|
|
/// </summary>
|
|
public override string ToString()
|
|
{
|
|
return $"COW Memory Estimate:\n" +
|
|
$" Nodes: {NodeCount}, Submaps: {SubmapCount}, Constraints: {ConstraintCount}\n" +
|
|
$" TrajectoryNodes: {TrajectoryNodesBytes / 1024.0:F1} KB\n" +
|
|
$" TrajectoryNodePoses: {TrajectoryNodePosesBytes / 1024.0:F1} KB\n" +
|
|
$" SubmapPoses: {SubmapPosesBytes / 1024.0:F1} KB\n" +
|
|
$" Constraints: {ConstraintsBytes / 1024.0:F1} KB\n" +
|
|
$" Peak Overhead: {PeakOverheadMB:F2} MB";
|
|
}
|
|
}
|
|
|
|
/// <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:
|
|
/// 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 PoseGraph2D : PoseGraph, IDisposable
|
|
{
|
|
private readonly PoseGraphOptions _options;
|
|
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 = [];
|
|
|
|
// CRITICAL FIX: Match C++ data_.global_submap_poses_2d
|
|
// This is a copy of optimization_problem_->submap_data() that gets updated after optimization.
|
|
// GetLocalToGlobalTransform reads from this instead of optimization_problem directly to avoid
|
|
// potential blocking/slowness when optimization_problem is being modified.
|
|
private readonly MapById<SubmapId, Optimization.SubmapSpec2D> _globalSubmapPoses2D = new();
|
|
|
|
// 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 = [];
|
|
|
|
// Get OptimizationProblemOptions from PoseGraphOptions
|
|
private readonly OptimizationProblem2D _optimizationProblem;
|
|
|
|
// ConstraintBuilder2D for inter-submap constraint computation
|
|
private readonly ConstraintBuilder2D? _constraintBuilder;
|
|
|
|
// Match C++: Thread pool for parallel constraint computation (pose_graph_2d.cc line 70)
|
|
private readonly Common.Threading.ThreadPool _threadPool;
|
|
|
|
// Work queue for serializing operations (similar to Cartographer C++)
|
|
private readonly WorkQueue _workQueue;
|
|
|
|
// Optimization tracking counters (thread-safe with Interlocked)
|
|
// Track how many optimization requests were made vs completed
|
|
private long _optimizationRequested = 0;
|
|
private long _optimizationCompleted = 0;
|
|
|
|
// Monotonically increasing version counter, incremented each time a node is appended.
|
|
// Used by OccupancyGridManager to skip regeneration when no new data has been added.
|
|
private int _nodeInsertionVersion;
|
|
|
|
// Mutex for data structures (only accessed from work queue thread)
|
|
private readonly object _dataLock = new();
|
|
|
|
// Periodic optimization tracking
|
|
// FIXED: Match C++ - use GLOBAL counter instead of per-trajectory
|
|
private int _numNodesSinceLastLoopClosure = 0;
|
|
private readonly Lock _optimizationLock = new();
|
|
private Thread? _optimizationThread;
|
|
private bool _optimizationInProgress = false;
|
|
|
|
// Track pending optimization requests (when optimization is already running)
|
|
private long _pendingOptimizationCount = 0;
|
|
|
|
// Track node-to-submap insertions for constraint builder.
|
|
// SortedSet for O(1) access to Max (last node), matching C++ std::set with rbegin().
|
|
private readonly Dictionary<SubmapId, SortedSet<NodeId>> _submapNodeInsertions = [];
|
|
|
|
// Cache of finished submap IDs to avoid scanning all submaps every node.
|
|
// Updated when a submap transitions to InsertionFinished = true.
|
|
private readonly HashSet<SubmapId> _finishedSubmapIds = [];
|
|
|
|
// Track nodes for active submaps (before SubmapId is created) by submap reference
|
|
// Key: submap reference (object), Value: set of NodeIds that were inserted into this submap
|
|
private readonly Dictionary<Submap, HashSet<NodeId>> _activeSubmapNodeInsertions = [];
|
|
|
|
// Transform from local map frame to global map frame (map origin). Set when loading from pbstream (C++ transform_to_map).
|
|
private Rigid3d? _transformToMap;
|
|
|
|
// Match C++: trajectory_connectivity_state, localization, relocalization
|
|
private readonly TrajectoryConnectivityState _trajectoryConnectivityState = new();
|
|
private bool _localizationMode;
|
|
private List<Rigid3d> _localizationInitialPoses = [];
|
|
// Match C++ data_.initial_trajectory_poses: from_trajectory_id -> (to_trajectory_id, pose, time). Used in InitializeGlobalSubmapPoses to Connect when adding first submap.
|
|
private readonly Dictionary<int, (int ToTrajectoryId, Rigid3d Pose, long Time)> _initialTrajectoryPoses = [];
|
|
private int _numRelocalizationConstraintSearch;
|
|
private readonly Dictionary<int, FixedRatioSampler> _globalLocalizationSamplers = [];
|
|
private bool _isRelocalized;
|
|
// Localization result callback - for manual relocalization (LocalizationResultCallback) and search timeout (LocalizationSearchCallback)
|
|
private LocalizationResultCallback? _localizationResultCallback;
|
|
private LocalizationSearchCallback? _localizationSearchCallback;
|
|
|
|
// Match C++: current_trajectory_id - tracks the current trajectory being processed
|
|
private int _currentTrajectoryId;
|
|
|
|
// Match C++: matching_score_enabled - enables pose confidence scoring
|
|
private bool _matchingScoreEnabled;
|
|
|
|
// Match C++: node_scores_ - tracks pose confidence scores over time (max 30 entries)
|
|
private readonly LinkedList<(long Time, double Score)> _nodeScores = new();
|
|
private const int MaxNodeScoresCount = 30;
|
|
|
|
// Constructor: Initialize with PoseGraphOptions and ThreadPool
|
|
// Match C++ (pose_graph_2d.cc line 63-70): Accept thread_pool parameter
|
|
// According to Cartographer C++ implementation, PoseGraphOptions is used to:
|
|
// 1. Initialize OptimizationProblem with optimization_problem_options
|
|
// 2. Setup OverlappingSubmapsTrimmer2D if configured
|
|
// 3. Store matcher weights (matcher_translation_weight, matcher_rotation_weight) for constraint building
|
|
// 4. Configure optimization frequency (optimize_every_n_nodes)
|
|
// 5. Setup ConstraintBuilder2D for inter-submap constraint computation
|
|
public PoseGraph2D(PoseGraphOptions options, Common.Threading.ThreadPool threadPool) : base()
|
|
{
|
|
_options = options;
|
|
_threadPool = threadPool;
|
|
|
|
// Initialize work queue for thread-safe operations (similar to Cartographer C++)
|
|
_workQueue = new WorkQueue();
|
|
_workQueue.OptimizationNeeded += OnOptimizationNeeded;
|
|
|
|
// Initialize optimization problem with options from PoseGraphOptions
|
|
var optimizationProblemOptions = _options.OptimizationProblemOptions ?? new OptimizationProblemOptions();
|
|
_optimizationProblem = new OptimizationProblem2D(optimizationProblemOptions);
|
|
|
|
// Setup trimmer if configured
|
|
// This follows the C++ implementation where OverlappingSubmapsTrimmer2D is created from options
|
|
if (_options.OverlappingSubmapsTrimmer2D.HasValue)
|
|
{
|
|
var trimmerOptions = _options.OverlappingSubmapsTrimmer2D.Value;
|
|
var trimmer = new OverlappingSubmapsTrimmer2D(
|
|
trimmerOptions.FreshSubmapsCount,
|
|
trimmerOptions.MinCoveredArea,
|
|
trimmerOptions.MinAddedSubmapsCount);
|
|
AddTrimmer(trimmer);
|
|
}
|
|
|
|
// Setup ConstraintBuilder2D if options are provided
|
|
// Match C++ (pose_graph_2d.cc line 69): Pass thread_pool to ConstraintBuilder2D
|
|
if (_options.ConstraintBuilderOptions.HasValue)
|
|
{
|
|
var constraintBuilderOptions = _options.ConstraintBuilderOptions.Value;
|
|
_constraintBuilder = new ConstraintBuilder2D(constraintBuilderOptions, _threadPool);
|
|
}
|
|
|
|
// Note: _options.MatcherTranslationWeight and _options.MatcherRotationWeight are used
|
|
// for intra-submap constraints (non-loop-closure)
|
|
// _options.OptimizeEveryNNodes controls when to run periodic optimization
|
|
}
|
|
|
|
/// <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)
|
|
{
|
|
// Wrap the work item for logging
|
|
WorkItemResult wrappedWorkItem()
|
|
{
|
|
// Execute the actual work item
|
|
var result = workItem();
|
|
return result;
|
|
}
|
|
|
|
_workQueue.AddWorkItem(new WorkItem(wrappedWorkItem));
|
|
}
|
|
|
|
// PoseGraphInterface implementation
|
|
// Match C++ implementation (xloc): RunFinalOptimization() sets max_num_iterations via work queue and waits
|
|
// The actual optimization (Solve) happens in RunOptimization() which is triggered by work queue
|
|
public override void RunFinalOptimization()
|
|
{
|
|
// Match C++: Add work items to set max_num_iterations and trigger optimization
|
|
AddWorkItem("RunFinalOptimization Add work items to set max_num_iterations and trigger optimization", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Set max_num_iterations to max_num_final_iterations for final optimization
|
|
var maxNumIterations = _options.MaxNumFinalIterations > 0
|
|
? _options.MaxNumFinalIterations
|
|
: (_options.OptimizationProblemOptions?.MaxNumIterations ?? 50);
|
|
_optimizationProblem.SetMaxNumIterations(maxNumIterations);
|
|
}
|
|
return WorkItemResult.RunOptimization; // Trigger RunOptimization()
|
|
});
|
|
|
|
AddWorkItem("RunFinalOptimization Reset max_num_iterations back to default after optimization", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Reset max_num_iterations back to default after optimization
|
|
var defaultMaxIterations = _options.OptimizationProblemOptions?.MaxNumIterations ?? 50;
|
|
_optimizationProblem.SetMaxNumIterations(defaultMaxIterations);
|
|
}
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
});
|
|
|
|
// Match C++: Wait for all computations to complete before returning
|
|
WaitForAllComputations();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Waits for all computations to complete (work queue, constraint builder, and optimization thread).
|
|
/// Match C++ WaitForAllComputations() implementation (line 875-936).
|
|
/// </summary>
|
|
private void WaitForAllComputations()
|
|
{
|
|
// Timeout for waiting: 60 minutes (sufficient for large maps with many constraints)
|
|
var waitTimeout = TimeSpan.FromMinutes(60);
|
|
|
|
// Match C++ (line 876-880): Get number of trajectory nodes
|
|
int numTrajectoryNodes;
|
|
lock (_dataLock)
|
|
{
|
|
numTrajectoryNodes = _trajectoryNodes.Count;
|
|
}
|
|
|
|
// Match C++ (line 882-898): Progress reporting function with detailed logging
|
|
void ReportProgress(string phase)
|
|
{
|
|
var numFinishedNodes = _constraintBuilder?.GetNumFinishedNodes() ?? 0;
|
|
var remainingNodes = numTrajectoryNodes - numFinishedNodes;
|
|
//Console.WriteLine($"[WaitForAllComputations] {phase}: FinishedNodes={numFinishedNodes}/{numTrajectoryNodes}, Remaining={remainingNodes}");
|
|
}
|
|
|
|
// Match C++ (line 900-913): First wait for the work queue to drain
|
|
// so that it's safe to schedule a WhenDone() callback
|
|
var workQueueStartTime = DateTime.UtcNow;
|
|
while (!_workQueue.IsEmpty && (DateTime.UtcNow - workQueueStartTime) < waitTimeout)
|
|
{
|
|
ReportProgress("WorkQueue draining");
|
|
Thread.Sleep(1000); // Check every second (match C++ 1s timeout)
|
|
}
|
|
|
|
// Match C++ (line 915-933): Now wait for any pending constraint computations to finish
|
|
if (_constraintBuilder != null)
|
|
{
|
|
// Match C++: Use WhenDone callback to wait for constraint builder to finish
|
|
var constraintBuilderFinished = false;
|
|
var constraintBuilderLock = new object();
|
|
var constraintsFromBuilder = new List<IPoseGraph.Constraint>();
|
|
|
|
_constraintBuilder.WhenDone((result) =>
|
|
{
|
|
lock (constraintBuilderLock)
|
|
{
|
|
if (result.Constraints != null)
|
|
{
|
|
constraintsFromBuilder.AddRange(result.Constraints);
|
|
}
|
|
constraintBuilderFinished = true;
|
|
Monitor.PulseAll(constraintBuilderLock);
|
|
}
|
|
});
|
|
|
|
// Match C++ (line 930-933): Wait with progress reporting (60 minutes timeout)
|
|
var constraintStartTime = DateTime.UtcNow;
|
|
var lastLogTime = DateTime.UtcNow;
|
|
lock (constraintBuilderLock)
|
|
{
|
|
while (!constraintBuilderFinished && (DateTime.UtcNow - constraintStartTime) < waitTimeout)
|
|
{
|
|
// Log progress every 5 seconds to avoid log spam
|
|
if ((DateTime.UtcNow - lastLogTime) >= TimeSpan.FromSeconds(5))
|
|
{
|
|
var elapsed = DateTime.UtcNow - constraintStartTime;
|
|
ReportProgress($"Waiting for ConstraintBuilder (elapsed: {elapsed.TotalSeconds:F1}s)");
|
|
lastLogTime = DateTime.UtcNow;
|
|
}
|
|
Monitor.Wait(constraintBuilderLock, TimeSpan.FromSeconds(1));
|
|
}
|
|
}
|
|
|
|
var totalElapsed = DateTime.UtcNow - constraintStartTime;
|
|
if (!constraintBuilderFinished)
|
|
{
|
|
ReportProgress("Timeout state");
|
|
}
|
|
|
|
// Match C++ (line 923-924): Add constraints from builder to main constraints list
|
|
if (constraintsFromBuilder.Count > 0)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_constraints.AddRange(constraintsFromBuilder);
|
|
}
|
|
}
|
|
|
|
// Match C++ (line 934): CHECK_EQ constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes
|
|
var numFinishedNodes = _constraintBuilder.GetNumFinishedNodes();
|
|
}
|
|
|
|
// CRITICAL FIX: Wait for optimization thread to complete
|
|
// This ensures RunFinalOptimization() doesn't return until optimization is fully done
|
|
// No timeout - wait indefinitely until optimization thread completes
|
|
while (true)
|
|
{
|
|
bool isOptimizationInProgress;
|
|
Thread? currentOptimizationThread;
|
|
|
|
lock (_optimizationLock)
|
|
{
|
|
isOptimizationInProgress = _optimizationInProgress;
|
|
currentOptimizationThread = _optimizationThread;
|
|
}
|
|
|
|
if (!isOptimizationInProgress)
|
|
{
|
|
// Optimization is not in progress, check if thread is still alive
|
|
if (currentOptimizationThread != null && currentOptimizationThread.IsAlive)
|
|
{
|
|
// Thread is still alive but optimization flag is false - thread might be finishing
|
|
// Wait a bit more
|
|
Thread.Sleep(100);
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
// Optimization is complete
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Wait a bit before checking again
|
|
Thread.Sleep(100);
|
|
}
|
|
|
|
// Final check: Wait for optimization thread to finish if it's still alive
|
|
// No timeout - wait indefinitely until thread completes
|
|
if (_optimizationThread != null && _optimizationThread.IsAlive)
|
|
{
|
|
_optimizationThread.Join(); // Wait indefinitely
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Runs optimization synchronously in the calling thread.
|
|
/// Match C++ RunOptimization() implementation.
|
|
/// Thread-safe: only one optimization runs at a time.
|
|
/// If optimization is already running, increments pending count and returns.
|
|
/// The running optimization will check pending count and run again if needed.
|
|
/// </summary>
|
|
private void RunOptimizationSync()
|
|
{
|
|
// Increment optimization requested counter (always, even if skipped)
|
|
Interlocked.Increment(ref _optimizationRequested);
|
|
|
|
lock (_optimizationLock)
|
|
{
|
|
// If optimization is already in progress, increment pending count and return
|
|
if (_optimizationInProgress)
|
|
{
|
|
Interlocked.Increment(ref _pendingOptimizationCount);
|
|
return;
|
|
}
|
|
|
|
_optimizationInProgress = true;
|
|
}
|
|
|
|
// Run optimization loop until no pending requests
|
|
while (true)
|
|
{
|
|
try
|
|
{
|
|
// Use Copy-on-Write implementation if enabled, otherwise use original
|
|
if (_useCopyOnWriteOptimization)
|
|
{
|
|
RunOptimizationSyncInternal_CopyOnWrite();
|
|
}
|
|
else
|
|
{
|
|
RunOptimizationSyncInternal();
|
|
}
|
|
|
|
// Increment completed counter
|
|
Interlocked.Increment(ref _optimizationCompleted);
|
|
}
|
|
catch
|
|
{
|
|
throw;
|
|
}
|
|
|
|
// Check if there are pending optimization requests
|
|
lock (_optimizationLock)
|
|
{
|
|
var pending = Interlocked.Read(ref _pendingOptimizationCount);
|
|
if (pending > 0)
|
|
{
|
|
// Reset pending count and continue loop
|
|
Interlocked.Exchange(ref _pendingOptimizationCount, 0);
|
|
}
|
|
else
|
|
{
|
|
// No pending requests, exit loop
|
|
_optimizationInProgress = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Internal implementation of optimization (extracted from RunOptimizationSync).
|
|
/// Assumes _optimizationInProgress is true.
|
|
/// </summary>
|
|
private void RunOptimizationSyncInternal()
|
|
{
|
|
try
|
|
{
|
|
// Match C++ RunOptimization(): Check if optimization problem is empty
|
|
if (_optimizationProblem.SubmapData().IsEmpty)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Get trajectory states
|
|
var trajectoryStates = GetTrajectoryStates();
|
|
|
|
// Get landmark nodes
|
|
var landmarkNodes = GetLandmarkNodes();
|
|
|
|
var optimizationSubmapData = _optimizationProblem.SubmapData();
|
|
var optimizationNodeData = _optimizationProblem.NodeData();
|
|
|
|
// FIX: Match C++ (lines 1208-1229): Remove invalid constraints before optimization
|
|
// CRITICAL: Must be inside lock to prevent race condition with OnConstraintBuilderResult()
|
|
// which adds constraints inside a lock. Without lock, RemoveAll can run concurrently
|
|
// with constraint additions, causing data corruption or lost constraints.
|
|
List<IPoseGraph.Constraint> constraints;
|
|
lock (_dataLock)
|
|
{
|
|
var removedCount = _constraints.RemoveAll(c =>
|
|
!optimizationNodeData.Contains(c.NodeId) ||
|
|
!optimizationSubmapData.Contains(c.SubmapId));
|
|
|
|
// Get constraints snapshot inside lock to ensure consistency
|
|
constraints = Constraints();
|
|
}
|
|
|
|
// Solve optimization problem (will use max_num_iterations set via SetMaxNumIterations)
|
|
_optimizationProblem.Solve(constraints, trajectoryStates, landmarkNodes, maxNumIterationsOverride: null);
|
|
|
|
// Invoke global SLAM optimization callback if set
|
|
InvokeGlobalSlamOptimizationCallback();
|
|
|
|
// Prepare callback data outside lock scope, but populate inside lock
|
|
LocalizationResult? localizationResultToInvoke = null;
|
|
(int TrajectoryId, long Time, Rigid3d Pose)? localizationSearchToInvoke = null;
|
|
|
|
lock (_dataLock) // ===== SINGLE LOCK START =====
|
|
{
|
|
// Store old submap poses before updating (for extrapolation)
|
|
var oldSubmapPoses = new MapById<SubmapId, Rigid2d>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
oldSubmapPoses.Insert(kvp.Id, TransformOperations.Project2D(kvp.Data.Pose));
|
|
}
|
|
|
|
// Get optimized data
|
|
var optimizedNodeData = _optimizationProblem.NodeData();
|
|
var optimizedSubmapData = _optimizationProblem.SubmapData();
|
|
|
|
// Update ALL nodes (no lock per iteration)
|
|
foreach (var kvp in optimizedNodeData)
|
|
{
|
|
var nodeId = kvp.Id;
|
|
var nodeSpec = kvp.Data;
|
|
|
|
if (_trajectoryNodes.Contains(nodeId))
|
|
{
|
|
var node = _trajectoryNodes[nodeId];
|
|
// Match C++: global_pose = Embed3D(global_pose_2d) * Rotation(gravity_alignment)
|
|
var optimizedPose3D = TransformOperations.Embed3D(nodeSpec.GlobalPose2D) *
|
|
Rigid3d.FromRotation(node.ConstantData?.GravityAlignment ?? Quaternion.Identity);
|
|
|
|
// Update trajectory node pose
|
|
if (_trajectoryNodePoses.Contains(nodeId))
|
|
{
|
|
var nodePose = _trajectoryNodePoses[nodeId];
|
|
nodePose.GlobalPose = optimizedPose3D;
|
|
_trajectoryNodePoses[nodeId] = nodePose;
|
|
}
|
|
|
|
// Update trajectory node
|
|
node.GlobalPose = optimizedPose3D;
|
|
_trajectoryNodes[nodeId] = node;
|
|
}
|
|
}
|
|
|
|
// Extrapolate nodes not included in optimization
|
|
foreach (var trajectoryId in optimizedNodeData.TrajectoryIds)
|
|
{
|
|
if (optimizedNodeData.SizeOfTrajectoryOrZero(trajectoryId) == 0)
|
|
continue;
|
|
|
|
// Compute local_to_new_global from optimized submap data
|
|
var localToNewGlobal = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
|
|
|
|
// Compute local_to_old_global from old submap poses
|
|
var localToOldGlobal = ComputeLocalToGlobalTransformFromSubmapPoses(oldSubmapPoses, trajectoryId);
|
|
|
|
// Compute transform from old global to new global
|
|
var oldGlobalToNewGlobal = localToNewGlobal * localToOldGlobal.Inverse();
|
|
|
|
// Find last optimized node
|
|
var trajectoryNodes = optimizedNodeData.BeginOfTrajectory(trajectoryId)
|
|
.Select(kvp => kvp.Id)
|
|
.OrderBy(id => id.NodeIndex)
|
|
.ToList();
|
|
|
|
if (trajectoryNodes.Count > 0)
|
|
{
|
|
var lastOptimizedNodeId = trajectoryNodes[^1];
|
|
|
|
// Find all nodes in trajectory after lastOptimizedNodeId
|
|
var nodesToUpdate = _trajectoryNodes
|
|
.Where(kvp => kvp.Id.TrajectoryId == trajectoryId &&
|
|
kvp.Id.NodeIndex > lastOptimizedNodeId.NodeIndex)
|
|
.ToList();
|
|
|
|
foreach (var kvp in nodesToUpdate)
|
|
{
|
|
var node = kvp.Data;
|
|
node.GlobalPose = oldGlobalToNewGlobal * node.GlobalPose;
|
|
_trajectoryNodes[kvp.Id] = node;
|
|
|
|
// Update trajectory node pose if exists
|
|
if (_trajectoryNodePoses.Contains(kvp.Id))
|
|
{
|
|
var nodePose = _trajectoryNodePoses[kvp.Id];
|
|
nodePose.GlobalPose = node.GlobalPose;
|
|
_trajectoryNodePoses[kvp.Id] = nodePose;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Update _globalSubmapPoses2D (ONLY - match C++)
|
|
// Match C++ line 1280: data_.global_submap_poses_2d = submap_data
|
|
var existingSubmapIds = _globalSubmapPoses2D.Select(kvp => kvp.Id).ToList();
|
|
foreach (var existingId in existingSubmapIds)
|
|
{
|
|
_globalSubmapPoses2D.Trim(existingId);
|
|
}
|
|
foreach (var kvp in optimizedSubmapData)
|
|
{
|
|
_globalSubmapPoses2D.Insert(kvp.Id, kvp.Data);
|
|
}
|
|
|
|
// === LOOP CLOSURE OPTIMIZATION RESULT LOG ===
|
|
// Count InterSubmap constraints (loop closures)
|
|
var interSubmapCount = _constraints.Count(c => c.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap);
|
|
var intraSubmapCount = _constraints.Count(c => c.ConstraintTag == IPoseGraph.Constraint.Tag.IntraSubmap);
|
|
|
|
if (optimizedSubmapData.Count > 0)
|
|
{
|
|
var submapPosesStr = string.Join(", ", optimizedSubmapData.Select(kvp =>
|
|
$"[{kvp.Id.TrajectoryId}:{kvp.Id.SubmapIndex}]=({kvp.Data.GlobalPose.Translation.X:F3},{kvp.Data.GlobalPose.Translation.Y:F3},{kvp.Data.GlobalPose.Rotation * 180 / Math.PI:F1}°)"));
|
|
|
|
// Log node poses if available
|
|
var nodePosesStr = string.Join(", ", _trajectoryNodes.Take(10).Select(kvp =>
|
|
$"[{kvp.Id.TrajectoryId}:{kvp.Id.NodeIndex}]=({kvp.Data.GlobalPose.Translation.X:F3},{kvp.Data.GlobalPose.Translation.Y:F3})"));
|
|
}
|
|
|
|
// REFACTOR: DO NOT update _submapData.Pose (removed dual source of truth)
|
|
// C++ does NOT update data_.submap_data with global pose after optimization.
|
|
// C++ only updates data_.global_submap_poses_2d and calculates pose dynamically in GetSubmapDataUnderLock().
|
|
// GetSubmapData() will calculate pose dynamically from _globalSubmapPoses2D (source of truth).
|
|
|
|
// Update landmark poses
|
|
var landmarkData = _optimizationProblem.LandmarkData();
|
|
foreach (var kvp in landmarkData)
|
|
{
|
|
_landmarkPoses[kvp.Key] = kvp.Value;
|
|
}
|
|
|
|
// Update local to global transforms
|
|
// Match C++: Use _globalSubmapPoses2D (which was just updated above)
|
|
foreach (var trajectoryId in _trajectoryStates.Keys)
|
|
{
|
|
var transform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
|
|
_localToGlobalTransforms[trajectoryId] = transform;
|
|
}
|
|
|
|
// Match C++ (lines 1282-1301): Check for relocalization when first inter-submap constraint is found
|
|
// Prepare callback data (don't call yet - will call outside lock)
|
|
if (!_isRelocalized && _localizationMode)
|
|
{
|
|
foreach (var constraint in _constraints)
|
|
{
|
|
if (constraint.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap &&
|
|
constraint.NodeId.TrajectoryId == _currentTrajectoryId)
|
|
{
|
|
_isRelocalized = true;
|
|
|
|
// Prepare callback data (invoke outside lock)
|
|
if (_trajectoryNodes.Contains(constraint.NodeId))
|
|
{
|
|
var matchingNode = _trajectoryNodes[constraint.NodeId];
|
|
|
|
localizationSearchToInvoke = (
|
|
constraint.NodeId.TrajectoryId,
|
|
matchingNode.ConstantData?.Time ?? 0,
|
|
matchingNode.GlobalPose
|
|
);
|
|
|
|
localizationResultToInvoke = new LocalizationResult
|
|
{
|
|
NodeId = constraint.NodeId,
|
|
SubmapId = constraint.SubmapId,
|
|
GlobalPose = matchingNode.GlobalPose,
|
|
Score = constraint.Score
|
|
};
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
} // ===== SINGLE LOCK END - match C++ unlock here =====
|
|
|
|
// REFACTOR: Call callbacks OUTSIDE lock (matching C++ behavior)
|
|
if (localizationSearchToInvoke.HasValue)
|
|
{
|
|
_localizationSearchCallback?.Invoke(
|
|
localizationSearchToInvoke.Value.TrajectoryId,
|
|
localizationSearchToInvoke.Value.Time,
|
|
localizationSearchToInvoke.Value.Pose);
|
|
}
|
|
if (localizationResultToInvoke != null)
|
|
{
|
|
_localizationResultCallback?.Invoke(localizationResultToInvoke.Value);
|
|
}
|
|
|
|
// Run trimmers after optimization
|
|
RunTrimmers();
|
|
}
|
|
catch
|
|
{
|
|
throw;
|
|
}
|
|
// Note: _optimizationInProgress is managed by outer RunOptimizationSync function
|
|
}
|
|
|
|
#region Copy-on-Write Optimization
|
|
|
|
/// <summary>
|
|
/// Flag to enable Copy-on-Write optimization mode.
|
|
/// When true, RunOptimizationSyncInternal_CopyOnWrite is used instead of RunOptimizationSyncInternal.
|
|
/// Default: false (use original blocking implementation for backward compatibility).
|
|
/// </summary>
|
|
private bool _useCopyOnWriteOptimization = false;
|
|
|
|
/// <summary>
|
|
/// Enables or disables Copy-on-Write optimization mode.
|
|
/// COW mode significantly reduces lock contention during optimization by:
|
|
/// 1. Preparing updated data structures outside of lock
|
|
/// 2. Swapping references atomically with minimal lock time
|
|
/// This reduces AddNode blocking time by ~95-97% depending on node count.
|
|
/// </summary>
|
|
public void SetCopyOnWriteOptimization(bool enabled)
|
|
{
|
|
_useCopyOnWriteOptimization = enabled;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets whether Copy-on-Write optimization is enabled.
|
|
/// </summary>
|
|
public bool IsCopyOnWriteOptimizationEnabled => _useCopyOnWriteOptimization;
|
|
|
|
/// <summary>
|
|
/// Copy-on-Write implementation of RunOptimizationSyncInternal.
|
|
/// Minimizes lock contention by preparing updated data outside of lock,
|
|
/// then swapping references atomically.
|
|
///
|
|
/// Phase 1 (NO LOCK or MINIMAL LOCK):
|
|
/// - Snapshot current constraints
|
|
/// - Solve optimization problem (CPU intensive)
|
|
/// - Build new MapById instances with updated poses
|
|
///
|
|
/// Phase 2 (MINIMAL LOCK ~1-5ms):
|
|
/// - Merge nodes added during Phase 1
|
|
/// - Swap references atomically
|
|
/// - Update remaining dictionaries
|
|
/// </summary>
|
|
private void RunOptimizationSyncInternal_CopyOnWrite()
|
|
{
|
|
try
|
|
{
|
|
// ═══════════════════════════════════════════════════════════════
|
|
// EARLY EXIT: Check if optimization problem is empty
|
|
// ═══════════════════════════════════════════════════════════════
|
|
if (_optimizationProblem.SubmapData().IsEmpty)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// ═══════════════════════════════════════════════════════════════
|
|
// PHASE 1: PREPARE - Minimal lock for snapshots, then NO LOCK
|
|
// ═══════════════════════════════════════════════════════════════
|
|
|
|
// 1.1 Get input data (read-only access to optimization problem)
|
|
var trajectoryStates = GetTrajectoryStates();
|
|
var landmarkNodes = GetLandmarkNodes();
|
|
var optimizationSubmapData = _optimizationProblem.SubmapData();
|
|
var optimizationNodeData = _optimizationProblem.NodeData();
|
|
|
|
// 1.2 Snapshot constraints and current state with minimal lock
|
|
List<IPoseGraph.Constraint> constraintsSnapshot;
|
|
MapById<SubmapId, Rigid2d> oldSubmapPosesSnapshot; // Rigid2d for ComputeLocalToGlobalTransformFromSubmapPoses
|
|
MapById<SubmapId, Optimization.SubmapSpec2D> oldGlobalSubmapPoses2DSnapshot; // SubmapSpec2D for reference
|
|
MapById<NodeId, TrajectoryNode> currentNodesSnapshot;
|
|
MapById<NodeId, TrajectoryNodePose> currentNodePosesSnapshot;
|
|
Dictionary<int, IPoseGraph.TrajectoryState> trajectoryStatesSnapshot;
|
|
int currentTrajectoryIdSnapshot;
|
|
bool localizationModeSnapshot;
|
|
bool isRelocalizedSnapshot;
|
|
|
|
lock (_dataLock) // Lock #1: ~0.5-2ms - snapshot only
|
|
{
|
|
// Remove invalid constraints (must be done under lock)
|
|
_constraints.RemoveAll(c =>
|
|
!optimizationNodeData.Contains(c.NodeId) ||
|
|
!optimizationSubmapData.Contains(c.SubmapId));
|
|
|
|
// Snapshot current state (shallow clone - fast for structs)
|
|
constraintsSnapshot = new List<IPoseGraph.Constraint>(_constraints);
|
|
|
|
// Create oldSubmapPosesSnapshot as MapById<SubmapId, Rigid2d> for ComputeLocalToGlobalTransformFromSubmapPoses
|
|
oldSubmapPosesSnapshot = new MapById<SubmapId, Rigid2d>();
|
|
foreach (var kvp in _globalSubmapPoses2D)
|
|
{
|
|
oldSubmapPosesSnapshot.Insert(kvp.Id, kvp.Data.GlobalPose);
|
|
}
|
|
oldGlobalSubmapPoses2DSnapshot = _globalSubmapPoses2D.ShallowClone();
|
|
|
|
currentNodesSnapshot = _trajectoryNodes.ShallowClone();
|
|
currentNodePosesSnapshot = _trajectoryNodePoses.ShallowClone();
|
|
trajectoryStatesSnapshot = new Dictionary<int, IPoseGraph.TrajectoryState>(_trajectoryStates);
|
|
currentTrajectoryIdSnapshot = _currentTrajectoryId;
|
|
localizationModeSnapshot = _localizationMode;
|
|
isRelocalizedSnapshot = _isRelocalized;
|
|
}
|
|
|
|
// 1.3 Solve optimization problem (CPU intensive, NO LOCK)
|
|
// This is the main computational work - can take 50-200ms
|
|
_optimizationProblem.Solve(constraintsSnapshot, trajectoryStates, landmarkNodes, maxNumIterationsOverride: null);
|
|
|
|
// Invoke global SLAM optimization callback (outside lock)
|
|
InvokeGlobalSlamOptimizationCallback();
|
|
|
|
// 1.4 Get optimized results
|
|
var optimizedNodeData = _optimizationProblem.NodeData();
|
|
var optimizedSubmapData = _optimizationProblem.SubmapData();
|
|
|
|
// 1.5 Build NEW data structures (NO LOCK - this is where COW saves time)
|
|
var newTrajectoryNodes = new MapById<NodeId, TrajectoryNode>();
|
|
var newTrajectoryNodePoses = new MapById<NodeId, TrajectoryNodePose>();
|
|
var newGlobalSubmapPoses2D = new MapById<SubmapId, Optimization.SubmapSpec2D>();
|
|
var newLandmarkPoses = new Dictionary<string, Rigid3d>();
|
|
var newLocalToGlobalTransforms = new Dictionary<int, Rigid3d>();
|
|
|
|
// 1.5.1 Update optimized nodes
|
|
// CRITICAL: TrajectoryNode is a class (reference type), so we must clone
|
|
// to avoid modifying the original while other threads may be reading it.
|
|
foreach (var kvp in optimizedNodeData)
|
|
{
|
|
var nodeId = kvp.Id;
|
|
var nodeSpec = kvp.Data;
|
|
|
|
if (currentNodesSnapshot.Contains(nodeId))
|
|
{
|
|
var originalNode = currentNodesSnapshot[nodeId];
|
|
// CRITICAL FIX: Clone the node to avoid data race!
|
|
// ShallowClone shares ConstantData (immutable) but creates new GlobalPose
|
|
var node = originalNode.ShallowClone();
|
|
|
|
// Match C++: global_pose = Embed3D(global_pose_2d) * Rotation(gravity_alignment)
|
|
var optimizedPose3D = TransformOperations.Embed3D(nodeSpec.GlobalPose2D) *
|
|
Rigid3d.FromRotation(node.ConstantData?.GravityAlignment ?? Quaternion.Identity);
|
|
|
|
// Now safe to modify - this is our own copy
|
|
node.GlobalPose = optimizedPose3D;
|
|
newTrajectoryNodes.Insert(nodeId, node);
|
|
|
|
// Create updated node pose
|
|
var nodePose = currentNodePosesSnapshot.Contains(nodeId)
|
|
? currentNodePosesSnapshot[nodeId]
|
|
: new TrajectoryNodePose();
|
|
nodePose.GlobalPose = optimizedPose3D;
|
|
newTrajectoryNodePoses.Insert(nodeId, nodePose);
|
|
}
|
|
}
|
|
|
|
// 1.5.2 Extrapolate nodes not included in optimization
|
|
// Pre-build optimized submap poses once (avoid rebuilding in loop)
|
|
var optimizedSubmapPosesMap = new MapById<SubmapId, Optimization.SubmapSpec2D>();
|
|
foreach (var kvp in optimizedSubmapData)
|
|
{
|
|
optimizedSubmapPosesMap.Insert(kvp.Id, kvp.Data);
|
|
}
|
|
|
|
foreach (var trajectoryId in optimizedNodeData.TrajectoryIds)
|
|
{
|
|
if (optimizedNodeData.SizeOfTrajectoryOrZero(trajectoryId) == 0)
|
|
continue;
|
|
|
|
// Compute local_to_new_global from optimized submap poses
|
|
var localToNewGlobal = ComputeLocalToGlobalTransform(optimizedSubmapPosesMap, trajectoryId);
|
|
|
|
// Compute local_to_old_global from snapshot
|
|
var localToOldGlobal = ComputeLocalToGlobalTransformFromSubmapPoses(oldSubmapPosesSnapshot, trajectoryId);
|
|
|
|
// Compute transform from old global to new global
|
|
var oldGlobalToNewGlobal = localToNewGlobal * localToOldGlobal.Inverse();
|
|
|
|
// Find last optimized node
|
|
var trajectoryNodes = optimizedNodeData.BeginOfTrajectory(trajectoryId)
|
|
.Select(kvp => kvp.Id)
|
|
.OrderBy(id => id.NodeIndex)
|
|
.ToList();
|
|
|
|
if (trajectoryNodes.Count > 0)
|
|
{
|
|
var lastOptimizedNodeId = trajectoryNodes[^1];
|
|
|
|
// Find all nodes in snapshot after lastOptimizedNodeId
|
|
var nodesToExtrapolate = currentNodesSnapshot
|
|
.Where(kvp => kvp.Id.TrajectoryId == trajectoryId &&
|
|
kvp.Id.NodeIndex > lastOptimizedNodeId.NodeIndex)
|
|
.ToList();
|
|
|
|
foreach (var kvp in nodesToExtrapolate)
|
|
{
|
|
// CRITICAL FIX: Clone the node to avoid data race!
|
|
var node = kvp.Data.ShallowClone();
|
|
node.GlobalPose = oldGlobalToNewGlobal * kvp.Data.GlobalPose;
|
|
|
|
// Only insert if not already in newTrajectoryNodes
|
|
if (!newTrajectoryNodes.Contains(kvp.Id))
|
|
{
|
|
newTrajectoryNodes.Insert(kvp.Id, node);
|
|
}
|
|
|
|
// Update or insert node pose
|
|
var nodePose = currentNodePosesSnapshot.Contains(kvp.Id)
|
|
? currentNodePosesSnapshot[kvp.Id]
|
|
: new TrajectoryNodePose();
|
|
nodePose.GlobalPose = node.GlobalPose;
|
|
if (!newTrajectoryNodePoses.Contains(kvp.Id))
|
|
{
|
|
newTrajectoryNodePoses.Insert(kvp.Id, nodePose);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// 1.5.3 Build new submap poses
|
|
foreach (var kvp in optimizedSubmapData)
|
|
{
|
|
newGlobalSubmapPoses2D.Insert(kvp.Id, kvp.Data);
|
|
}
|
|
|
|
// 1.5.4 Update landmark poses
|
|
var landmarkData = _optimizationProblem.LandmarkData();
|
|
foreach (var kvp in landmarkData)
|
|
{
|
|
newLandmarkPoses[kvp.Key] = kvp.Value;
|
|
}
|
|
|
|
// 1.5.5 Compute new transforms using new submap poses
|
|
foreach (var trajectoryId in trajectoryStatesSnapshot.Keys)
|
|
{
|
|
var transform = ComputeLocalToGlobalTransform(newGlobalSubmapPoses2D, trajectoryId);
|
|
newLocalToGlobalTransforms[trajectoryId] = transform;
|
|
}
|
|
|
|
// ═══════════════════════════════════════════════════════════════
|
|
// PHASE 2: SWAP - Minimal lock for atomic reference swap
|
|
// ═══════════════════════════════════════════════════════════════
|
|
|
|
LocalizationResult? localizationResultToInvoke = null;
|
|
(int TrajectoryId, long Time, Rigid3d Pose)? localizationSearchToInvoke = null;
|
|
|
|
lock (_dataLock) // Lock #2: ~1-5ms - merge and swap only
|
|
{
|
|
// 2.1 Merge nodes that were added during Phase 1
|
|
// These are nodes in _trajectoryNodes that are NOT in currentNodesSnapshot
|
|
var nodesAddedDuringPhase1 = _trajectoryNodes.GetIdsDifference(currentNodesSnapshot).ToList();
|
|
|
|
if (nodesAddedDuringPhase1.Count > 0)
|
|
{
|
|
// Compute transform delta to apply to newly added nodes
|
|
foreach (var nodeId in nodesAddedDuringPhase1)
|
|
{
|
|
var node = _trajectoryNodes[nodeId];
|
|
|
|
// Compute transform from old global to new global for this trajectory
|
|
if (newLocalToGlobalTransforms.TryGetValue(nodeId.TrajectoryId, out var newTransform) &&
|
|
_localToGlobalTransforms.TryGetValue(nodeId.TrajectoryId, out var oldTransform))
|
|
{
|
|
var oldToNew = newTransform * oldTransform.Inverse();
|
|
node.GlobalPose = oldToNew * node.GlobalPose;
|
|
}
|
|
|
|
// Add to new structures
|
|
if (!newTrajectoryNodes.Contains(nodeId))
|
|
{
|
|
newTrajectoryNodes.Insert(nodeId, node);
|
|
}
|
|
else
|
|
{
|
|
newTrajectoryNodes[nodeId] = node;
|
|
}
|
|
|
|
// Update node pose
|
|
var nodePose = new TrajectoryNodePose { GlobalPose = node.GlobalPose };
|
|
if (!newTrajectoryNodePoses.Contains(nodeId))
|
|
{
|
|
newTrajectoryNodePoses.Insert(nodeId, nodePose);
|
|
}
|
|
else
|
|
{
|
|
newTrajectoryNodePoses[nodeId] = nodePose;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 2.2 Also merge any submaps added during Phase 1
|
|
var submapsAddedDuringPhase1 = _globalSubmapPoses2D.GetIdsDifference(oldGlobalSubmapPoses2DSnapshot).ToList();
|
|
foreach (var submapId in submapsAddedDuringPhase1)
|
|
{
|
|
if (!newGlobalSubmapPoses2D.Contains(submapId))
|
|
{
|
|
var submapPose = _globalSubmapPoses2D[submapId];
|
|
newGlobalSubmapPoses2D.Insert(submapId, submapPose);
|
|
}
|
|
}
|
|
|
|
// 2.3 Atomic swap - replace old data with new data
|
|
_trajectoryNodes.ReplaceWith(newTrajectoryNodes);
|
|
_trajectoryNodePoses.ReplaceWith(newTrajectoryNodePoses);
|
|
_globalSubmapPoses2D.ReplaceWith(newGlobalSubmapPoses2D);
|
|
|
|
// 2.4 Update dictionaries
|
|
foreach (var kvp in newLandmarkPoses)
|
|
{
|
|
_landmarkPoses[kvp.Key] = kvp.Value;
|
|
}
|
|
foreach (var kvp in newLocalToGlobalTransforms)
|
|
{
|
|
_localToGlobalTransforms[kvp.Key] = kvp.Value;
|
|
}
|
|
|
|
// 2.5 Check for relocalization (same logic as original)
|
|
if (!_isRelocalized && _localizationMode)
|
|
{
|
|
foreach (var constraint in _constraints)
|
|
{
|
|
if (constraint.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap &&
|
|
constraint.NodeId.TrajectoryId == _currentTrajectoryId)
|
|
{
|
|
_isRelocalized = true;
|
|
|
|
if (_trajectoryNodes.Contains(constraint.NodeId))
|
|
{
|
|
var matchingNode = _trajectoryNodes[constraint.NodeId];
|
|
|
|
localizationSearchToInvoke = (
|
|
constraint.NodeId.TrajectoryId,
|
|
matchingNode.ConstantData?.Time ?? 0,
|
|
matchingNode.GlobalPose
|
|
);
|
|
|
|
localizationResultToInvoke = new LocalizationResult
|
|
{
|
|
NodeId = constraint.NodeId,
|
|
SubmapId = constraint.SubmapId,
|
|
GlobalPose = matchingNode.GlobalPose,
|
|
Score = constraint.Score
|
|
};
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
} // End of Phase 2 lock
|
|
|
|
// Invoke callbacks OUTSIDE lock (matching C++ behavior)
|
|
if (localizationSearchToInvoke.HasValue)
|
|
{
|
|
_localizationSearchCallback?.Invoke(
|
|
localizationSearchToInvoke.Value.TrajectoryId,
|
|
localizationSearchToInvoke.Value.Time,
|
|
localizationSearchToInvoke.Value.Pose);
|
|
}
|
|
if (localizationResultToInvoke != null)
|
|
{
|
|
_localizationResultCallback?.Invoke(localizationResultToInvoke.Value);
|
|
}
|
|
|
|
// Run trimmers after optimization
|
|
RunTrimmers();
|
|
}
|
|
catch
|
|
{
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets memory usage estimation for Copy-on-Write overhead.
|
|
/// Returns the peak additional memory used during COW optimization in bytes.
|
|
/// </summary>
|
|
public CopyOnWriteMemoryEstimate GetCopyOnWriteMemoryEstimate()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// TrajectoryNode: class with Data (~200 bytes) + GlobalPose (56 bytes) = ~256 bytes
|
|
const int trajectoryNodeSize = 256;
|
|
|
|
// TrajectoryNodePose: struct with GlobalPose (56 bytes) + optional ConstantPoseData (64 bytes) = ~120 bytes
|
|
const int trajectoryNodePoseSize = 120;
|
|
|
|
// SubmapSpec2D: struct with Rigid2d (24 bytes) = ~24 bytes
|
|
const int submapSpec2DSize = 24;
|
|
|
|
// Constraint: struct ~100 bytes
|
|
const int constraintSize = 100;
|
|
|
|
var nodeCount = _trajectoryNodes.Count;
|
|
var submapCount = _globalSubmapPoses2D.Count;
|
|
var constraintCount = _constraints.Count;
|
|
|
|
// Calculate memory for each structure
|
|
var trajectoryNodesMemory = _trajectoryNodes.EstimateMemoryUsageBytes(trajectoryNodeSize);
|
|
var trajectoryNodePosesMemory = _trajectoryNodePoses.EstimateMemoryUsageBytes(trajectoryNodePoseSize);
|
|
var submapPosesMemory = _globalSubmapPoses2D.EstimateMemoryUsageBytes(submapSpec2DSize);
|
|
var constraintsMemory = constraintCount * constraintSize + 32; // List overhead
|
|
|
|
// During COW, we create copies of all these structures
|
|
var peakOverhead = trajectoryNodesMemory + trajectoryNodePosesMemory + submapPosesMemory + constraintsMemory;
|
|
|
|
return new CopyOnWriteMemoryEstimate
|
|
{
|
|
NodeCount = nodeCount,
|
|
SubmapCount = submapCount,
|
|
ConstraintCount = constraintCount,
|
|
TrajectoryNodesBytes = trajectoryNodesMemory,
|
|
TrajectoryNodePosesBytes = trajectoryNodePosesMemory,
|
|
SubmapPosesBytes = submapPosesMemory,
|
|
ConstraintsBytes = constraintsMemory,
|
|
PeakOverheadBytes = peakOverhead,
|
|
PeakOverheadMB = peakOverhead / (1024.0 * 1024.0)
|
|
};
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// Runs optimization asynchronously on a separate thread (legacy method).
|
|
/// DEPRECATED: This method is kept for backward compatibility but should not be used
|
|
/// from work queue context. Use RunOptimizationSync() instead when called from work queue.
|
|
/// </summary>
|
|
private void RunOptimization()
|
|
{
|
|
lock (_optimizationLock)
|
|
{
|
|
// If optimization is already in progress, skip
|
|
if (_optimizationInProgress) return;
|
|
}
|
|
|
|
// Run optimization on dedicated thread
|
|
_optimizationThread = new Thread(() =>
|
|
{
|
|
Thread.BeginThreadAffinity();
|
|
try
|
|
{
|
|
// Call the synchronous version (which sets _optimizationInProgress itself)
|
|
RunOptimizationSync();
|
|
}
|
|
catch
|
|
{
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
Thread.EndThreadAffinity();
|
|
}
|
|
})
|
|
{
|
|
Priority = ThreadPriority.BelowNormal,
|
|
IsBackground = true,
|
|
Name = "CartographerOptimization2D"
|
|
};
|
|
_optimizationThread.Start();
|
|
}
|
|
|
|
/// <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);
|
|
}
|
|
|
|
public override MapById<SubmapId, IPoseGraph.SubmapData> GetAllSubmapData()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Match C++ GetSubmapDataUnderLock(): Calculate global pose dynamically from _globalSubmapPoses2D
|
|
var result = new MapById<SubmapId, IPoseGraph.SubmapData>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
result.Insert(kvp.Id, GetSubmapDataUnsafe(kvp.Id));
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Monotonically increasing version counter. Incremented each time a node is
|
|
/// appended via AddNode (i.e. each successful range data insertion).
|
|
/// Safe to read without locking (volatile read).
|
|
/// </summary>
|
|
public int NodeInsertionVersion => Volatile.Read(ref _nodeInsertionVersion);
|
|
|
|
/// <summary>
|
|
/// Non-blocking attempt to get all submap data + transforms for visualization.
|
|
/// Uses Monitor.TryEnter to avoid blocking the sensor data processing path.
|
|
/// Returns false if the lock is currently held (e.g., during optimization or AddNode).
|
|
/// Also returns the current nodeInsertionVersion for change detection.
|
|
/// </summary>
|
|
public bool TryGetSubmapSnapshot(
|
|
out MapById<SubmapId, IPoseGraph.SubmapData>? submapData,
|
|
out Rigid3d transformToMap,
|
|
out int snapshotVersion)
|
|
{
|
|
submapData = null;
|
|
transformToMap = Rigid3d.Identity;
|
|
snapshotVersion = 0;
|
|
|
|
if (!Monitor.TryEnter(_dataLock))
|
|
return false;
|
|
|
|
try
|
|
{
|
|
var result = new MapById<SubmapId, IPoseGraph.SubmapData>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
result.Insert(kvp.Id, GetSubmapDataUnsafe(kvp.Id));
|
|
}
|
|
submapData = result;
|
|
transformToMap = _transformToMap ?? Rigid3d.Identity;
|
|
snapshotVersion = _nodeInsertionVersion;
|
|
return true;
|
|
}
|
|
finally
|
|
{
|
|
Monitor.Exit(_dataLock);
|
|
}
|
|
}
|
|
|
|
public override IPoseGraph.SubmapData GetSubmapData(SubmapId submapId)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return GetSubmapDataUnsafe(submapId);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets submap data with dynamic global pose calculation (matches C++ GetSubmapDataUnderLock).
|
|
/// Assumes lock held.
|
|
/// </summary>
|
|
private IPoseGraph.SubmapData GetSubmapDataUnsafe(SubmapId submapId)
|
|
{
|
|
// Match C++ GetSubmapDataUnderLock() (dòng 1565-1582)
|
|
if (!_submapData.Contains(submapId))
|
|
{
|
|
return new IPoseGraph.SubmapData(null, Rigid3d.Identity);
|
|
}
|
|
|
|
var submapData = _submapData[submapId];
|
|
var submap = submapData.Submap;
|
|
|
|
// Match C++: if (data_.global_submap_poses_2d.Contains(submap_id))
|
|
if (_globalSubmapPoses2D.Contains(submapId))
|
|
{
|
|
// We already have an optimized pose.
|
|
// Match C++: return {submap, transform::Embed3D(data_.global_submap_poses_2d.at(submap_id).global_pose)}
|
|
var optimizedPose2D = _globalSubmapPoses2D[submapId].GlobalPose;
|
|
var optimizedPose3D = TransformOperations.Embed3D(optimizedPose2D);
|
|
return new IPoseGraph.SubmapData(submap, optimizedPose3D);
|
|
}
|
|
|
|
// We have to extrapolate.
|
|
// Match C++: return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, submap_id.trajectory_id) * submap->local_pose()}
|
|
var transform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, submapId.TrajectoryId);
|
|
if (submap is Mapping.D2D.Submap2D submap2D)
|
|
{
|
|
var extrapolatedPose = transform * submap2D.LocalPose;
|
|
return new IPoseGraph.SubmapData(submap, extrapolatedPose);
|
|
}
|
|
|
|
// Fallback for non-2D submaps
|
|
return new IPoseGraph.SubmapData(submap, Rigid3d.Identity);
|
|
}
|
|
|
|
public override MapById<SubmapId, IPoseGraph.SubmapPose> GetAllSubmapPoses()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Match C++ GetAllSubmapPoses() (dòng 1527-1539): Calculate pose dynamically using GetSubmapDataUnderLock
|
|
var result = new MapById<SubmapId, IPoseGraph.SubmapPose>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
// Match C++: auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
|
|
var submapData = GetSubmapDataUnsafe(kvp.Id);
|
|
|
|
var version = 0;
|
|
if (submapData.Submap is Mapping.D2D.Submap2D submap2D)
|
|
{
|
|
version = submap2D.NumRangeData;
|
|
}
|
|
// Match C++: submap_poses.Insert(..., SubmapPose{submap_data.submap->num_range_data(), submap_data.pose})
|
|
result.Insert(kvp.Id, new IPoseGraph.SubmapPose(version, submapData.Pose));
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public override Rigid3d GetLocalToGlobalTransform(int trajectoryId)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return GetLocalToGlobalTransformUnsafe(trajectoryId);
|
|
}
|
|
}
|
|
|
|
public override MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Return a copy to avoid external modification
|
|
var result = new MapById<NodeId, TrajectoryNode>();
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
result.Insert(kvp.Id, kvp.Data);
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// CRITICAL FIX: Match C++ implementation - compute from trajectory_nodes on demand
|
|
// instead of using separate _trajectoryNodePoses member variable.
|
|
// This matches C++ GetTrajectoryNodePoses() which creates a new MapById and
|
|
// populates it from data_.trajectory_nodes.
|
|
var result = new MapById<NodeId, TrajectoryNodePose>();
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
ConstantPoseData? constantPoseData = null;
|
|
if (kvp.Data.ConstantData != null)
|
|
{
|
|
constantPoseData = new ConstantPoseData
|
|
{
|
|
Time = kvp.Data.ConstantData.Time,
|
|
LocalPose = kvp.Data.ConstantData.LocalPose
|
|
};
|
|
}
|
|
result.Insert(kvp.Id, new TrajectoryNodePose
|
|
{
|
|
GlobalPose = kvp.Data.GlobalPose,
|
|
ConstantPoseData = constantPoseData
|
|
});
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public override Dictionary<int, IPoseGraph.TrajectoryState> GetTrajectoryStates()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return new Dictionary<int, IPoseGraph.TrajectoryState>(_trajectoryStates);
|
|
}
|
|
}
|
|
|
|
public override Dictionary<string, Rigid3d> GetLandmarkPoses()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return new Dictionary<string, Rigid3d>(_landmarkPoses);
|
|
}
|
|
}
|
|
|
|
public override void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_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 void DeleteTrajectory(int trajectoryId)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_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);
|
|
_finishedSubmapIds.Remove(submapId);
|
|
}
|
|
|
|
// Remove constraints involving this trajectory
|
|
_constraints.RemoveAll(c =>
|
|
c.SubmapId.TrajectoryId == trajectoryId ||
|
|
c.NodeId.TrajectoryId == trajectoryId);
|
|
}
|
|
|
|
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 Dictionary<int, IPoseGraph.TrajectoryData> GetTrajectoryData()
|
|
{
|
|
// FIX: Added lock to match C++ (pose_graph_2d.cc:1429)
|
|
lock (_dataLock)
|
|
{
|
|
return new Dictionary<int, IPoseGraph.TrajectoryData>(_trajectoryData);
|
|
}
|
|
}
|
|
|
|
public override List<IPoseGraph.Constraint> Constraints()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return [.. _constraints];
|
|
}
|
|
}
|
|
|
|
public override Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps)
|
|
{
|
|
// FIX: Added lock for thread safety - reads multiple member variables
|
|
lock (_dataLock)
|
|
{
|
|
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)
|
|
{
|
|
// CRITICAL FIX: Read GlobalPose from _trajectoryNodes instead of _trajectoryNodePoses
|
|
// because _trajectoryNodePoses is only populated when loading from proto
|
|
var nodePose = kvp.Data.GlobalPose;
|
|
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.D2D.Submap2D submap2D)
|
|
{
|
|
if (includeUnfinishedSubmaps || submap2D.InsertionFinished)
|
|
{
|
|
trajectory.Submaps.Add(new Models.Mapping.Trajectory.Submap(
|
|
kvp.Id.SubmapIndex,
|
|
(Rigid3dProto)kvp.Data.Pose
|
|
));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
proto.Trajectories.Add(trajectory);
|
|
}
|
|
|
|
// Add landmark poses
|
|
foreach (var kvp in _landmarkPoses)
|
|
{
|
|
proto.LandmarkPoses.Add(new Models.Mapping.PoseGraph.LandmarkPose
|
|
{
|
|
LandmarkId = kvp.Key,
|
|
GlobalPose = (Rigid3dProto)kvp.Value
|
|
});
|
|
}
|
|
|
|
// Match C++: serialize transform_to_map when set
|
|
if (_transformToMap.HasValue)
|
|
{
|
|
proto.TransformToMap = (Rigid3dProto)_transformToMap.Value;
|
|
}
|
|
|
|
return proto;
|
|
}
|
|
}
|
|
|
|
public override void SetTransformToMap(Rigid3d transform)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_transformToMap = transform;
|
|
}
|
|
}
|
|
|
|
public override Rigid3d GetTransformToMap()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return _transformToMap ?? Rigid3d.Identity;
|
|
}
|
|
}
|
|
|
|
public override void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback)
|
|
{
|
|
_globalSlamOptimizationCallback = callback;
|
|
}
|
|
|
|
// PoseGraph implementation
|
|
// Thread-safe: Uses work queue pattern similar to Cartographer C++
|
|
public override void AddImuData(int trajectoryId, ImuData imuData)
|
|
{
|
|
// Capture by value to avoid closure issues
|
|
var capturedTrajectoryId = trajectoryId;
|
|
var capturedImuData = imuData;
|
|
|
|
AddWorkItem("", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (!_imuData.TryGetValue(capturedTrajectoryId, out var data))
|
|
{
|
|
data = [];
|
|
_imuData[capturedTrajectoryId] = data;
|
|
}
|
|
data.Add(capturedImuData);
|
|
}
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
});
|
|
}
|
|
|
|
public override void AddOdometryData(int trajectoryId, OdometryData odometryData)
|
|
{
|
|
var capturedTrajectoryId = trajectoryId;
|
|
var capturedOdometryData = odometryData;
|
|
|
|
AddWorkItem("", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (!_odometryData.TryGetValue(capturedTrajectoryId, out var data))
|
|
{
|
|
data = [];
|
|
_odometryData[capturedTrajectoryId] = data;
|
|
}
|
|
data.Add(capturedOdometryData);
|
|
}
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
});
|
|
}
|
|
|
|
public override void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData)
|
|
{
|
|
var capturedTrajectoryId = trajectoryId;
|
|
var capturedFixedFramePoseData = fixedFramePoseData;
|
|
|
|
AddWorkItem($"AddFixedFramePoseData trajectoryId={trajectoryId}", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (!_fixedFramePoseData.TryGetValue(capturedTrajectoryId, out var data))
|
|
{
|
|
data = [];
|
|
_fixedFramePoseData[capturedTrajectoryId] = data;
|
|
}
|
|
data.Add(capturedFixedFramePoseData);
|
|
}
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
});
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trims sensor data for a finished trajectory, removing entries older than the
|
|
/// oldest remaining node's time. This bounds memory growth for long-running sessions.
|
|
/// Must be called under _dataLock.
|
|
/// </summary>
|
|
private void TrimSensorDataForFinishedTrajectory(int trajectoryId)
|
|
{
|
|
// Find the oldest node time for this trajectory
|
|
long oldestNodeTime = long.MaxValue;
|
|
foreach (var kvp in _trajectoryNodes.BeginOfTrajectory(trajectoryId))
|
|
{
|
|
var time = kvp.Data.ConstantData?.Time ?? long.MaxValue;
|
|
if (time < oldestNodeTime)
|
|
oldestNodeTime = time;
|
|
}
|
|
|
|
if (oldestNodeTime == long.MaxValue)
|
|
return; // No nodes found, nothing to trim
|
|
|
|
// Trim IMU data: remove entries before oldest node, keeping last one before for interpolation
|
|
if (_imuData.TryGetValue(trajectoryId, out var imuList) && imuList.Count > 1)
|
|
{
|
|
int lastBeforeIdx = -1;
|
|
for (int i = 0; i < imuList.Count; i++)
|
|
{
|
|
if (imuList[i].Time < oldestNodeTime)
|
|
lastBeforeIdx = i;
|
|
else
|
|
break; // List is sorted by time (added chronologically)
|
|
}
|
|
// Keep one entry before oldest node for interpolation, remove the rest
|
|
if (lastBeforeIdx > 0)
|
|
{
|
|
imuList.RemoveRange(0, lastBeforeIdx);
|
|
}
|
|
}
|
|
|
|
// Trim odometry data
|
|
if (_odometryData.TryGetValue(trajectoryId, out var odomList) && odomList.Count > 1)
|
|
{
|
|
int lastBeforeIdx = -1;
|
|
for (int i = 0; i < odomList.Count; i++)
|
|
{
|
|
if (odomList[i].Time < oldestNodeTime)
|
|
lastBeforeIdx = i;
|
|
else
|
|
break;
|
|
}
|
|
if (lastBeforeIdx > 0)
|
|
{
|
|
odomList.RemoveRange(0, lastBeforeIdx);
|
|
}
|
|
}
|
|
|
|
// Trim fixed frame pose data
|
|
if (_fixedFramePoseData.TryGetValue(trajectoryId, out var ffpList) && ffpList.Count > 1)
|
|
{
|
|
int lastBeforeIdx = -1;
|
|
for (int i = 0; i < ffpList.Count; i++)
|
|
{
|
|
if (ffpList[i].Time < oldestNodeTime)
|
|
lastBeforeIdx = i;
|
|
else
|
|
break;
|
|
}
|
|
if (lastBeforeIdx > 0)
|
|
{
|
|
ffpList.RemoveRange(0, lastBeforeIdx);
|
|
}
|
|
}
|
|
}
|
|
|
|
public override void AddLandmarkData(int trajectoryId, LandmarkData landmarkData)
|
|
{
|
|
var capturedTrajectoryId = trajectoryId;
|
|
var capturedLandmarkData = landmarkData;
|
|
|
|
AddWorkItem("", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
foreach (var observation in capturedLandmarkData.LandmarkObservations)
|
|
{
|
|
if (!_landmarkNodes.TryGetValue(observation.Id, out var node))
|
|
{
|
|
node = new IPoseGraph.LandmarkNode();
|
|
}
|
|
node.LandmarkObservations.Add(new IPoseGraph.LandmarkNode.LandmarkObservation(
|
|
capturedTrajectoryId,
|
|
capturedLandmarkData.Time,
|
|
observation.LandmarkToTrackingTransform,
|
|
observation.TranslationWeight,
|
|
observation.RotationWeight
|
|
));
|
|
_landmarkNodes[observation.Id] = node;
|
|
}
|
|
}
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
});
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually compute a constraint between a node and submap using global scan matching.
|
|
/// Match C++: manualComputeConstraint (pose_graph_2d.cc:677-696)
|
|
/// Note: C++ does not lock here, but we use a single lock for thread safety in C#.
|
|
/// </summary>
|
|
public override (double Score, IPoseGraph.Constraint? Constraint) ManualComputeConstraint(
|
|
NodeId nodeId, SubmapId submapId)
|
|
{
|
|
// Match C++ (pose_graph_2d.cc:677-696)
|
|
// FIX: Merged two separate locks into one to prevent data changes between locks
|
|
|
|
TrajectoryNode.Data? constantData;
|
|
Mapping.D2D.Submap2D? submap;
|
|
|
|
lock (_dataLock)
|
|
{
|
|
// 1. Validate and get node data
|
|
if (!_trajectoryNodes.Contains(nodeId))
|
|
return (0, null);
|
|
|
|
var node = _trajectoryNodes[nodeId];
|
|
constantData = node.ConstantData;
|
|
if (constantData == null)
|
|
return (0, null);
|
|
|
|
// 2. Validate and get submap data
|
|
if (!_submapData.Contains(submapId))
|
|
return (0, null);
|
|
|
|
var submapData = _submapData[submapId];
|
|
submap = submapData.Submap as Mapping.D2D.Submap2D;
|
|
if (submap == null)
|
|
return (0, null);
|
|
}
|
|
|
|
// 3. Delegate to constraint builder (no lock needed for computation)
|
|
if (_constraintBuilder == null)
|
|
return (0, null);
|
|
|
|
return _constraintBuilder.ManualComputeGlobalConstraint(
|
|
submapId, submap, nodeId, constantData, minScore: 0);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually compute constraint score from an initial pose estimate.
|
|
/// Match C++: manualComputeConstraintScore (pose_graph_2d.cc:698-713)
|
|
/// Note: C++ does not lock here, but we use a single lock for thread safety in C#.
|
|
/// </summary>
|
|
public override double ManualComputeConstraintScore(
|
|
NodeId nodeId, SubmapId submapId, Rigid3d initialPose)
|
|
{
|
|
// Match C++ (pose_graph_2d.cc:698-713)
|
|
// FIX: Merged two separate locks into one to prevent data changes between locks
|
|
|
|
TrajectoryNode.Data? constantData;
|
|
Mapping.D2D.Submap2D? submap;
|
|
|
|
lock (_dataLock)
|
|
{
|
|
// Get node constant data
|
|
if (!_trajectoryNodes.Contains(nodeId))
|
|
return 0;
|
|
var node = _trajectoryNodes[nodeId];
|
|
constantData = node.ConstantData;
|
|
if (constantData == null)
|
|
return 0;
|
|
|
|
// Get submap
|
|
if (!_submapData.Contains(submapId))
|
|
return 0;
|
|
var submapData = _submapData[submapId];
|
|
submap = submapData.Submap as Mapping.D2D.Submap2D;
|
|
if (submap == null)
|
|
return 0;
|
|
}
|
|
|
|
// Delegate to constraint builder
|
|
if (_constraintBuilder == null)
|
|
return 0;
|
|
|
|
return _constraintBuilder.ManualComputeConstraintScore(
|
|
submapId, submap, nodeId, constantData, minScore: 0, initialPose);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually compute scan matcher score with refined pose output.
|
|
/// Match C++: manualComputeScanMatcher (pose_graph_2d.cc:715-729)
|
|
/// </summary>
|
|
public override double ManualComputeScanMatcher(
|
|
NodeId nodeId, SubmapId submapId, Rigid3d initialPose,
|
|
out Rigid3d poseManualEstimate)
|
|
{
|
|
// Match C++ (pose_graph_2d.cc:715-729)
|
|
|
|
poseManualEstimate = Rigid3d.Identity;
|
|
|
|
// Get node and submap (same pattern as above)
|
|
TrajectoryNode.Data? constantData;
|
|
Mapping.D2D.Submap2D? submap;
|
|
|
|
lock (_dataLock)
|
|
{
|
|
if (!_trajectoryNodes.Contains(nodeId))
|
|
return 0;
|
|
var node = _trajectoryNodes[nodeId];
|
|
constantData = node.ConstantData;
|
|
|
|
if (!_submapData.Contains(submapId))
|
|
return 0;
|
|
var submapData = _submapData[submapId];
|
|
submap = submapData.Submap as Mapping.D2D.Submap2D;
|
|
}
|
|
|
|
if (constantData == null || submap == null)
|
|
return 0;
|
|
|
|
if (_constraintBuilder == null)
|
|
return 0;
|
|
|
|
return _constraintBuilder.ManualComputeScanMatcher(
|
|
submapId, submap, nodeId, constantData, minScore: 0,
|
|
initialPose, out poseManualEstimate);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually relocalize a trajectory against finished submaps.
|
|
/// Match C++: ManualRelocalization (pose_graph_2d.cc:565-675)
|
|
/// </summary>
|
|
public override bool ManualRelocalization(
|
|
int trajectoryId, out double score, Rigid2d initialPose,
|
|
LocalizationResultCallback? callback)
|
|
{
|
|
// Match C++ (pose_graph_2d.cc:565-675)
|
|
// FIX: Merged 4 separate locks into 1 to prevent data changes between locks
|
|
|
|
score = 0.0;
|
|
|
|
NodeId lastNodeId;
|
|
TrajectoryNode.Data? constantData;
|
|
var finishedSubmapIds = new List<SubmapId>();
|
|
var submaps = new List<Mapping.D2D.Submap2D>();
|
|
var relativePoses = new List<Rigid2d>();
|
|
|
|
// Single lock for all data reading operations
|
|
lock (_dataLock)
|
|
{
|
|
// 1. Store callback
|
|
if (callback != null)
|
|
{
|
|
_localizationResultCallback = callback;
|
|
}
|
|
|
|
// 2. Get last node of trajectory (match C++ line 580: std::prev(EndOfTrajectory))
|
|
var nodesInTrajectory = _trajectoryNodes
|
|
.Where(kvp => kvp.Id.TrajectoryId == trajectoryId)
|
|
.OrderBy(kvp => kvp.Data.ConstantData?.Time ?? 0)
|
|
.ToList();
|
|
|
|
if (nodesInTrajectory.Count == 0)
|
|
return false;
|
|
|
|
var lastNode = nodesInTrajectory[^1];
|
|
lastNodeId = lastNode.Id;
|
|
constantData = lastNode.Data.ConstantData;
|
|
|
|
if (constantData == null)
|
|
return false;
|
|
|
|
// 3. Get finished submaps (trajectory 0 only, match C++ line 585)
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
var submapId = kvp.Id;
|
|
var submapData = kvp.Data;
|
|
|
|
// Match C++: trajectory_id == 0 && state == kFinished
|
|
// Check if submap is finished by checking InsertionFinished property
|
|
if (submapId.TrajectoryId == 0 &&
|
|
submapData.Submap is Mapping.D2D.Submap2D submap2D &&
|
|
submap2D.InsertionFinished)
|
|
{
|
|
finishedSubmapIds.Add(submapId);
|
|
}
|
|
}
|
|
|
|
// 4. Compute relative poses for each submap
|
|
foreach (var submapId in finishedSubmapIds)
|
|
{
|
|
if (!_submapData.Contains(submapId))
|
|
continue;
|
|
|
|
var submapData = _submapData[submapId];
|
|
if (submapData.Submap is not Mapping.D2D.Submap2D submap)
|
|
continue;
|
|
|
|
// Get global pose from optimization problem (already 2D)
|
|
if (!_globalSubmapPoses2D.Contains(submapId))
|
|
continue;
|
|
|
|
var globalSubmapPose2D = _globalSubmapPoses2D[submapId].GlobalPose;
|
|
|
|
// Compute relative pose: submap.inverse() * initial_pose
|
|
var relativePose = globalSubmapPose2D.Inverse() * initialPose;
|
|
|
|
// Filter by max constraint distance (match C++ line 600-606)
|
|
if (_options.ConstraintBuilderOptions.HasValue &&
|
|
relativePose.Translation.Length() < _options.ConstraintBuilderOptions.Value.MaxConstraintDistance)
|
|
{
|
|
submaps.Add(submap);
|
|
relativePoses.Add(relativePose);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 5. Check if we have submaps to relocalize against
|
|
if (submaps.Count == 0)
|
|
return false;
|
|
|
|
// 6. Call constraint builder for relocalization
|
|
if (_constraintBuilder == null)
|
|
return false;
|
|
|
|
var (Score, Constraint) = _constraintBuilder.ManualComputeRelocalizationConstraint(
|
|
finishedSubmapIds, submaps, relativePoses,
|
|
lastNodeId, constantData, minScore: 0);
|
|
|
|
score = Score;
|
|
|
|
// 7. If relocalization failed, return false
|
|
if (Score == 0 || Constraint == null)
|
|
return false;
|
|
|
|
// 8. Add constraint and run optimization
|
|
lock (_dataLock)
|
|
{
|
|
// Verify node and submap still exist (match C++ line 656-663)
|
|
if (!_trajectoryNodes.Contains(Constraint.Value.NodeId))
|
|
return false;
|
|
if (!_submapData.Contains(Constraint.Value.SubmapId))
|
|
return false;
|
|
|
|
_constraints.Add(Constraint.Value);
|
|
|
|
// Update trajectory connectivity (match C++ line 667-669)
|
|
UpdateTrajectoryConnectivityUnsafe(Constraint.Value);
|
|
}
|
|
|
|
// 9. Run optimization synchronously (matches C++ ManualRelocalization behavior)
|
|
RunOptimizationSync();
|
|
|
|
// 10. Invoke callback with result
|
|
if (_localizationResultCallback != null)
|
|
{
|
|
// Get optimized pose from trajectory node poses
|
|
Rigid3d globalPose = Rigid3d.Identity;
|
|
lock (_dataLock)
|
|
{
|
|
if (_trajectoryNodePoses.Contains(lastNodeId))
|
|
{
|
|
globalPose = _trajectoryNodePoses[lastNodeId].GlobalPose;
|
|
}
|
|
}
|
|
|
|
var localizationResult = new LocalizationResult
|
|
{
|
|
NodeId = lastNodeId,
|
|
SubmapId = Constraint.Value.SubmapId,
|
|
GlobalPose = globalPose,
|
|
Score = Score
|
|
};
|
|
_localizationResultCallback(localizationResult);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Drains the work queue. Match C++ DrainWorkQueue() implementation (line 849-873).
|
|
/// Processes work items until queue is empty or optimization is needed.
|
|
/// When queue is empty, schedules WhenDone callback to HandleWorkQueue (OnConstraintBuilderResult).
|
|
/// </summary>
|
|
public override void DrainWorkQueue()
|
|
{
|
|
// Match C++ DrainWorkQueue (line 849-866): Process work items until queue is empty or optimization is needed
|
|
// Use WorkQueue's DrainWorkQueue method which handles the processing logic
|
|
_workQueue.DrainWorkQueue();
|
|
|
|
// Match C++ (line 867): Log remaining work items
|
|
// Note: WorkQueue exposes Count property, but we skip logging in production
|
|
|
|
// Match C++ (line 868-872): Schedule WhenDone callback when queue is empty
|
|
// In C++, this schedules HandleWorkQueue to be called when constraint builder finishes
|
|
// In C#, OnConstraintBuilderResult already serves as the HandleWorkQueue equivalent
|
|
// and is called automatically when constraints are ready via MaybeAddConstraint callback
|
|
// So we don't need to schedule it again here
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finishes a trajectory (matches xloc_cpp FinishTrajectory implementation).
|
|
/// Marks trajectory and all its submaps as finished, and triggers optimization.
|
|
/// </summary>
|
|
public override void FinishTrajectory(int trajectoryId)
|
|
{
|
|
// Match C++: Add work item to work queue (not direct lock)
|
|
// This ensures thread-safe serialization with other operations
|
|
AddWorkItem($"FinishTrajectory {trajectoryId}", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Check if trajectory is already finished (match C++ CHECK)
|
|
if (_trajectoryStates.TryGetValue(trajectoryId, out IPoseGraph.TrajectoryState value) && value == IPoseGraph.TrajectoryState.Finished)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"Trajectory {trajectoryId} is already finished");
|
|
}
|
|
|
|
// Mark trajectory as FINISHED (match C++ line 968)
|
|
_trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Finished;
|
|
|
|
// Mark all submaps of this trajectory as finished (match C++ lines 970-972)
|
|
// In C++, this sets data_.submap_data.at(submap.id).state = SubmapState::kFinished
|
|
// In RobotApp, we mark submaps as insertion finished since we don't have separate state tracking
|
|
foreach (var submapEntry in _submapData.BeginOfTrajectory(trajectoryId))
|
|
{
|
|
submapEntry.Data.Submap?.InsertionFinished = true;
|
|
_finishedSubmapIds.Add(submapEntry.Id);
|
|
}
|
|
|
|
// Trim old sensor data to bound memory growth for finished trajectories
|
|
TrimSensorDataForFinishedTrajectory(trajectoryId);
|
|
}
|
|
|
|
// Match C++: Return kRunOptimization to trigger optimization after finishing
|
|
// This ensures optimization runs after trajectory is marked as finished
|
|
return WorkItemResult.RunOptimization;
|
|
});
|
|
}
|
|
|
|
public override void FreezeTrajectory(int trajectoryId)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Frozen;
|
|
}
|
|
}
|
|
|
|
public override void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap)
|
|
{
|
|
Submap? runtimeSubmap = null;
|
|
if (submap.Submap2D.HasValue)
|
|
{
|
|
// Convert from proto to runtime Submap2D
|
|
var conversionTables = new ValueConversionTables();
|
|
runtimeSubmap = new Mapping.D2D.Submap2D(submap.Submap2D.Value, conversionTables);
|
|
}
|
|
|
|
var submapId = submap.SubmapId;
|
|
var runtimeSubmapId = new SubmapId(submapId.TrajectoryId, submapId.SubmapIndex);
|
|
_submapData.Insert(runtimeSubmapId, new IPoseGraph.SubmapData(runtimeSubmap, globalPose));
|
|
|
|
// Populate finished submap cache for submaps loaded from proto
|
|
if (runtimeSubmap is Mapping.D2D.Submap2D submap2D && submap2D.InsertionFinished)
|
|
{
|
|
_finishedSubmapIds.Add(runtimeSubmapId);
|
|
}
|
|
}
|
|
|
|
public override void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node)
|
|
{
|
|
var nodeData = TrajectoryNodeOperations.FromProto(node.NodeData);
|
|
var trajectoryNode = new TrajectoryNode
|
|
{
|
|
ConstantData = nodeData,
|
|
GlobalPose = globalPose
|
|
};
|
|
|
|
var nodeId = node.NodeId;
|
|
var runtimeNodeId = new NodeId(nodeId.TrajectoryId, nodeId.NodeIndex);
|
|
|
|
_trajectoryNodes.Insert(runtimeNodeId, trajectoryNode);
|
|
_trajectoryNodePoses.Insert(runtimeNodeId, new TrajectoryNodePose
|
|
{
|
|
GlobalPose = globalPose,
|
|
ConstantPoseData = new ConstantPoseData
|
|
{
|
|
Time = nodeData.Time,
|
|
LocalPose = nodeData.LocalPose
|
|
}
|
|
});
|
|
}
|
|
|
|
public override void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data)
|
|
{
|
|
// Convert from proto to runtime TrajectoryData
|
|
var trajectoryData = new IPoseGraph.TrajectoryData(
|
|
data.GravityConstant,
|
|
data.ImuCalibration.HasValue ? (Quaternion?)data.ImuCalibration.Value : null,
|
|
data.FixedFrameOriginInMap.HasValue ? (Rigid3d?)data.FixedFrameOriginInMap.Value : null
|
|
);
|
|
_trajectoryData[data.TrajectoryId] = trajectoryData;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a node to a submap (matches C++ AddNodeToSubmap).
|
|
/// C++: Only inserts node_id into submap_data.node_ids, does NOT create constraints here.
|
|
/// Constraints are created in ComputeConstraintsForNode.
|
|
/// </summary>
|
|
public override void AddNodeToSubmap(NodeId nodeId, SubmapId submapId)
|
|
{
|
|
// Match C++: AddWorkItem to work queue (line 1120)
|
|
var capturedNodeId = nodeId;
|
|
var capturedSubmapId = submapId;
|
|
|
|
AddWorkItem($"AddNodeToSubmap TrajectoryId={nodeId.TrajectoryId} SubmapIndex={submapId.SubmapIndex} NodeIndex={nodeId.NodeIndex}", () =>
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Match C++: if (CanAddWorkItemModifying(submap_id.trajectory_id))
|
|
// Check if trajectory is finished or deleted
|
|
if (!_trajectoryStates.TryGetValue(capturedSubmapId.TrajectoryId, out var state) ||
|
|
state == IPoseGraph.TrajectoryState.Finished ||
|
|
state == IPoseGraph.TrajectoryState.Deleted)
|
|
{
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
}
|
|
|
|
// Match C++: data_.submap_data.at(submap_id).node_ids.insert(node_id)
|
|
// Track node-to-submap insertion for constraint building
|
|
if (!_submapNodeInsertions.TryGetValue(capturedSubmapId, out var nodes))
|
|
{
|
|
nodes = [];
|
|
_submapNodeInsertions[capturedSubmapId] = nodes;
|
|
}
|
|
nodes.Add(capturedNodeId);
|
|
}
|
|
return WorkItemResult.DoNotRunOptimization;
|
|
});
|
|
}
|
|
|
|
public override void AddSerializedConstraints(List<IPoseGraph.Constraint> constraints)
|
|
{
|
|
_constraints.AddRange(constraints);
|
|
}
|
|
|
|
private readonly List<PoseGraphTrimmer> _trimmers = [];
|
|
|
|
public override void AddTrimmer(PoseGraphTrimmer trimmer)
|
|
{
|
|
_trimmers.Add(trimmer);
|
|
}
|
|
|
|
private void RunTrimmers()
|
|
{
|
|
if (_trimmers.Count == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
foreach (var trimmer in _trimmers)
|
|
{
|
|
trimmer.Trim(new TrimmablePoseGraph2D(this));
|
|
}
|
|
|
|
lock (_dataLock)
|
|
{
|
|
var totalSubmapsAfter = _submapData.Count;
|
|
var submapsByTrajectoryAfter = new Dictionary<int, int>();
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
var trajId = kvp.Id.TrajectoryId;
|
|
submapsByTrajectoryAfter.TryGetValue(trajId, out var count);
|
|
submapsByTrajectoryAfter[trajId] = count + 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
private class TrimmablePoseGraph2D(PoseGraph2D poseGraph) : ITrimmable
|
|
{
|
|
public int NumSubmaps(int trajectoryId)
|
|
{
|
|
lock (poseGraph._dataLock)
|
|
{
|
|
int count = 0;
|
|
foreach (var kvp in poseGraph._submapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
count++;
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
}
|
|
|
|
public List<SubmapId> GetSubmapIds(int trajectoryId)
|
|
{
|
|
lock (poseGraph._dataLock)
|
|
{
|
|
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()
|
|
{
|
|
lock (poseGraph._dataLock)
|
|
{
|
|
// Return a copy to avoid external modification
|
|
var result = new MapById<SubmapId, IPoseGraph.SubmapData>();
|
|
foreach (var kvp in poseGraph._submapData)
|
|
{
|
|
result.Insert(kvp.Id, kvp.Data);
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public MapById<NodeId, TrajectoryNode> GetTrajectoryNodes()
|
|
{
|
|
lock (poseGraph._dataLock)
|
|
{
|
|
// Return a copy to avoid external modification
|
|
var result = new MapById<NodeId, TrajectoryNode>();
|
|
foreach (var kvp in poseGraph._trajectoryNodes)
|
|
{
|
|
result.Insert(kvp.Id, kvp.Data);
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public List<IPoseGraph.Constraint> GetConstraints()
|
|
{
|
|
lock (poseGraph._dataLock)
|
|
{
|
|
return [.. poseGraph._constraints];
|
|
}
|
|
}
|
|
|
|
public void TrimSubmap(SubmapId submapId)
|
|
{
|
|
lock (poseGraph._dataLock)
|
|
{
|
|
// Match C++ TrimSubmap (pose_graph_2d.cc line 1638-1729)
|
|
if (!poseGraph._submapData.Contains(submapId))
|
|
return;
|
|
|
|
// Nodes that are still INTRA_SUBMAP constrained to other submaps once this submap is gone
|
|
var nodesToRetain = new HashSet<NodeId>();
|
|
foreach (var kvp in poseGraph._submapData)
|
|
{
|
|
if (!kvp.Id.Equals(submapId) && poseGraph._submapNodeInsertions.TryGetValue(kvp.Id, out var nodeIds))
|
|
{
|
|
foreach (var n in nodeIds)
|
|
nodesToRetain.Add(n);
|
|
}
|
|
}
|
|
|
|
// Nodes exclusively associated to submapId
|
|
var nodesToRemove = new HashSet<NodeId>();
|
|
if (poseGraph._submapNodeInsertions.TryGetValue(submapId, out var thisSubmapNodeIds))
|
|
{
|
|
foreach (var n in thisSubmapNodeIds)
|
|
{
|
|
if (!nodesToRetain.Contains(n))
|
|
nodesToRemove.Add(n);
|
|
}
|
|
}
|
|
|
|
// Remove all constraints related to submap_id
|
|
poseGraph._constraints.RemoveAll(c => c.SubmapId.Equals(submapId));
|
|
|
|
// Remove all constraints related to nodes_to_remove; track other submaps losing constraints
|
|
var otherSubmapIdsLosingConstraints = new HashSet<SubmapId>();
|
|
var constraintsAfterRemoval = new List<IPoseGraph.Constraint>();
|
|
foreach (var c in poseGraph._constraints)
|
|
{
|
|
if (!nodesToRemove.Contains(c.NodeId))
|
|
constraintsAfterRemoval.Add(c);
|
|
else
|
|
otherSubmapIdsLosingConstraints.Add(c.SubmapId);
|
|
}
|
|
poseGraph._constraints.Clear();
|
|
poseGraph._constraints.AddRange(constraintsAfterRemoval);
|
|
|
|
// Keep only submaps that have no inter-submap constraints left
|
|
foreach (var c in poseGraph._constraints)
|
|
{
|
|
if (c.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap &&
|
|
otherSubmapIdsLosingConstraints.Contains(c.SubmapId))
|
|
otherSubmapIdsLosingConstraints.Remove(c.SubmapId);
|
|
}
|
|
foreach (var sid in otherSubmapIdsLosingConstraints)
|
|
poseGraph.DeleteConstraintBuilderScanMatcher(sid);
|
|
|
|
// Mark submap as trimmed and remove its data
|
|
poseGraph._submapData.Trim(submapId);
|
|
poseGraph.DeleteConstraintBuilderScanMatcher(submapId);
|
|
poseGraph._optimizationProblem.TrimSubmap(submapId);
|
|
if (poseGraph._globalSubmapPoses2D.Contains(submapId))
|
|
poseGraph._globalSubmapPoses2D.Trim(submapId);
|
|
poseGraph._submapNodeInsertions.Remove(submapId);
|
|
poseGraph._finishedSubmapIds.Remove(submapId);
|
|
|
|
// Remove nodes_to_remove from pose graph and optimization problem
|
|
foreach (var nodeId in nodesToRemove)
|
|
{
|
|
poseGraph._trajectoryNodes.Trim(nodeId);
|
|
poseGraph._trajectoryNodePoses.Trim(nodeId);
|
|
poseGraph._optimizationProblem.TrimTrajectoryNode(nodeId);
|
|
foreach (var set in poseGraph._submapNodeInsertions.Values)
|
|
set.Remove(nodeId);
|
|
}
|
|
}
|
|
}
|
|
|
|
public bool IsFinished(int trajectoryId)
|
|
{
|
|
return poseGraph.IsTrajectoryFinished(trajectoryId);
|
|
}
|
|
|
|
public void SetTrajectoryState(int trajectoryId, IPoseGraph.TrajectoryState state)
|
|
{
|
|
lock (poseGraph._dataLock)
|
|
{
|
|
poseGraph._trajectoryStates[trajectoryId] = state;
|
|
}
|
|
}
|
|
}
|
|
|
|
public override List<List<int>> GetConnectedTrajectories()
|
|
{
|
|
// Analyze constraints to find connected components of trajectories
|
|
var visited = new HashSet<int>();
|
|
var result = new List<List<int>>();
|
|
|
|
// Get all trajectory IDs
|
|
var allTrajectoryIds = new HashSet<int>();
|
|
foreach (var kvp in _trajectoryNodes)
|
|
{
|
|
allTrajectoryIds.Add(kvp.Id.TrajectoryId);
|
|
}
|
|
foreach (var kvp in _submapData)
|
|
{
|
|
allTrajectoryIds.Add(kvp.Id.TrajectoryId);
|
|
}
|
|
|
|
// Build connectivity graph from constraints
|
|
var graph = new Dictionary<int, HashSet<int>>();
|
|
foreach (var constraint in _constraints)
|
|
{
|
|
var nodeTrajectoryId = constraint.NodeId.TrajectoryId;
|
|
var submapTrajectoryId = constraint.SubmapId.TrajectoryId;
|
|
|
|
if (!graph.TryGetValue(nodeTrajectoryId, out var nodeNeighbors))
|
|
{
|
|
nodeNeighbors = [];
|
|
graph[nodeTrajectoryId] = nodeNeighbors;
|
|
}
|
|
nodeNeighbors.Add(submapTrajectoryId);
|
|
|
|
if (!graph.TryGetValue(submapTrajectoryId, out var submapNeighbors))
|
|
{
|
|
submapNeighbors = [];
|
|
graph[submapTrajectoryId] = submapNeighbors;
|
|
}
|
|
submapNeighbors.Add(nodeTrajectoryId);
|
|
}
|
|
|
|
// Find connected components using DFS
|
|
foreach (var trajectoryId in allTrajectoryIds)
|
|
{
|
|
if (visited.Contains(trajectoryId))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var component = new List<int>();
|
|
var stack = new Stack<int>();
|
|
stack.Push(trajectoryId);
|
|
visited.Add(trajectoryId);
|
|
|
|
while (stack.Count > 0)
|
|
{
|
|
var current = stack.Pop();
|
|
component.Add(current);
|
|
|
|
if (graph.TryGetValue(current, out var neighbors))
|
|
{
|
|
foreach (var neighbor in neighbors)
|
|
{
|
|
if (!visited.Add(neighbor))
|
|
{
|
|
stack.Push(neighbor);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (component.Count > 0)
|
|
{
|
|
result.Add(component);
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
public override Dictionary<int, List<ImuData>> GetImuData()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Return deep copy to avoid external modification
|
|
var result = new Dictionary<int, List<ImuData>>();
|
|
foreach (var kvp in _imuData)
|
|
{
|
|
result[kvp.Key] = [.. kvp.Value];
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public override Dictionary<int, List<OdometryData>> GetOdometryData()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Return deep copy to avoid external modification
|
|
var result = new Dictionary<int, List<OdometryData>>();
|
|
foreach (var kvp in _odometryData)
|
|
{
|
|
result[kvp.Key] = [.. kvp.Value];
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public override Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
// Return deep copy to avoid external modification
|
|
var result = new Dictionary<int, List<FixedFramePoseData>>();
|
|
foreach (var kvp in _fixedFramePoseData)
|
|
{
|
|
result[kvp.Key] = [.. kvp.Value];
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public override Dictionary<string, IPoseGraph.LandmarkNode> GetLandmarkNodes()
|
|
{
|
|
return new Dictionary<string, IPoseGraph.LandmarkNode>(_landmarkNodes);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ SetInitialTrajectoryPose: store (to_trajectory_id, pose, time) and set local-to-global for from_trajectory.
|
|
/// Used when adding first submap in InitializeGlobalSubmapPoses to Connect(from, to, time).
|
|
/// </summary>
|
|
public override void SetInitialTrajectoryPose(int fromTrajectoryId, int toTrajectoryId, Rigid3d pose, long time)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_initialTrajectoryPoses[fromTrajectoryId] = (toTrajectoryId, pose, time);
|
|
|
|
if (!_localToGlobalTransforms.ContainsKey(fromTrajectoryId))
|
|
{
|
|
var targetTransform = _localToGlobalTransforms.TryGetValue(toTrajectoryId, out var transform)
|
|
? transform
|
|
: GetLocalToGlobalTransformUnsafe(toTrajectoryId);
|
|
var sourceTransform = targetTransform * pose;
|
|
_localToGlobalTransforms[fromTrajectoryId] = sourceTransform;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds a new node with constant_data. Its constant_data.LocalPose was
|
|
/// determined by scan matching against insertion_submaps.front() and the
|
|
/// node data was inserted into the insertion_submaps. If
|
|
/// insertion_submaps.front().InsertionFinished is true, data was inserted into
|
|
/// this submap for the last time.
|
|
///
|
|
/// Matches C++ implementation: AddNode calls AppendNode SYNCHRONOUSLY,
|
|
/// then queues ComputeConstraintsForNode to work queue. This ensures fast return
|
|
/// while constraint building happens asynchronously.
|
|
/// </summary>
|
|
public NodeId AddNode(
|
|
TrajectoryNode.Data constantData,
|
|
int trajectoryId,
|
|
List<Mapping.D2D.Submap2D> insertionSubmaps)
|
|
{
|
|
if (insertionSubmaps == null || insertionSubmaps.Count == 0)
|
|
{
|
|
throw new ArgumentException("insertion_submaps cannot be null or empty");
|
|
}
|
|
|
|
// Compute optimized pose (same as C++ GetLocalToGlobalTransform)
|
|
var localToGlobalTransform = GetLocalToGlobalTransform(trajectoryId);
|
|
var localPose = constantData.LocalPose;
|
|
var optimizedPose = localToGlobalTransform * localPose;
|
|
|
|
// Validate poses
|
|
if (double.IsNaN(localPose.Translation.X) || double.IsNaN(localPose.Translation.Y) ||
|
|
double.IsInfinity(localPose.Translation.X) || double.IsInfinity(localPose.Translation.Y))
|
|
{
|
|
throw new InvalidOperationException($"Local pose is NaN or Infinity: {localPose.Translation.X}, {localPose.Translation.Y}");
|
|
}
|
|
if (double.IsNaN(optimizedPose.Translation.X) || double.IsNaN(optimizedPose.Translation.Y) ||
|
|
double.IsInfinity(optimizedPose.Translation.X) || double.IsInfinity(optimizedPose.Translation.Y))
|
|
{
|
|
throw new InvalidOperationException($"Optimized pose is NaN or Infinity: {optimizedPose.Translation.X}, {optimizedPose.Translation.Y}");
|
|
}
|
|
|
|
// Call AppendNodeUnsafe SYNCHRONOUSLY with lock (matches C++ AppendNode behavior)
|
|
// This is fast and blocking, but necessary to return nodeId immediately
|
|
NodeId nodeId;
|
|
bool newlyFinishedSubmap;
|
|
lock (_dataLock)
|
|
{
|
|
nodeId = AppendNodeUnsafe(constantData, trajectoryId, insertionSubmaps, optimizedPose);
|
|
// Check if submap is newly finished (must check here before queuing)
|
|
newlyFinishedSubmap = 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 capturedNewlyFinishedSubmap = newlyFinishedSubmap;
|
|
|
|
AddWorkItem($"AddNode trajectoryId={trajectoryId}", () =>
|
|
{
|
|
// Match C++ ComputeConstraintsForNode execution order:
|
|
// 1. lock(mutex_) { build constraints, collect tasks } unlock
|
|
// 2. ComputeConstraint for each finished submap (no lock)
|
|
// 3. ComputeConstraint for old nodes vs newly finished submap (no lock)
|
|
// 4. NotifyEndOfNode()
|
|
// 5. lock(mutex_) { ++counter, check optimization } unlock
|
|
List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> finishedSubmapConstraintTasks;
|
|
List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> newlyFinishedSubmapConstraintTasks;
|
|
|
|
// Step 1: lock - build constraints, collect tasks
|
|
lock (_dataLock)
|
|
{
|
|
ComputeConstraintsForNodeUnsafe(
|
|
capturedNodeId,
|
|
capturedConstantData,
|
|
capturedTrajectoryId,
|
|
capturedInsertionSubmaps,
|
|
capturedNewlyFinishedSubmap,
|
|
out finishedSubmapConstraintTasks,
|
|
out newlyFinishedSubmapConstraintTasks);
|
|
}
|
|
|
|
// Step 2-3: ComputeConstraint dispatches without lock (match C++ line 445-474)
|
|
if (_constraintBuilder != null)
|
|
{
|
|
void ProcessTask(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)
|
|
{
|
|
// Match C++: None means ComputeConstraint decided not to create any constraint
|
|
if (kind == ConstraintTaskKind.None)
|
|
return;
|
|
if (kind == ConstraintTaskKind.Global)
|
|
_constraintBuilder.MaybeAddGlobalConstraint(submapId, taskNodeId, submap, node, callback: OnConstraintBuilderResult);
|
|
else if (kind == ConstraintTaskKind.Localization && localizationPoses != null)
|
|
_constraintBuilder.MaybeAddLocalizationConstraint(submapId, taskNodeId, submap, node, localizationPoses, callback: OnConstraintBuilderResult);
|
|
else
|
|
_constraintBuilder.MaybeAddConstraint(submapId, taskNodeId, submap, node, initialRelativePose, callback: OnConstraintBuilderResult);
|
|
}
|
|
foreach (var (submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind) in finishedSubmapConstraintTasks)
|
|
ProcessTask(submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind);
|
|
foreach (var (submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind) in newlyFinishedSubmapConstraintTasks)
|
|
ProcessTask(submapId, taskNodeId, submap, node, initialRelativePose, localizationPoses, kind);
|
|
|
|
// Step 4: NotifyEndOfNode (match C++ line 476)
|
|
_constraintBuilder.NotifyEndOfNode();
|
|
}
|
|
|
|
// Step 5: lock - increment counter and check optimization (match C++ line 477-488)
|
|
lock (_dataLock)
|
|
{
|
|
return CheckIfOptimizationNeededUnsafe(capturedTrajectoryId)
|
|
? WorkItemResult.RunOptimization
|
|
: WorkItemResult.DoNotRunOptimization;
|
|
}
|
|
});
|
|
|
|
return nodeId;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Appends a node to the trajectory (matches C++ AppendNode).
|
|
/// Only appends node and submap to data structures - 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.D2D.Submap2D> insertionSubmaps,
|
|
Rigid3d optimizedPose)
|
|
{
|
|
// Ensure trajectory state exists
|
|
AddTrajectoryIfNeededUnsafe(trajectoryId);
|
|
|
|
if (!_trajectoryStates.TryGetValue(trajectoryId, out IPoseGraph.TrajectoryState value))
|
|
{
|
|
value = IPoseGraph.TrajectoryState.Active;
|
|
_trajectoryStates[trajectoryId] = value;
|
|
}
|
|
|
|
// Check if trajectory is finished or deleted
|
|
if (value == IPoseGraph.TrajectoryState.Finished || value == IPoseGraph.TrajectoryState.Deleted)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"Cannot add node to finished or deleted trajectory {trajectoryId}");
|
|
}
|
|
|
|
// Append node to trajectory (matches C++ AppendNode)
|
|
var nodeId = _trajectoryNodes.Append(trajectoryId, new TrajectoryNode
|
|
{
|
|
ConstantData = constantData,
|
|
GlobalPose = optimizedPose
|
|
});
|
|
|
|
// Bump version so OccupancyGridManager knows new data is available.
|
|
_nodeInsertionVersion++;
|
|
|
|
// CRITICAL FIX: Do NOT insert into _trajectoryNodePoses here.
|
|
// In C++ code, GetTrajectoryNodePoses() computes the result from trajectory_nodes
|
|
// on demand, not from a separate member variable. Inserting here causes
|
|
// CanAppend to be set to false for the trajectory in _trajectoryNodePoses,
|
|
// which can cause issues when appending subsequent nodes.
|
|
// The _trajectoryNodePoses member variable is only used when loading from proto
|
|
// (AddNodeFromProto), not during normal operation.
|
|
|
|
// Match C++: Test if the 'insertion_submap.back()' is one we never saw before.
|
|
// C++ line 152-162: if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
|
|
// std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))->data.submap != insertion_submaps.back())
|
|
// CRITICAL FIX: Check if trajectory is frozen before checking for existing submaps
|
|
// Frozen trajectories cannot be appended to, so we must always create new submaps for new trajectories
|
|
bool isNewSubmap = true;
|
|
bool trajectoryIsFrozen = _trajectoryStates.TryGetValue(trajectoryId, out var trajectoryState) &&
|
|
trajectoryState == IPoseGraph.TrajectoryState.Frozen;
|
|
|
|
if (!trajectoryIsFrozen && _submapData.SizeOfTrajectoryOrZero(trajectoryId) > 0)
|
|
{
|
|
// Find the last submap in the trajectory (highest SubmapIndex)
|
|
SubmapId? lastSubmapId = null;
|
|
int maxIndex = -1;
|
|
foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
|
|
{
|
|
if (kvp.Id.SubmapIndex > maxIndex)
|
|
{
|
|
maxIndex = kvp.Id.SubmapIndex;
|
|
lastSubmapId = kvp.Id;
|
|
}
|
|
}
|
|
|
|
if (lastSubmapId.HasValue && _submapData.Contains(lastSubmapId.Value))
|
|
{
|
|
var lastSubmap = _submapData[lastSubmapId.Value].Submap;
|
|
if (lastSubmap == insertionSubmaps[^1])
|
|
{
|
|
isNewSubmap = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Match C++: We grow 'data_.submap_data' as needed. This code assumes that the first
|
|
// time we see a new submap is as 'insertion_submaps.back()'.
|
|
// CRITICAL FIX: For new trajectories (not frozen), always append new submaps
|
|
// For frozen trajectories, submaps should have been loaded from map, so this should not happen
|
|
if (isNewSubmap)
|
|
{
|
|
// Match C++: const SubmapId submap_id = data_.submap_data.Append(trajectory_id, InternalSubmapData());
|
|
// Match C++: data_.submap_data.at(submap_id).submap = insertion_submaps.back();
|
|
_ = _submapData.Append(trajectoryId, new IPoseGraph.SubmapData(insertionSubmaps[^1], Rigid3d.Identity));
|
|
|
|
// CRITICAL FIX: Update localToGlobalTransform immediately after creating the first submap.
|
|
// For localization trajectories we set initial pose via SetInitialTrajectoryPose; _globalSubmapPoses2D
|
|
// is only updated later in InitializeGlobalSubmapPosesUnsafe (called from async work item). So
|
|
// ComputeLocalToGlobalTransform would return Identity here and would overwrite the MCL pose.
|
|
// Preserve existing _localToGlobalTransforms when this trajectory has an initial pose (localization).
|
|
if (_submapData.SizeOfTrajectoryOrZero(trajectoryId) == 1)
|
|
{
|
|
if (!_initialTrajectoryPoses.ContainsKey(trajectoryId))
|
|
{
|
|
_localToGlobalTransforms[trajectoryId] = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
|
|
}
|
|
// else: keep _localToGlobalTransforms[trajectoryId] from SetInitialTrajectoryPose (MCL pose)
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Not a new submap - node is being added to existing submap
|
|
//
|
|
// WARNING: ADDITIONAL C# LOGIC NOT IN ORIGINAL C++ CODE
|
|
// =====================================================
|
|
// This zero-pose submap correction is C# specific and does NOT exist in the original
|
|
// Cartographer C++ implementation (pose_graph_2d.cc). It was added to handle an edge case
|
|
// where the first submap is initialized with zero global pose but the first node has a
|
|
// non-zero pose (e.g., when using MCL/localization with initial pose).
|
|
//
|
|
// Purpose: Synchronize the submap's global pose with the node's pose to ensure
|
|
// ComputeLocalToGlobalTransform returns correct values for subsequent nodes.
|
|
//
|
|
// Trigger conditions:
|
|
// 1. Only one submap exists in the trajectory
|
|
// 2. Submap has zero global pose (X, Y, rotation all < 1e-6)
|
|
// 3. Node has non-zero global pose (any of X, Y, rotation > 1e-6)
|
|
//
|
|
// Potential impacts:
|
|
// - May cause different behavior from C++ in edge cases
|
|
// - Recommended to verify MCL initialization scenarios work correctly
|
|
// - Consider removing if upstream C++ code is updated to handle this case
|
|
//
|
|
// TODO: Verify with domain experts if this is the correct approach
|
|
if (_submapData.SizeOfTrajectoryOrZero(trajectoryId) == 1)
|
|
{
|
|
// Only one submap exists - check if it has zero pose and node has non-zero pose
|
|
SubmapId? firstSubmapId = null;
|
|
foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
|
|
{
|
|
if (firstSubmapId == null || kvp.Id.SubmapIndex < firstSubmapId.Value.SubmapIndex)
|
|
{
|
|
firstSubmapId = kvp.Id;
|
|
}
|
|
}
|
|
|
|
if (firstSubmapId.HasValue && _submapData.Contains(firstSubmapId.Value))
|
|
{
|
|
var submapData = _submapData[firstSubmapId.Value];
|
|
var currentGlobalPose = submapData.Pose;
|
|
var currentGlobalPose2D = TransformOperations.Project2D(currentGlobalPose);
|
|
var optimizedPose2D = TransformOperations.Project2D(optimizedPose);
|
|
|
|
// Check if submap has zero pose and node has non-zero pose
|
|
bool submapHasZeroPose = Math.Abs(currentGlobalPose2D.Translation.X) < 1e-6 &&
|
|
Math.Abs(currentGlobalPose2D.Translation.Y) < 1e-6 &&
|
|
Math.Abs(currentGlobalPose2D.Rotation) < 1e-6;
|
|
bool nodeHasNonZeroPose = Math.Abs(optimizedPose2D.Translation.X) > 1e-6 ||
|
|
Math.Abs(optimizedPose2D.Translation.Y) > 1e-6 ||
|
|
Math.Abs(optimizedPose2D.Rotation) > 1e-6;
|
|
|
|
if (submapHasZeroPose && nodeHasNonZeroPose)
|
|
{
|
|
// Update submap global pose from node pose
|
|
var updatedGlobalPose = optimizedPose;
|
|
_submapData[firstSubmapId.Value] = new IPoseGraph.SubmapData(submapData.Submap, updatedGlobalPose);
|
|
|
|
// Update localToGlobalTransform
|
|
var transform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
|
|
_localToGlobalTransforms[trajectoryId] = transform;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return nodeId;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds trajectory if needed (assumes lock held). Match C++ AddTrajectoryIfNeeded: trajectory_connectivity_state.Add, global_localization_samplers_.
|
|
/// </summary>
|
|
private void AddTrajectoryIfNeededUnsafe(int trajectoryId)
|
|
{
|
|
if (!_trajectoryStates.ContainsKey(trajectoryId))
|
|
{
|
|
_trajectoryStates[trajectoryId] = IPoseGraph.TrajectoryState.Active;
|
|
}
|
|
_trajectoryConnectivityState.Add(trajectoryId);
|
|
if (!_globalLocalizationSamplers.ContainsKey(trajectoryId))
|
|
{
|
|
var ratio = _options.GlobalSamplingRatio;
|
|
if (ratio <= 0) ratio = 0.003;
|
|
_globalLocalizationSamplers[trajectoryId] = new FixedRatioSampler(ratio);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get latest node time for (node_id, submap_id) (match C++ GetLatestNodeTime). Assumes lock held.
|
|
/// </summary>
|
|
private long GetLatestNodeTimeUnsafe(NodeId nodeId, SubmapId submapId)
|
|
{
|
|
if (!_trajectoryNodes.Contains(nodeId))
|
|
return 0;
|
|
if (!_submapData.Contains(submapId))
|
|
return _trajectoryNodes[nodeId].ConstantData?.Time ?? 0;
|
|
var time = _trajectoryNodes[nodeId].ConstantData?.Time ?? 0;
|
|
if (_submapNodeInsertions.TryGetValue(submapId, out var nodeIds) && nodeIds.Count > 0)
|
|
{
|
|
// SortedSet.Max is O(log N), matching C++ std::set::rbegin() which is O(1).
|
|
var lastNodeId = nodeIds.Max;
|
|
if (_trajectoryNodes.Contains(lastNodeId))
|
|
{
|
|
var lastTime = _trajectoryNodes[lastNodeId].ConstantData?.Time ?? 0;
|
|
if (lastTime > time) time = lastTime;
|
|
}
|
|
}
|
|
return time;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets localization initial poses for relocalizing against the map.
|
|
/// Match C++: SetLocalizationInitialPoses (pose_graph_2d.cc:1467-1475)
|
|
/// Sets initial poses, isRelocalized=false, ToggleSearchingForRelocalization(true), num_relocalization_constraint_search_=0.
|
|
/// </summary>
|
|
public override void SetLocalizationInitialPoses(IReadOnlyList<Rigid3d> localizationInitialPoses)
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_localizationInitialPoses = localizationInitialPoses?.ToList() ?? [];
|
|
_isRelocalized = false;
|
|
_constraintBuilder?.ToggleSearchingForRelocalization(true);
|
|
_numRelocalizationConstraintSearch = 0;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ SetLocalizationMode.
|
|
/// </summary>
|
|
public void SetLocalizationMode(bool value)
|
|
{
|
|
lock (_dataLock) { _localizationMode = value; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ SetLocalizationCallback: set callback invoked when relocalization search reaches max nodes without success.
|
|
/// </summary>
|
|
public void SetLocalizationCallback(LocalizationSearchCallback? callback)
|
|
{
|
|
_localizationSearchCallback = callback;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ matching_score_enabled: enable/disable pose confidence scoring.
|
|
/// </summary>
|
|
public void SetMatchingScoreEnabled(bool value)
|
|
{
|
|
lock (_dataLock) { _matchingScoreEnabled = value; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the current node scores for pose confidence tracking.
|
|
/// Match C++: node_scores_ accessor.
|
|
/// </summary>
|
|
public List<(long Time, double Score)> GetNodeScores()
|
|
{
|
|
lock (_dataLock) { return [.. _nodeScores]; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes pose confidence for a node against finished submaps.
|
|
/// Match C++: ComputeNodePoseConfident (pose_graph_2d.cc:491-563)
|
|
/// </summary>
|
|
private void ComputeNodePoseConfident(NodeId nodeId, List<SubmapId> finishedSubmapIds)
|
|
{
|
|
// Get node constant data
|
|
if (!_trajectoryNodes.Contains(nodeId))
|
|
return;
|
|
|
|
var node = _trajectoryNodes[nodeId];
|
|
var constantData = node.ConstantData;
|
|
if (constantData == null)
|
|
return;
|
|
|
|
var pointCloud = constantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0)
|
|
return;
|
|
|
|
// Track best probability for each point
|
|
var pointcloudScore = new double[pointCloud.Count];
|
|
for (int i = 0; i < pointcloudScore.Length; i++)
|
|
pointcloudScore[i] = 0.0;
|
|
|
|
// Get optimization problem data
|
|
var optimizationNodeData = _optimizationProblem.NodeData();
|
|
var optimizationSubmapData = _optimizationProblem.SubmapData();
|
|
|
|
if (!optimizationNodeData.Contains(nodeId))
|
|
return;
|
|
|
|
var nodeGlobalPose2D = optimizationNodeData[nodeId].GlobalPose2D;
|
|
|
|
// Match C++ (line 506-544): iterate through finished submaps
|
|
foreach (var submapId in finishedSubmapIds)
|
|
{
|
|
// Match C++ (line 508-510): skip same trajectory
|
|
if (nodeId.TrajectoryId == submapId.TrajectoryId)
|
|
continue;
|
|
|
|
if (!optimizationSubmapData.Contains(submapId))
|
|
continue;
|
|
|
|
// Match C++ (line 512-516): compute initial_relative_pose
|
|
var submapGlobalPose2D = optimizationSubmapData[submapId].GlobalPose;
|
|
var initialRelativePose = submapGlobalPose2D.Inverse() * nodeGlobalPose2D;
|
|
|
|
// Match C++ (line 518-521): skip if distance > max_constraint_distance
|
|
if (_options.ConstraintBuilderOptions.HasValue &&
|
|
initialRelativePose.Translation.Length() > _options.ConstraintBuilderOptions.Value.MaxConstraintDistance)
|
|
continue;
|
|
|
|
// Get submap and grid
|
|
if (!_submapData.Contains(submapId))
|
|
continue;
|
|
|
|
var submapData = _submapData[submapId];
|
|
if (submapData.Submap is not Mapping.D2D.Submap2D submap2D)
|
|
continue;
|
|
|
|
var grid = submap2D.Grid;
|
|
if (grid is not Mapping.D2D.ProbabilityGrid probabilityGrid)
|
|
continue;
|
|
|
|
// Match C++ (line 526-527): compute node_pose_in_local
|
|
var submapLocalPose2D = TransformOperations.Project2D(submap2D.LocalPose);
|
|
var nodePoseInLocal = submapLocalPose2D * initialRelativePose;
|
|
|
|
var limits = probabilityGrid.Limits;
|
|
|
|
// Match C++ (line 531-544): compute probability for each point
|
|
for (int i = 0; i < pointCloud.Count; i++)
|
|
{
|
|
var point = pointCloud[i];
|
|
// Match C++: node_pose_in_local.cast<double>() * point_cloud[i].position.head<2>()
|
|
var transformedP = nodePoseInLocal * new Vector2(point.Position.X, point.Position.Y);
|
|
var cellIndex = limits.GetCellIndex(transformedP);
|
|
var prob = probabilityGrid.GetProbability(cellIndex);
|
|
|
|
if (prob > pointcloudScore[i])
|
|
pointcloudScore[i] = prob;
|
|
}
|
|
}
|
|
|
|
// Match C++ (line 550-556): compute sum_prob (count of points with score >= 0.5)
|
|
double sumProb = 0.0;
|
|
foreach (var s in pointcloudScore)
|
|
{
|
|
if (s >= 0.5)
|
|
sumProb += 1;
|
|
}
|
|
|
|
// Match C++ (line 556): best_score = sum_prob / pointcloud_score.size()
|
|
double bestScore = sumProb / pointcloudScore.Length;
|
|
|
|
// Match C++ (line 559-562): add to node_scores_ with max 30 entries
|
|
_nodeScores.AddLast((constantData.Time, bestScore));
|
|
if (_nodeScores.Count > MaxNodeScoresCount)
|
|
_nodeScores.RemoveFirst();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ UpdateTrajectoryConnectivity: update last connection time for (node_id.trajectory_id, submap_id.trajectory_id).
|
|
/// Assumes lock (_dataLock) held by caller.
|
|
/// </summary>
|
|
private void UpdateTrajectoryConnectivityUnsafe(IPoseGraph.Constraint constraint)
|
|
{
|
|
if (constraint.ConstraintTag != IPoseGraph.Constraint.Tag.InterSubmap) return;
|
|
var time = GetLatestNodeTimeUnsafe(constraint.NodeId, constraint.SubmapId);
|
|
_trajectoryConnectivityState.Connect(constraint.NodeId.TrajectoryId, constraint.SubmapId.TrajectoryId, time);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ ComputeConstraint (line 273-313): decide Local, Localization, or Global for (nodeId, submapId). Assumes lock held.
|
|
/// Extended for single-trajectory loop closure: when EnableSingleTrajectoryLoopClosure is true and
|
|
/// initialRelativePose distance exceeds threshold, use Global search instead of Local.
|
|
/// </summary>
|
|
private ConstraintTaskKind ComputeConstraintKindUnsafe(
|
|
NodeId nodeId,
|
|
SubmapId submapId,
|
|
MapById<SubmapId, Optimization.SubmapSpec2D> optimizationSubmapData,
|
|
Rigid2d? initialRelativePose = null)
|
|
{
|
|
var nodeTime = GetLatestNodeTimeUnsafe(nodeId, submapId);
|
|
var lastConnectionTime = _trajectoryConnectivityState.LastConnectionTime(nodeId.TrajectoryId, submapId.TrajectoryId);
|
|
// C++ global_constraint_search_after_n_seconds: add seconds to time (assume time is in microseconds)
|
|
var threshold = lastConnectionTime + (long)(_options.GlobalConstraintSearchAfterNSeconds * 1e6);
|
|
var searchingRelocalization = _constraintBuilder?.IsSearchingForRelocalization ?? false;
|
|
|
|
// === SINGLE-TRAJECTORY LOOP CLOSURE EXTENSION ===
|
|
// When the node and submap are on the same trajectory, but the initial relative pose
|
|
// distance is large (indicating possible loop closure), use Global search instead of Local.
|
|
// This allows detecting loop closures when robot returns to a previously visited area.
|
|
if (nodeId.TrajectoryId == submapId.TrajectoryId &&
|
|
_options.EnableSingleTrajectoryLoopClosure &&
|
|
initialRelativePose.HasValue)
|
|
{
|
|
var distance = initialRelativePose.Value.Translation.Length();
|
|
var loopClosureThreshold = _options.SingleTrajectoryLoopClosureDistanceThreshold;
|
|
|
|
// If distance is large but within max constraint distance, this could be a loop closure
|
|
// Use Global search (MatchFullSubmap) for better detection
|
|
var maxConstraintDistance = _options.ConstraintBuilderOptions?.MaxConstraintDistance ?? 15.0;
|
|
if (distance > loopClosureThreshold && distance <= maxConstraintDistance)
|
|
{
|
|
return ConstraintTaskKind.Global;
|
|
}
|
|
}
|
|
// === END EXTENSION ===
|
|
|
|
bool maybeLocal = nodeId.TrajectoryId == submapId.TrajectoryId
|
|
|| (!_localizationMode && nodeTime < threshold)
|
|
|| (_localizationMode && !searchingRelocalization);
|
|
if (maybeLocal) return ConstraintTaskKind.Local;
|
|
if (_localizationMode && searchingRelocalization) return ConstraintTaskKind.Localization;
|
|
if (_globalLocalizationSamplers.TryGetValue(nodeId.TrajectoryId, out var sampler) && sampler.Pulse())
|
|
{
|
|
return ConstraintTaskKind.Global;
|
|
}
|
|
// Match C++: when Pulse() returns false, ComputeConstraint does nothing (no constraint created)
|
|
return ConstraintTaskKind.None;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Constraint task kind (match C++ ComputeConstraint: local / localization / global).
|
|
/// </summary>
|
|
private enum ConstraintTaskKind { None, Local, Localization, Global }
|
|
|
|
/// <summary>
|
|
/// Computes constraints for a node (matches C++ ComputeConstraintsForNode).
|
|
/// This is called from work queue after AppendNodeUnsafe completes.
|
|
/// Assumes lock held.
|
|
/// Outputs constraint tasks to be executed outside lock to avoid blocking AddNode.
|
|
/// Optimization check is done separately in the caller after NotifyEndOfNode (match C++ line 477-488).
|
|
/// </summary>
|
|
private void ComputeConstraintsForNodeUnsafe(
|
|
NodeId nodeId,
|
|
TrajectoryNode.Data constantData,
|
|
int trajectoryId,
|
|
List<Mapping.D2D.Submap2D> insertionSubmaps,
|
|
bool newlyFinishedSubmap,
|
|
out List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> finishedSubmapConstraintTasks,
|
|
out List<(SubmapId submapId, NodeId taskNodeId, Mapping.D2D.Submap2D submap, TrajectoryNode node, Rigid2d initialRelativePose, List<Rigid2d>? localizationPoses, ConstraintTaskKind kind)> newlyFinishedSubmapConstraintTasks)
|
|
{
|
|
// CRITICAL FIX: Cache SubmapData() and NodeData() at the start to avoid multiple calls
|
|
var optimizationSubmapData = _optimizationProblem.SubmapData();
|
|
var optimizationNodeData = _optimizationProblem.NodeData();
|
|
|
|
// Get optimized pose for the node - read from _trajectoryNodes instead of _trajectoryNodePoses
|
|
// because _trajectoryNodePoses is only populated when loading from proto, not during normal operation
|
|
var optimizedPose = _trajectoryNodes[nodeId].GlobalPose;
|
|
var localToGlobal = GetLocalToGlobalTransformUnsafe(trajectoryId);
|
|
|
|
// Initialize global submap poses and get submap IDs (match C++: pass time for initial_trajectory_poses Connect)
|
|
var submapIds = InitializeGlobalSubmapPosesUnsafe(trajectoryId, constantData.Time, insertionSubmaps);
|
|
|
|
// Match C++: local_pose_2d = Project2D(local_pose * Rotation(gravity_alignment.inverse()))
|
|
var gravityAlignmentInverse = Quaternion.Conjugate(constantData.GravityAlignment);
|
|
var localPoseWithGravityRotation = constantData.LocalPose * new Rigid3d(Vector3.Zero, gravityAlignmentInverse);
|
|
var localPose2D = TransformOperations.Project2D(localPoseWithGravityRotation);
|
|
|
|
// Match C++: global_pose_2d = submap_data.at(matching_id).global_pose * ComputeSubmapPose(insertion_submaps.front()).inverse() * local_pose_2d
|
|
var matchingId = submapIds[0];
|
|
var matchingSubmapPose2D = optimizationSubmapData[matchingId].GlobalPose;
|
|
var frontSubmapPose2D = TransformOperations.Project2D(insertionSubmaps[0].LocalPose);
|
|
var globalPose2D = matchingSubmapPose2D * frontSubmapPose2D.Inverse() * localPose2D;
|
|
|
|
// Add node to optimization problem
|
|
_optimizationProblem.AddNode(
|
|
trajectoryId,
|
|
constantData.Time,
|
|
localPose2D,
|
|
globalPose2D,
|
|
TransformOperations.Embed3D(globalPose2D) * new Rigid3d(Vector3.Zero, constantData.GravityAlignment),
|
|
constantData.GravityAlignment);
|
|
|
|
// FIXED: Match C++ order - Create INTRA_SUBMAP constraints FIRST (lines 406-423)
|
|
// Then collect finished submaps (lines 429-434)
|
|
var translationWeight = _options.MatcherTranslationWeight;
|
|
var rotationWeight = _options.MatcherRotationWeight;
|
|
|
|
// Match C++: Create constraints for all insertion_submaps using submap_ids
|
|
for (int i = 0; i < insertionSubmaps.Count; i++)
|
|
{
|
|
var submapId = submapIds[i];
|
|
var insertionSubmap = insertionSubmaps[i];
|
|
|
|
// Match C++: Track node_ids for this submap (line 412: data_.submap_data.at(submap_id).node_ids.emplace(node_id))
|
|
if (!_submapNodeInsertions.TryGetValue(submapId, out var insertionNodes))
|
|
{
|
|
insertionNodes = [];
|
|
_submapNodeInsertions[submapId] = insertionNodes;
|
|
}
|
|
insertionNodes.Add(nodeId);
|
|
|
|
// Match C++: constraint_transform = ComputeSubmapPose(*insertion_submaps[i]).inverse() * local_pose_2d
|
|
// Note: local_pose_2d is already gravity-aligned (computed above)
|
|
var computeSubmapPose = TransformOperations.Project2D(insertionSubmap.LocalPose);
|
|
var constraintTransform2D = computeSubmapPose.Inverse() * localPose2D;
|
|
var constraintTransform = TransformOperations.Embed3D(constraintTransform2D);
|
|
|
|
// FIXED: Removed identity skip check - match C++ which creates ALL constraints
|
|
// FIXED: Removed duplicate check - match C++ which uses push_back without checking
|
|
|
|
// Match C++: Constraint{submap_id, node_id, {Embed3D(constraint_transform), matcher_translation_weight, matcher_rotation_weight}, INTRA_SUBMAP}
|
|
_constraints.Add(new IPoseGraph.Constraint(
|
|
submapId,
|
|
nodeId,
|
|
new IPoseGraph.Constraint.Pose(
|
|
constraintTransform,
|
|
translationWeight,
|
|
rotationWeight
|
|
),
|
|
IPoseGraph.Constraint.Tag.IntraSubmap
|
|
));
|
|
}
|
|
|
|
// Match C++: Find finished submaps to compute constraints against (line 429-434)
|
|
// C++ ComputeConstraint (line 273-370) chooses per (node_id, submap_id): Local, Localization, or Global.
|
|
finishedSubmapConstraintTasks = [];
|
|
newlyFinishedSubmapConstraintTasks = [];
|
|
|
|
// Collect finished submap constraint tasks using cached _finishedSubmapIds
|
|
// instead of scanning all submaps. O(_finishedSubmapIds.Count) with O(1) lookups.
|
|
if (_trajectoryNodes.Contains(nodeId))
|
|
{
|
|
var node = _trajectoryNodes[nodeId];
|
|
var nodeGlobalPose2D = optimizationNodeData[nodeId].GlobalPose2D;
|
|
|
|
foreach (var finishedSubmapId in _finishedSubmapIds)
|
|
{
|
|
// Skip submaps that contain this node (INTRA already handled)
|
|
if (_submapNodeInsertions.TryGetValue(finishedSubmapId, out var insertedNodeIds) && insertedNodeIds.Contains(nodeId))
|
|
continue;
|
|
if (!optimizationSubmapData.Contains(finishedSubmapId))
|
|
continue;
|
|
if (!_submapData.Contains(finishedSubmapId))
|
|
continue;
|
|
var submapDataEntry = _submapData[finishedSubmapId];
|
|
if (submapDataEntry.Submap is not Mapping.D2D.Submap2D submap2D)
|
|
continue;
|
|
|
|
var submapGlobalPose2D = optimizationSubmapData[finishedSubmapId].GlobalPose;
|
|
var initialRelativePose = submapGlobalPose2D.Inverse() * nodeGlobalPose2D;
|
|
var kind = ComputeConstraintKindUnsafe(nodeId, finishedSubmapId, optimizationSubmapData, initialRelativePose);
|
|
if (kind == ConstraintTaskKind.None)
|
|
continue;
|
|
List<Rigid2d>? localizationPoses = null;
|
|
if (kind == ConstraintTaskKind.Localization)
|
|
{
|
|
if (_localizationInitialPoses.Count == 0)
|
|
localizationPoses = [initialRelativePose];
|
|
else
|
|
{
|
|
localizationPoses = [];
|
|
foreach (var globalPose3d in _localizationInitialPoses)
|
|
{
|
|
var rel2d = submapGlobalPose2D.Inverse() * TransformOperations.Project2D(globalPose3d);
|
|
localizationPoses.Add(rel2d);
|
|
}
|
|
}
|
|
}
|
|
finishedSubmapConstraintTasks.Add((finishedSubmapId, nodeId, submap2D, node, initialRelativePose, localizationPoses, kind));
|
|
}
|
|
}
|
|
|
|
// Match C++: Handle newly_finished_submap (line 435-442)
|
|
HashSet<NodeId>? newlyFinishedSubmapNodeIds = null;
|
|
if (newlyFinishedSubmap)
|
|
{
|
|
var newlyFinishedSubmapId = submapIds[0];
|
|
// FIXED: Match C++ - set state to kFinished (use InsertionFinished = true)
|
|
if (_submapData.Contains(newlyFinishedSubmapId))
|
|
{
|
|
var submapData = _submapData[newlyFinishedSubmapId];
|
|
if (submapData.Submap is Mapping.D2D.Submap2D finishedSubmap2D)
|
|
{
|
|
// Match C++: finished_submap_data.state = SubmapState::kFinished
|
|
finishedSubmap2D.InsertionFinished = true;
|
|
_finishedSubmapIds.Add(newlyFinishedSubmapId);
|
|
}
|
|
}
|
|
// Track node IDs for this newly finished submap
|
|
if (_submapNodeInsertions.TryGetValue(newlyFinishedSubmapId, out var nodeIds))
|
|
{
|
|
newlyFinishedSubmapNodeIds = [.. nodeIds];
|
|
}
|
|
}
|
|
|
|
// Match C++: Compute constraints for finished submaps (line 445-447)
|
|
// C++: for (const auto& submap_id : finished_submap_ids) { ComputeConstraint(node_id, submap_id); }
|
|
// C# ComputeConstraintKindUnsafe + ProcessTask dispatch: Local -> MaybeAddConstraint, Localization -> MaybeAddLocalizationConstraint (with _localizationInitialPoses), Global -> MaybeAddGlobalConstraint.
|
|
// Constraint tasks are executed outside lock to avoid blocking AddNode.
|
|
|
|
// Match C++: Handle newly_finished_submap - compute constraints for old nodes (line 465-475)
|
|
if (newlyFinishedSubmap && _constraintBuilder != null)
|
|
{
|
|
var newlyFinishedSubmapId = submapIds[0];
|
|
if (_submapData.Contains(newlyFinishedSubmapId) &&
|
|
_submapData[newlyFinishedSubmapId].Submap is Mapping.D2D.Submap2D newlyFinishedSubmap2D &&
|
|
optimizationSubmapData.Contains(newlyFinishedSubmapId))
|
|
{
|
|
// Use EnumerateUnsafe() - safe here because we hold _dataLock and
|
|
// optimizationNodeData is not modified during this iteration.
|
|
// Avoids O(N) snapshot allocation that GetEnumerator().ToList() would create.
|
|
var newlyFinishedSubmapPose2D = optimizationSubmapData[newlyFinishedSubmapId].GlobalPose;
|
|
|
|
foreach (var nodeKvp in optimizationNodeData.EnumerateUnsafe())
|
|
{
|
|
var oldNodeId = nodeKvp.Id;
|
|
// Match C++: if (newly_finished_submap_node_ids.count(node_id) == 0)
|
|
if (newlyFinishedSubmapNodeIds != null && newlyFinishedSubmapNodeIds.Contains(oldNodeId))
|
|
continue;
|
|
if (!_trajectoryNodes.Contains(oldNodeId))
|
|
continue;
|
|
|
|
var oldNode = _trajectoryNodes[oldNodeId];
|
|
var oldNodeGlobalPose2D = nodeKvp.Data.GlobalPose2D;
|
|
var initialRelativePose = newlyFinishedSubmapPose2D.Inverse() * oldNodeGlobalPose2D;
|
|
var kind = ComputeConstraintKindUnsafe(oldNodeId, newlyFinishedSubmapId, optimizationSubmapData, initialRelativePose);
|
|
if (kind == ConstraintTaskKind.None)
|
|
continue;
|
|
List<Rigid2d>? localizationPoses = null;
|
|
if (kind == ConstraintTaskKind.Localization)
|
|
{
|
|
if (_localizationInitialPoses.Count == 0)
|
|
localizationPoses = [initialRelativePose];
|
|
else
|
|
{
|
|
localizationPoses = [];
|
|
foreach (var globalPose3d in _localizationInitialPoses)
|
|
{
|
|
var rel2d = newlyFinishedSubmapPose2D.Inverse() * TransformOperations.Project2D(globalPose3d);
|
|
localizationPoses.Add(rel2d);
|
|
}
|
|
}
|
|
}
|
|
newlyFinishedSubmapConstraintTasks.Add((newlyFinishedSubmapId, oldNodeId, newlyFinishedSubmap2D, oldNode, initialRelativePose, localizationPoses, kind));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Match C++ (line 449-458): relocalization counter and callback
|
|
if (_localizationMode && (_constraintBuilder?.IsSearchingForRelocalization ?? false))
|
|
{
|
|
_numRelocalizationConstraintSearch++;
|
|
var maxNodes = _options.MaxNumberRelocalizationNodes > 0 ? _options.MaxNumberRelocalizationNodes : 12;
|
|
if (_numRelocalizationConstraintSearch >= maxNodes)
|
|
{
|
|
_localizationSearchCallback?.Invoke(-1, 0, Rigid3d.Identity);
|
|
_constraintBuilder?.ToggleSearchingForRelocalization(false);
|
|
}
|
|
}
|
|
|
|
// Match C++ (line 460-461): Compute pose confidence if enabled and relocated
|
|
if (_isRelocalized && _matchingScoreEnabled)
|
|
{
|
|
// Collect finished submap IDs for ComputeNodePoseConfident
|
|
var finishedSubmapIds = finishedSubmapConstraintTasks.Select(t => t.submapId).ToList();
|
|
ComputeNodePoseConfident(nodeId, finishedSubmapIds);
|
|
}
|
|
|
|
// Match C++ (line 463): current_trajectory_id = node_id.trajectory_id
|
|
_currentTrajectoryId = trajectoryId;
|
|
|
|
// Match C++: constraint_builder_.NotifyEndOfNode() and optimization check
|
|
// are done in the caller after constraint dispatch (match C++ line 476-488)
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes global submap poses (matches C++ InitializeGlobalSubmapPoses).
|
|
/// Assumes lock held.
|
|
/// </summary>
|
|
private List<SubmapId> InitializeGlobalSubmapPosesUnsafe(int trajectoryId, long time, List<Mapping.D2D.Submap2D> insertionSubmaps)
|
|
{
|
|
if (insertionSubmaps.Count == 0)
|
|
{
|
|
throw new ArgumentException("insertion_submaps cannot be empty");
|
|
}
|
|
|
|
var optimizationSubmapData = _optimizationProblem.SubmapData();
|
|
|
|
// Handle case with 1 insertion_submap.
|
|
// C++ assumes this only happens at initialization (SubmapId index 0).
|
|
// C# ForceNewSubmapAndInsert can produce 1-submap results mid-operation,
|
|
// so we find the matching SubmapId by object reference instead of hardcoding index 0.
|
|
if (insertionSubmaps.Count == 1)
|
|
{
|
|
// Find the SubmapId in _submapData that matches insertionSubmaps[0] by object reference
|
|
SubmapId? matchingSubmapId = null;
|
|
foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
|
|
{
|
|
if (kvp.Data.Submap == insertionSubmaps[0])
|
|
{
|
|
matchingSubmapId = kvp.Id;
|
|
}
|
|
}
|
|
|
|
if (!matchingSubmapId.HasValue)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"Cannot find matching submap in _submapData for trajectory {trajectoryId}, " +
|
|
$"insertionSubmap={insertionSubmaps[0].GetHashCode()}, " +
|
|
$"_submapData.size={_submapData.SizeOfTrajectoryOrZero(trajectoryId)}, " +
|
|
$"optimization.size={optimizationSubmapData.SizeOfTrajectoryOrZero(trajectoryId)}");
|
|
}
|
|
|
|
// Ensure the submap is in the optimization problem
|
|
if (!optimizationSubmapData.Contains(matchingSubmapId.Value))
|
|
{
|
|
if (_initialTrajectoryPoses.TryGetValue(trajectoryId, out var initialPose))
|
|
{
|
|
_trajectoryConnectivityState.Connect(trajectoryId, initialPose.ToTrajectoryId, time);
|
|
}
|
|
|
|
var localToGlobal = GetLocalToGlobalTransformUnsafe(trajectoryId);
|
|
var submapPose2D = TransformOperations.Project2D(localToGlobal * insertionSubmaps[0].LocalPose);
|
|
_optimizationProblem.AddSubmap(matchingSubmapId.Value, submapPose2D);
|
|
|
|
if (!_globalSubmapPoses2D.Contains(matchingSubmapId.Value))
|
|
_globalSubmapPoses2D.Insert(matchingSubmapId.Value, new Optimization.SubmapSpec2D(submapPose2D));
|
|
}
|
|
|
|
return [matchingSubmapId.Value];
|
|
}
|
|
|
|
// Match C++: Handle case with 2 insertion_submaps
|
|
if (insertionSubmaps.Count != 2)
|
|
{
|
|
throw new ArgumentException($"Expected 1 or 2 insertion_submaps, got {insertionSubmaps.Count}");
|
|
}
|
|
|
|
// Find last submap ID (highest SubmapIndex) from optimization problem
|
|
SubmapId? lastSubmapId = null;
|
|
int maxIndex = -1;
|
|
foreach (var kvp in optimizationSubmapData)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId && kvp.Id.SubmapIndex > maxIndex)
|
|
{
|
|
maxIndex = kvp.Id.SubmapIndex;
|
|
lastSubmapId = kvp.Id;
|
|
}
|
|
}
|
|
|
|
if (lastSubmapId == null)
|
|
{
|
|
throw new InvalidOperationException($"No submaps in optimization problem for trajectory {trajectoryId}");
|
|
}
|
|
|
|
if (!lastSubmapId.HasValue || !_submapData.Contains(lastSubmapId.Value))
|
|
{
|
|
throw new InvalidOperationException($"Last submap not found for trajectory {trajectoryId}");
|
|
}
|
|
|
|
// Match C++: Check if last_submap_id.submap == insertion_submaps.front()
|
|
if (_submapData[lastSubmapId.Value].Submap == insertionSubmaps[0])
|
|
{
|
|
// Match C++: insertion_submaps.back() is new
|
|
// C++: optimization_problem_->AddSubmap(trajectory_id, first_submap_pose * ComputeSubmapPose(insertion_submaps[0]).inverse() * ComputeSubmapPose(insertion_submaps[1]))
|
|
var firstSubmapPose = optimizationSubmapData[lastSubmapId.Value].GlobalPose;
|
|
var computeSubmapPose0 = TransformOperations.Project2D(insertionSubmaps[0].LocalPose);
|
|
var computeSubmapPose1 = TransformOperations.Project2D(insertionSubmaps[1].LocalPose);
|
|
var newSubmapPose2D = firstSubmapPose * computeSubmapPose0.Inverse() * computeSubmapPose1;
|
|
_optimizationProblem.AddSubmap(trajectoryId, newSubmapPose2D);
|
|
|
|
// CRITICAL FIX: Also update _globalSubmapPoses2D to match C++ behavior
|
|
// This ensures GetLocalToGlobalTransform can read from _globalSubmapPoses2D immediately
|
|
var newSubmapId = new SubmapId(trajectoryId, lastSubmapId.Value.SubmapIndex + 1);
|
|
if (!_globalSubmapPoses2D.Contains(newSubmapId))
|
|
_globalSubmapPoses2D.Insert(newSubmapId, new Optimization.SubmapSpec2D(newSubmapPose2D));
|
|
|
|
// Match C++: return {last_submap_id, SubmapId{trajectory_id, last_submap_id.submap_index + 1}}
|
|
return [lastSubmapId.Value, newSubmapId];
|
|
}
|
|
|
|
// Match C++: Check if last_submap_id.submap == insertion_submaps.back()
|
|
if (_submapData[lastSubmapId.Value].Submap == insertionSubmaps[1])
|
|
{
|
|
// Match C++: front_submap_id = {trajectory_id, last_submap_id.submap_index - 1}
|
|
var frontSubmapId = new SubmapId(trajectoryId, lastSubmapId.Value.SubmapIndex - 1);
|
|
|
|
// Match C++: CHECK(data_.submap_data.at(front_submap_id).submap == insertion_submaps.front())
|
|
if (!_submapData.Contains(frontSubmapId) || _submapData[frontSubmapId].Submap != insertionSubmaps[0])
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"Front submap mismatch for trajectory {trajectoryId}: " +
|
|
$"frontSubmapId={frontSubmapId}, lastSubmapId={lastSubmapId.Value}, " +
|
|
$"frontExists={_submapData.Contains(frontSubmapId)}, " +
|
|
$"_submapData.size={_submapData.SizeOfTrajectoryOrZero(trajectoryId)}, " +
|
|
$"optimization.size={optimizationSubmapData.SizeOfTrajectoryOrZero(trajectoryId)}");
|
|
}
|
|
|
|
return [frontSubmapId, lastSubmapId.Value];
|
|
}
|
|
|
|
// C# FIX: Handle case where last optimization submap doesn't match either insertion submap.
|
|
// This can happen when ForceNewSubmapAndInsert (C#-only) disrupts the normal submap lifecycle,
|
|
// or when trimming causes the optimization problem to be out of sync with _submapData.
|
|
// Find the actual submap IDs by scanning _submapData for matching submap objects.
|
|
SubmapId? foundFrontId = null;
|
|
SubmapId? foundBackId = null;
|
|
foreach (var kvp in _submapData.BeginOfTrajectory(trajectoryId))
|
|
{
|
|
if (kvp.Data.Submap == insertionSubmaps[0])
|
|
foundFrontId = kvp.Id;
|
|
if (kvp.Data.Submap == insertionSubmaps[1])
|
|
foundBackId = kvp.Id;
|
|
}
|
|
|
|
if (!foundFrontId.HasValue || !foundBackId.HasValue)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"Submap mismatch for trajectory {trajectoryId}: " +
|
|
$"lastSubmapId={lastSubmapId.Value}, " +
|
|
$"lastSubmap={_submapData[lastSubmapId.Value].Submap?.GetHashCode()}, " +
|
|
$"insertionSubmaps[0]={insertionSubmaps[0].GetHashCode()}, " +
|
|
$"insertionSubmaps[1]={insertionSubmaps[1].GetHashCode()}, " +
|
|
$"foundFront={foundFrontId.HasValue}, foundBack={foundBackId.HasValue}, " +
|
|
$"_submapData.size={_submapData.SizeOfTrajectoryOrZero(trajectoryId)}, " +
|
|
$"optimization.size={optimizationSubmapData.SizeOfTrajectoryOrZero(trajectoryId)}");
|
|
}
|
|
|
|
// Add missing submaps to the optimization problem
|
|
Debug.WriteLine($"[PoseGraph2D] Submap chain recovery for trajectory {trajectoryId}: " +
|
|
$"lastOptimization={lastSubmapId.Value.SubmapIndex}, " +
|
|
$"front={foundFrontId.Value.SubmapIndex}, back={foundBackId.Value.SubmapIndex}");
|
|
|
|
// Ensure front submap is in the optimization problem
|
|
if (!optimizationSubmapData.Contains(foundFrontId.Value))
|
|
{
|
|
var localToGlobal = GetLocalToGlobalTransformUnsafe(trajectoryId);
|
|
var frontPose2D = TransformOperations.Project2D(localToGlobal * insertionSubmaps[0].LocalPose);
|
|
_optimizationProblem.AddSubmap(foundFrontId.Value, frontPose2D);
|
|
if (!_globalSubmapPoses2D.Contains(foundFrontId.Value))
|
|
_globalSubmapPoses2D.Insert(foundFrontId.Value, new Optimization.SubmapSpec2D(frontPose2D));
|
|
}
|
|
|
|
// Ensure back submap is in the optimization problem
|
|
if (!optimizationSubmapData.Contains(foundBackId.Value))
|
|
{
|
|
var frontPose = optimizationSubmapData.Contains(foundFrontId.Value)
|
|
? optimizationSubmapData[foundFrontId.Value].GlobalPose
|
|
: TransformOperations.Project2D(GetLocalToGlobalTransformUnsafe(trajectoryId) * insertionSubmaps[0].LocalPose);
|
|
var computeSubmapPose0 = TransformOperations.Project2D(insertionSubmaps[0].LocalPose);
|
|
var computeSubmapPose1 = TransformOperations.Project2D(insertionSubmaps[1].LocalPose);
|
|
var backPose2D = frontPose * computeSubmapPose0.Inverse() * computeSubmapPose1;
|
|
_optimizationProblem.AddSubmap(foundBackId.Value, backPose2D);
|
|
if (!_globalSubmapPoses2D.Contains(foundBackId.Value))
|
|
_globalSubmapPoses2D.Insert(foundBackId.Value, new Optimization.SubmapSpec2D(backPose2D));
|
|
}
|
|
|
|
return [foundFrontId.Value, foundBackId.Value];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Checks if optimization is needed (assumes lock held).
|
|
/// </summary>
|
|
/// <summary>
|
|
/// Match C++ (line 481-489): run optimization if optimize_every_n_nodes reached, or in localization mode before relocalized and under max relocalization nodes.
|
|
/// FIXED: Use GLOBAL counter like C++, not per-trajectory.
|
|
/// FIXED: Check > instead of >= to match C++ logic.
|
|
/// Assumes lock (_dataLock) held by caller.
|
|
/// </summary>
|
|
private bool CheckIfOptimizationNeededUnsafe(int trajectoryId)
|
|
{
|
|
// Match C++: ++num_nodes_since_last_loop_closure_ (line 478)
|
|
_numNodesSinceLastLoopClosure++;
|
|
|
|
// Match C++: Check > not >= (line 482)
|
|
if (_options.OptimizeEveryNNodes > 0 && _numNodesSinceLastLoopClosure > _options.OptimizeEveryNNodes)
|
|
{
|
|
Debug.WriteLine($"[PoseGraph2D] Optimization triggered: nodes_since_last={_numNodesSinceLastLoopClosure} > OptimizeEveryNNodes={_options.OptimizeEveryNNodes}");
|
|
return true;
|
|
}
|
|
var maxReloc = _options.MaxNumberRelocalizationNodes > 0 ? _options.MaxNumberRelocalizationNodes : 12;
|
|
if (_localizationMode && !_isRelocalized && _numRelocalizationConstraintSearch < maxReloc)
|
|
{
|
|
Debug.WriteLine($"[PoseGraph2D] Optimization triggered (localization mode): is_relocalized={_isRelocalized}, reloc_search={_numRelocalizationConstraintSearch}/{maxReloc}");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Unsafe version of GetLocalToGlobalTransform (assumes lock held).
|
|
/// Match C++: Reads from data_.global_submap_poses_2d (not optimization_problem directly).
|
|
/// </summary>
|
|
private Rigid3d GetLocalToGlobalTransformUnsafe(int trajectoryId)
|
|
{
|
|
if (_localToGlobalTransforms.TryGetValue(trajectoryId, out var transform))
|
|
{
|
|
return transform;
|
|
}
|
|
// CRITICAL FIX: Match C++ behavior - read from _globalSubmapPoses2D instead of optimization_problem
|
|
// This avoids potential blocking/slowness when optimization_problem is being modified.
|
|
// Match C++ line 1501: ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id)
|
|
var computedTransform = ComputeLocalToGlobalTransform(_globalSubmapPoses2D, trajectoryId);
|
|
// Cache the computed transform for next time
|
|
// Note: ComputeLocalToGlobalTransform already logs when returning Identity
|
|
_localToGlobalTransforms[trajectoryId] = computedTransform;
|
|
return computedTransform;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ lines 1477-1496: GetInterpolatedGlobalTrajectoryPose(trajectory_id, time)
|
|
/// Returns the interpolated global pose for a trajectory at a given time.
|
|
/// Used by ComputeLocalToGlobalTransform when no submaps exist but initial_trajectory_poses has an entry.
|
|
/// Assumes lock (_dataLock) held by caller.
|
|
/// </summary>
|
|
private Rigid3d GetInterpolatedGlobalTrajectoryPose(int trajectoryId, long time)
|
|
{
|
|
// Match C++ line 1479: CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0)
|
|
var trajectorySize = _trajectoryNodes.SizeOfTrajectoryOrZero(trajectoryId);
|
|
if (trajectorySize == 0)
|
|
{
|
|
// No nodes in trajectory, return identity
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
// Match C++ line 1480: const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time)
|
|
// LowerBound returns null when time > all node times (EndOfTrajectory case)
|
|
var it = _trajectoryNodes.LowerBound(trajectoryId, time, node => node.Time);
|
|
|
|
// Get first and last nodes for boundary checks
|
|
var lastNode = _trajectoryNodes.GetLastOfTrajectory(trajectoryId);
|
|
if (lastNode == null)
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
// Match C++ line 1485-1488: if (it == EndOfTrajectory) return last node's global_pose
|
|
// In C#, LowerBound returns null when time > all node times
|
|
if (it == null)
|
|
{
|
|
return lastNode.Value.Data.GlobalPose;
|
|
}
|
|
|
|
// Match C++ line 1481-1484: if (it == BeginOfTrajectory) return first node's global_pose
|
|
var beginItEnumerable = _trajectoryNodes.BeginOfTrajectory(trajectoryId);
|
|
var hasBeginIt = beginItEnumerable.Any();
|
|
if (!hasBeginIt)
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
var beginIt = beginItEnumerable.First();
|
|
|
|
if (it.Value.Id.Equals(beginIt.Id))
|
|
{
|
|
// Return first node's global pose
|
|
return beginIt.Data.GlobalPose;
|
|
}
|
|
|
|
// Get current node from iterator (safe now - it is not null)
|
|
var itNode = it.Value;
|
|
|
|
// Match C++ lines 1489-1496: Interpolate between prev(it) and it
|
|
// Find the previous node
|
|
MapById<NodeId, TrajectoryNode>.IdDataReference? prevNode = null;
|
|
foreach (var kvp in _trajectoryNodes.BeginOfTrajectory(trajectoryId))
|
|
{
|
|
if (kvp.Id.Equals(itNode.Id))
|
|
{
|
|
break;
|
|
}
|
|
prevNode = kvp;
|
|
}
|
|
|
|
if (prevNode == null)
|
|
{
|
|
// No previous node, return current node's pose
|
|
return itNode.Data.GlobalPose;
|
|
}
|
|
|
|
// Match C++ lines 1489-1495: Interpolate between prev(it) and it
|
|
return TransformOperations.Interpolate(
|
|
prevNode.Value.Data.GlobalPose,
|
|
prevNode.Value.Data.Time,
|
|
itNode.Data.GlobalPose,
|
|
itNode.Data.Time,
|
|
time);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes the local to global map frame transform based on the given global submap poses.
|
|
/// Match C++: ComputeLocalToGlobalTransform(global_submap_poses, trajectory_id)
|
|
/// C++ uses the LAST optimized submap (std::prev(end_it)->id), not the first
|
|
/// CRITICAL FIX: Use GetLastOfTrajectory() instead of BeginOfTrajectory().OrderBy().ToList()
|
|
/// to avoid O(n log n) sorting overhead when there are many submaps.
|
|
/// </summary>
|
|
private Rigid3d ComputeLocalToGlobalTransform(MapById<SubmapId, Optimization.SubmapSpec2D> globalSubmapPoses, int trajectoryId)
|
|
{
|
|
// Match C++: Find the LAST submap for this trajectory (std::prev(end_it)->id)
|
|
// CRITICAL FIX: Use GetLastOfTrajectory() instead of OrderBy().ToList() for O(1) access
|
|
var lastSubmapRef = globalSubmapPoses.GetLastOfTrajectory(trajectoryId);
|
|
if (lastSubmapRef == null)
|
|
{
|
|
// Match C++ lines 1547-1554: Check initial_trajectory_poses if no submaps
|
|
if (_initialTrajectoryPoses.TryGetValue(trajectoryId, out var initialPose))
|
|
{
|
|
// Match C++ lines 1549-1551: return GetInterpolatedGlobalTrajectoryPose(...) * relative_pose
|
|
return GetInterpolatedGlobalTrajectoryPose(initialPose.ToTrajectoryId, initialPose.Time)
|
|
* initialPose.Pose;
|
|
}
|
|
// WARNING: No submaps and no initial pose - returning Identity means local pose = global pose
|
|
// This is expected during initialization but may indicate a problem if it persists.
|
|
Debug.WriteLine($"[PoseGraph2D] ComputeLocalToGlobalTransform returning Identity: " +
|
|
$"trajectoryId={trajectoryId}, no submaps in globalSubmapPoses and no initialTrajectoryPose");
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
// Match C++: Use LAST optimized submap (std::prev(end_it)->id)
|
|
var lastOptimizedSubmapId = lastSubmapRef.Value.Id;
|
|
var optimizedSubmapSpec = lastSubmapRef.Value.Data;
|
|
|
|
// Get the submap from our data
|
|
if (!_submapData.Contains(lastOptimizedSubmapId))
|
|
{
|
|
Debug.WriteLine($"[PoseGraph2D] ComputeLocalToGlobalTransform returning Identity: " +
|
|
$"trajectoryId={trajectoryId}, lastOptimizedSubmapId={lastOptimizedSubmapId} not found in _submapData");
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
if (_submapData[lastOptimizedSubmapId].Submap is not Mapping.D2D.Submap2D submap)
|
|
{
|
|
Debug.WriteLine($"[PoseGraph2D] ComputeLocalToGlobalTransform returning Identity: " +
|
|
$"trajectoryId={trajectoryId}, submap at {lastOptimizedSubmapId} is not Submap2D");
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
// Match C++: Compute transform: Embed3D(global_pose) * local_pose.inverse()
|
|
var globalSubmapPose = TransformOperations.Embed3D(optimizedSubmapSpec.GlobalPose);
|
|
var localSubmapPose = submap.LocalPose;
|
|
var transform = globalSubmapPose * localSubmapPose.Inverse();
|
|
return transform;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes local to global transform from old submap poses (for extrapolation).
|
|
/// Match C++: ComputeLocalToGlobalTransform(global_submap_poses_2d, trajectory_id)
|
|
/// </summary>
|
|
private Rigid3d ComputeLocalToGlobalTransformFromSubmapPoses(
|
|
MapById<SubmapId, Rigid2d> oldSubmapPoses,
|
|
int trajectoryId)
|
|
{
|
|
// Find the LAST submap for this trajectory (match C++: std::prev(end_it)->id)
|
|
SubmapId? lastSubmapId = null;
|
|
foreach (var kvp in oldSubmapPoses)
|
|
{
|
|
if (kvp.Id.TrajectoryId == trajectoryId)
|
|
{
|
|
if (lastSubmapId == null || kvp.Id.SubmapIndex > lastSubmapId.Value.SubmapIndex)
|
|
{
|
|
lastSubmapId = kvp.Id;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (lastSubmapId == null)
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
// Get the submap from _submapData to access LocalPose
|
|
if (!_submapData.Contains(lastSubmapId.Value))
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
var submapData = _submapData[lastSubmapId.Value];
|
|
if (submapData.Submap is not Mapping.D2D.Submap2D submap)
|
|
{
|
|
return Rigid3d.Identity;
|
|
}
|
|
|
|
// Compute transform: global_submap_pose * local_submap_pose^-1
|
|
var globalSubmapPose2D = oldSubmapPoses[lastSubmapId.Value];
|
|
var globalSubmapPose = TransformOperations.Embed3D(globalSubmapPose2D);
|
|
var localSubmapPose = submap.LocalPose;
|
|
var transform = globalSubmapPose * localSubmapPose.Inverse();
|
|
return transform;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ (line 1706, 1714): Delete scan matcher for submap when trimming.
|
|
/// Called by TrimmablePoseGraph2D.TrimSubmap.
|
|
/// </summary>
|
|
internal void DeleteConstraintBuilderScanMatcher(SubmapId submapId)
|
|
{
|
|
_constraintBuilder?.DeleteScanMatcher(submapId);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Callback invoked when ConstraintBuilder2D finishes computing constraints.
|
|
/// Matches C++ HandleWorkQueue logic (line 772-847):
|
|
/// 1. Add constraints to data structures (with dataLock)
|
|
/// 2. Run optimization
|
|
/// 3. Invoke global SLAM callback
|
|
/// 4. Update trajectory connectivity
|
|
/// 5. Delete trajectories if needed
|
|
/// 6. Handle trimming
|
|
/// 7. Reset loop closure counter
|
|
/// 8. Drain work queue
|
|
/// </summary>
|
|
private void OnConstraintBuilderResult(ConstraintBuilder2DResult result)
|
|
{
|
|
if (result.Constraints == null || result.Constraints.Count == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// === LOOP CLOSURE CONSTRAINTS RECEIVED LOG ===
|
|
var interSubmapConstraints = result.Constraints.Where(c => c.ConstraintTag == IPoseGraph.Constraint.Tag.InterSubmap).ToList();
|
|
|
|
// Match C++ HandleWorkQueue (line 774-778): Add constraints with dataLock (mutex_)
|
|
// C++ uses data_.constraints.insert() without duplicate checking
|
|
// FIXED: Removed O(n) duplicate check - match C++ which just inserts all constraints
|
|
lock (_dataLock)
|
|
{
|
|
foreach (var constraint in result.Constraints)
|
|
{
|
|
_constraints.Add(constraint);
|
|
UpdateTrajectoryConnectivityUnsafe(constraint);
|
|
}
|
|
}
|
|
|
|
// Match C++ HandleWorkQueue (line 780): Run optimization SYNCHRONOUSLY
|
|
// This matches C++ behavior where RunOptimization() is synchronous
|
|
RunOptimizationSync();
|
|
|
|
// Match C++ HandleWorkQueue (line 782-805): Invoke global SLAM optimization callback
|
|
InvokeGlobalSlamOptimizationCallback();
|
|
|
|
// Match C++ HandleWorkQueue (line 807-844): Process constraints and cleanup
|
|
lock (_dataLock)
|
|
{
|
|
// Match C++ HandleWorkQueue (line 809-811): Update trajectory connectivity (done in Add constraints above via UpdateTrajectoryConnectivityUnsafe)
|
|
|
|
// Match C++ HandleWorkQueue (line 812): Delete trajectories if needed
|
|
// Note: DeleteTrajectoriesIfNeeded requires deletion_state tracking which is not implemented in C#
|
|
// For now, we skip this as trajectories are deleted immediately in C#
|
|
|
|
// Match C++ HandleWorkQueue (line 813-822): Handle trimming
|
|
var trimmingHandle = new TrimmablePoseGraph2D(this);
|
|
foreach (var trimmer in _trimmers)
|
|
{
|
|
trimmer.Trim(trimmingHandle);
|
|
}
|
|
|
|
// Match C++ HandleWorkQueue (line 817-822): Remove finished trimmers
|
|
// Note: PoseGraphTrimmer in C# doesn't have IsFinished() method, so we skip this
|
|
// In C++, trimmers can be marked as finished and removed, but C# implementation doesn't support this
|
|
|
|
// Match C++ HandleWorkQueue (line 824): Reset loop closure counter
|
|
// FIXED: Reset GLOBAL counter like C++
|
|
var oldCounter = _numNodesSinceLastLoopClosure;
|
|
_numNodesSinceLastLoopClosure = 0;
|
|
|
|
// Match C++ HandleWorkQueue (line 826-843): Update metrics
|
|
// Note: Metrics are not implemented in C# version, so we skip this
|
|
}
|
|
|
|
// Match C++ HandleWorkQueue (line 846): Drain work queue
|
|
DrainWorkQueue();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the total number of optimization requests made.
|
|
/// This tracks how many times RunOptimizationSync() was called.
|
|
/// </summary>
|
|
public override int WorkItemsAdded => (int)Interlocked.Read(ref _optimizationRequested);
|
|
|
|
/// <summary>
|
|
/// Gets the total number of optimizations completed.
|
|
/// This tracks how many times optimization actually finished executing.
|
|
/// </summary>
|
|
public override int WorkItemsCompleted => (int)Interlocked.Read(ref _optimizationCompleted);
|
|
|
|
/// <summary>
|
|
/// Gets the number of pending optimization requests.
|
|
/// This is the difference between requested and completed optimizations.
|
|
/// </summary>
|
|
public override int WorkItemsPending => (int)(Interlocked.Read(ref _optimizationRequested) - Interlocked.Read(ref _optimizationCompleted));
|
|
|
|
/// <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 (matches _trajectoryNodes.Count in WaitForAllComputations).
|
|
/// </summary>
|
|
public override int TrajectoryNodesCount
|
|
{
|
|
get
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
return _trajectoryNodes.Count;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the total number of constraint tasks dispatched for scan matching.
|
|
/// </summary>
|
|
public override int ConstraintTasksTotal => _constraintBuilder?.GetNumConstraintTasksDispatched() ?? 0;
|
|
|
|
/// <summary>
|
|
/// Gets the number of constraint tasks that have finished scan matching.
|
|
/// </summary>
|
|
public override int ConstraintTasksFinished => _constraintBuilder?.GetNumConstraintTasksFinished() ?? 0;
|
|
|
|
|
|
|
|
/// <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;
|
|
|
|
// Wait for optimization thread to complete (with timeout)
|
|
if (_optimizationThread != null)
|
|
{
|
|
if (_optimizationThread.IsAlive)
|
|
{
|
|
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
|
|
// CRITICAL: Try to acquire lock with timeout to avoid deadlock
|
|
bool lockAcquired = false;
|
|
try
|
|
{
|
|
if (Monitor.TryEnter(_dataLock, TimeSpan.FromSeconds(5)))
|
|
{
|
|
lockAcquired = true;
|
|
_imuData.Clear();
|
|
_odometryData.Clear();
|
|
_fixedFramePoseData.Clear();
|
|
_submapNodeInsertions.Clear();
|
|
_finishedSubmapIds.Clear();
|
|
_activeSubmapNodeInsertions.Clear();
|
|
}
|
|
else
|
|
{
|
|
// Continue anyway - collections will be GC'd when object is disposed
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
if (lockAcquired)
|
|
{
|
|
Monitor.Exit(_dataLock);
|
|
}
|
|
}
|
|
|
|
_workQueue?.Dispose();
|
|
|
|
// Dispose constraint builder (disposes CeresScanMatcher2D + SemaphoreSlim)
|
|
_constraintBuilder?.Dispose();
|
|
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
}
|