Initial commit
This commit is contained in:
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
* 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.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// Options for 3D submaps.
|
||||
/// </summary>
|
||||
public struct SubmapsOptions3D(
|
||||
double highResolution,
|
||||
double highResolutionMaxRange,
|
||||
double lowResolution,
|
||||
int numRangeData,
|
||||
RangeDataInserterOptions3D rangeDataInserterOptions)
|
||||
{
|
||||
public double HighResolution { get; set; } = highResolution;
|
||||
public double HighResolutionMaxRange { get; set; } = highResolutionMaxRange;
|
||||
public double LowResolution { get; set; } = lowResolution;
|
||||
public int NumRangeData { get; set; } = numRangeData;
|
||||
public RangeDataInserterOptions3D RangeDataInserterOptions { get; set; } = rangeDataInserterOptions;
|
||||
}
|
||||
|
||||
/// <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.
|
||||
/// </summary>
|
||||
public class ActiveSubmaps3D
|
||||
{
|
||||
private readonly SubmapsOptions3D _options;
|
||||
private readonly List<Submap3D> _submaps = [];
|
||||
private readonly RangeDataInserter3D _rangeDataInserter;
|
||||
|
||||
public ActiveSubmaps3D(SubmapsOptions3D options)
|
||||
{
|
||||
if (options.NumRangeData <= 0)
|
||||
{
|
||||
throw new ArgumentException("num_range_data must be greater than 0", nameof(options));
|
||||
}
|
||||
|
||||
_options = options;
|
||||
_rangeDataInserter = new RangeDataInserter3D(options.RangeDataInserterOptions);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Inserts 'range_data_in_local' into the Submap collection.
|
||||
/// 'local_from_gravity_aligned' is used for the orientation of new submaps so
|
||||
/// that the z axis approximately aligns with gravity.
|
||||
/// 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
|
||||
/// submaps of the Submap collection.
|
||||
/// </summary>
|
||||
public List<Submap3D> InsertData(
|
||||
RangeData rangeDataInLocal,
|
||||
Quaternion localFromGravityAligned,
|
||||
List<double> rotationalScanMatcherHistogramInGravity)
|
||||
{
|
||||
// Create new submap if needed
|
||||
if (_submaps.Count == 0 ||
|
||||
_submaps[^1].NumRangeData == _options.NumRangeData)
|
||||
{
|
||||
var localSubmapPose = new Rigid3d(
|
||||
(Vector3)rangeDataInLocal.Origin,
|
||||
localFromGravityAligned);
|
||||
AddSubmap(localSubmapPose, rotationalScanMatcherHistogramInGravity.Count);
|
||||
}
|
||||
|
||||
// Insert into all active submaps
|
||||
foreach (var submap in _submaps)
|
||||
{
|
||||
submap.InsertData(
|
||||
rangeDataInLocal,
|
||||
_rangeDataInserter,
|
||||
_options.HighResolutionMaxRange,
|
||||
localFromGravityAligned,
|
||||
rotationalScanMatcherHistogramInGravity);
|
||||
}
|
||||
|
||||
// Finish the first submap if it has reached 2 * num_range_data
|
||||
if (_submaps.Count > 0 && _submaps[0].NumRangeData == 2 * _options.NumRangeData)
|
||||
{
|
||||
_submaps[0].Finish();
|
||||
}
|
||||
|
||||
return [.. _submaps];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current active submaps.
|
||||
/// </summary>
|
||||
public List<Submap3D> Submaps()
|
||||
{
|
||||
return [.. _submaps];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds a new submap to the collection.
|
||||
/// </summary>
|
||||
private void AddSubmap(Rigid3d localSubmapPose, int rotationalScanMatcherHistogramSize)
|
||||
{
|
||||
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");
|
||||
}
|
||||
|
||||
// We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use
|
||||
// active submaps and their associated intensity hybrid grids for scan
|
||||
// matching, we call `ForgetIntensityHybridGrid` once we remove the submap
|
||||
// from active submaps and no longer need the intensity hybrid grid.
|
||||
_submaps[0].ForgetIntensityHybridGrid();
|
||||
_submaps.RemoveAt(0);
|
||||
}
|
||||
|
||||
var initialRotationalScanMatcherHistogram = new List<double>(rotationalScanMatcherHistogramSize);
|
||||
for (int i = 0; i < rotationalScanMatcherHistogramSize; i++)
|
||||
{
|
||||
initialRotationalScanMatcherHistogram.Add(0.0);
|
||||
}
|
||||
|
||||
var submap = new Submap3D(
|
||||
_options.HighResolution,
|
||||
_options.LowResolution,
|
||||
localSubmapPose,
|
||||
initialRotationalScanMatcherHistogram);
|
||||
|
||||
_submaps.Add(submap);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,848 @@
|
||||
/*
|
||||
* 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.Math;
|
||||
using System.Collections;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// Utility functions for HybridGrid indexing.
|
||||
/// </summary>
|
||||
internal static class HybridGridUtils
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat z-major index.
|
||||
/// </summary>
|
||||
public static int ToFlatIndex(Array3i index, int bits)
|
||||
{
|
||||
if (index.X < 0 || index.Y < 0 || index.Z < 0 ||
|
||||
index.X >= (1 << bits) || index.Y >= (1 << bits) || index.Z >= (1 << bits))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(index),
|
||||
$"Index {index} is out of range for bits={bits}");
|
||||
}
|
||||
return (((index.Z << bits) + index.Y) << bits) + index.X;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts a flat z-major 'index' to a 3-dimensional index with each dimension
|
||||
/// from 0 to 2^'bits' - 1.
|
||||
/// </summary>
|
||||
public static Array3i To3DIndex(int index, int bits)
|
||||
{
|
||||
if (index < 0 || index >= (1 << (3 * bits)))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(index),
|
||||
$"Index {index} is out of range for bits={bits}");
|
||||
}
|
||||
int mask = (1 << bits) - 1;
|
||||
return new Array3i(
|
||||
index & mask,
|
||||
(index >> bits) & mask,
|
||||
(index >> bits) >> bits);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Checks if a value is the default value.
|
||||
/// </summary>
|
||||
public static bool IsDefaultValue<T>(T value) where T : struct
|
||||
{
|
||||
return EqualityComparer<T>.Default.Equals(value, default);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Checks if a list is empty (default value for collections).
|
||||
/// </summary>
|
||||
public static bool IsDefaultValue<T>(List<T>? value)
|
||||
{
|
||||
return value == null || value.Count == 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of
|
||||
/// type 'TValueType' in contiguous memory. Indices in each dimension are 0-based.
|
||||
/// </summary>
|
||||
internal class FlatGrid<TValueType> where TValueType : struct
|
||||
{
|
||||
private const int kBits = 3; // Fixed at 3 bits = 8x8x8 = 512 cells
|
||||
private const int kGridSize = 1 << kBits; // 8
|
||||
private const int kTotalCells = 1 << (3 * kBits); // 512
|
||||
|
||||
private readonly TValueType[] _cells;
|
||||
|
||||
public FlatGrid()
|
||||
{
|
||||
_cells = new TValueType[kTotalCells];
|
||||
// Values are already default-initialized
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the number of voxels per dimension.
|
||||
/// </summary>
|
||||
public static int GridSize => kGridSize;
|
||||
|
||||
/// <summary>
|
||||
/// Returns the value stored at 'index', each dimension of 'index' being
|
||||
/// between 0 and grid_size() - 1.
|
||||
/// </summary>
|
||||
public TValueType GetValue(Array3i index)
|
||||
{
|
||||
return _cells[HybridGridUtils.ToFlatIndex(index, kBits)];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns a reference to the value at 'index' to allow changing it.
|
||||
/// </summary>
|
||||
public ref TValueType GetMutableValue(Array3i index)
|
||||
{
|
||||
return ref _cells[HybridGridUtils.ToFlatIndex(index, kBits)];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Iterator for iterating over all values not comparing equal to the default constructed value.
|
||||
/// </summary>
|
||||
public class Iterator : IEnumerator<(Array3i Index, TValueType Value)>
|
||||
{
|
||||
private readonly FlatGrid<TValueType> _grid;
|
||||
private int _currentIndex;
|
||||
private (Array3i Index, TValueType Value)? _current;
|
||||
|
||||
public Iterator(FlatGrid<TValueType> grid)
|
||||
{
|
||||
_grid = grid;
|
||||
_currentIndex = -1;
|
||||
MoveNext();
|
||||
}
|
||||
|
||||
public bool MoveNext()
|
||||
{
|
||||
_currentIndex++;
|
||||
while (_currentIndex < _grid._cells.Length)
|
||||
{
|
||||
var value = _grid._cells[_currentIndex];
|
||||
if (!HybridGridUtils.IsDefaultValue(value))
|
||||
{
|
||||
var index = HybridGridUtils.To3DIndex(_currentIndex, kBits);
|
||||
_current = (index, value);
|
||||
return true;
|
||||
}
|
||||
_currentIndex++;
|
||||
}
|
||||
_current = null;
|
||||
return false;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
_currentIndex = -1;
|
||||
_current = null;
|
||||
}
|
||||
|
||||
public (Array3i Index, TValueType Value) Current => _current!.Value;
|
||||
|
||||
object IEnumerator.Current => Current;
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets an enumerator for all non-default values.
|
||||
/// </summary>
|
||||
public Iterator GetEnumerator()
|
||||
{
|
||||
return new Iterator(this);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type 'FlatGrid'.
|
||||
/// Wrapped grids are constructed on first access via 'GetMutableValue()'.
|
||||
/// This is a concrete implementation for the specific case: NestedGrid<FlatGrid<TValueType>, 3>
|
||||
/// </summary>
|
||||
internal class NestedGrid<TValueType> where TValueType : struct
|
||||
{
|
||||
private const int kBits = 3; // Fixed at 3 bits = 8x8x8 = 512 meta cells
|
||||
private const int kWrappedGridSize = 8; // FlatGrid<TValueType>.GridSize = 8
|
||||
private readonly FlatGrid<TValueType>?[] _metaCells;
|
||||
|
||||
public NestedGrid()
|
||||
{
|
||||
_metaCells = new FlatGrid<TValueType>?[1 << (3 * kBits)]; // 512
|
||||
}
|
||||
|
||||
public static int GridSize => kWrappedGridSize << kBits; // 8 * 8 = 64
|
||||
|
||||
public TValueType GetValue(Array3i index)
|
||||
{
|
||||
var metaIndex = NestedGrid<TValueType>.GetMetaIndex(index);
|
||||
var metaCell = _metaCells[HybridGridUtils.ToFlatIndex(metaIndex, kBits)];
|
||||
if (metaCell == null)
|
||||
{
|
||||
return default;
|
||||
}
|
||||
var innerIndex = index - metaIndex * kWrappedGridSize;
|
||||
return metaCell.GetValue(innerIndex);
|
||||
}
|
||||
|
||||
public ref TValueType GetMutableValue(Array3i index)
|
||||
{
|
||||
var metaIndex = NestedGrid<TValueType>.GetMetaIndex(index);
|
||||
var flatIndex = HybridGridUtils.ToFlatIndex(metaIndex, kBits);
|
||||
if (_metaCells[flatIndex] == null)
|
||||
{
|
||||
_metaCells[flatIndex] = new FlatGrid<TValueType>();
|
||||
}
|
||||
var innerIndex = index - metaIndex * kWrappedGridSize;
|
||||
return ref _metaCells[flatIndex]!.GetMutableValue(innerIndex);
|
||||
}
|
||||
|
||||
public IEnumerator<(Array3i Index, TValueType Value)> GetEnumerator()
|
||||
{
|
||||
return new Iterator(this);
|
||||
}
|
||||
|
||||
private static Array3i GetMetaIndex(Array3i index)
|
||||
{
|
||||
if (index.X < 0 || index.Y < 0 || index.Z < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(index), $"Index {index} has negative components");
|
||||
}
|
||||
var metaIndex = index / kWrappedGridSize;
|
||||
if (metaIndex.X >= (1 << kBits) || metaIndex.Y >= (1 << kBits) || metaIndex.Z >= (1 << kBits))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(index), $"Meta index {metaIndex} is out of range");
|
||||
}
|
||||
return metaIndex;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Iterator for iterating over all non-default values.
|
||||
/// </summary>
|
||||
public class Iterator : IEnumerator<(Array3i Index, TValueType Value)>
|
||||
{
|
||||
private readonly NestedGrid<TValueType> _grid;
|
||||
private int _currentMetaIndex;
|
||||
private FlatGrid<TValueType>.Iterator? _nestedIterator;
|
||||
private (Array3i Index, TValueType Value)? _current;
|
||||
|
||||
public Iterator(NestedGrid<TValueType> grid)
|
||||
{
|
||||
_grid = grid;
|
||||
_currentMetaIndex = -1;
|
||||
AdvanceToValidNestedIterator();
|
||||
}
|
||||
|
||||
private void AdvanceToValidNestedIterator()
|
||||
{
|
||||
while (_currentMetaIndex < _grid._metaCells.Length - 1)
|
||||
{
|
||||
_currentMetaIndex++;
|
||||
if (_currentMetaIndex >= _grid._metaCells.Length)
|
||||
{
|
||||
_nestedIterator = null;
|
||||
_current = null;
|
||||
return;
|
||||
}
|
||||
|
||||
var metaCell = _grid._metaCells[_currentMetaIndex];
|
||||
if (metaCell != null)
|
||||
{
|
||||
_nestedIterator = metaCell.GetEnumerator();
|
||||
if (_nestedIterator.MoveNext())
|
||||
{
|
||||
var (innerIndex, value) = _nestedIterator.Current;
|
||||
var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, kBits);
|
||||
var fullIndex = metaIndex * kWrappedGridSize + innerIndex;
|
||||
_current = (fullIndex, value);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
_nestedIterator = null;
|
||||
_current = null;
|
||||
}
|
||||
|
||||
public bool MoveNext()
|
||||
{
|
||||
if (_nestedIterator != null && _nestedIterator.MoveNext())
|
||||
{
|
||||
var (innerIndex, value) = _nestedIterator.Current;
|
||||
var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, kBits);
|
||||
var fullIndex = metaIndex * kWrappedGridSize + innerIndex;
|
||||
_current = (fullIndex, value);
|
||||
return true;
|
||||
}
|
||||
|
||||
AdvanceToValidNestedIterator();
|
||||
return _current.HasValue;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
_currentMetaIndex = -1;
|
||||
_nestedIterator = null;
|
||||
_current = null;
|
||||
AdvanceToValidNestedIterator();
|
||||
}
|
||||
|
||||
public (Array3i Index, TValueType Value) Current => _current!.Value;
|
||||
|
||||
object IEnumerator.Current => Current;
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
_nestedIterator?.Dispose();
|
||||
_current = null;
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A grid consisting of 2x2x2 grids of type 'NestedGrid' initially. Wrapped grids
|
||||
/// are constructed on first access via 'GetMutableValue()'. If necessary, the grid
|
||||
/// grows to twice the size in each dimension. The range of indices is (almost)
|
||||
/// symmetric around the origin, i.e. negative indices are allowed.
|
||||
/// </summary>
|
||||
internal class DynamicGrid<TValueType> where TValueType : struct
|
||||
{
|
||||
private const int kWrappedGridSize = 64; // NestedGrid<TValueType>.GridSize = 64
|
||||
private int _bits; // Starts at 1 (2x2x2 = 8 meta cells)
|
||||
private NestedGrid<TValueType>?[] _metaCells;
|
||||
|
||||
public DynamicGrid()
|
||||
{
|
||||
_bits = 1;
|
||||
_metaCells = new NestedGrid<TValueType>?[8]; // 2^3 = 8
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the current number of voxels per dimension.
|
||||
/// </summary>
|
||||
public int GridSize => kWrappedGridSize << _bits;
|
||||
|
||||
/// <summary>
|
||||
/// Returns the value stored at 'index'.
|
||||
/// </summary>
|
||||
public TValueType GetValue(Array3i index)
|
||||
{
|
||||
var shiftedIndex = index + new Array3i(GridSize >> 1, GridSize >> 1, GridSize >> 1);
|
||||
|
||||
// Check bounds using unsigned comparison for performance
|
||||
if (shiftedIndex.X < 0 || shiftedIndex.Y < 0 || shiftedIndex.Z < 0 ||
|
||||
shiftedIndex.X >= GridSize || shiftedIndex.Y >= GridSize || shiftedIndex.Z >= GridSize)
|
||||
{
|
||||
return default;
|
||||
}
|
||||
|
||||
var metaIndex = GetMetaIndex(shiftedIndex);
|
||||
var metaCell = _metaCells[HybridGridUtils.ToFlatIndex(metaIndex, _bits)];
|
||||
if (metaCell == null)
|
||||
{
|
||||
return default;
|
||||
}
|
||||
var innerIndex = shiftedIndex - metaIndex * kWrappedGridSize;
|
||||
return metaCell.GetValue(innerIndex);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns a reference to the value at 'index' to allow changing it, dynamically
|
||||
/// growing the DynamicGrid and constructing new NestedGrids as needed.
|
||||
/// </summary>
|
||||
public ref TValueType GetMutableValue(Array3i index)
|
||||
{
|
||||
var shiftedIndex = index + new Array3i(GridSize >> 1, GridSize >> 1, GridSize >> 1);
|
||||
|
||||
// Check bounds using unsigned comparison for performance
|
||||
if (shiftedIndex.X < 0 || shiftedIndex.Y < 0 || shiftedIndex.Z < 0 ||
|
||||
shiftedIndex.X >= GridSize || shiftedIndex.Y >= GridSize || shiftedIndex.Z >= GridSize)
|
||||
{
|
||||
// SAFEGUARD: Store old bits to detect if Grow() actually increased size
|
||||
var oldBits = _bits;
|
||||
|
||||
// Grow the grid
|
||||
Grow();
|
||||
|
||||
// SAFEGUARD: Check if Grow() actually increased size (prevent infinite recursion)
|
||||
if (_bits == oldBits)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Cannot grow grid further. Index {index} is out of bounds even after grow attempt. " +
|
||||
$"Current bits={_bits}, GridSize={GridSize}, shiftedIndex={shiftedIndex}");
|
||||
}
|
||||
|
||||
// SAFEGUARD: Recalculate shiftedIndex after grow and check bounds again
|
||||
shiftedIndex = index + new Array3i(GridSize >> 1, GridSize >> 1, GridSize >> 1);
|
||||
|
||||
// SAFEGUARD: If still out of bounds after grow, throw exception instead of infinite recursion
|
||||
if (shiftedIndex.X < 0 || shiftedIndex.Y < 0 || shiftedIndex.Z < 0 ||
|
||||
shiftedIndex.X >= GridSize || shiftedIndex.Y >= GridSize || shiftedIndex.Z >= GridSize)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(index),
|
||||
$"Index {index} is out of bounds even after growing grid to maximum size. " +
|
||||
$"GridSize={GridSize}, shiftedIndex={shiftedIndex}, bits={_bits}");
|
||||
}
|
||||
|
||||
return ref GetMutableValue(index); // Recursive call after grow (now safe)
|
||||
}
|
||||
|
||||
var metaIndex = GetMetaIndex(shiftedIndex);
|
||||
var flatIndex = HybridGridUtils.ToFlatIndex(metaIndex, _bits);
|
||||
if (_metaCells[flatIndex] == null)
|
||||
{
|
||||
_metaCells[flatIndex] = new NestedGrid<TValueType>();
|
||||
}
|
||||
var innerIndex = shiftedIndex - metaIndex * kWrappedGridSize;
|
||||
return ref _metaCells[flatIndex]!.GetMutableValue(innerIndex);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Iterator for iterating over all values not comparing equal to the default constructed value.
|
||||
/// </summary>
|
||||
public class Iterator : IEnumerator<(Array3i Index, TValueType Value)>
|
||||
{
|
||||
private readonly DynamicGrid<TValueType> _grid;
|
||||
private readonly int _bits;
|
||||
private int _currentMetaIndex;
|
||||
private IEnumerator<(Array3i Index, TValueType Value)>? _nestedIterator;
|
||||
private (Array3i Index, TValueType Value)? _current;
|
||||
|
||||
public Iterator(DynamicGrid<TValueType> grid)
|
||||
{
|
||||
_grid = grid;
|
||||
_bits = grid._bits;
|
||||
_currentMetaIndex = -1;
|
||||
AdvanceToValidNestedIterator();
|
||||
}
|
||||
|
||||
private void AdvanceToValidNestedIterator()
|
||||
{
|
||||
while (_currentMetaIndex < _grid._metaCells.Length - 1)
|
||||
{
|
||||
_currentMetaIndex++;
|
||||
if (_currentMetaIndex >= _grid._metaCells.Length)
|
||||
{
|
||||
_nestedIterator = null;
|
||||
_current = null;
|
||||
return;
|
||||
}
|
||||
|
||||
var metaCell = _grid._metaCells[_currentMetaIndex];
|
||||
if (metaCell != null)
|
||||
{
|
||||
_nestedIterator = metaCell.GetEnumerator();
|
||||
if (_nestedIterator.MoveNext())
|
||||
{
|
||||
var (innerIndex, value) = _nestedIterator.Current;
|
||||
var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, _bits);
|
||||
var shiftedIndex = metaIndex * kWrappedGridSize + innerIndex;
|
||||
var originalIndex = shiftedIndex - new Array3i(
|
||||
(1 << (_bits - 1)) * kWrappedGridSize,
|
||||
(1 << (_bits - 1)) * kWrappedGridSize,
|
||||
(1 << (_bits - 1)) * kWrappedGridSize);
|
||||
_current = (originalIndex, value);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
_nestedIterator = null;
|
||||
_current = null;
|
||||
}
|
||||
|
||||
public bool MoveNext()
|
||||
{
|
||||
if (_nestedIterator != null && _nestedIterator.MoveNext())
|
||||
{
|
||||
var (innerIndex, value) = _nestedIterator.Current;
|
||||
var metaIndex = HybridGridUtils.To3DIndex(_currentMetaIndex, _bits);
|
||||
var shiftedIndex = metaIndex * kWrappedGridSize + innerIndex;
|
||||
var originalIndex = shiftedIndex - new Array3i(
|
||||
(1 << (_bits - 1)) * kWrappedGridSize,
|
||||
(1 << (_bits - 1)) * kWrappedGridSize,
|
||||
(1 << (_bits - 1)) * kWrappedGridSize);
|
||||
_current = (originalIndex, value);
|
||||
return true;
|
||||
}
|
||||
|
||||
AdvanceToValidNestedIterator();
|
||||
return _current.HasValue;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
_currentMetaIndex = -1;
|
||||
_nestedIterator = null;
|
||||
_current = null;
|
||||
AdvanceToValidNestedIterator();
|
||||
}
|
||||
|
||||
public (Array3i Index, TValueType Value) Current => _current!.Value;
|
||||
|
||||
object IEnumerator.Current => Current;
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
_nestedIterator?.Dispose();
|
||||
_current = null;
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Advances iterator to end (for end() implementation).
|
||||
/// </summary>
|
||||
public void AdvanceToEnd()
|
||||
{
|
||||
_currentMetaIndex = _grid._metaCells.Length;
|
||||
_nestedIterator = null;
|
||||
_current = null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets an enumerator for all non-default values.
|
||||
/// </summary>
|
||||
public Iterator GetEnumerator()
|
||||
{
|
||||
return new Iterator(this);
|
||||
}
|
||||
|
||||
private Array3i GetMetaIndex(Array3i index)
|
||||
{
|
||||
if (index.X < 0 || index.Y < 0 || index.Z < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(index), $"Index {index} has negative components");
|
||||
}
|
||||
var metaIndex = index / kWrappedGridSize;
|
||||
if (metaIndex.X >= (1 << _bits) || metaIndex.Y >= (1 << _bits) || metaIndex.Z >= (1 << _bits))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(index), $"Meta index {metaIndex} is out of range");
|
||||
}
|
||||
return metaIndex;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Grows this grid by a factor of 2 in each of the 3 dimensions.
|
||||
/// </summary>
|
||||
private void Grow()
|
||||
{
|
||||
var newBits = _bits + 1;
|
||||
if (newBits > 8)
|
||||
{
|
||||
throw new InvalidOperationException($"Cannot grow grid beyond bits=8 (current bits={_bits})");
|
||||
}
|
||||
|
||||
var newMetaCells = new NestedGrid<TValueType>?[8 * _metaCells.Length];
|
||||
|
||||
for (int z = 0; z < (1 << _bits); z++)
|
||||
{
|
||||
for (int y = 0; y < (1 << _bits); y++)
|
||||
{
|
||||
for (int x = 0; x < (1 << _bits); x++)
|
||||
{
|
||||
var originalMetaIndex = new Array3i(x, y, z);
|
||||
var newMetaIndex = originalMetaIndex + new Array3i(1 << (_bits - 1), 1 << (_bits - 1), 1 << (_bits - 1));
|
||||
var originalFlatIndex = HybridGridUtils.ToFlatIndex(originalMetaIndex, _bits);
|
||||
var newFlatIndex = HybridGridUtils.ToFlatIndex(newMetaIndex, newBits);
|
||||
newMetaCells[newFlatIndex] = _metaCells[originalFlatIndex];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_metaCells = newMetaCells;
|
||||
_bits = newBits;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Represents a 3D grid as a wide, shallow tree.
|
||||
/// This is the base class for HybridGrid and IntensityHybridGrid.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a new tree-based grid with voxels having edge length 'resolution'
|
||||
/// around the origin which becomes the center of the cell at index (0, 0, 0).
|
||||
/// </remarks>
|
||||
public class HybridGridBase<TValueType>(double resolution) where TValueType : struct
|
||||
{
|
||||
private readonly DynamicGrid<TValueType> _grid = new();
|
||||
|
||||
/// <summary>
|
||||
/// Returns the resolution (edge length of each voxel).
|
||||
/// </summary>
|
||||
public double Resolution => resolution;
|
||||
|
||||
/// <summary>
|
||||
/// Returns the value stored at 'index'.
|
||||
/// </summary>
|
||||
protected TValueType GetValue(Array3i index)
|
||||
{
|
||||
return _grid.GetValue(index);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns a reference to the value at 'index' to allow changing it.
|
||||
/// </summary>
|
||||
protected ref TValueType GetMutableValue(Array3i index)
|
||||
{
|
||||
return ref _grid.GetMutableValue(index);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the index of the cell containing the 'point'. Indices are integer
|
||||
/// vectors identifying cells, for this the coordinates are rounded to the next
|
||||
/// multiple of the resolution.
|
||||
/// </summary>
|
||||
public Array3i GetCellIndex(Vector3 point)
|
||||
{
|
||||
var index = new Vector3(point.X / resolution, point.Y / resolution, point.Z / resolution);
|
||||
return new Array3i(
|
||||
(int)System.Math.Round(index.X),
|
||||
(int)System.Math.Round(index.Y),
|
||||
(int)System.Math.Round(index.Z));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1).
|
||||
/// </summary>
|
||||
public static Array3i GetOctant(int i)
|
||||
{
|
||||
if (i < 0 || i >= 8)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(i), $"Octant index {i} must be in range [0, 7]");
|
||||
}
|
||||
return new Array3i(
|
||||
(i & 1) != 0 ? 1 : 0,
|
||||
(i & 2) != 0 ? 1 : 0,
|
||||
(i & 4) != 0 ? 1 : 0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the center of the cell at 'index'.
|
||||
/// </summary>
|
||||
public Vector3 GetCenterOfCell(Array3i index)
|
||||
{
|
||||
return new Vector3(
|
||||
index.X * resolution,
|
||||
index.Y * resolution,
|
||||
index.Z * resolution);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets an enumerator for all non-default values.
|
||||
/// </summary>
|
||||
public IEnumerator<(Array3i Index, TValueType Value)> GetEnumerator()
|
||||
{
|
||||
return _grid.GetEnumerator();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A grid containing probability values stored using 15 bits, and an update
|
||||
/// marker per voxel.
|
||||
/// Points are expected to be close to the origin. Points far from the origin
|
||||
/// require the grid to grow dynamically. For centimeter resolution, points
|
||||
/// can only be tens of meters from the origin.
|
||||
/// The hard limit of cell indexes is +/- 8192 around the origin.
|
||||
/// </summary>
|
||||
public class HybridGrid : HybridGridBase<ushort>
|
||||
{
|
||||
private const ushort kUpdateMarker = (ushort)(1u << 15);
|
||||
private readonly List<Array3i> _updateIndices;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new HybridGrid with the specified resolution.
|
||||
/// </summary>
|
||||
public HybridGrid(double resolution) : base(resolution)
|
||||
{
|
||||
_updateIndices = [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a HybridGrid from a proto.
|
||||
/// </summary>
|
||||
public HybridGrid(Models.Mapping.HybridGrid proto) : base(proto.Resolution)
|
||||
{
|
||||
_updateIndices = [];
|
||||
|
||||
if (proto.XIndices == null || proto.YIndices == null || proto.ZIndices == null || proto.Values == null)
|
||||
{
|
||||
throw new ArgumentException("Proto must have valid indices and values", nameof(proto));
|
||||
}
|
||||
|
||||
if (proto.XIndices.Count != proto.Values.Count ||
|
||||
proto.YIndices.Count != proto.Values.Count ||
|
||||
proto.ZIndices.Count != proto.Values.Count)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"Proto indices and values count mismatch: X={proto.XIndices.Count}, Y={proto.YIndices.Count}, Z={proto.ZIndices.Count}, Values={proto.Values.Count}",
|
||||
nameof(proto));
|
||||
}
|
||||
|
||||
for (int i = 0; i < proto.Values.Count; i++)
|
||||
{
|
||||
var index = new Array3i(proto.XIndices[i], proto.YIndices[i], proto.ZIndices[i]);
|
||||
var probability = ProbabilityValues.ValueToProbability((ushort)proto.Values[i]);
|
||||
SetProbability(index, probability);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the probability of the cell at 'index' to the given 'probability'.
|
||||
/// </summary>
|
||||
public void SetProbability(Array3i index, double probability)
|
||||
{
|
||||
var clampedProbability = ProbabilityValues.ClampProbability(probability);
|
||||
var value = ProbabilityValues.ProbabilityToValue(clampedProbability);
|
||||
GetMutableValue(index) = value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finishes the update sequence by removing update markers from all updated cells.
|
||||
/// </summary>
|
||||
public void FinishUpdate()
|
||||
{
|
||||
foreach (var index in _updateIndices)
|
||||
{
|
||||
ref var cell = ref GetMutableValue(index);
|
||||
if (cell >= kUpdateMarker)
|
||||
{
|
||||
cell = (ushort)(cell - kUpdateMarker);
|
||||
}
|
||||
}
|
||||
_updateIndices.Clear();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Applies the 'table' (lookup table from ComputeLookupTableToApplyOdds) to the
|
||||
/// probability of the cell at 'index' if the cell has not already been updated.
|
||||
/// Multiple updates of the same cell will be ignored until FinishUpdate() is called.
|
||||
/// Returns true if the cell was updated.
|
||||
///
|
||||
/// If this is the first call to ApplyLookupTable() for the specified cell, its value
|
||||
/// will be set to probability corresponding to the table entry.
|
||||
/// </summary>
|
||||
public bool ApplyLookupTable(Array3i index, List<ushort> table)
|
||||
{
|
||||
if (table == null || table.Count != kUpdateMarker)
|
||||
{
|
||||
throw new ArgumentException($"Table must have size {kUpdateMarker}", nameof(table));
|
||||
}
|
||||
|
||||
ref var cell = ref GetMutableValue(index);
|
||||
if (cell >= kUpdateMarker)
|
||||
{
|
||||
return false; // Already updated
|
||||
}
|
||||
|
||||
_updateIndices.Add(index);
|
||||
cell = table[cell];
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the probability of the cell with 'index'.
|
||||
/// </summary>
|
||||
public double GetProbability(Array3i index)
|
||||
{
|
||||
return ProbabilityValues.ValueToProbability(GetValue(index));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns true if the probability at the specified 'index' is known.
|
||||
/// </summary>
|
||||
public bool IsKnown(Array3i index)
|
||||
{
|
||||
return GetValue(index) != 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts this HybridGrid to a proto.
|
||||
/// </summary>
|
||||
public Models.Mapping.HybridGrid ToProto()
|
||||
{
|
||||
if (_updateIndices.Count > 0)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"Serializing a grid during an update is not supported. Finish the update first.");
|
||||
}
|
||||
|
||||
var result = new Models.Mapping.HybridGrid
|
||||
{
|
||||
Resolution = Resolution,
|
||||
XIndices = [],
|
||||
YIndices = [],
|
||||
ZIndices = [],
|
||||
Values = []
|
||||
};
|
||||
|
||||
foreach (var (index, value) in this)
|
||||
{
|
||||
result.XIndices.Add(index.X);
|
||||
result.YIndices.Add(index.Y);
|
||||
result.ZIndices.Add(index.Z);
|
||||
result.Values.Add(value);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Average intensity data structure for IntensityHybridGrid.
|
||||
/// </summary>
|
||||
public struct AverageIntensityData
|
||||
{
|
||||
public double Sum { get; set; }
|
||||
public int Count { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid grid for storing intensity data (average intensity per voxel).
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a new IntensityHybridGrid with the specified resolution.
|
||||
/// </remarks>
|
||||
public class IntensityHybridGrid(double resolution) : HybridGridBase<AverageIntensityData>(resolution)
|
||||
{
|
||||
|
||||
/// <summary>
|
||||
/// Adds intensity value to the cell at 'index'.
|
||||
/// </summary>
|
||||
public void AddIntensity(Array3i index, double intensity)
|
||||
{
|
||||
ref var cell = ref GetMutableValue(index);
|
||||
cell.Count += 1;
|
||||
cell.Sum += intensity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the average intensity of the cell at 'index'.
|
||||
/// </summary>
|
||||
public double GetIntensity(Array3i index)
|
||||
{
|
||||
var cell = GetValue(index);
|
||||
if (cell.Count == 0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
return cell.Sum / cell.Count;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
/*
|
||||
* 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.Math;
|
||||
using CartographerSharp.Sensor;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// Options for 3D range data inserter.
|
||||
/// </summary>
|
||||
public struct RangeDataInserterOptions3D(
|
||||
double hitProbability,
|
||||
double missProbability,
|
||||
int numFreeSpaceVoxels,
|
||||
double intensityThreshold)
|
||||
{
|
||||
public double HitProbability { get; set; } = hitProbability;
|
||||
public double MissProbability { get; set; } = missProbability;
|
||||
public int NumFreeSpaceVoxels { get; set; } = numFreeSpaceVoxels;
|
||||
public double IntensityThreshold { get; set; } = intensityThreshold;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Range data inserter for 3D hybrid grids.
|
||||
/// </summary>
|
||||
public class RangeDataInserter3D
|
||||
{
|
||||
private readonly RangeDataInserterOptions3D _options;
|
||||
private readonly List<ushort> _hitTable;
|
||||
private readonly List<ushort> _missTable;
|
||||
|
||||
public RangeDataInserter3D(RangeDataInserterOptions3D options)
|
||||
{
|
||||
if (options.HitProbability <= 0.5)
|
||||
{
|
||||
throw new ArgumentException("hit_probability must be greater than 0.5", nameof(options));
|
||||
}
|
||||
if (options.MissProbability >= 0.5)
|
||||
{
|
||||
throw new ArgumentException("miss_probability must be less than 0.5", nameof(options));
|
||||
}
|
||||
|
||||
_options = options;
|
||||
_hitTable = ProbabilityValues.ComputeLookupTableToApplyOdds(
|
||||
ProbabilityValues.Odds(options.HitProbability));
|
||||
_missTable = ProbabilityValues.ComputeLookupTableToApplyOdds(
|
||||
ProbabilityValues.Odds(options.MissProbability));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Inserts 'range_data' into 'hybrid_grid' and optionally into 'intensity_hybrid_grid'.
|
||||
/// </summary>
|
||||
public void Insert(
|
||||
RangeData rangeData,
|
||||
HybridGrid hybridGrid,
|
||||
IntensityHybridGrid? intensityHybridGrid)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(hybridGrid);
|
||||
|
||||
// Insert hits
|
||||
foreach (var hit in rangeData.Returns.Points)
|
||||
{
|
||||
var hitCell = hybridGrid.GetCellIndex(hit.Position);
|
||||
hybridGrid.ApplyLookupTable(hitCell, _hitTable);
|
||||
}
|
||||
|
||||
// By not starting a new update after hits are inserted, we give hits priority
|
||||
// (i.e. no hits will be ignored because of a miss in the same cell).
|
||||
InsertMissesIntoGrid(_missTable, rangeData.Origin, rangeData.Returns, hybridGrid, _options.NumFreeSpaceVoxels);
|
||||
|
||||
if (intensityHybridGrid != null)
|
||||
{
|
||||
InsertIntensitiesIntoGrid(rangeData.Returns, intensityHybridGrid, _options.IntensityThreshold);
|
||||
}
|
||||
|
||||
hybridGrid.FinishUpdate();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Inserts misses into the grid along rays from origin to returns.
|
||||
/// </summary>
|
||||
private static void InsertMissesIntoGrid(
|
||||
List<ushort> missTable,
|
||||
Vector3 origin,
|
||||
PointCloud returns,
|
||||
HybridGrid hybridGrid,
|
||||
int numFreeSpaceVoxels)
|
||||
{
|
||||
var originCell = hybridGrid.GetCellIndex(origin);
|
||||
|
||||
foreach (var hit in returns.Points)
|
||||
{
|
||||
var hitCell = hybridGrid.GetCellIndex(hit.Position);
|
||||
var delta = hitCell - originCell;
|
||||
|
||||
// Calculate the maximum absolute component of delta
|
||||
var numSamples = Math.Max(Math.Max(Math.Abs(delta.X), Math.Abs(delta.Y)), Math.Abs(delta.Z));
|
||||
|
||||
if (numSamples >= (1 << 15))
|
||||
{
|
||||
throw new InvalidOperationException($"Number of samples {numSamples} exceeds maximum");
|
||||
}
|
||||
|
||||
// 'numSamples' is the number of samples we equi-distantly place on the
|
||||
// line between 'origin' and 'hit'. (including a fractional part for sub-
|
||||
// voxels) It is chosen so that between two samples we change from one voxel
|
||||
// to the next on the fastest changing dimension.
|
||||
//
|
||||
// Only the last 'numFreeSpaceVoxels' are updated for performance.
|
||||
var startPosition = Math.Max(0, numSamples - numFreeSpaceVoxels);
|
||||
for (int position = startPosition; position < numSamples; position++)
|
||||
{
|
||||
var missCell = originCell + new Array3i(
|
||||
delta.X * position / numSamples,
|
||||
delta.Y * position / numSamples,
|
||||
delta.Z * position / numSamples);
|
||||
hybridGrid.ApplyLookupTable(missCell, missTable);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Inserts intensities into the intensity hybrid grid.
|
||||
/// </summary>
|
||||
private static void InsertIntensitiesIntoGrid(
|
||||
PointCloud returns,
|
||||
IntensityHybridGrid intensityHybridGrid,
|
||||
double intensityThreshold)
|
||||
{
|
||||
if (returns.Intensities.Count > 0)
|
||||
{
|
||||
for (int i = 0; i < returns.Count; i++)
|
||||
{
|
||||
if (i >= returns.Intensities.Count)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if (returns.Intensities[i] > intensityThreshold)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var hitCell = intensityHybridGrid.GetCellIndex(returns.Points[i].Position);
|
||||
intensityHybridGrid.AddIntensity(hitCell, returns.Intensities[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,263 @@
|
||||
/*
|
||||
* 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.Models.Transform;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using RotationalScanMatcher = CartographerSharp.Mapping.Internal.D3D.ScanMatching.RotationalScanMatcher;
|
||||
|
||||
namespace CartographerSharp.Mapping.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// 3D Submap implementation.
|
||||
/// </summary>
|
||||
public class Submap3D : Submap
|
||||
{
|
||||
private HybridGrid _highResolutionHybridGrid;
|
||||
private HybridGrid _lowResolutionHybridGrid;
|
||||
private IntensityHybridGrid? _highResolutionIntensityHybridGrid;
|
||||
private List<double> _rotationalScanMatcherHistogram;
|
||||
|
||||
public Submap3D(
|
||||
double highResolution,
|
||||
double lowResolution,
|
||||
Rigid3d localSubmapPose,
|
||||
List<double> rotationalScanMatcherHistogram)
|
||||
: base(localSubmapPose)
|
||||
{
|
||||
_highResolutionHybridGrid = new HybridGrid(highResolution);
|
||||
_lowResolutionHybridGrid = new HybridGrid(lowResolution);
|
||||
_highResolutionIntensityHybridGrid = new IntensityHybridGrid(highResolution);
|
||||
_rotationalScanMatcherHistogram = [.. rotationalScanMatcherHistogram];
|
||||
}
|
||||
|
||||
public Submap3D(Models.Mapping.Submap3D proto)
|
||||
: base((Rigid3d)proto.LocalPose)
|
||||
{
|
||||
// Initialize with default values first
|
||||
_highResolutionHybridGrid = new HybridGrid(0.05f); // Default resolution
|
||||
_lowResolutionHybridGrid = new HybridGrid(0.05f);
|
||||
_rotationalScanMatcherHistogram = [];
|
||||
UpdateFromProto(proto);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the high resolution hybrid grid.
|
||||
/// </summary>
|
||||
public HybridGrid HighResolutionHybridGrid => _highResolutionHybridGrid;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the low resolution hybrid grid.
|
||||
/// </summary>
|
||||
public HybridGrid LowResolutionHybridGrid => _lowResolutionHybridGrid;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the high resolution intensity hybrid grid.
|
||||
/// </summary>
|
||||
public IntensityHybridGrid? HighResolutionIntensityHybridGrid => _highResolutionIntensityHybridGrid;
|
||||
|
||||
/// <summary>
|
||||
/// Forgets the intensity hybrid grid to reduce memory usage.
|
||||
/// </summary>
|
||||
public void ForgetIntensityHybridGrid()
|
||||
{
|
||||
_highResolutionIntensityHybridGrid = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the rotational scan matcher histogram.
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> RotationalScanMatcherHistogram => _rotationalScanMatcherHistogram;
|
||||
|
||||
/// <summary>
|
||||
/// Insert 'range_data' into this submap using 'range_data_inserter'. The
|
||||
/// submap must not be finished yet.
|
||||
/// </summary>
|
||||
public void InsertData(
|
||||
RangeData rangeDataInLocal,
|
||||
RangeDataInserter3D rangeDataInserter,
|
||||
double highResolutionMaxRange,
|
||||
Quaternion localFromGravityAligned,
|
||||
List<double> scanHistogramInGravity)
|
||||
{
|
||||
if (InsertionFinished)
|
||||
{
|
||||
throw new InvalidOperationException("Cannot insert data into finished submap");
|
||||
}
|
||||
|
||||
// Transform range data into submap frame
|
||||
var submapInverse = LocalPose.Inverse();
|
||||
var submapInverseFloat = new Rigid3f(
|
||||
(Vector3)submapInverse.Translation,
|
||||
submapInverse.Rotation);
|
||||
var transformedRangeData = RangeDataOperations.Transform(rangeDataInLocal, submapInverseFloat);
|
||||
|
||||
// Filter range data by max range for high resolution grid
|
||||
var filteredRangeData = FilterRangeDataByMaxRange(transformedRangeData, highResolutionMaxRange);
|
||||
|
||||
// Insert into high resolution grid with intensity
|
||||
rangeDataInserter.Insert(
|
||||
filteredRangeData,
|
||||
_highResolutionHybridGrid,
|
||||
_highResolutionIntensityHybridGrid);
|
||||
|
||||
// Insert into low resolution grid without intensity
|
||||
rangeDataInserter.Insert(
|
||||
transformedRangeData,
|
||||
_lowResolutionHybridGrid,
|
||||
null);
|
||||
|
||||
NumRangeData++;
|
||||
|
||||
// Update rotational scan matcher histogram
|
||||
// C++: yaw_in_submap_from_gravity = GetYaw(local_pose().inverse().rotation() * local_from_gravity_aligned)
|
||||
// rotational_scan_matcher_histogram_ += RotationalScanMatcher::RotateHistogram(scan_histogram_in_gravity, yaw_in_submap_from_gravity)
|
||||
var yawInSubmapFromGravity = TransformOperations.GetYaw(submapInverse.Rotation * localFromGravityAligned);
|
||||
|
||||
if (_rotationalScanMatcherHistogram.Count == scanHistogramInGravity.Count)
|
||||
{
|
||||
var rotatedHistogram = RotationalScanMatcher.RotateHistogram(
|
||||
scanHistogramInGravity.ToArray(),
|
||||
yawInSubmapFromGravity);
|
||||
|
||||
for (int i = 0; i < rotatedHistogram.Length; i++)
|
||||
{
|
||||
_rotationalScanMatcherHistogram[i] += rotatedHistogram[i];
|
||||
}
|
||||
}
|
||||
else if (scanHistogramInGravity.Count > 0)
|
||||
{
|
||||
// Log warning for histogram size mismatch - this can cause rotational matching failures
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"Warning: Histogram size mismatch in Submap3D.InsertData: " +
|
||||
$"expected {_rotationalScanMatcherHistogram.Count}, got {scanHistogramInGravity.Count}. " +
|
||||
"Rotational scan matching may not work correctly.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finishes the submap.
|
||||
/// </summary>
|
||||
public void Finish()
|
||||
{
|
||||
if (InsertionFinished)
|
||||
{
|
||||
throw new InvalidOperationException("Submap is already finished");
|
||||
}
|
||||
InsertionFinished = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts to proto representation.
|
||||
/// </summary>
|
||||
public override Models.Mapping.Submap ToProto(bool includeGridData)
|
||||
{
|
||||
Models.Mapping.HybridGrid highResGrid;
|
||||
Models.Mapping.HybridGrid lowResGrid;
|
||||
|
||||
if (includeGridData)
|
||||
{
|
||||
highResGrid = _highResolutionHybridGrid.ToProto();
|
||||
lowResGrid = _lowResolutionHybridGrid.ToProto();
|
||||
}
|
||||
else
|
||||
{
|
||||
// Create empty grids with just resolution
|
||||
highResGrid = new Models.Mapping.HybridGrid
|
||||
{
|
||||
Resolution = _highResolutionHybridGrid.Resolution,
|
||||
XIndices = [],
|
||||
YIndices = [],
|
||||
ZIndices = [],
|
||||
Values = []
|
||||
};
|
||||
lowResGrid = new Models.Mapping.HybridGrid
|
||||
{
|
||||
Resolution = _lowResolutionHybridGrid.Resolution,
|
||||
XIndices = [],
|
||||
YIndices = [],
|
||||
ZIndices = [],
|
||||
Values = []
|
||||
};
|
||||
}
|
||||
|
||||
var submap3D = new Models.Mapping.Submap3D(
|
||||
(Rigid3dProto)LocalPose,
|
||||
NumRangeData,
|
||||
InsertionFinished,
|
||||
highResGrid,
|
||||
lowResGrid,
|
||||
[.. _rotationalScanMatcherHistogram]);
|
||||
|
||||
// Note: SubmapId will be set by caller
|
||||
return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), null, submap3D);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Updates from proto representation.
|
||||
/// </summary>
|
||||
public override void UpdateFromProto(Models.Mapping.Submap proto)
|
||||
{
|
||||
if (!proto.Submap3D.HasValue)
|
||||
{
|
||||
throw new ArgumentException("Proto must contain Submap3D", nameof(proto));
|
||||
}
|
||||
|
||||
UpdateFromProto(proto.Submap3D.Value);
|
||||
}
|
||||
|
||||
private void UpdateFromProto(Models.Mapping.Submap3D submap3D)
|
||||
{
|
||||
NumRangeData = submap3D.NumRangeData;
|
||||
InsertionFinished = submap3D.Finished;
|
||||
|
||||
if (submap3D.HighResolutionHybridGrid.Values != null && submap3D.HighResolutionHybridGrid.Values.Count > 0)
|
||||
{
|
||||
_highResolutionHybridGrid = new HybridGrid(submap3D.HighResolutionHybridGrid);
|
||||
}
|
||||
|
||||
if (submap3D.LowResolutionHybridGrid.Values != null && submap3D.LowResolutionHybridGrid.Values.Count > 0)
|
||||
{
|
||||
_lowResolutionHybridGrid = new HybridGrid(submap3D.LowResolutionHybridGrid);
|
||||
}
|
||||
|
||||
_rotationalScanMatcherHistogram = [.. submap3D.RotationalScanMatcherHistogram ?? []];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Filters 'range_data', retaining only the returns that have no more than
|
||||
/// 'max_range' distance from the origin. Removes misses.
|
||||
/// </summary>
|
||||
public static RangeData FilterRangeDataByMaxRange(RangeData rangeData, double maxRange)
|
||||
{
|
||||
var filteredReturns = new List<RangefinderPoint>();
|
||||
|
||||
foreach (var point in rangeData.Returns.Points)
|
||||
{
|
||||
var distance = Vector3.Distance(point.Position, rangeData.Origin);
|
||||
if (distance <= maxRange)
|
||||
{
|
||||
filteredReturns.Add(point);
|
||||
}
|
||||
}
|
||||
|
||||
return new RangeData(
|
||||
rangeData.Origin,
|
||||
new PointCloud(filteredReturns),
|
||||
new PointCloud()); // Misses are removed
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user