QUYVN/Denso
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

273
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/TSDFRangeDataInserter2D.cs Normal file
View File

@@ -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);
}
}
}
}