274 lines
11 KiB
C#
274 lines
11 KiB
C#
/*
|
|
* Copyright 2018 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.Mapping.Internal.D2D;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Sensor;
|
|
using System.Runtime.InteropServices;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace CartographerSharp.Mapping.D2D;
|
|
|
|
/// <summary>
|
|
/// Range data inserter for TSDF grids in 2D.
|
|
/// </summary>
|
|
public class TSDFRangeDataInserter2D : IRangeDataInserter
|
|
{
|
|
private const int kSubpixelScale = 1000;
|
|
private const double kMinRangeMeters = 1e-6;
|
|
private static readonly double kSqrtTwoPi = Math.Sqrt(2.0 * Math.PI);
|
|
|
|
private readonly TSDFRangeDataInserterOptions2D _options;
|
|
|
|
public TSDFRangeDataInserter2D(TSDFRangeDataInserterOptions2D options)
|
|
{
|
|
_options = options;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Inserts 'range_data' into 'grid'.
|
|
/// </summary>
|
|
public void Insert(RangeData rangeData, IGrid grid)
|
|
{
|
|
if (grid is not TSDF2D tsdf)
|
|
{
|
|
throw new ArgumentException("Grid must be a TSDF2D", nameof(grid));
|
|
}
|
|
|
|
// Match C++: No FinishUpdate() before GrowAsNeeded()
|
|
var truncationDistance = _options.TruncationDistance;
|
|
GrowAsNeeded(rangeData, truncationDistance, tsdf);
|
|
|
|
// Compute normals if needed
|
|
bool scaleUpdateWeightAngleScanNormalToRay =
|
|
_options.UpdateWeightAngleScanNormalToRayKernelBandwidth != 0.0;
|
|
RangeData sortedRangeData = rangeData;
|
|
List<double> normals = [];
|
|
|
|
if (_options.ProjectSdfDistanceToScanNormal || scaleUpdateWeightAngleScanNormalToRay)
|
|
{
|
|
// Sort range data by angle from origin
|
|
var returns = new List<RangefinderPoint>(rangeData.Returns.Points);
|
|
returns.Sort(new RangeDataSorter(rangeData.Origin));
|
|
|
|
sortedRangeData = new RangeData(
|
|
rangeData.Origin,
|
|
new PointCloud(returns),
|
|
rangeData.Misses);
|
|
|
|
normals = NormalEstimation2D.EstimateNormals(
|
|
sortedRangeData,
|
|
_options.NormalEstimationOptions);
|
|
}
|
|
|
|
var origin = new Vector2(sortedRangeData.Origin.X, sortedRangeData.Origin.Y);
|
|
// Reusable buffer for ray computation - avoids per-hit List<Array2i> allocation
|
|
var rayBuffer = new List<Array2i>(512);
|
|
for (int hitIndex = 0; hitIndex < sortedRangeData.Returns.Count; hitIndex++)
|
|
{
|
|
var hitPoint = sortedRangeData.Returns.Points[hitIndex];
|
|
var hit = new Vector2(hitPoint.Position.X, hitPoint.Position.Y);
|
|
var normal = normals.Count > 0 ? normals[hitIndex] : double.NaN;
|
|
InsertHit(hit, origin, normal, tsdf, rayBuffer);
|
|
}
|
|
|
|
tsdf.FinishUpdate();
|
|
}
|
|
|
|
private void InsertHit(Vector2 hit, Vector2 origin, double normal, TSDF2D tsdf, List<Array2i> rayBuffer)
|
|
{
|
|
var ray = hit - origin;
|
|
var range = ray.Length();
|
|
var truncationDistance = _options.TruncationDistance;
|
|
|
|
if (range < truncationDistance) return;
|
|
|
|
var truncationRatio = truncationDistance / range;
|
|
var rayBegin = _options.UpdateFreeSpace
|
|
? origin
|
|
: origin + (1.0 - truncationRatio) * ray;
|
|
var rayEnd = origin + (1.0 + truncationRatio) * ray;
|
|
|
|
var superscaledRay = SuperscaleRay(rayBegin, rayEnd, tsdf);
|
|
rayBuffer.Clear();
|
|
RayToPixelMask.ComputeInto(
|
|
superscaledRay.Item1, superscaledRay.Item2, kSubpixelScale, rayBuffer);
|
|
|
|
// Precompute weight factors
|
|
double weightFactorAngleRayNormal = 1.0;
|
|
if (_options.UpdateWeightAngleScanNormalToRayKernelBandwidth != 0.0)
|
|
{
|
|
var negativeRay = -ray;
|
|
double angleRayNormal = MathUtils.NormalizeAngleDifference(
|
|
normal - Math.Atan2(negativeRay.Y, negativeRay.X));
|
|
weightFactorAngleRayNormal = GaussianKernel(
|
|
angleRayNormal,
|
|
_options.UpdateWeightAngleScanNormalToRayKernelBandwidth);
|
|
}
|
|
|
|
double weightFactorRange = 1.0;
|
|
if (_options.UpdateWeightRangeExponent != 0)
|
|
{
|
|
weightFactorRange = ComputeRangeWeightFactor(
|
|
range, _options.UpdateWeightRangeExponent);
|
|
}
|
|
|
|
// Update cells using Span for bounds-check-free iteration
|
|
var raySpan = CollectionsMarshal.AsSpan(rayBuffer);
|
|
for (int i = 0; i < raySpan.Length; i++)
|
|
{
|
|
var cellIndex = raySpan[i];
|
|
if (tsdf.CellIsUpdated(cellIndex)) continue;
|
|
|
|
var cellCenter = tsdf.Limits.GetCellCenter(cellIndex);
|
|
double distanceCellToOrigin = (cellCenter - origin).Length();
|
|
double updateTSD = range - distanceCellToOrigin;
|
|
|
|
// Match C++: if (options_.project_sdf_distance_to_scan_normal()) {
|
|
// No NaN check in C++
|
|
if (_options.ProjectSdfDistanceToScanNormal)
|
|
{
|
|
double normalOrientation = normal;
|
|
var normalVector = new Vector2(
|
|
Math.Cos(normalOrientation),
|
|
Math.Sin(normalOrientation));
|
|
updateTSD = Vector2.Dot(cellCenter - hit, normalVector);
|
|
}
|
|
|
|
updateTSD = Math.Clamp(updateTSD, -truncationDistance, truncationDistance);
|
|
|
|
double updateWeight = weightFactorRange * weightFactorAngleRayNormal;
|
|
if (_options.UpdateWeightDistanceCellToHitKernelBandwidth != 0.0)
|
|
{
|
|
updateWeight *= GaussianKernel(
|
|
updateTSD,
|
|
_options.UpdateWeightDistanceCellToHitKernelBandwidth);
|
|
}
|
|
|
|
UpdateCell(cellIndex, updateTSD, updateWeight, tsdf);
|
|
}
|
|
}
|
|
|
|
private void UpdateCell(Array2i cell, double updateSdf, double updateWeight, TSDF2D tsdf)
|
|
{
|
|
if (updateWeight == 0.0) return;
|
|
|
|
var (currentTSD, currentWeight) = tsdf.GetTSDAndWeight(cell);
|
|
double updatedWeight = currentWeight + updateWeight;
|
|
double updatedSDF = (currentTSD * currentWeight + updateSdf * updateWeight) / updatedWeight;
|
|
updatedWeight = Math.Min(updatedWeight, _options.MaximumWeight);
|
|
|
|
tsdf.SetCell(cell, updatedSDF, updatedWeight);
|
|
}
|
|
|
|
private static void GrowAsNeeded(RangeData rangeData, double truncationDistance, TSDF2D tsdf)
|
|
{
|
|
// Match C++: Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
|
|
// Then only extend end_position, not hit.position
|
|
var origin2D = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
|
|
var boundingBoxMin = origin2D;
|
|
var boundingBoxMax = origin2D;
|
|
|
|
foreach (var hit in rangeData.Returns.Points)
|
|
{
|
|
var hit2D = new Vector2(hit.Position.X, hit.Position.Y);
|
|
var direction = Vector2.Normalize(hit2D - origin2D);
|
|
var endPosition = hit2D + truncationDistance * direction;
|
|
|
|
// Match C++: bounding_box.extend(end_position.head<2>());
|
|
// Only extend end_position, not hit.position
|
|
boundingBoxMin.X = Math.Min(boundingBoxMin.X, endPosition.X);
|
|
boundingBoxMin.Y = Math.Min(boundingBoxMin.Y, endPosition.Y);
|
|
boundingBoxMax.X = Math.Max(boundingBoxMax.X, endPosition.X);
|
|
boundingBoxMax.Y = Math.Max(boundingBoxMax.Y, endPosition.Y);
|
|
}
|
|
|
|
const double kPadding = 1e-6;
|
|
tsdf.GrowLimits(boundingBoxMin - new Vector2(kPadding, kPadding));
|
|
tsdf.GrowLimits(boundingBoxMax + new Vector2(kPadding, kPadding));
|
|
}
|
|
|
|
private static (Array2i, Array2i) SuperscaleRay(
|
|
Vector2 begin, Vector2 end, TSDF2D tsdf)
|
|
{
|
|
// Match C++: const MapLimits superscaled_limits(
|
|
// superscaled_resolution, limits.max(), ...);
|
|
// Use limits.max() directly, no calculation
|
|
var limits = tsdf.Limits;
|
|
var superscaledResolution = limits.Resolution / kSubpixelScale;
|
|
var superscaledCellLimits = new CellLimits(
|
|
limits.CellLimits.NumXCells * kSubpixelScale,
|
|
limits.CellLimits.NumYCells * kSubpixelScale);
|
|
var superscaledLimits = new MapLimits(
|
|
superscaledResolution, limits.Max, superscaledCellLimits);
|
|
|
|
var superscaledBegin = superscaledLimits.GetCellIndex(begin);
|
|
var superscaledEnd = superscaledLimits.GetCellIndex(end);
|
|
|
|
// Match C++: return std::make_pair(superscaled_begin, superscaled_end);
|
|
// No multiplication by kSubpixelScale - GetCellIndex already returns superscaled indices
|
|
return (superscaledBegin, superscaledEnd);
|
|
}
|
|
|
|
// Match C++: No sigma == 0 check
|
|
private static double GaussianKernel(double x, double sigma)
|
|
{
|
|
return 1.0 / (kSqrtTwoPi * sigma) * Math.Exp(-0.5 * x * x / (sigma * sigma));
|
|
}
|
|
|
|
private static double ComputeRangeWeightFactor(double range, int exponent)
|
|
{
|
|
if (Math.Abs(range) <= kMinRangeMeters) return 0.0;
|
|
return 1.0 / Math.Pow(range, exponent);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sorts range data points by angle from origin.
|
|
/// </summary>
|
|
private class RangeDataSorter : IComparer<RangefinderPoint>
|
|
{
|
|
private readonly Vector2 _origin;
|
|
|
|
public RangeDataSorter(Vector3 origin)
|
|
{
|
|
_origin = new Vector2(origin.X, origin.Y);
|
|
}
|
|
|
|
public int Compare(RangefinderPoint lhs, RangefinderPoint rhs)
|
|
{
|
|
var deltaLhs = Vector2.Normalize(
|
|
new Vector2(lhs.Position.X, lhs.Position.Y) - _origin);
|
|
var deltaRhs = Vector2.Normalize(
|
|
new Vector2(rhs.Position.X, rhs.Position.Y) - _origin);
|
|
|
|
if ((deltaLhs.Y < 0.0) != (deltaRhs.Y < 0.0))
|
|
{
|
|
return deltaLhs.Y < 0.0 ? -1 : 1;
|
|
}
|
|
else if (deltaLhs.Y < 0.0)
|
|
{
|
|
return deltaLhs.X < deltaRhs.X ? -1 : (deltaLhs.X > deltaRhs.X ? 1 : 0);
|
|
}
|
|
else
|
|
{
|
|
return deltaLhs.X > deltaRhs.X ? -1 : (deltaLhs.X < deltaRhs.X ? 1 : 0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|