Initial commit
This commit is contained in:
@@ -0,0 +1,273 @@
|
||||
/*
|
||||
* 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user