Initial commit
This commit is contained in:
@@ -0,0 +1,838 @@
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,580 @@
|
||||
/*
|
||||
* 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.D3D.ScanMatching;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using Submap3D = CartographerSharp.Mapping.D3D.Submap3D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.Constraints;
|
||||
|
||||
/// <summary>
|
||||
/// Result of constraint building for 3D.
|
||||
/// </summary>
|
||||
public record struct ConstraintBuilder3DResult(List<IPoseGraph.Constraint> Constraints);
|
||||
|
||||
/// <summary>
|
||||
/// Callback for constraint building completion.
|
||||
/// </summary>
|
||||
public delegate void ConstraintBuilder3DCallback(ConstraintBuilder3DResult result);
|
||||
|
||||
/// <summary>
|
||||
/// Builds constraints for the 3D pose graph by matching nodes against submaps.
|
||||
/// Match C++ ConstraintBuilder3D (constraint_builder_3d.h/cc)
|
||||
/// </summary>
|
||||
public class ConstraintBuilder3D : IDisposable
|
||||
{
|
||||
private bool _disposed;
|
||||
private readonly ConstraintBuilderOptions _options;
|
||||
private readonly object _mutex = new();
|
||||
private readonly Dictionary<SubmapId, FixedRatioSampler> _perSubmapSampler = [];
|
||||
|
||||
// Scan matchers
|
||||
private readonly CeresScanMatcher3D? _ceresScanMatcher;
|
||||
|
||||
// Match C++: num_started_nodes_, num_finished_nodes_ (constraint_builder_3d.h line 157-159)
|
||||
private int _numStartedNodes;
|
||||
private int _numFinishedNodes;
|
||||
|
||||
// Match C++: thread_pool_ (constraint_builder_3d.cc line 63)
|
||||
private readonly Common.Threading.ThreadPoolInterface _threadPool;
|
||||
|
||||
// Match C++: finish_node_task_, when_done_task_ (constraint_builder_3d.h line 161-163)
|
||||
private Common.Threading.Task _finishNodeTask;
|
||||
private Common.Threading.Task _whenDoneTask;
|
||||
|
||||
/// <summary>
|
||||
/// Submap scan matcher structure.
|
||||
/// Match C++ SubmapScanMatcher (constraint_builder_3d.h line 117-123)
|
||||
/// </summary>
|
||||
private class SubmapScanMatcher
|
||||
{
|
||||
public Mapping.D3D.HybridGrid? HighResolutionHybridGrid { get; set; }
|
||||
public Mapping.D3D.HybridGrid? LowResolutionHybridGrid { get; set; }
|
||||
public Mapping.D3D.IntensityHybridGrid? HighResolutionIntensityHybridGrid { get; set; }
|
||||
public RealTimeCorrelativeScanMatcher3D? FastCorrelativeScanMatcher { get; set; }
|
||||
public WeakReference<Common.Threading.Task>? CreationTaskHandle { get; set; }
|
||||
}
|
||||
|
||||
// Match C++: submap_scan_matchers_ (constraint_builder_3d.h line 171-172)
|
||||
private readonly Dictionary<SubmapId, SubmapScanMatcher> _submapScanMatchers = [];
|
||||
|
||||
// Match C++: constraints_ deque (constraint_builder_3d.h line 168)
|
||||
private readonly List<IPoseGraph.Constraint?> _pendingConstraints = [];
|
||||
|
||||
// Match C++: when_done_ callback (constraint_builder_3d.h line 150-151)
|
||||
private ConstraintBuilder3DCallback? _whenDoneCallback;
|
||||
|
||||
// Match C++: Constructor accepts thread_pool (constraint_builder_3d.cc line 61-68)
|
||||
public ConstraintBuilder3D(ConstraintBuilderOptions options, Common.Threading.ThreadPoolInterface threadPool)
|
||||
{
|
||||
_options = options;
|
||||
_threadPool = threadPool;
|
||||
|
||||
// Initialize Ceres scan matcher if options are provided
|
||||
if (options.CeresScanMatcherOptions3D != null)
|
||||
{
|
||||
_ceresScanMatcher = new CeresScanMatcher3D(options.CeresScanMatcherOptions3D.Value);
|
||||
}
|
||||
|
||||
// Match C++ (constraint_builder_3d.cc line 66-67): Initialize task objects
|
||||
_finishNodeTask = new Common.Threading.Task();
|
||||
_whenDoneTask = new Common.Threading.Task();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Schedules exploring a new constraint between 'submap' identified by
|
||||
/// 'submap_id', and the point cloud for 'node_id'.
|
||||
/// Match C++ MaybeAddConstraint (constraint_builder_3d.cc line 79-114)
|
||||
/// </summary>
|
||||
public void MaybeAddConstraint(
|
||||
SubmapId submapId,
|
||||
NodeId nodeId,
|
||||
Submap3D submap,
|
||||
TrajectoryNode node,
|
||||
Rigid3d globalNodePose,
|
||||
Rigid3d globalSubmapPose)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(submap);
|
||||
ArgumentNullException.ThrowIfNull(node);
|
||||
if (node.ConstantData == null)
|
||||
return;
|
||||
|
||||
// Check distance threshold
|
||||
var distance = (globalNodePose.Translation - globalSubmapPose.Translation).Length();
|
||||
if (distance > _options.MaxConstraintDistance)
|
||||
return;
|
||||
|
||||
// Check sampling ratio
|
||||
if (!GetOrCreateSampler(submapId).Pulse())
|
||||
return;
|
||||
|
||||
// Get point cloud from node
|
||||
var pointCloud = node.ConstantData.HighResolutionPointCloud;
|
||||
if (pointCloud == null || pointCloud.Count == 0)
|
||||
return;
|
||||
|
||||
// Match C++ (constraint_builder_3d.cc line 95-113)
|
||||
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);
|
||||
|
||||
// Get or create scan matcher (may schedule async construction)
|
||||
var scanMatcher = DispatchScanMatcherConstruction(submapId, submap);
|
||||
if (scanMatcher == null)
|
||||
return;
|
||||
|
||||
// Schedule constraint computation task
|
||||
var constraintTask = new Common.Threading.Task();
|
||||
constraintTask.SetWorkItem(() =>
|
||||
{
|
||||
ComputeConstraint(submapId, nodeId, false, node.ConstantData,
|
||||
globalNodePose, globalSubmapPose, scanMatcher, constraintIndex);
|
||||
});
|
||||
|
||||
// Add dependency on scan matcher construction (match C++ line 110)
|
||||
constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
|
||||
|
||||
var constraintTaskHandle = _threadPool.Schedule(constraintTask);
|
||||
|
||||
// Add dependency to finish_node_task (match C++ line 113)
|
||||
_finishNodeTask.AddDependency(constraintTaskHandle);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Schedules exploring a new global constraint (full submap matching).
|
||||
/// Match C++ MaybeAddGlobalConstraint (constraint_builder_3d.cc line 116-142)
|
||||
/// </summary>
|
||||
public void MaybeAddGlobalConstraint(
|
||||
SubmapId submapId,
|
||||
NodeId nodeId,
|
||||
Submap3D submap,
|
||||
TrajectoryNode node,
|
||||
Quaternion globalNodeRotation,
|
||||
Quaternion globalSubmapRotation)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(submap);
|
||||
ArgumentNullException.ThrowIfNull(node);
|
||||
if (node.ConstantData == null)
|
||||
return;
|
||||
|
||||
// Get point cloud from node
|
||||
var pointCloud = node.ConstantData.HighResolutionPointCloud;
|
||||
if (pointCloud == null || pointCloud.Count == 0)
|
||||
return;
|
||||
|
||||
// Match C++ (constraint_builder_3d.cc line 121-141)
|
||||
lock (_mutex)
|
||||
{
|
||||
if (_whenDoneCallback != null)
|
||||
{
|
||||
// LOG(WARNING): MaybeAddGlobalConstraint was called while WhenDone was scheduled
|
||||
}
|
||||
|
||||
var constraintIndex = _pendingConstraints.Count;
|
||||
_pendingConstraints.Add(null);
|
||||
|
||||
var scanMatcher = DispatchScanMatcherConstruction(submapId, submap);
|
||||
if (scanMatcher == null)
|
||||
return;
|
||||
|
||||
// Create poses with only rotation (yaw is ignored for global matching)
|
||||
var globalNodePose = Rigid3d.FromRotation(globalNodeRotation);
|
||||
var globalSubmapPose = Rigid3d.FromRotation(globalSubmapRotation);
|
||||
|
||||
var constraintTask = new Common.Threading.Task();
|
||||
constraintTask.SetWorkItem(() =>
|
||||
{
|
||||
ComputeConstraint(submapId, nodeId, true, node.ConstantData,
|
||||
globalNodePose, globalSubmapPose, scanMatcher, constraintIndex);
|
||||
});
|
||||
|
||||
constraintTask.AddDependency(scanMatcher.CreationTaskHandle);
|
||||
var constraintTaskHandle = _threadPool.Schedule(constraintTask);
|
||||
_finishNodeTask.AddDependency(constraintTaskHandle);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Must be called after all computations related to one node have been added.
|
||||
/// Match C++ NotifyEndOfNode (constraint_builder_3d.cc line 144-156)
|
||||
/// </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>
|
||||
/// Registers the callback to be called with the results, after all
|
||||
/// computations triggered by MaybeAdd*Constraint have finished.
|
||||
/// Match C++ WhenDone (constraint_builder_3d.cc line 158-168)
|
||||
/// </summary>
|
||||
public void WhenDone(ConstraintBuilder3DCallback 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_3d.cc line 307-333)
|
||||
/// </summary>
|
||||
private void RunWhenDoneCallback()
|
||||
{
|
||||
List<IPoseGraph.Constraint> result = [];
|
||||
ConstraintBuilder3DCallback? 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 ConstraintBuilder3DResult(result));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the number of consecutive finished nodes.
|
||||
/// </summary>
|
||||
public int GetNumFinishedNodes()
|
||||
{
|
||||
lock (_mutex) return _numFinishedNodes;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the number of started nodes.
|
||||
/// </summary>
|
||||
public int GetNumStartedNodes()
|
||||
{
|
||||
lock (_mutex) return _numStartedNodes;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Delete data related to 'submap_id'.
|
||||
/// </summary>
|
||||
public void DeleteScanMatcher(SubmapId submapId)
|
||||
{
|
||||
lock (_mutex)
|
||||
{
|
||||
_submapScanMatchers.Remove(submapId);
|
||||
_perSubmapSampler.Remove(submapId);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the computed constraints.
|
||||
/// </summary>
|
||||
public List<IPoseGraph.Constraint> GetConstraints()
|
||||
{
|
||||
lock (_mutex)
|
||||
{
|
||||
return _pendingConstraints.Where(c => c != null).Select(c => c!.Value).ToList();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clears all constraints.
|
||||
/// </summary>
|
||||
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>
|
||||
/// Dispatches scan matcher construction for a submap.
|
||||
/// Match C++ DispatchScanMatcherConstruction (constraint_builder_3d.cc line 170-198)
|
||||
/// MUST be called with _mutex held.
|
||||
/// </summary>
|
||||
private SubmapScanMatcher? DispatchScanMatcherConstruction(SubmapId submapId, Submap3D submap)
|
||||
{
|
||||
// Check if scan matcher already exists
|
||||
if (_submapScanMatchers.TryGetValue(submapId, out var existingMatcher))
|
||||
{
|
||||
return existingMatcher;
|
||||
}
|
||||
|
||||
// Create new scan matcher entry
|
||||
var scanMatcher = new SubmapScanMatcher
|
||||
{
|
||||
HighResolutionHybridGrid = submap.HighResolutionHybridGrid,
|
||||
LowResolutionHybridGrid = submap.LowResolutionHybridGrid,
|
||||
HighResolutionIntensityHybridGrid = submap.HighResolutionIntensityHybridGrid
|
||||
};
|
||||
|
||||
if (scanMatcher.HighResolutionHybridGrid == null)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
_submapScanMatchers[submapId] = scanMatcher;
|
||||
|
||||
var fastOptions = _options.FastCorrelativeScanMatcherOptions3D ??
|
||||
new FastCorrelativeScanMatcherOptions3D();
|
||||
|
||||
// Get rotational scan matcher histogram from submap if available
|
||||
double[]? histogram = null;
|
||||
if (submap is Mapping.D3D.Submap3D submap3D)
|
||||
{
|
||||
var histogramList = submap3D.RotationalScanMatcherHistogram;
|
||||
if (histogramList != null && histogramList.Count > 0)
|
||||
{
|
||||
histogram = histogramList.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
// Capture values for closure
|
||||
var highResGrid = scanMatcher.HighResolutionHybridGrid;
|
||||
var lowResGrid = scanMatcher.LowResolutionHybridGrid;
|
||||
|
||||
// Schedule async construction of FastCorrelativeScanMatcher
|
||||
var scanMatcherTask = new Common.Threading.Task();
|
||||
scanMatcherTask.SetWorkItem(() =>
|
||||
{
|
||||
var matcher = new RealTimeCorrelativeScanMatcher3D(
|
||||
highResGrid,
|
||||
lowResGrid,
|
||||
histogram,
|
||||
fastOptions);
|
||||
|
||||
lock (_mutex)
|
||||
{
|
||||
scanMatcher.FastCorrelativeScanMatcher = matcher;
|
||||
}
|
||||
});
|
||||
|
||||
scanMatcher.CreationTaskHandle = _threadPool.Schedule(scanMatcherTask);
|
||||
|
||||
return scanMatcher;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes a constraint between a node and submap.
|
||||
/// Match C++ ComputeConstraint (constraint_builder_3d.cc line 200-305)
|
||||
/// </summary>
|
||||
private void ComputeConstraint(
|
||||
SubmapId submapId,
|
||||
NodeId nodeId,
|
||||
bool matchFullSubmap,
|
||||
TrajectoryNode.Data constantData,
|
||||
Rigid3d globalNodePose,
|
||||
Rigid3d globalSubmapPose,
|
||||
SubmapScanMatcher scanMatcher,
|
||||
int constraintIndex)
|
||||
{
|
||||
// Get the scan matcher (should be ready by now due to task dependency)
|
||||
RealTimeCorrelativeScanMatcher3D? fastMatcher;
|
||||
lock (_mutex)
|
||||
{
|
||||
if (scanMatcher.FastCorrelativeScanMatcher == null)
|
||||
{
|
||||
return; // Scan matcher not ready (shouldn't happen with proper dependencies)
|
||||
}
|
||||
fastMatcher = scanMatcher.FastCorrelativeScanMatcher;
|
||||
}
|
||||
|
||||
if (scanMatcher.HighResolutionHybridGrid == null)
|
||||
return;
|
||||
|
||||
var pointCloud = constantData.HighResolutionPointCloud;
|
||||
if (pointCloud == null || pointCloud.Count == 0)
|
||||
return;
|
||||
|
||||
// Step 1: Fast correlative scan matching for initial estimate
|
||||
FastCorrelativeScanMatcher3DResult? matchResult;
|
||||
if (matchFullSubmap)
|
||||
{
|
||||
matchResult = fastMatcher.MatchFullSubmap(
|
||||
globalNodePose.Rotation,
|
||||
globalSubmapPose.Rotation,
|
||||
constantData,
|
||||
_options.GlobalLocalizationMinScore);
|
||||
}
|
||||
else
|
||||
{
|
||||
matchResult = fastMatcher.Match(
|
||||
globalNodePose,
|
||||
globalSubmapPose,
|
||||
constantData,
|
||||
_options.MinScore);
|
||||
}
|
||||
|
||||
if (matchResult == null)
|
||||
return; // Score too low
|
||||
|
||||
var poseEstimate = matchResult.Value.PoseEstimate;
|
||||
|
||||
// Step 2: Refine with Ceres scan matcher
|
||||
if (_ceresScanMatcher != null)
|
||||
{
|
||||
var pointCloudsAndGrids = new List<PointCloudAndHybridGridsPointers>();
|
||||
|
||||
// Add high resolution point cloud and grid
|
||||
if (scanMatcher.HighResolutionHybridGrid != null)
|
||||
{
|
||||
pointCloudsAndGrids.Add(new PointCloudAndHybridGridsPointers
|
||||
{
|
||||
PointCloud = pointCloud,
|
||||
HybridGrid = scanMatcher.HighResolutionHybridGrid,
|
||||
IntensityHybridGrid = scanMatcher.HighResolutionIntensityHybridGrid
|
||||
});
|
||||
}
|
||||
|
||||
// Add low resolution point cloud and grid if available
|
||||
if (scanMatcher.LowResolutionHybridGrid != null && constantData.LowResolutionPointCloud != null)
|
||||
{
|
||||
pointCloudsAndGrids.Add(new PointCloudAndHybridGridsPointers
|
||||
{
|
||||
PointCloud = constantData.LowResolutionPointCloud,
|
||||
HybridGrid = scanMatcher.LowResolutionHybridGrid,
|
||||
IntensityHybridGrid = null
|
||||
});
|
||||
}
|
||||
|
||||
if (pointCloudsAndGrids.Count > 0)
|
||||
{
|
||||
CeresSharp.SolverSummary? summary = null;
|
||||
try
|
||||
{
|
||||
_ceresScanMatcher.Match(
|
||||
poseEstimate.Translation,
|
||||
poseEstimate,
|
||||
pointCloudsAndGrids,
|
||||
out poseEstimate,
|
||||
out summary
|
||||
);
|
||||
}
|
||||
finally
|
||||
{
|
||||
summary?.Dispose();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Step 3: Create constraint
|
||||
// CRITICAL FIX: Match C++ (constraint_builder_3d.cc line 303-304)
|
||||
// constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate
|
||||
// poseEstimate is in global frame, constraint must be relative to submap's local frame
|
||||
var constraintTransform = globalSubmapPose.Inverse() * poseEstimate;
|
||||
var constraint = new IPoseGraph.Constraint(
|
||||
submapId,
|
||||
nodeId,
|
||||
new IPoseGraph.Constraint.Pose(
|
||||
constraintTransform,
|
||||
_options.LoopClosureTranslationWeight,
|
||||
_options.LoopClosureRotationWeight
|
||||
),
|
||||
IPoseGraph.Constraint.Tag.InterSubmap,
|
||||
matchResult.Value.Score,
|
||||
IPoseGraph.Constraint.State.Enabled
|
||||
);
|
||||
|
||||
// Store constraint at the pre-allocated index
|
||||
lock (_mutex)
|
||||
{
|
||||
_pendingConstraints[constraintIndex] = constraint;
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_ceresScanMatcher?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user