Initial commit
This commit is contained in:
@@ -0,0 +1,346 @@
|
||||
/*
|
||||
* 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 System.Threading.Channels;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.D2D;
|
||||
|
||||
/// <summary>
|
||||
/// The first active submap will be created on the insertion of the first range
|
||||
/// data. Except during this initialization when no or only one single submap
|
||||
/// exists, there are always two submaps into which range data is inserted: an
|
||||
/// old submap that is used for matching, and a new one, which will be used for
|
||||
/// matching next, that is being initialized.
|
||||
///
|
||||
/// Once a certain number of range data have been inserted, the new submap is
|
||||
/// considered initialized: the old submap is no longer changed, the "new" submap
|
||||
/// is now the "old" submap and is used for scan-to-map matching. Moreover, a
|
||||
/// "new" submap gets created. The "old" submap is forgotten by this object.
|
||||
///
|
||||
/// The front (old) submap is always inserted synchronously since it is used for
|
||||
/// scan matching immediately. The back (new) submap is inserted asynchronously
|
||||
/// via a background worker to reduce per-frame blocking latency.
|
||||
/// </summary>
|
||||
public class ActiveSubmaps2D(SubmapsOptions2D options) : IDisposable
|
||||
{
|
||||
private const int kInitialSubmapSize = 100;
|
||||
|
||||
private readonly SubmapsOptions2D _options = options;
|
||||
private readonly List<Submap2D> _submaps = [];
|
||||
private readonly ValueConversionTables _conversionTables = new();
|
||||
private IRangeDataInserter? _rangeDataInserter;
|
||||
private readonly Lock _insertLock = new(); // Lock to prevent concurrent inserts
|
||||
|
||||
// Async back submap insertion infrastructure.
|
||||
// The back submap is queued to a Channel and processed by a single long-running
|
||||
// worker task, preserving insertion order (SingleReader). The front submap is
|
||||
// always inserted synchronously for immediate scan matching availability.
|
||||
private readonly Channel<(RangeData rangeData, Submap2D submap, IRangeDataInserter inserter)>
|
||||
_backSubmapChannel = Channel.CreateUnbounded<(RangeData, Submap2D, IRangeDataInserter)>(
|
||||
new UnboundedChannelOptions { SingleReader = true });
|
||||
private Task? _backSubmapWorker;
|
||||
private int _backSubmapExpectedCount; // Tracks intended NumRangeData of back submap (including queued)
|
||||
private int _backSubmapPendingCount; // Items queued but not yet processed (Interlocked)
|
||||
|
||||
/// <summary>
|
||||
/// Inserts 'range_data' into the Submap collection.
|
||||
/// Front submap: synchronous (used for scan matching immediately).
|
||||
/// Back submap: queued to background worker (fire-and-forget).
|
||||
/// </summary>
|
||||
public List<Submap2D> InsertRangeData(RangeData rangeData)
|
||||
{
|
||||
lock (_insertLock)
|
||||
{
|
||||
var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
|
||||
var submapPose = new Rigid3d(
|
||||
new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
|
||||
Quaternion.Identity);
|
||||
|
||||
// Use _backSubmapExpectedCount instead of back submap's NumRangeData
|
||||
// because the back submap's actual count may lag (async insertions).
|
||||
if (_submaps.Count == 0 ||
|
||||
(_submaps.Count > 0 && _backSubmapExpectedCount == _options.NumRangeData))
|
||||
{
|
||||
// Drain all pending back submap insertions before submap rotation
|
||||
// so the back submap is fully up-to-date when it becomes the front.
|
||||
DrainBackSubmapQueue();
|
||||
AddSubmap(submapPose);
|
||||
_backSubmapExpectedCount = 0;
|
||||
}
|
||||
|
||||
_rangeDataInserter ??= CreateRangeDataInserter();
|
||||
|
||||
if (_submaps.Count >= 2)
|
||||
{
|
||||
// Front submap: SYNCHRONOUS (used for scan matching immediately)
|
||||
_submaps[0].InsertRangeData(rangeData, _rangeDataInserter);
|
||||
|
||||
// Back submap: ASYNC (queue to background worker)
|
||||
// Increment pending BEFORE writing to channel to ensure drain correctness.
|
||||
Interlocked.Increment(ref _backSubmapPendingCount);
|
||||
_backSubmapChannel.Writer.TryWrite((rangeData, _submaps[1], _rangeDataInserter));
|
||||
_backSubmapWorker ??= Task.Factory.StartNew(
|
||||
ProcessBackSubmapQueue, TaskCreationOptions.LongRunning);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Only 1 submap - insert synchronously
|
||||
for (int si = 0; si < _submaps.Count; si++)
|
||||
{
|
||||
_submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
|
||||
}
|
||||
}
|
||||
_backSubmapExpectedCount++;
|
||||
|
||||
// Finish front submap when it reaches 2x threshold.
|
||||
// Front is always up-to-date (synchronous insert).
|
||||
if (_submaps.Count > 0 && _submaps[0].NumRangeData == _options.NumRangeData * 2)
|
||||
{
|
||||
_submaps[0].Finish();
|
||||
}
|
||||
|
||||
return [.. _submaps];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Forces the current front submap to finish, creates a new submap at the range data origin,
|
||||
/// and inserts range data into all active submaps. Used to break the deadlock when the robot
|
||||
/// enters genuinely new territory and consecutive hard-limit Ceres failures accumulate.
|
||||
/// </summary>
|
||||
public List<Submap2D> ForceNewSubmapAndInsert(RangeData rangeData)
|
||||
{
|
||||
lock (_insertLock)
|
||||
{
|
||||
// Drain any pending back submap insertions before manipulating submaps.
|
||||
DrainBackSubmapQueue();
|
||||
|
||||
var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
|
||||
var submapPose = new Rigid3d(
|
||||
new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
|
||||
Quaternion.Identity);
|
||||
|
||||
if (_submaps.Count == 0)
|
||||
{
|
||||
// No submaps yet - just create the first one via normal flow
|
||||
AddSubmap(submapPose);
|
||||
}
|
||||
else if (_submaps.Count == 1)
|
||||
{
|
||||
// FIX: When only 1 submap exists, finish it and REMOVE it before creating the new one.
|
||||
// Previously, finishing + AddSubmap would leave [finished, new] and the subsequent
|
||||
// InsertRangeData loop would crash on the finished front submap.
|
||||
if (!_submaps[0].InsertionFinished)
|
||||
{
|
||||
_submaps[0].Finish();
|
||||
}
|
||||
_submaps.RemoveAt(0);
|
||||
AddSubmap(submapPose);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 2 submaps: finish front if needed, then AddSubmap removes it and creates new
|
||||
if (!_submaps[0].InsertionFinished)
|
||||
{
|
||||
_submaps[0].Finish();
|
||||
}
|
||||
AddSubmap(submapPose);
|
||||
}
|
||||
|
||||
// Reset expected count after submap manipulation.
|
||||
_backSubmapExpectedCount = 0;
|
||||
|
||||
// Insert range data into all active submaps sequentially.
|
||||
// ForceNewSubmap is a rare recovery path (consecutive Ceres failures),
|
||||
// so sequential insert is simpler and avoids thread-safety risks.
|
||||
_rangeDataInserter ??= CreateRangeDataInserter();
|
||||
|
||||
for (int si = 0; si < _submaps.Count; si++)
|
||||
{
|
||||
_submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
|
||||
}
|
||||
_backSubmapExpectedCount++;
|
||||
|
||||
return [.. _submaps];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current active submaps.
|
||||
/// </summary>
|
||||
public List<Submap2D> Submaps()
|
||||
{
|
||||
return [.. _submaps];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Background worker that sequentially processes queued back submap insertions.
|
||||
/// Uses per-item error handling for resilience - a single failed insertion
|
||||
/// should not crash the entire SLAM pipeline.
|
||||
/// </summary>
|
||||
private async Task ProcessBackSubmapQueue()
|
||||
{
|
||||
await foreach (var (rangeData, submap, inserter) in _backSubmapChannel.Reader.ReadAllAsync())
|
||||
{
|
||||
try
|
||||
{
|
||||
submap.InsertRangeData(rangeData, inserter);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"[SUBMAP_ASYNC] Back submap insert error: {ex.Message}");
|
||||
}
|
||||
Interlocked.Decrement(ref _backSubmapPendingCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Waits for all pending back submap insertions to complete.
|
||||
/// Called before submap rotation (AddSubmap) to ensure the back submap is
|
||||
/// fully up-to-date before it becomes the front submap used for scan matching.
|
||||
/// Called infrequently (every NumRangeData frames, typically ~90).
|
||||
/// </summary>
|
||||
private void DrainBackSubmapQueue()
|
||||
{
|
||||
if (Volatile.Read(ref _backSubmapPendingCount) == 0) return;
|
||||
var sw = new SpinWait();
|
||||
while (Volatile.Read(ref _backSubmapPendingCount) > 0)
|
||||
{
|
||||
sw.SpinOnce();
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
_backSubmapChannel.Writer.Complete();
|
||||
_backSubmapWorker?.GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
private IRangeDataInserter CreateRangeDataInserter()
|
||||
{
|
||||
// Match C++ logic: switch case with LOG(FATAL) for unknown types
|
||||
var options = _options.RangeDataInserterOptions;
|
||||
|
||||
switch (options.RangeDataInserterTypeValue)
|
||||
{
|
||||
case RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D:
|
||||
if (options.ProbabilityGridRangeDataInserterOptions2D.HasValue)
|
||||
{
|
||||
return new ProbabilityGridRangeDataInserter2D(options.ProbabilityGridRangeDataInserterOptions2D.Value);
|
||||
}
|
||||
throw new ArgumentException("ProbabilityGridRangeDataInserterOptions2D is required for ProbabilityGrid inserter");
|
||||
|
||||
case RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D:
|
||||
if (options.TsdfRangeDataInserterOptions2D.HasValue)
|
||||
{
|
||||
return new TSDFRangeDataInserter2D(options.TsdfRangeDataInserterOptions2D.Value);
|
||||
}
|
||||
throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF inserter");
|
||||
|
||||
default:
|
||||
throw new ArgumentException($"Unknown RangeDataInserterType: {options.RangeDataInserterTypeValue}");
|
||||
}
|
||||
}
|
||||
|
||||
private Grid2D CreateGrid(Vector2 origin)
|
||||
{
|
||||
return CreateGridWithOptions(origin, _options.GridOptions2D);
|
||||
}
|
||||
|
||||
private Grid2D? CreateHighResGrid(Vector2 origin)
|
||||
{
|
||||
if (_options.HighResGridOptions2D == null)
|
||||
return null;
|
||||
return CreateGridWithOptions(origin, _options.HighResGridOptions2D.Value);
|
||||
}
|
||||
|
||||
private Grid2D CreateGridWithOptions(Vector2 origin, GridOptions2D gridOptions)
|
||||
{
|
||||
var resolution = gridOptions.Resolution;
|
||||
// Calculate initial size to cover ±18m (matching MaxRange) at this resolution
|
||||
var initialSize = Math.Max(100, (int)(kInitialSubmapSize * _options.GridOptions2D.Resolution / resolution));
|
||||
var mapLimits = new MapLimits(
|
||||
resolution,
|
||||
new Vector2(
|
||||
origin.X + 0.5 * initialSize * resolution,
|
||||
origin.Y + 0.5 * initialSize * resolution
|
||||
),
|
||||
new CellLimits(initialSize, initialSize)
|
||||
);
|
||||
|
||||
// Match C++ logic: switch case with LOG(FATAL) for unknown types
|
||||
switch (gridOptions.GridTypeValue)
|
||||
{
|
||||
case GridOptions2D.GridType.ProbabilityGrid:
|
||||
return new ProbabilityGrid(mapLimits, _conversionTables);
|
||||
|
||||
case GridOptions2D.GridType.Tsdf:
|
||||
// Match C++: Get truncation_distance and maximum_weight from range_data_inserter_options
|
||||
// C++: options_.range_data_inserter_options().tsdf_range_data_inserter_options_2d()
|
||||
if (_options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.HasValue)
|
||||
{
|
||||
var tsdfOptions = _options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.Value;
|
||||
return new TSDF2D(
|
||||
mapLimits,
|
||||
tsdfOptions.TruncationDistance,
|
||||
tsdfOptions.MaximumWeight,
|
||||
_conversionTables
|
||||
);
|
||||
}
|
||||
throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF grid type");
|
||||
|
||||
case GridOptions2D.GridType.InvalidGrid:
|
||||
throw new ArgumentException("Invalid grid type specified");
|
||||
|
||||
default:
|
||||
throw new ArgumentException($"Unknown grid type: {gridOptions.GridTypeValue}");
|
||||
}
|
||||
}
|
||||
|
||||
private void AddSubmap(Rigid3d localSubmapPose)
|
||||
{
|
||||
// Match C++ logic: if (submaps_.size() >= 2) { CHECK(submaps_.front()->insertion_finished()); submaps_.erase(submaps_.begin()); }
|
||||
if (_submaps.Count >= 2)
|
||||
{
|
||||
// This will crop the finished Submap before inserting a new Submap to
|
||||
// reduce peak memory usage a bit.
|
||||
if (!_submaps[0].InsertionFinished)
|
||||
{
|
||||
throw new InvalidOperationException("First submap must be finished before adding a new one");
|
||||
}
|
||||
_submaps.RemoveAt(0);
|
||||
}
|
||||
|
||||
// Match C++: Extract origin from pose (C++ passes Vector2f directly, but we have Rigid3d)
|
||||
var origin = new Vector2(localSubmapPose.Translation.X, localSubmapPose.Translation.Y);
|
||||
var grid = CreateGrid(origin);
|
||||
var highResGrid = CreateHighResGrid(origin);
|
||||
|
||||
// Match C++: Submap2D(origin, grid, conversion_tables)
|
||||
// C++ constructor takes Vector2f origin, but C# Submap2D takes Rigid3d (which includes origin)
|
||||
var submap = new Submap2D(
|
||||
localSubmapPose, // Pass the full Rigid3d pose including rotation
|
||||
grid,
|
||||
_conversionTables,
|
||||
highResGrid
|
||||
);
|
||||
_submaps.Add(submap);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user