839 lines
32 KiB
C#
839 lines
32 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.D2D.ScanMatching;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Sensor;
|
|
using CartographerSharp.Transform;
|
|
using Submap2D = CartographerSharp.Mapping.D2D.Submap2D;
|
|
using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
|
|
|
|
namespace CartographerSharp.Mapping.Internal.Constraints;
|
|
|
|
/// <summary>
|
|
/// Result of constraint building.
|
|
/// </summary>
|
|
public record struct ConstraintBuilder2DResult(List<IPoseGraph.Constraint> Constraints);
|
|
|
|
/// <summary>
|
|
/// Callback for constraint building completion.
|
|
/// </summary>
|
|
public delegate void ConstraintBuilder2DCallback(ConstraintBuilder2DResult result);
|
|
|
|
/// <summary>
|
|
/// Builds constraints for the pose graph by matching nodes against submaps.
|
|
/// Matches C++ ConstraintBuilder2D (xloc) including localization and manual compute APIs.
|
|
/// </summary>
|
|
public class ConstraintBuilder2D : IDisposable
|
|
{
|
|
private bool _disposed;
|
|
private readonly ConstraintBuilderOptions _options;
|
|
private readonly Lock _mutex = new();
|
|
|
|
private readonly CeresScanMatcher2D _ceresScanMatcher;
|
|
private readonly FastCorrelativeScanMatcherOptions2D? _fastCorrelativeScanMatcherOptions;
|
|
private readonly Dictionary<SubmapId, FixedRatioSampler> _perSubmapSampler = [];
|
|
|
|
// Match C++: localization_mode_, is_search_for_relocalization_
|
|
private bool _localizationMode;
|
|
private bool _isSearchForRelocalization;
|
|
|
|
// Match C++: num_started_nodes_, num_finished_nodes_ (constraint_builder_2d.h line 177-179)
|
|
private int _numStartedNodes;
|
|
private int _numFinishedNodes;
|
|
|
|
// Constraint task progress counters (for MapSaveProcessor progress tracking)
|
|
private int _numConstraintTasksDispatched;
|
|
private int _numConstraintTasksFinished;
|
|
|
|
// Match C++: thread_pool_ (constraint_builder_2d.cc line 63)
|
|
private readonly Common.Threading.ThreadPoolInterface _threadPool;
|
|
|
|
// Match C++: finish_node_task_, when_done_task_ (constraint_builder_2d.h line 181-183)
|
|
private Common.Threading.Task _finishNodeTask;
|
|
private Common.Threading.Task _whenDoneTask;
|
|
|
|
// Match C++: SubmapScanMatcher struct (constraint_builder_2d.h line 140-145)
|
|
// Stores the grid, fast correlative scan matcher, and creation task handle
|
|
private class SubmapScanMatcher
|
|
{
|
|
public Grid2D? Grid { get; set; }
|
|
public FastCorrelativeScanMatcher2D? FastCorrelativeScanMatcher { get; set; }
|
|
public WeakReference<Common.Threading.Task>? CreationTaskHandle { get; set; }
|
|
}
|
|
|
|
// Match C++: submap_scan_matchers_ (constraint_builder_2d.h line 191-192)
|
|
private readonly Dictionary<SubmapId, SubmapScanMatcher> _submapScanMatchers = [];
|
|
|
|
// Match C++: constraints_ deque (constraint_builder_2d.h line 188)
|
|
// We use a list of nullable constraints since computation may fail
|
|
private readonly List<IPoseGraph.Constraint?> _pendingConstraints = [];
|
|
|
|
// Match C++: when_done_ callback (constraint_builder_2d.h line 170-171)
|
|
private ConstraintBuilder2DCallback? _whenDoneCallback;
|
|
|
|
// === MEMORY OPTIMIZATION: Limit concurrent MatchFullSubmap calls ===
|
|
// Each MatchFullSubmap can allocate 150-250MB, running 10+ concurrently causes 3-4GB spikes
|
|
// Limit to 2 concurrent calls to prevent memory exhaustion while still allowing parallelism
|
|
private readonly SemaphoreSlim _matchFullSubmapSemaphore = new(16, 16);
|
|
private int _activeMatchFullSubmapCount;
|
|
|
|
// Match C++: Constructor accepts thread_pool (constraint_builder_2d.cc line 59-66)
|
|
public ConstraintBuilder2D(ConstraintBuilderOptions options, Common.Threading.ThreadPoolInterface threadPool)
|
|
{
|
|
_options = options;
|
|
_threadPool = threadPool;
|
|
_fastCorrelativeScanMatcherOptions = options.FastCorrelativeScanMatcherOptions ??
|
|
new FastCorrelativeScanMatcherOptions2D(linearSearchWindow: 7.0, angularSearchWindow: Math.PI / 6.0, branchAndBoundDepth: 7);
|
|
var ceresOptions = options.CeresScanMatcherOptions ?? new CeresScanMatcherOptions2D(20.0, 0.1, 0.1);
|
|
_ceresScanMatcher = new CeresScanMatcher2D(ceresOptions);
|
|
|
|
// Match C++ (constraint_builder_2d.cc line 64-65): Initialize task objects
|
|
_finishNodeTask = new Common.Threading.Task();
|
|
_whenDoneTask = new Common.Threading.Task();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: MaybeAddConstraint - one initial_relative_pose, Match() then Ceres.
|
|
/// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
|
|
/// </summary>
|
|
public void MaybeAddConstraint(
|
|
SubmapId submapId,
|
|
NodeId nodeId,
|
|
Submap2D submap,
|
|
TrajectoryNode node,
|
|
Rigid2d initialRelativePose,
|
|
ConstraintBuilder2DCallback? callback = null)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(submap);
|
|
ArgumentNullException.ThrowIfNull(node);
|
|
if (node.ConstantData == null) return;
|
|
|
|
if (initialRelativePose.Translation.Length() > _options.MaxConstraintDistance)
|
|
return;
|
|
|
|
if (!GetOrCreateSampler(submapId).Pulse())
|
|
return;
|
|
|
|
var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0) return;
|
|
|
|
var grid = submap.Grid;
|
|
if (grid == null) return;
|
|
|
|
// Match C++ (constraint_builder_2d.cc line 92-111)
|
|
lock (_mutex)
|
|
{
|
|
if (_whenDoneCallback != null)
|
|
{
|
|
// LOG(WARNING): MaybeAddConstraint was called while WhenDone was scheduled
|
|
}
|
|
|
|
// Add placeholder for constraint result
|
|
var constraintIndex = _pendingConstraints.Count;
|
|
_pendingConstraints.Add(null);
|
|
_numConstraintTasksDispatched++;
|
|
|
|
// Get or create scan matcher (may schedule async construction)
|
|
var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);
|
|
|
|
// Schedule constraint computation task
|
|
var constraintTask = new Common.Threading.Task();
|
|
constraintTask.SetWorkItem(() =>
|
|
{
|
|
ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
|
|
matchFullSubmap: false, [initialRelativePose], constraintIndex);
|
|
Interlocked.Increment(ref _numConstraintTasksFinished);
|
|
});
|
|
|
|
// Add dependency on scan matcher construction (match C++ line 108)
|
|
constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
|
|
|
|
var constraintTaskHandle = _threadPool.Schedule(constraintTask);
|
|
|
|
// Add dependency to finish_node_task (match C++ line 111)
|
|
_finishNodeTask.AddDependency(constraintTaskHandle);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: MaybeAddLocalizationConstraint - list of initial_relative_poses, LocalizationMatch then Ceres.
|
|
/// Only adds a constraint when IsSearchingForRelocalization is true; then clears the flag on success.
|
|
/// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
|
|
/// </summary>
|
|
public void MaybeAddLocalizationConstraint(
|
|
SubmapId submapId,
|
|
NodeId nodeId,
|
|
Submap2D submap,
|
|
TrajectoryNode node,
|
|
IReadOnlyList<Rigid2d> initialRelativePoses,
|
|
ConstraintBuilder2DCallback? callback = null)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(submap);
|
|
ArgumentNullException.ThrowIfNull(node);
|
|
if (node.ConstantData == null) return;
|
|
if (initialRelativePoses == null || initialRelativePoses.Count == 0) return;
|
|
|
|
var filtered = initialRelativePoses
|
|
.Where(p => p.Translation.Length() <= _options.MaxConstraintDistance)
|
|
.ToList();
|
|
if (filtered.Count == 0) return;
|
|
|
|
_localizationMode = true;
|
|
|
|
var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0) return;
|
|
|
|
var grid = submap.Grid;
|
|
if (grid == null) return;
|
|
|
|
// Match C++ (constraint_builder_2d.cc line 138-157)
|
|
lock (_mutex)
|
|
{
|
|
if (_whenDoneCallback != null)
|
|
{
|
|
// LOG(WARNING): MaybeAddConstraint was called while WhenDone was scheduled
|
|
}
|
|
|
|
var constraintIndex = _pendingConstraints.Count;
|
|
_pendingConstraints.Add(null);
|
|
_numConstraintTasksDispatched++;
|
|
|
|
var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);
|
|
|
|
var constraintTask = new Common.Threading.Task();
|
|
constraintTask.SetWorkItem(() =>
|
|
{
|
|
ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
|
|
matchFullSubmap: true, filtered, constraintIndex);
|
|
Interlocked.Increment(ref _numConstraintTasksFinished);
|
|
});
|
|
|
|
constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
|
|
var constraintTaskHandle = _threadPool.Schedule(constraintTask);
|
|
_finishNodeTask.AddDependency(constraintTaskHandle);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: MaybeAddGlobalConstraint - full submap match (MatchFullSubmap then Ceres).
|
|
/// Schedules constraint computation asynchronously on ThreadPool (matches C++ behavior).
|
|
/// </summary>
|
|
public void MaybeAddGlobalConstraint(
|
|
SubmapId submapId,
|
|
NodeId nodeId,
|
|
Submap2D submap,
|
|
TrajectoryNode node,
|
|
ConstraintBuilder2DCallback? callback = null)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(submap);
|
|
ArgumentNullException.ThrowIfNull(node);
|
|
if (node.ConstantData == null) return;
|
|
|
|
var pointCloud = node.ConstantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0) return;
|
|
|
|
var grid = submap.Grid;
|
|
if (grid == null) return;
|
|
|
|
// Match C++ (constraint_builder_2d.cc line 160-182)
|
|
lock (_mutex)
|
|
{
|
|
if (_whenDoneCallback != null)
|
|
{
|
|
// LOG(WARNING): MaybeAddGlobalConstraint was called while WhenDone was scheduled
|
|
}
|
|
|
|
var constraintIndex = _pendingConstraints.Count;
|
|
_pendingConstraints.Add(null);
|
|
_numConstraintTasksDispatched++;
|
|
|
|
var scanMatcher = DispatchScanMatcherConstruction(submapId, grid);
|
|
|
|
var constraintTask = new Common.Threading.Task();
|
|
constraintTask.SetWorkItem(() =>
|
|
{
|
|
ComputeConstraint(submapId, nodeId, submap, grid, pointCloud,
|
|
matchFullSubmap: true, [Rigid2d.Identity], constraintIndex);
|
|
Interlocked.Increment(ref _numConstraintTasksFinished);
|
|
});
|
|
|
|
constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
|
|
var constraintTaskHandle = _threadPool.Schedule(constraintTask);
|
|
_finishNodeTask.AddDependency(constraintTaskHandle);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: manualComputeGlobalConstraint - MatchFullSubmap, Ceres, then MatchWithCustomizeParameters(0.1, 0.1, 0.01) for score.
|
|
/// </summary>
|
|
public (double Score, IPoseGraph.Constraint? Constraint) ManualComputeGlobalConstraint(
|
|
SubmapId submapId,
|
|
Submap2D submap,
|
|
NodeId nodeId,
|
|
TrajectoryNode.Data constantData,
|
|
double minScore)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(submap);
|
|
ArgumentNullException.ThrowIfNull(constantData);
|
|
var pointCloud = constantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0)
|
|
return (0, null);
|
|
|
|
var grid = submap.Grid;
|
|
if (grid == null) return (0, null);
|
|
|
|
var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
|
|
var submapPose = ComputeSubmapPose(submap);
|
|
if (!fastMatcher.MatchFullSubmap(pointCloud, 0, out _, out var poseEstimate))
|
|
return (0, null);
|
|
|
|
_ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary1);
|
|
ceresSummary1?.Dispose();
|
|
if (fastMatcher.MatchWithCustomizeParameters(0.1, 0.1, 0.01f, poseEstimate, pointCloud, 0, out double score, out poseEstimate))
|
|
{
|
|
// Re-calculated score
|
|
}
|
|
|
|
var constraintTransform = submapPose.Inverse() * poseEstimate;
|
|
// Match C++: include score and state (constraint_builder_2d.cc)
|
|
var constraint = new IPoseGraph.Constraint(
|
|
submapId, nodeId,
|
|
new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
|
|
IPoseGraph.Constraint.Tag.InterSubmap,
|
|
score,
|
|
IPoseGraph.Constraint.State.Enabled);
|
|
return (score, constraint);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: manualComputeRelocalizationConstraint - try LocalizationMatch on each submap/pose, pick best, Ceres, return constraint.
|
|
/// </summary>
|
|
public (double Score, IPoseGraph.Constraint? Constraint) ManualComputeRelocalizationConstraint(
|
|
IReadOnlyList<SubmapId> submapIds,
|
|
IReadOnlyList<Submap2D> submaps,
|
|
IReadOnlyList<Rigid2d> relativePoses,
|
|
NodeId nodeId,
|
|
TrajectoryNode.Data constantData,
|
|
double minScore)
|
|
{
|
|
if (submapIds == null || submaps == null || relativePoses == null || submapIds.Count != submaps.Count || submapIds.Count != relativePoses.Count)
|
|
return (0, null);
|
|
ArgumentNullException.ThrowIfNull(constantData);
|
|
var pointCloud = constantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0) return (0, null);
|
|
|
|
double bestScore = 0;
|
|
Rigid2d bestPoseEstimate = Rigid2d.Identity;
|
|
Submap2D? bestSubmap = null;
|
|
SubmapId bestSubmapId = default;
|
|
|
|
for (int i = 0; i < submaps.Count; i++)
|
|
{
|
|
var submap = submaps[i];
|
|
var grid = submap.Grid;
|
|
if (grid == null) continue;
|
|
|
|
var fastMatcher = GetOrCreateFastMatcherSync(submapIds[i], grid);
|
|
var localizationInitialPose = ComputeSubmapPose(submap) * relativePoses[i];
|
|
|
|
if (!fastMatcher.LocalizationMatch(localizationInitialPose, pointCloud, minScore, out var score, out var poseEstimate))
|
|
continue;
|
|
|
|
if (score > bestScore)
|
|
{
|
|
bestScore = score;
|
|
bestPoseEstimate = poseEstimate;
|
|
bestSubmap = submap;
|
|
bestSubmapId = submapIds[i];
|
|
}
|
|
}
|
|
|
|
if (bestSubmap == null) return (0, null);
|
|
|
|
_ceresScanMatcher.Match(bestPoseEstimate.Translation, bestPoseEstimate, pointCloud, bestSubmap.Grid!, out bestPoseEstimate, out var ceresSummary2);
|
|
ceresSummary2?.Dispose();
|
|
var constraintTransform = ComputeSubmapPose(bestSubmap).Inverse() * bestPoseEstimate;
|
|
// Match C++: include score and state (constraint_builder_2d.cc)
|
|
var constraint = new IPoseGraph.Constraint(
|
|
bestSubmapId, nodeId,
|
|
new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
|
|
IPoseGraph.Constraint.Tag.InterSubmap,
|
|
bestScore,
|
|
IPoseGraph.Constraint.State.Enabled);
|
|
return (bestScore, constraint);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: manualComputeConstraintScore - MatchWithCustomizeParameters(1.5, 1.5, 0.05) from initial_pose.
|
|
/// </summary>
|
|
public double ManualComputeConstraintScore(
|
|
SubmapId submapId,
|
|
Submap2D submap,
|
|
NodeId nodeId,
|
|
TrajectoryNode.Data constantData,
|
|
double minScore,
|
|
Rigid3d initialPose)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(submap);
|
|
ArgumentNullException.ThrowIfNull(constantData);
|
|
var pointCloud = constantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0) return 0;
|
|
|
|
var grid = submap.Grid;
|
|
if (grid == null) return 0;
|
|
|
|
var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
|
|
var poseEstimate = TransformOperations.Project2D(initialPose);
|
|
fastMatcher.MatchWithCustomizeParameters(1.5, 1.5, 0.05f, poseEstimate, pointCloud, 0, out var constraintScore, out _);
|
|
return constraintScore;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: manualComputeScanMatcher - Ceres then MatchWithCustomizeParameters(0.2, 0.2, 0.01), output pose_manual_estimate.
|
|
/// </summary>
|
|
public double ManualComputeScanMatcher(
|
|
SubmapId submapId,
|
|
Submap2D submap,
|
|
NodeId nodeId,
|
|
TrajectoryNode.Data constantData,
|
|
double minScore,
|
|
Rigid3d initialPose,
|
|
out Rigid3d poseManualEstimate)
|
|
{
|
|
poseManualEstimate = default;
|
|
ArgumentNullException.ThrowIfNull(submap);
|
|
ArgumentNullException.ThrowIfNull(constantData);
|
|
var pointCloud = constantData.FilteredGravityAlignedPointCloud;
|
|
if (pointCloud == null || pointCloud.Count == 0) return 0;
|
|
|
|
var grid = submap.Grid;
|
|
if (grid == null) return 0;
|
|
|
|
var fastMatcher = GetOrCreateFastMatcherSync(submapId, grid);
|
|
var poseEstimate = TransformOperations.Project2D(initialPose);
|
|
|
|
_ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary3);
|
|
ceresSummary3?.Dispose();
|
|
|
|
if (fastMatcher.MatchWithCustomizeParameters(0.2, 0.2, 0.01f, poseEstimate, pointCloud, 0, out double score, out poseEstimate))
|
|
{
|
|
poseManualEstimate = TransformOperations.Embed3D(poseEstimate);
|
|
return score;
|
|
}
|
|
poseManualEstimate = TransformOperations.Embed3D(poseEstimate);
|
|
return score;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: NotifyEndOfNode - must be called after all computations for one node have been added.
|
|
/// Match C++ (constraint_builder_2d.cc line 403-415)
|
|
/// </summary>
|
|
public void NotifyEndOfNode()
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
// Set work item for finish_node_task to increment num_finished_nodes
|
|
_finishNodeTask.SetWorkItem(() =>
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
_numFinishedNodes++;
|
|
}
|
|
});
|
|
|
|
// Schedule finish_node_task
|
|
var finishNodeTaskHandle = _threadPool.Schedule(_finishNodeTask);
|
|
|
|
// Create new finish_node_task for next node
|
|
_finishNodeTask = new Common.Threading.Task();
|
|
|
|
// Add dependency to when_done_task
|
|
_whenDoneTask.AddDependency(finishNodeTaskHandle);
|
|
|
|
_numStartedNodes++;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ WhenDone: Registers callback to be called after all computations finish.
|
|
/// Match C++ (constraint_builder_2d.cc line 417-427)
|
|
/// </summary>
|
|
public void WhenDone(ConstraintBuilder2DCallback callback)
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
if (_whenDoneCallback != null)
|
|
{
|
|
throw new InvalidOperationException("WhenDone() called while another WhenDone() was pending");
|
|
}
|
|
|
|
_whenDoneCallback = callback;
|
|
|
|
// Set work item for when_done_task to run callback
|
|
_whenDoneTask.SetWorkItem(RunWhenDoneCallback);
|
|
|
|
// Schedule when_done_task (it will wait for all dependencies)
|
|
_threadPool.Schedule(_whenDoneTask);
|
|
|
|
// Create new when_done_task for next cycle
|
|
_whenDoneTask = new Common.Threading.Task();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ RunWhenDoneCallback (constraint_builder_2d.cc line 596-617)
|
|
/// </summary>
|
|
private void RunWhenDoneCallback()
|
|
{
|
|
List<IPoseGraph.Constraint> result = [];
|
|
ConstraintBuilder2DCallback? callback;
|
|
|
|
lock (_mutex)
|
|
{
|
|
if (_whenDoneCallback == null)
|
|
{
|
|
throw new InvalidOperationException("RunWhenDoneCallback called without callback set");
|
|
}
|
|
|
|
// Collect all non-null constraints
|
|
foreach (var constraint in _pendingConstraints)
|
|
{
|
|
if (constraint != null)
|
|
{
|
|
result.Add(constraint.Value);
|
|
}
|
|
}
|
|
|
|
// Clear pending constraints
|
|
_pendingConstraints.Clear();
|
|
|
|
// Take callback and clear
|
|
callback = _whenDoneCallback;
|
|
_whenDoneCallback = null;
|
|
}
|
|
|
|
// Invoke callback outside lock
|
|
callback(new ConstraintBuilder2DResult(result));
|
|
}
|
|
|
|
public List<IPoseGraph.Constraint> GetConstraints()
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
return [.. _pendingConstraints.Where(c => c != null).Select(c => c!.Value)];
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: GetNumFinishedNodes().
|
|
/// </summary>
|
|
public int GetNumFinishedNodes()
|
|
{
|
|
lock (_mutex) return _numFinishedNodes;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: DeleteScanMatcher(submap_id).
|
|
/// </summary>
|
|
public void DeleteScanMatcher(SubmapId submapId)
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
_submapScanMatchers.Remove(submapId);
|
|
_perSubmapSampler.Remove(submapId);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: GetNumStartedNodes().
|
|
/// </summary>
|
|
public int GetNumStartedNodes()
|
|
{
|
|
lock (_mutex) return _numStartedNodes;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the total number of constraint tasks dispatched for computation.
|
|
/// </summary>
|
|
public int GetNumConstraintTasksDispatched()
|
|
{
|
|
lock (_mutex) return _numConstraintTasksDispatched;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the number of constraint tasks that have finished computation.
|
|
/// </summary>
|
|
public int GetNumConstraintTasksFinished()
|
|
{
|
|
return Interlocked.CompareExchange(ref _numConstraintTasksFinished, 0, 0);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++: IsSearchingForRelocalization().
|
|
/// </summary>
|
|
public bool IsSearchingForRelocalization => _isSearchForRelocalization;
|
|
|
|
/// <summary>
|
|
/// Match C++: ToggleSearchingForRelocalization(enable).
|
|
/// </summary>
|
|
public void ToggleSearchingForRelocalization(bool enable)
|
|
{
|
|
_isSearchForRelocalization = enable;
|
|
}
|
|
|
|
public void Clear()
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
_pendingConstraints.Clear();
|
|
}
|
|
}
|
|
|
|
private FixedRatioSampler GetOrCreateSampler(SubmapId submapId)
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
if (!_perSubmapSampler.TryGetValue(submapId, out var sampler))
|
|
{
|
|
sampler = new FixedRatioSampler(_options.SamplingRatio);
|
|
_perSubmapSampler[submapId] = sampler;
|
|
}
|
|
return sampler;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Match C++ DispatchScanMatcherConstruction (constraint_builder_2d.cc line 429-450)
|
|
/// Creates or returns existing SubmapScanMatcher, scheduling async construction if needed.
|
|
/// MUST be called with _mutex held.
|
|
/// </summary>
|
|
private SubmapScanMatcher DispatchScanMatcherConstruction(SubmapId submapId, Grid2D grid)
|
|
{
|
|
// Check if scan matcher already exists
|
|
if (_submapScanMatchers.TryGetValue(submapId, out var existingMatcher))
|
|
{
|
|
return existingMatcher;
|
|
}
|
|
|
|
// Create new scan matcher entry
|
|
var submapScanMatcher = new SubmapScanMatcher
|
|
{
|
|
Grid = grid
|
|
};
|
|
_submapScanMatchers[submapId] = submapScanMatcher;
|
|
|
|
var scanMatcherOptions = _fastCorrelativeScanMatcherOptions ??
|
|
new FastCorrelativeScanMatcherOptions2D(7.0, Math.PI / 6.0, 7);
|
|
|
|
// Schedule async construction of FastCorrelativeScanMatcher2D
|
|
var scanMatcherTask = new Common.Threading.Task();
|
|
scanMatcherTask.SetWorkItem(() =>
|
|
{
|
|
// Create the scan matcher (this may be expensive)
|
|
var gridLimits = grid.Limits.CellLimits;
|
|
var matcher = new FastCorrelativeScanMatcher2D(grid, scanMatcherOptions);
|
|
lock (_mutex)
|
|
{
|
|
submapScanMatcher.FastCorrelativeScanMatcher = matcher;
|
|
}
|
|
});
|
|
|
|
submapScanMatcher.CreationTaskHandle = _threadPool.Schedule(scanMatcherTask);
|
|
|
|
return submapScanMatcher;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the FastCorrelativeScanMatcher for a submap (for manual compute methods).
|
|
/// This blocks until the scan matcher is ready.
|
|
/// </summary>
|
|
private FastCorrelativeScanMatcher2D GetOrCreateFastMatcherSync(SubmapId submapId, Grid2D grid)
|
|
{
|
|
SubmapScanMatcher? scanMatcher;
|
|
lock (_mutex)
|
|
{
|
|
if (!_submapScanMatchers.TryGetValue(submapId, out scanMatcher))
|
|
{
|
|
// Create synchronously for manual methods
|
|
var options = _fastCorrelativeScanMatcherOptions ??
|
|
new FastCorrelativeScanMatcherOptions2D(7.0, Math.PI / 6.0, 7);
|
|
var matcher = new FastCorrelativeScanMatcher2D(grid, options);
|
|
scanMatcher = new SubmapScanMatcher
|
|
{
|
|
Grid = grid,
|
|
FastCorrelativeScanMatcher = matcher
|
|
};
|
|
_submapScanMatchers[submapId] = scanMatcher;
|
|
return matcher;
|
|
}
|
|
}
|
|
|
|
// Wait for async construction if needed
|
|
if (scanMatcher.FastCorrelativeScanMatcher == null &&
|
|
scanMatcher.CreationTaskHandle != null &&
|
|
scanMatcher.CreationTaskHandle.TryGetTarget(out var task))
|
|
{
|
|
while (task.GetState() != Common.Threading.TaskState.Completed)
|
|
{
|
|
System.Threading.Thread.Sleep(1);
|
|
}
|
|
}
|
|
|
|
lock (_mutex)
|
|
{
|
|
return scanMatcher.FastCorrelativeScanMatcher!;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Single internal compute: handles MaybeAddConstraint (matchFullSubmap=false, single pose),
|
|
/// MaybeAddLocalizationConstraint (matchFullSubmap=true, localizationMode, many poses),
|
|
/// MaybeAddGlobalConstraint (matchFullSubmap=true, single Identity pose).
|
|
/// Match C++ ComputeConstraint (constraint_builder_2d.cc line 452-594)
|
|
/// </summary>
|
|
private void ComputeConstraint(
|
|
SubmapId submapId,
|
|
NodeId nodeId,
|
|
Submap2D submap,
|
|
Grid2D grid,
|
|
PointCloud pointCloud,
|
|
bool matchFullSubmap,
|
|
IReadOnlyList<Rigid2d> initialRelativePoses,
|
|
int constraintIndex)
|
|
{
|
|
// Get the scan matcher (should be ready by now due to task dependency)
|
|
FastCorrelativeScanMatcher2D? fastMatcher;
|
|
lock (_mutex)
|
|
{
|
|
if (!_submapScanMatchers.TryGetValue(submapId, out var scanMatcher) ||
|
|
scanMatcher.FastCorrelativeScanMatcher == null)
|
|
{
|
|
return; // Scan matcher not ready (shouldn't happen with proper dependencies)
|
|
}
|
|
fastMatcher = scanMatcher.FastCorrelativeScanMatcher;
|
|
}
|
|
|
|
var submapPose = ComputeSubmapPose(submap);
|
|
double score = 0;
|
|
Rigid2d poseEstimate = Rigid2d.Identity;
|
|
|
|
if (matchFullSubmap)
|
|
{
|
|
if (_localizationMode)
|
|
{
|
|
lock (_mutex)
|
|
{
|
|
if (!_isSearchForRelocalization)
|
|
return;
|
|
}
|
|
|
|
double bestScore = 0;
|
|
Rigid2d bestPoseEstimate = Rigid2d.Identity;
|
|
foreach (var rel in initialRelativePoses)
|
|
{
|
|
var localizationInitialPose = submapPose * rel;
|
|
if (fastMatcher.LocalizationMatch(localizationInitialPose, pointCloud, _options.GlobalLocalizationMinScore, out score, out poseEstimate))
|
|
{
|
|
if (score > bestScore)
|
|
{
|
|
bestScore = score;
|
|
bestPoseEstimate = poseEstimate;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (bestScore < _options.GlobalLocalizationMinScore)
|
|
return;
|
|
|
|
_isSearchForRelocalization = false;
|
|
score = bestScore;
|
|
poseEstimate = bestPoseEstimate;
|
|
}
|
|
else
|
|
{
|
|
// === MEMORY OPTIMIZATION: Limit concurrent MatchFullSubmap calls ===
|
|
// Each call allocates 150-250MB, running many concurrently causes GB-level spikes
|
|
_matchFullSubmapSemaphore.Wait();
|
|
_ = Interlocked.Increment(ref _activeMatchFullSubmapCount);
|
|
try
|
|
{
|
|
// === DEBUG: Track memory before/after MatchFullSubmap ===
|
|
var ramBeforeMatch = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);
|
|
|
|
if (!fastMatcher.MatchFullSubmap(pointCloud, _options.GlobalLocalizationMinScore, out score, out poseEstimate))
|
|
{
|
|
var ramAfterFail = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);
|
|
return;
|
|
}
|
|
|
|
var ramAfterMatch = System.Diagnostics.Process.GetCurrentProcess().PrivateMemorySize64 / (1024 * 1024);
|
|
|
|
if (score <= _options.GlobalLocalizationMinScore)
|
|
return;
|
|
}
|
|
finally
|
|
{
|
|
Interlocked.Decrement(ref _activeMatchFullSubmapCount);
|
|
_matchFullSubmapSemaphore.Release();
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
var initialPose = submapPose * initialRelativePoses[0];
|
|
if (!fastMatcher.Match(initialPose, pointCloud, _options.MinScore, out score, out poseEstimate))
|
|
return;
|
|
if (score <= _options.MinScore)
|
|
return;
|
|
}
|
|
|
|
_ceresScanMatcher.Match(poseEstimate.Translation, poseEstimate, pointCloud, grid, out poseEstimate, out var ceresSummary4);
|
|
ceresSummary4?.Dispose();
|
|
|
|
var constraintTransform = submapPose.Inverse() * poseEstimate;
|
|
// Match C++: include score and state (constraint_builder_2d.cc lines 567-574)
|
|
var constraint = new IPoseGraph.Constraint(
|
|
submapId, nodeId,
|
|
new IPoseGraph.Constraint.Pose(TransformOperations.Embed3D(constraintTransform), _options.LoopClosureTranslationWeight, _options.LoopClosureRotationWeight),
|
|
IPoseGraph.Constraint.Tag.InterSubmap,
|
|
score, // CRITICAL FIX: include score from scan matching
|
|
IPoseGraph.Constraint.State.Enabled); // Match C++: Constraint::ENABLED
|
|
|
|
// Store constraint at the pre-allocated index
|
|
lock (_mutex)
|
|
{
|
|
_pendingConstraints[constraintIndex] = constraint;
|
|
}
|
|
}
|
|
|
|
private static Rigid2d ComputeSubmapPose(Submap2D submap)
|
|
{
|
|
return TransformOperations.Project2D(submap.LocalPose);
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
if (!_disposed)
|
|
{
|
|
_ceresScanMatcher?.Dispose();
|
|
_matchFullSubmapSemaphore?.Dispose();
|
|
_disposed = true;
|
|
}
|
|
}
|
|
}
|