QUYVN/BQP
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-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

218
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/CeresScanMatcher3D.cs Normal file
View File

@@ -0,0 +1,218 @@
/*
* 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.Mapping;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using CeresSharp;
using CeresSharp.Enums;
using RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Point cloud and hybrid grids pointers structure.
/// </summary>
public struct PointCloudAndHybridGridsPointers
{
public PointCloud? PointCloud { get; set; }
public Mapping.D3D.HybridGrid? HybridGrid { get; set; }
public Mapping.D3D.IntensityHybridGrid? IntensityHybridGrid { get; set; } // optional
}
/// <summary>
/// This scan matcher uses Ceres to align scans with an existing 3D map.
/// </summary>
public class CeresScanMatcher3D : IDisposable
{
private readonly CeresScanMatcherOptions3D _options;
private readonly SolverOptions _solverOptions;
private bool _disposed;
public CeresScanMatcher3D(CeresScanMatcherOptions3D options)
{
_options = options;
// Initialize CeresSharp solver options
_solverOptions = new SolverOptions
{
// Set linear solver type to DENSE_QR for 3D scan matching
LinearSolverType = LinearSolverType.DenseQr,
// Configure from CeresSolverOptions if available, otherwise use defaults
MaxNumIterations = options.CeresSolverOptions?.MaxNumIterations ?? 20, // Default for scan matching
NumThreads = options.CeresSolverOptions?.NumThreads ?? 1, // Single thread for scan matching
UseNonmonotonicSteps = options.CeresSolverOptions?.UseNonmonotonicSteps ?? false
};
}
/// <summary>
/// Aligns 'point_clouds' within the 'hybrid_grids' given an
/// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
/// 'summary'.
/// </summary>
public void Match(
Vector3 targetTranslation,
Rigid3d initialPoseEstimate,
List<PointCloudAndHybridGridsPointers> pointCloudsAndHybridGrids,
out Rigid3d poseEstimate,
out SolverSummary summary)
{
if (pointCloudsAndHybridGrids == null || pointCloudsAndHybridGrids.Count == 0)
{
poseEstimate = initialPoseEstimate;
using var emptyProblem = new Problem();
using var emptyOptions = new SolverOptions();
summary = emptyProblem.Solve(emptyOptions);
return;
}
// Validate weights
if (_options.OccupiedSpaceWeight.Count != pointCloudsAndHybridGrids.Count)
{
throw new ArgumentException(
$"OccupiedSpaceWeight count ({_options.OccupiedSpaceWeight.Count}) must match pointCloudsAndHybridGrids count ({pointCloudsAndHybridGrids.Count})",
nameof(pointCloudsAndHybridGrids));
}
for (int i = 0; i < _options.OccupiedSpaceWeight.Count; i++)
{
if (_options.OccupiedSpaceWeight[i] <= 0.0)
{
throw new ArgumentException($"OccupiedSpaceWeight[{i}] must be positive", nameof(_options));
}
}
if (_options.TranslationWeight <= 0.0)
throw new ArgumentException("TranslationWeight must be positive", nameof(_options));
if (_options.RotationWeight <= 0.0)
throw new ArgumentException("RotationWeight must be positive", nameof(_options));
// Initialize pose parameters
// For 3D: [translation[3], rotation[4]]
var translationParams = new double[3]
{
initialPoseEstimate.Translation.X,
initialPoseEstimate.Translation.Y,
initialPoseEstimate.Translation.Z
};
var rotationParams = new double[4]
{
initialPoseEstimate.Rotation.W,
initialPoseEstimate.Rotation.X,
initialPoseEstimate.Rotation.Y,
initialPoseEstimate.Rotation.Z
};
// Create Ceres problem
using var problem = new Problem();
// Add parameter blocks
problem.AddParameterBlock(translationParams, 3);
problem.AddParameterBlock(rotationParams, 4);
// Set quaternion manifold (Ceres 2.2.0 uses Manifold instead of Parameterization)
// TODO: When OnlyOptimizeYaw is true, use a YawOnlyQuaternionManifold instead
// (C++ uses YawOnlyQuaternionPlus local parameterization for this case)
using var quaternionManifold = new QuaternionManifold();
problem.SetManifold(rotationParams, quaternionManifold);
// Add occupied space cost functions for each point cloud/grid pair
for (int i = 0; i < pointCloudsAndHybridGrids.Count; i++)
{
var pcAndGrid = pointCloudsAndHybridGrids[i];
if (pcAndGrid.PointCloud == null || pcAndGrid.HybridGrid == null)
continue;
if (pcAndGrid.PointCloud.Count == 0)
continue;
var occupiedSpaceCost = OccupiedSpaceCostFunction3D.CreateAutoDiffCostFunction(
_options.OccupiedSpaceWeight[i] / Math.Sqrt(pcAndGrid.PointCloud.Count),
pcAndGrid.PointCloud,
pcAndGrid.HybridGrid
);
problem.AddResidualBlock(occupiedSpaceCost, null, [translationParams, rotationParams]);
// Add intensity cost function if intensity grid is available
if (pcAndGrid.IntensityHybridGrid != null &&
_options.IntensityCostFunctionOptions != null &&
_options.IntensityCostFunctionOptions.Count > i)
{
var intensityOptions = _options.IntensityCostFunctionOptions[i];
var intensityCost = IntensityCostFunction3D.CreateAutoDiffCostFunction(
intensityOptions.Weight / Math.Sqrt(pcAndGrid.PointCloud.Count),
intensityOptions.IntensityThreshold,
pcAndGrid.PointCloud,
pcAndGrid.IntensityHybridGrid
);
// Do NOT use 'using' here - Problem takes ownership of the loss function
// via MarkOwnedByProblem() and will manage its lifetime
var huberLoss = new HuberLoss(intensityOptions.HuberScale);
problem.AddResidualBlock(intensityCost, huberLoss, [translationParams, rotationParams]);
}
}
// Add translation delta cost function
var translationCost = TranslationDeltaCostFunctor3D.CreateAutoDiffCostFunction(
_options.TranslationWeight,
targetTranslation
);
problem.AddResidualBlock(translationCost, null, [translationParams]);
// Add rotation delta cost function
var rotationCost = RotationDeltaCostFunctor3D.CreateAutoDiffCostFunction(
_options.RotationWeight,
initialPoseEstimate.Rotation
);
problem.AddResidualBlock(rotationCost, null, [rotationParams]);
// Solve
summary = problem.Solve(_solverOptions);
// Extract result
var newTranslation = new Vector3(
translationParams[0],
translationParams[1],
translationParams[2]
);
// rotationParams = [w, x, y, z] from Ceres
// System.Numerics.Quaternion constructor is (x, y, z, w)
var newRotation = new Quaternion(
rotationParams[1], // x
rotationParams[2], // y
rotationParams[3], // z
rotationParams[0] // w
);
// Normalize to ensure unit quaternion after Ceres optimization
// C++ uses EigenQuaternionParameterization which maintains unit norm,
// but CeresSharp may not have the same guarantee
newRotation = Quaternion.Normalize(newRotation);
poseEstimate = new Rigid3d(newTranslation, newRotation);
}
public void Dispose()
{
if (!_disposed)
{
_solverOptions?.Dispose();
_disposed = true;
}
}
}

141
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/IntensityCostFunction3D.cs Normal file
View File

@@ -0,0 +1,141 @@
/*
* Copyright 2019 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.Mapping.D3D;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using CeresSharp;
using RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
/// 'translation' and 'rotation'. The cost increases when points fall into space
/// for which different intensity has been observed, i.e. at voxels with different
/// values. Only points up to a certain threshold are evaluated which is intended
/// to ignore data from retroreflections.
/// </summary>
/// <remarks>
/// Creates an intensity cost function for 3D scan matching.
/// </remarks>
/// <param name="scalingFactor">Weight factor (typically intensity_weight / sqrt(point_cloud.size())).</param>
/// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
/// <param name="pointCloud">Point cloud to match (must have intensities).</param>
/// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
public class IntensityCostFunction3D(
double scalingFactor,
double intensityThreshold,
PointCloud pointCloud,
IntensityHybridGrid hybridGrid) : IDisposable
{
private readonly PointCloud _pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
private readonly InterpolatedIntensityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
private static readonly int[] parameterBlockSizes = [3, 4];
/// <summary>
/// Creates a DynamicAutoDiff cost function for intensity matching.
/// </summary>
/// <param name="scalingFactor">Weight factor.</param>
/// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
/// <param name="pointCloud">Point cloud to match.</param>
/// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
/// <returns>DynamicAutoDiff cost function.</returns>
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
double scalingFactor,
double intensityThreshold,
PointCloud pointCloud,
IntensityHybridGrid hybridGrid)
{
var costFunction = new IntensityCostFunction3D(scalingFactor, intensityThreshold, pointCloud, hybridGrid);
return new DynamicAutoDiffCostFunction(
costFunction.Evaluate,
numResiduals: pointCloud.Count,
parameterBlockSizes: parameterBlockSizes);
}
/// <summary>
/// Evaluates the cost function.
/// </summary>
/// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
/// <param name="residuals">Output residuals (one per point).</param>
/// <returns>True on success.</returns>
private bool Evaluate(double[][] parameters, double[] residuals)
{
if (parameters == null || parameters.Length < 2)
return false;
if (parameters[0].Length < 3 || parameters[1].Length < 4)
return false;
if (residuals == null || residuals.Length < _pointCloud.Count)
return false;
var translation = parameters[0];
var rotation = parameters[1]; // [w, x, y, z] from Ceres
// Create transform from translation and rotation
// C++ line 48-50: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
// where rotation = [w, x, y, z]
// System.Numerics.Quaternion constructor is (x, y, z, w)
var transform = new Rigid3d(
new Vector3(translation[0], translation[1], translation[2]),
new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
);
// Transform each point and compute residual
for (int i = 0; i < _pointCloud.Count; i++)
{
var point = _pointCloud[i];
// Get intensity from point cloud if available, otherwise use 0
double intensity = 0.0;
if (_pointCloud.Intensities.Count > 0 && i < _pointCloud.Intensities.Count)
{
intensity = _pointCloud.Intensities[i];
}
// Ignore points with intensity above threshold (retroreflections)
if (intensity > intensityThreshold)
{
residuals[i] = 0.0;
continue;
}
// Transform point from local frame to world frame
var worldPoint = transform * point.Position;
// Get interpolated intensity value at world point
var interpolatedIntensity = _interpolatedGrid.GetInterpolatedValue(
worldPoint.X,
worldPoint.Y,
worldPoint.Z
);
// Residual = scaling_factor * (interpolated_intensity - intensity)
residuals[i] = scalingFactor * (interpolatedIntensity - intensity);
}
return true;
}
/// <summary>
/// Disposes managed resources.
/// </summary>
public void Dispose()
{
// InterpolatedIntensityGrid doesn't need disposal, but we implement IDisposable for consistency
GC.SuppressFinalize(this);
}
}

238
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/InterpolatedGrid.cs Normal file
View File

@@ -0,0 +1,238 @@
/*
* 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.Mapping.D3D;
using RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Interpolates between HybridGrid voxels using tricubic interpolation.
/// This class is designed to work with Ceres autodiff, so the interpolation
/// scheme must be continuously differentiable.
/// </summary>
public class InterpolatedProbabilityGrid(HybridGrid _hybridGrid)
{
/// <summary>
/// Returns the interpolated value at (x, y, z) of the HybridGrid.
/// Uses tricubic interpolation (piecewise cubic polynomials).
/// </summary>
public double GetInterpolatedValue(double x, double y, double z)
{
ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
var q111 = GetValue(index1);
var q112 = GetValue(index1 + new Array3i(0, 0, 1));
var q121 = GetValue(index1 + new Array3i(0, 1, 0));
var q122 = GetValue(index1 + new Array3i(0, 1, 1));
var q211 = GetValue(index1 + new Array3i(1, 0, 0));
var q212 = GetValue(index1 + new Array3i(1, 0, 1));
var q221 = GetValue(index1 + new Array3i(1, 1, 0));
var q222 = GetValue(index1 + new Array3i(1, 1, 1));
var normalizedX = (x - x1) / (x2 - x1);
var normalizedY = (y - y1) / (y2 - y1);
var normalizedZ = (z - z1) / (z2 - z1);
// Compute powers: t^2 and t^3
var normalizedXx = normalizedX * normalizedX;
var normalizedXxx = normalizedX * normalizedXx;
var normalizedYy = normalizedY * normalizedY;
var normalizedYyy = normalizedY * normalizedYy;
var normalizedZz = normalizedZ * normalizedZ;
var normalizedZzz = normalizedZ * normalizedZz;
// Interpolate in z, then y, then x
// Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
var q11 = (q111 - q112) * normalizedZzz * 2.0 +
(q112 - q111) * normalizedZz * 3.0 + q111;
var q12 = (q121 - q122) * normalizedZzz * 2.0 +
(q122 - q121) * normalizedZz * 3.0 + q121;
var q21 = (q211 - q212) * normalizedZzz * 2.0 +
(q212 - q211) * normalizedZz * 3.0 + q211;
var q22 = (q221 - q222) * normalizedZzz * 2.0 +
(q222 - q221) * normalizedZz * 3.0 + q221;
var q1 = (q11 - q12) * normalizedYyy * 2.0 +
(q12 - q11) * normalizedYy * 3.0 + q11;
var q2 = (q21 - q22) * normalizedYyy * 2.0 +
(q22 - q21) * normalizedYy * 3.0 + q21;
return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
}
/// <summary>
/// Computes interpolation data points (corners of the voxel containing the point).
/// </summary>
private void ComputeInterpolationDataPoints(
double x, double y, double z,
out double x1, out double y1, out double z1,
out double x2, out double y2, out double z2)
{
var lower = CenterOfLowerVoxel(x, y, z);
x1 = lower.X;
y1 = lower.Y;
z1 = lower.Z;
x2 = lower.X + _hybridGrid.Resolution;
y2 = lower.Y + _hybridGrid.Resolution;
z2 = lower.Z + _hybridGrid.Resolution;
}
/// <summary>
/// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
/// For each dimension, the largest voxel index so that the corresponding center
/// is at most the given coordinate.
/// </summary>
private Vector3 CenterOfLowerVoxel(double x, double y, double z)
{
// Center of the cell containing (x, y, z)
var center = _hybridGrid.GetCenterOfCell(
_hybridGrid.GetCellIndex(new Vector3(x, y, z))
);
// Move to the next lower voxel center
var resolution = _hybridGrid.Resolution;
if (center.X > x)
{
center.X -= resolution;
}
if (center.Y > y)
{
center.Y -= resolution;
}
if (center.Z > z)
{
center.Z -= resolution;
}
return center;
}
/// <summary>
/// Gets the probability value at the given cell index.
/// </summary>
private double GetValue(Array3i index)
{
// HybridGrid.GetProbability already returns probability in range [0, 1]
// It internally calls ProbabilityValues.ValueToProbability which does the conversion
// DO NOT divide by ushort.MaxValue - that was a bug!
return _hybridGrid.GetProbability(index);
}
}
/// <summary>
/// Interpolates between IntensityHybridGrid voxels using tricubic interpolation.
/// </summary>
public class InterpolatedIntensityGrid(IntensityHybridGrid _hybridGrid)
{
/// <summary>
/// Returns the interpolated value at (x, y, z) of the IntensityHybridGrid.
/// Uses tricubic interpolation (piecewise cubic polynomials).
/// </summary>
public double GetInterpolatedValue(double x, double y, double z)
{
ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
var q111 = GetValue(index1);
var q112 = GetValue(index1 + new Array3i(0, 0, 1));
var q121 = GetValue(index1 + new Array3i(0, 1, 0));
var q122 = GetValue(index1 + new Array3i(0, 1, 1));
var q211 = GetValue(index1 + new Array3i(1, 0, 0));
var q212 = GetValue(index1 + new Array3i(1, 0, 1));
var q221 = GetValue(index1 + new Array3i(1, 1, 0));
var q222 = GetValue(index1 + new Array3i(1, 1, 1));
var normalizedX = (x - x1) / (x2 - x1);
var normalizedY = (y - y1) / (y2 - y1);
var normalizedZ = (z - z1) / (z2 - z1);
// Compute powers: t^2 and t^3
var normalizedXx = normalizedX * normalizedX;
var normalizedXxx = normalizedX * normalizedXx;
var normalizedYy = normalizedY * normalizedY;
var normalizedYyy = normalizedY * normalizedYy;
var normalizedZz = normalizedZ * normalizedZ;
var normalizedZzz = normalizedZ * normalizedZz;
// Interpolate in z, then y, then x
// Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
var q11 = (q111 - q112) * normalizedZzz * 2.0 +
(q112 - q111) * normalizedZz * 3.0 + q111;
var q12 = (q121 - q122) * normalizedZzz * 2.0 +
(q122 - q121) * normalizedZz * 3.0 + q121;
var q21 = (q211 - q212) * normalizedZzz * 2.0 +
(q212 - q211) * normalizedZz * 3.0 + q211;
var q22 = (q221 - q222) * normalizedZzz * 2.0 +
(q222 - q221) * normalizedZz * 3.0 + q221;
var q1 = (q11 - q12) * normalizedYyy * 2.0 +
(q12 - q11) * normalizedYy * 3.0 + q11;
var q2 = (q21 - q22) * normalizedYyy * 2.0 +
(q22 - q21) * normalizedYy * 3.0 + q21;
return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
}
/// <summary>
/// Computes interpolation data points (corners of the voxel containing the point).
/// </summary>
private void ComputeInterpolationDataPoints(
double x, double y, double z,
out double x1, out double y1, out double z1,
out double x2, out double y2, out double z2)
{
var lower = CenterOfLowerVoxel(x, y, z);
x1 = lower.X;
y1 = lower.Y;
z1 = lower.Z;
x2 = lower.X + _hybridGrid.Resolution;
y2 = lower.Y + _hybridGrid.Resolution;
z2 = lower.Z + _hybridGrid.Resolution;
}
/// <summary>
/// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
/// </summary>
private Vector3 CenterOfLowerVoxel(double x, double y, double z)
{
// Center of the cell containing (x, y, z)
var center = _hybridGrid.GetCenterOfCell(
_hybridGrid.GetCellIndex(new Vector3(x, y, z))
);
// Move to the next lower voxel center
var resolution = _hybridGrid.Resolution;
if (center.X > x)
{
center.X -= resolution;
}
if (center.Y > y)
{
center.Y -= resolution;
}
if (center.Z > z)
{
center.Z -= resolution;
}
return center;
}
/// <summary>
/// Gets the intensity value at the given cell index.
/// </summary>
private double GetValue(Array3i index)
{
return _hybridGrid.GetIntensity(index);
}
}

121
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/OccupiedSpaceCostFunction3D.cs Normal file
View File

@@ -0,0 +1,121 @@
/*
* 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.Mapping.D3D;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using CeresSharp;
using RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
/// 'translation' and 'rotation'. The cost increases when points fall into less
/// occupied space, i.e. at voxels with lower values.
/// </summary>
/// <remarks>
/// Creates an occupied space cost function for 3D scan matching.
/// </remarks>
/// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
/// <param name="pointCloud">Point cloud to match.</param>
/// <param name="hybridGrid">Hybrid grid to match against.</param>
public class OccupiedSpaceCostFunction3D(
double scalingFactor,
PointCloud _pointCloud,
HybridGrid hybridGrid) : IDisposable
{
private readonly InterpolatedProbabilityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
/// <summary>
/// Creates a DynamicAutoDiff cost function for occupied space matching.
/// </summary>
/// <param name="scalingFactor">Weight factor.</param>
/// <param name="pointCloud">Point cloud to match.</param>
/// <param name="hybridGrid">Hybrid grid to match against.</param>
/// <returns>DynamicAutoDiff cost function.</returns>
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
double scalingFactor,
PointCloud pointCloud,
HybridGrid hybridGrid)
{
var costFunction = new OccupiedSpaceCostFunction3D(scalingFactor, pointCloud, hybridGrid);
return new DynamicAutoDiffCostFunction(
costFunction.Evaluate,
numResiduals: pointCloud.Count,
parameterBlockSizes: [3, 4] // [translation[3], rotation[4]]
);
}
/// <summary>
/// Evaluates the cost function.
/// </summary>
/// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
/// <param name="residuals">Output residuals (one per point).</param>
/// <returns>True on success.</returns>
private bool Evaluate(double[][] parameters, double[] residuals)
{
if (parameters == null || parameters.Length < 2)
return false;
if (parameters[0].Length < 3 || parameters[1].Length < 4)
return false;
if (residuals == null || residuals.Length < _pointCloud.Count)
return false;
var translation = parameters[0];
var rotation = parameters[1]; // [w, x, y, z] from Ceres
// Create transform from translation and rotation
// C++ line 52-53: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
// where rotation = [w, x, y, z]
// System.Numerics.Quaternion constructor is (x, y, z, w)
var transform = new Rigid3d(
new Vector3(translation[0], translation[1], translation[2]),
new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
);
// Transform each point and compute residual
for (int i = 0; i < _pointCloud.Count; i++)
{
var point = _pointCloud[i];
// Transform point from local frame to world frame
var worldPoint = transform * point.Position;
// Get interpolated probability value at world point
var probability = _interpolatedGrid.GetInterpolatedValue(
worldPoint.X,
worldPoint.Y,
worldPoint.Z
);
// Residual = scaling_factor * (1 - probability)
// Higher probability (occupied space) = lower residual = better match
residuals[i] = scalingFactor * (1.0 - probability);
}
return true;
}
/// <summary>
/// Disposes managed resources.
/// </summary>
public void Dispose()
{
// InterpolatedProbabilityGrid doesn't need disposal, but we implement IDisposable for consistency
GC.SuppressFinalize(this);
}
}

152
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/PrecomputationGrid3D.cs Normal file
View File

@@ -0,0 +1,152 @@
/*
* 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.Mapping.D3D;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Precomputation grid for 3D scan matching using 8-bit values instead of 16-bit.
/// This is used for branch-and-bound algorithm in Fast Correlative Scan Matcher.
/// </summary>
/// <remarks>
/// Creates a new PrecomputationGrid3D with the specified resolution.
/// </remarks>
public class PrecomputationGrid3D(double resolution) : HybridGridBase<byte>(resolution)
{
/// <summary>
/// Minimum probability value.
/// </summary>
public const double kMinProbability = 0.1;
/// <summary>
/// Maximum probability value.
/// </summary>
public const double kMaxProbability = 0.9;
/// <summary>
/// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
/// </summary>
public static double ToProbability(double value)
{
return kMinProbability +
value * ((kMaxProbability - kMinProbability) / 255.0);
}
/// <summary>
/// Gets the value at the given cell index.
/// </summary>
public new byte GetValue(Array3i index)
{
return base.GetValue(index);
}
/// <summary>
/// Sets the value at the given cell index.
/// </summary>
public void SetValue(Array3i index, byte value)
{
ref var cell = ref GetMutableValue(index);
cell = value;
}
}
/// <summary>
/// Converts a HybridGrid to a PrecomputationGrid3D representing the same data,
/// but only using 8 bit instead of 2 x 16 bit.
/// </summary>
public static class PrecomputationGrid3DOperations
{
/// <summary>
/// Converts a HybridGrid to a PrecomputationGrid3D.
/// </summary>
public static PrecomputationGrid3D ConvertToPrecomputationGrid(Mapping.D3D.HybridGrid hybridGrid)
{
var result = new PrecomputationGrid3D(hybridGrid.Resolution);
// Iterate through all cells in the hybrid grid
foreach (var (index, value) in hybridGrid)
{
// Convert probability (ushort) to byte [0, 255]
var probability = ProbabilityValues.ValueToProbability(value);
var cellValue = (int)Math.Round(
(probability - PrecomputationGrid3D.kMinProbability) *
(255.0 / (PrecomputationGrid3D.kMaxProbability - PrecomputationGrid3D.kMinProbability))
);
cellValue = Math.Max(0, Math.Min(255, cellValue));
result.SetValue(index, (byte)cellValue);
}
return result;
}
/// <summary>
/// Returns a grid of the same resolution containing the maximum value of
/// original voxels in 'grid'. This maximum is over the 8 voxels that have
/// any combination of index components optionally increased by 'shift'.
/// </summary>
public static PrecomputationGrid3D PrecomputeGrid(
PrecomputationGrid3D grid,
bool halfResolution,
Array3i shift)
{
var result = new PrecomputationGrid3D(grid.Resolution);
// Iterate through all cells in the input grid
foreach (var (index, value) in grid)
{
// Update 8 values in the resulting grid
for (int i = 0; i < 8; i++)
{
var octant = HybridGridBase<byte>.GetOctant(i);
// Element-wise multiplication: shift * octant
var shiftOctant = new Array3i(
shift.X * octant.X,
shift.Y * octant.Y,
shift.Z * octant.Z
);
var cellIndex = index - shiftOctant;
if (halfResolution)
{
// Convert to half resolution index
cellIndex = CellIndexAtHalfResolution(cellIndex);
}
// Take maximum value
var currentValue = result.GetValue(cellIndex);
var newValue = (byte)Math.Max(value, currentValue);
result.SetValue(cellIndex, newValue);
}
}
return result;
}
/// <summary>
/// Computes the half resolution index corresponding to the full resolution
/// 'cell_index'. Uses bit shift to round towards negative infinity.
/// </summary>
private static Array3i CellIndexAtHalfResolution(Array3i cellIndex)
{
return new Array3i(
cellIndex.X >> 1, // Divide by 2, rounding towards negative infinity
cellIndex.Y >> 1,
cellIndex.Z >> 1
);
}
}

90
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/PrecomputationGridStack3D.cs Normal file
View File

@@ -0,0 +1,90 @@
/*
* 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.Models.Mapping;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Stack of precomputation grids for branch-and-bound algorithm.
/// </summary>
public class PrecomputationGridStack3D
{
private readonly List<PrecomputationGrid3D> _precomputationGrids;
/// <summary>
/// Creates a precomputation grid stack from a hybrid grid.
/// </summary>
public PrecomputationGridStack3D(
Mapping.D3D.HybridGrid hybridGrid,
FastCorrelativeScanMatcherOptions3D options)
{
if (options.BranchAndBoundDepth < 1)
throw new ArgumentException("branch_and_bound_depth must be >= 1", nameof(options));
if (options.FullResolutionDepth < 1)
throw new ArgumentException("full_resolution_depth must be >= 1", nameof(options));
_precomputationGrids = new List<PrecomputationGrid3D>(options.BranchAndBoundDepth)
{
// First grid: convert from hybrid grid
PrecomputationGrid3DOperations.ConvertToPrecomputationGrid(hybridGrid)
};
var lastWidth = new Array3i(1, 1, 1);
// Create grids for each depth
for (int depth = 1; depth < options.BranchAndBoundDepth; depth++)
{
var halfResolution = depth >= options.FullResolutionDepth;
var nextWidth = new Array3i(1 << depth, 1 << depth, 1 << depth);
var fullVoxelsPerHighResolutionVoxel = 1 << Math.Max(0, depth - options.FullResolutionDepth);
// Element-wise division: (nextWidth - lastWidth + (fullVoxelsPerHighResolutionVoxel - 1)) / fullVoxelsPerHighResolutionVoxel
var numerator = nextWidth - lastWidth + new Array3i(fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1);
var shift = new Array3i(
numerator.X / fullVoxelsPerHighResolutionVoxel,
numerator.Y / fullVoxelsPerHighResolutionVoxel,
numerator.Z / fullVoxelsPerHighResolutionVoxel
);
_precomputationGrids.Add(
PrecomputationGrid3DOperations.PrecomputeGrid(
_precomputationGrids[^1],
halfResolution,
shift
)
);
lastWidth = nextWidth;
}
}
/// <summary>
/// Gets the precomputation grid at the specified depth.
/// </summary>
public PrecomputationGrid3D Get(int depth)
{
if (depth < 0 || depth >= _precomputationGrids.Count)
throw new ArgumentOutOfRangeException(nameof(depth));
return _precomputationGrids[depth];
}
/// <summary>
/// Gets the maximum depth (0-based).
/// </summary>
public int MaxDepth => _precomputationGrids.Count - 1;
}

632
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/RealTimeCorrelativeScanMatcher3D.cs Normal file
View File

@@ -0,0 +1,632 @@
/*
* 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.Models.Mapping;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using RobotNet10.Shared.Numbers;
using InterpolatedProbabilityGrid = CartographerSharp.Mapping.Internal.D3D.ScanMatching.InterpolatedProbabilityGrid;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Result of fast correlative scan matching for 3D.
/// </summary>
public struct FastCorrelativeScanMatcher3DResult(double score, Rigid3d poseEstimate, double rotationalScore, double lowResolutionScore)
{
public double Score { get; set; } = score;
public Rigid3d PoseEstimate { get; set; } = poseEstimate;
public double RotationalScore { get; set; } = rotationalScore;
public double LowResolutionScore { get; set; } = lowResolutionScore;
}
/// <summary>
/// Discrete scan structure for 3D scan matching.
/// </summary>
internal struct DiscreteScan3D
{
public Rigid3f Pose { get; set; }
public List<List<Array3i>> CellIndicesPerDepth { get; set; }
public double RotationalScore { get; set; }
}
/// <summary>
/// Candidate structure for branch-and-bound algorithm.
/// </summary>
internal struct Candidate3D(int scanIndex, Array3i offset) : IComparable<Candidate3D>
{
public int ScanIndex { get; set; } = scanIndex;
public Array3i Offset { get; set; } = offset;
public double Score { get; set; } = double.NegativeInfinity;
public double LowResolutionScore { get; set; } = 0.0;
public static Candidate3D Unsuccessful()
{
return new Candidate3D(0, Array3i.Zero);
}
public readonly int CompareTo(Candidate3D other)
{
return Score.CompareTo(other.Score);
}
public static bool operator <(Candidate3D left, Candidate3D right)
{
return left.Score < right.Score;
}
public static bool operator >(Candidate3D left, Candidate3D right)
{
return left.Score > right.Score;
}
}
/// <summary>
/// WARNING: NAMING MISMATCH WITH C++
///
/// This class is actually an implementation of FastCorrelativeScanMatcher3D (branch-and-bound algorithm),
/// NOT RealTimeCorrelativeScanMatcher3D (exhaustive search).
///
/// C++ differences:
/// - real_time_correlative_scan_matcher_3d.cc: Uses exhaustive search with 6 nested loops over
/// a SMALL search window (linear and angular). Simple O(n^6) brute force.
/// - fast_correlative_scan_matcher_3d.cc: Uses branch-and-bound optimization with precomputation
/// grids for efficient search over LARGE windows. This is what this class implements.
///
/// The class name was incorrectly chosen. For constraint building (loop closure), this branch-and-bound
/// implementation is actually correct since it can search over large windows efficiently.
/// For real-time scan matching in LocalTrajectoryBuilder3D, the exhaustive search version should be
/// used (smaller window, simpler, more predictable performance).
///
/// TODO: Consider renaming to FastCorrelativeScanMatcher3D and implementing a proper
/// RealTimeCorrelativeScanMatcher3D for local SLAM if needed.
/// </summary>
public class RealTimeCorrelativeScanMatcher3D(
Mapping.D3D.HybridGrid _hybridGrid,
Mapping.D3D.HybridGrid? lowResolutionHybridGrid,
double[]? rotationalScanMatcherHistogram,
FastCorrelativeScanMatcherOptions3D options)
{
private readonly double _resolution = _hybridGrid.Resolution;
private readonly int _widthInVoxels = 256;
private readonly PrecomputationGridStack3D _precomputationGridStack = new(_hybridGrid, options);
private readonly RotationalScanMatcher _rotationalScanMatcher = new(rotationalScanMatcherHistogram);
/// <summary>
/// Search parameters for branch-and-bound algorithm.
/// </summary>
private struct SearchParameters
{
public int LinearXyWindowSize { get; set; } // voxels
public int LinearZWindowSize { get; set; } // voxels
public double AngularSearchWindow { get; set; } // radians
public Func<Rigid3f, double>? LowResolutionMatcher { get; set; }
}
/// <summary>
/// Creates a low resolution matcher function.
/// </summary>
private static Func<Rigid3f, double>? CreateLowResolutionMatcher(
Mapping.D3D.HybridGrid? lowResolutionGrid,
PointCloud? points)
{
if (lowResolutionGrid == null || points == null || points.Count == 0)
return null;
return pose =>
{
double score = 0.0;
var transformedPoints = PointCloudOperations.Transform(points, pose);
var interpolatedGrid = new InterpolatedProbabilityGrid(lowResolutionGrid);
foreach (var point in transformedPoints)
{
// Use interpolated grid for better score
var probability = interpolatedGrid.GetInterpolatedValue(
point.Position.X,
point.Position.Y,
point.Position.Z);
score += probability;
}
return score / points.Count;
};
}
/// <summary>
/// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
/// given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
/// returned if a score above 'min_score' (excluding equality) is possible.
/// </summary>
public FastCorrelativeScanMatcher3DResult? Match(
Rigid3d globalNodePose,
Rigid3d globalSubmapPose,
TrajectoryNode.Data constantData,
double minScore)
{
ArgumentNullException.ThrowIfNull(constantData);
var pointCloud = constantData.HighResolutionPointCloud;
if (pointCloud == null || pointCloud.Count == 0)
{
return null;
}
var lowResolutionMatcher = CreateLowResolutionMatcher(
lowResolutionHybridGrid,
constantData.LowResolutionPointCloud);
var searchParameters = new SearchParameters
{
LinearXyWindowSize = (int)Math.Round(options.LinearXySearchWindow / _resolution),
LinearZWindowSize = (int)Math.Round(options.LinearZSearchWindow / _resolution),
AngularSearchWindow = options.AngularSearchWindow,
LowResolutionMatcher = lowResolutionMatcher
};
return MatchWithSearchParameters(
searchParameters,
new Rigid3f(globalNodePose.Translation, globalNodePose.Rotation),
new Rigid3f(globalSubmapPose.Translation, globalSubmapPose.Rotation),
pointCloud,
constantData.RotationalScanMatcherHistogram?.ToArray(),
constantData.GravityAlignment,
minScore);
}
/// <summary>
/// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
/// given rotations which are expected to be approximately gravity aligned.
/// 'Result' is only returned if a score above 'min_score' (excluding equality)
/// is possible.
/// </summary>
public FastCorrelativeScanMatcher3DResult? MatchFullSubmap(
Quaternion globalNodeRotation,
Quaternion globalSubmapRotation,
TrajectoryNode.Data constantData,
double minScore)
{
ArgumentNullException.ThrowIfNull(constantData);
var pointCloud = constantData.HighResolutionPointCloud;
if (pointCloud == null || pointCloud.Count == 0)
{
return null;
}
// Compute max point distance to determine search window
double maxPointDistance = 0.0;
foreach (var point in pointCloud)
{
maxPointDistance = Math.Max(maxPointDistance, point.Position.Length());
}
var linearWindowSize = (_widthInVoxels + 1) / 2 +
(int)Math.Round(maxPointDistance / _resolution + 0.5);
var lowResolutionMatcher = CreateLowResolutionMatcher(
lowResolutionHybridGrid,
constantData.LowResolutionPointCloud);
var searchParameters = new SearchParameters
{
LinearXyWindowSize = linearWindowSize,
LinearZWindowSize = linearWindowSize,
AngularSearchWindow = Math.PI,
LowResolutionMatcher = lowResolutionMatcher
};
var globalNodePose = Rigid3f.FromRotation(globalNodeRotation);
var globalSubmapPose = Rigid3f.FromRotation(globalSubmapRotation);
return MatchWithSearchParameters(
searchParameters,
globalNodePose,
globalSubmapPose,
pointCloud,
constantData.RotationalScanMatcherHistogram?.ToArray(),
constantData.GravityAlignment,
minScore);
}
/// <summary>
/// Matches with given search parameters.
/// </summary>
private FastCorrelativeScanMatcher3DResult? MatchWithSearchParameters(
SearchParameters searchParameters,
Rigid3f globalNodePose,
Rigid3f globalSubmapPose,
PointCloud pointCloud,
double[]? rotationalScanMatcherHistogram,
Quaternion gravityAlignment,
double minScore)
{
var discreteScans = GenerateDiscreteScans(
searchParameters,
pointCloud,
rotationalScanMatcherHistogram,
gravityAlignment,
globalNodePose,
globalSubmapPose);
var lowestResolutionCandidates = ComputeLowestResolutionCandidates(
searchParameters,
discreteScans);
var bestCandidate = BranchAndBound(
searchParameters,
discreteScans,
lowestResolutionCandidates,
_precomputationGridStack.MaxDepth,
minScore);
if (bestCandidate.Score > minScore)
{
var pose = GetPoseFromCandidate(discreteScans, bestCandidate);
return new FastCorrelativeScanMatcher3DResult(
bestCandidate.Score,
new Rigid3d(pose.Translation, pose.Rotation),
discreteScans[bestCandidate.ScanIndex].RotationalScore,
bestCandidate.LowResolutionScore);
}
return null;
}
/// <summary>
/// Discretizes a scan at different resolutions for branch-and-bound.
/// </summary>
private DiscreteScan3D DiscretizeScan(
SearchParameters searchParameters,
PointCloud pointCloud,
Rigid3f pose,
double rotationalScore)
{
var cellIndicesPerDepth = new List<List<Array3i>>();
var originalGrid = _precomputationGridStack.Get(0);
// Transform point cloud
var transformedPoints = PointCloudOperations.Transform(pointCloud, pose);
// Get full resolution cell indices
var fullResolutionCellIndices = new List<Array3i>();
foreach (var point in transformedPoints)
{
fullResolutionCellIndices.Add(originalGrid.GetCellIndex(point.Position));
}
var fullResolutionDepth = Math.Min(
options.FullResolutionDepth,
options.BranchAndBoundDepth);
if (fullResolutionDepth < 1)
fullResolutionDepth = 1;
// Add full resolution indices for each depth up to full_resolution_depth
for (int i = 0; i < fullResolutionDepth; i++)
{
cellIndicesPerDepth.Add([.. fullResolutionCellIndices]);
}
var lowResolutionDepth = options.BranchAndBoundDepth - fullResolutionDepth;
if (lowResolutionDepth < 0)
lowResolutionDepth = 0;
var searchWindowStart = new Array3i(
-searchParameters.LinearXyWindowSize,
-searchParameters.LinearXyWindowSize,
-searchParameters.LinearZWindowSize);
// Add low resolution indices
for (int i = 0; i < lowResolutionDepth; i++)
{
var reductionExponent = i + 1;
var lowResolutionSearchWindowStart = new Array3i(
searchWindowStart.X >> reductionExponent,
searchWindowStart.Y >> reductionExponent,
searchWindowStart.Z >> reductionExponent);
var lowResolutionIndices = new List<Array3i>();
foreach (var cellIndex in fullResolutionCellIndices)
{
var cellAtStart = cellIndex + searchWindowStart;
var lowResolutionCellAtStart = new Array3i(
cellAtStart.X >> reductionExponent,
cellAtStart.Y >> reductionExponent,
cellAtStart.Z >> reductionExponent);
lowResolutionIndices.Add(
lowResolutionCellAtStart - lowResolutionSearchWindowStart);
}
cellIndicesPerDepth.Add(lowResolutionIndices);
}
return new DiscreteScan3D
{
Pose = pose,
CellIndicesPerDepth = cellIndicesPerDepth,
RotationalScore = rotationalScore
};
}
/// <summary>
/// Generates discrete scans for different rotation angles.
/// </summary>
private List<DiscreteScan3D> GenerateDiscreteScans(
SearchParameters searchParameters,
PointCloud pointCloud,
double[]? rotationalScanMatcherHistogram,
Quaternion gravityAlignment,
Rigid3f globalNodePose,
Rigid3f globalSubmapPose)
{
var result = new List<DiscreteScan3D>();
// Compute max scan range
double maxScanRange = 3.0 * _resolution;
foreach (var point in pointCloud)
{
var range = point.Position.Length();
maxScanRange = Math.Max(range, maxScanRange);
}
const double kSafetyMargin = 1.0 - 1e-2;
var angularStepSize = kSafetyMargin * Math.Acos(
1.0 - MathUtils.Pow2(_resolution) / (2.0 * MathUtils.Pow2(maxScanRange)));
var angularWindowSize = (int)Math.Round(
searchParameters.AngularSearchWindow / angularStepSize);
var angles = new List<double>();
for (int rz = -angularWindowSize; rz <= angularWindowSize; rz++)
{
angles.Add(rz * angularStepSize);
}
var nodeToSubmap = globalSubmapPose.Inverse() * globalNodePose;
var initialAngle = TransformOperations.GetYaw(
nodeToSubmap.Rotation * Quaternion.Inverse(gravityAlignment));
var scores = _rotationalScanMatcher.Match(
rotationalScanMatcherHistogram ?? [],
initialAngle,
angles);
for (int i = 0; i < angles.Count; i++)
{
if (scores[i] < options.MinRotationalScore)
continue;
var angleAxis = new Vector3(0.0f, 0.0f, angles[i]);
// Apply rotation between translation and rotation of initial_pose
var pose = new Rigid3f(
nodeToSubmap.Translation,
Quaternion.Inverse(globalSubmapPose.Rotation) *
TransformOperations.AngleAxisVectorToRotationQuaternion(angleAxis) *
globalNodePose.Rotation);
result.Add(DiscretizeScan(searchParameters, pointCloud, pose, scores[i]));
}
return result;
}
/// <summary>
/// Generates candidates at the lowest resolution.
/// </summary>
private List<Candidate3D> GenerateLowestResolutionCandidates(
SearchParameters searchParameters,
int numDiscreteScans)
{
var linearStepSize = 1 << _precomputationGridStack.MaxDepth;
var numLowestResolutionLinearXyCandidates =
(2 * searchParameters.LinearXyWindowSize + linearStepSize) / linearStepSize;
var numLowestResolutionLinearZCandidates =
(2 * searchParameters.LinearZWindowSize + linearStepSize) / linearStepSize;
var numCandidates = numDiscreteScans *
MathUtils.Power(numLowestResolutionLinearXyCandidates, 2) *
numLowestResolutionLinearZCandidates;
var candidates = new List<Candidate3D>((int)numCandidates);
for (int scanIndex = 0; scanIndex < numDiscreteScans; scanIndex++)
{
for (int z = -searchParameters.LinearZWindowSize;
z <= searchParameters.LinearZWindowSize;
z += linearStepSize)
{
for (int y = -searchParameters.LinearXyWindowSize;
y <= searchParameters.LinearXyWindowSize;
y += linearStepSize)
{
for (int x = -searchParameters.LinearXyWindowSize;
x <= searchParameters.LinearXyWindowSize;
x += linearStepSize)
{
candidates.Add(new Candidate3D(scanIndex, new Array3i(x, y, z)));
}
}
}
}
return candidates;
}
/// <summary>
/// Scores candidates at a given depth.
/// </summary>
private void ScoreCandidates(
int depth,
List<DiscreteScan3D> discreteScans,
List<Candidate3D> candidates)
{
var reductionExponent = Math.Max(0, depth - options.FullResolutionDepth + 1);
for (int i = 0; i < candidates.Count; i++)
{
var candidate = candidates[i];
int sum = 0;
var discreteScan = discreteScans[candidate.ScanIndex];
var offset = new Array3i(
candidate.Offset.X >> reductionExponent,
candidate.Offset.Y >> reductionExponent,
candidate.Offset.Z >> reductionExponent);
if (depth >= discreteScan.CellIndicesPerDepth.Count)
continue;
var grid = _precomputationGridStack.Get(depth);
foreach (var cellIndex in discreteScan.CellIndicesPerDepth[depth])
{
var proposedCellIndex = cellIndex + offset;
sum += grid.GetValue(proposedCellIndex);
}
var newScore = PrecomputationGrid3D.ToProbability(
sum / discreteScan.CellIndicesPerDepth[depth].Count);
// Create new candidate with updated score
var updatedCandidate = new Candidate3D(candidate.ScanIndex, candidate.Offset)
{
Score = newScore,
LowResolutionScore = candidate.LowResolutionScore
};
candidates[i] = updatedCandidate;
}
// Sort candidates by score (descending)
candidates.Sort((a, b) => b.Score.CompareTo(a.Score));
}
/// <summary>
/// Computes candidates at the lowest resolution.
/// </summary>
private List<Candidate3D> ComputeLowestResolutionCandidates(
SearchParameters searchParameters,
List<DiscreteScan3D> discreteScans)
{
var lowestResolutionCandidates = GenerateLowestResolutionCandidates(
searchParameters,
discreteScans.Count);
ScoreCandidates(
_precomputationGridStack.MaxDepth,
discreteScans,
lowestResolutionCandidates);
return lowestResolutionCandidates;
}
/// <summary>
/// Gets pose from candidate.
/// </summary>
private Rigid3f GetPoseFromCandidate(
List<DiscreteScan3D> discreteScans,
Candidate3D candidate)
{
var translation = (_resolution) * candidate.Offset.ToVector3();
return Rigid3f.FromTranslation(translation) * discreteScans[candidate.ScanIndex].Pose;
}
/// <summary>
/// Branch-and-bound algorithm to find best candidate.
/// </summary>
private Candidate3D BranchAndBound(
SearchParameters searchParameters,
List<DiscreteScan3D> discreteScans,
List<Candidate3D> candidates,
int candidateDepth,
double minScore)
{
if (candidateDepth == 0)
{
foreach (var candidate in candidates)
{
if (candidate.Score <= minScore)
{
// Return if candidate is bad because following candidates won't be better
return Candidate3D.Unsuccessful();
}
if (searchParameters.LowResolutionMatcher == null)
continue;
var lowResolutionScore = searchParameters.LowResolutionMatcher(
GetPoseFromCandidate(discreteScans, candidate));
if (lowResolutionScore >= options.MinLowResolutionScore)
{
// Found best candidate that passes matching function
var bestCandidate = candidate;
bestCandidate.LowResolutionScore = lowResolutionScore;
return bestCandidate;
}
}
// All candidates have good scores but none passes matching function
return Candidate3D.Unsuccessful();
}
var bestHighResolutionCandidate = Candidate3D.Unsuccessful();
bestHighResolutionCandidate.Score = minScore;
foreach (var candidate in candidates)
{
if (candidate.Score <= minScore)
break;
var higherResolutionCandidates = new List<Candidate3D>();
var halfWidth = 1 << (candidateDepth - 1);
for (int z = 0; z <= halfWidth; z += halfWidth)
{
if (candidate.Offset.Z + z > searchParameters.LinearZWindowSize)
break;
for (int y = 0; y <= halfWidth; y += halfWidth)
{
if (candidate.Offset.Y + y > searchParameters.LinearXyWindowSize)
break;
for (int x = 0; x <= halfWidth; x += halfWidth)
{
if (candidate.Offset.X + x > searchParameters.LinearXyWindowSize)
break;
higherResolutionCandidates.Add(new Candidate3D(
candidate.ScanIndex,
candidate.Offset + new Array3i(x, y, z)));
}
}
}
ScoreCandidates(candidateDepth - 1, discreteScans, higherResolutionCandidates);
// C++ line 433-437: std::max(best_high_resolution_candidate, BranchAndBound(...))
// This ensures we always get the candidate with the highest score (or equal)
var bestCandidate = BranchAndBound(
searchParameters,
discreteScans,
higherResolutionCandidates,
candidateDepth - 1,
bestHighResolutionCandidate.Score);
// Use >= to match std::max behavior (prefer new candidate if score is equal or greater)
if (bestCandidate.Score >= bestHighResolutionCandidate.Score)
{
bestHighResolutionCandidate = bestCandidate;
}
}
return bestHighResolutionCandidate;
}
}

100
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/RotationDeltaCostFunctor3D.cs Normal file
View File

@@ -0,0 +1,100 @@
/*
* 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 CeresSharp;
using RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
/// Cost increases with the solution's distance from 'target_rotation'.
/// </summary>
/// <remarks>
/// Creates a rotation delta cost functor for 3D.
/// </remarks>
/// <param name="scalingFactor">Weight factor.</param>
/// <param name="targetRotation">Target rotation to match.</param>
public class RotationDeltaCostFunctor3D(double scalingFactor, Quaternion targetRotation)
{
private readonly double[] _targetRotationInverse =
[
targetRotation.W,
-targetRotation.X,
-targetRotation.Y,
-targetRotation.Z
]; // [w, x, y, z]
/// <summary>
/// Creates a DynamicAutoDiff cost function for rotation delta.
/// </summary>
/// <param name="scalingFactor">Weight factor.</param>
/// <param name="targetRotation">Target rotation to match.</param>
/// <returns>DynamicAutoDiff cost function.</returns>
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
double scalingFactor,
Quaternion targetRotation)
{
var functor = new RotationDeltaCostFunctor3D(scalingFactor, targetRotation);
return new DynamicAutoDiffCostFunction(
functor.Evaluate,
numResiduals: 3, // [x, y, z] - imaginary part of delta quaternion
parameterBlockSizes: [4] // [w, x, y, z] - quaternion
);
}
/// <summary>
/// Evaluates the cost function.
/// Computes delta = target_rotation_inverse * rotation_quaternion
/// Returns the imaginary part (x, y, z) of the delta quaternion.
/// </summary>
/// <param name="parameters">Rotation quaternion [w, x, y, z].</param>
/// <param name="residuals">Output residuals [x, y, z] - imaginary part of delta.</param>
/// <returns>True on success.</returns>
private bool Evaluate(double[][] parameters, double[] residuals)
{
if (parameters == null || parameters.Length < 1 || parameters[0].Length < 4)
return false;
if (residuals == null || residuals.Length < 3)
return false;
var rotation = parameters[0];
// Compute quaternion product: target_rotation_inverse * rotation
// delta = q_inv * q = [w1, x1, y1, z1] * [w2, x2, y2, z2]
// delta.w = w1*w2 - x1*x2 - y1*y2 - z1*z2
// delta.x = w1*x2 + x1*w2 + y1*z2 - z1*y2
// delta.y = w1*y2 - x1*z2 + y1*w2 + z1*x2
// delta.z = w1*z2 + x1*y2 - y1*x2 + z1*w2
var w1 = _targetRotationInverse[0];
var x1 = _targetRotationInverse[1];
var y1 = _targetRotationInverse[2];
var z1 = _targetRotationInverse[3];
var w2 = rotation[0];
var x2 = rotation[1];
var y2 = rotation[2];
var z2 = rotation[3];
// Compute delta quaternion (only need imaginary part for residual)
// The squared norm of the imaginary component is sin(phi/2)^2
residuals[0] = scalingFactor * (w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2);
residuals[1] = scalingFactor * (w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2);
residuals[2] = scalingFactor * (w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2);
return true;
}
}

312
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/RotationalScanMatcher.cs Normal file
View File

@@ -0,0 +1,312 @@
/*
* 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 RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Rotational scan matcher for 3D scan matching.
/// Computes histogram-based rotational matching scores.
/// Match C++ RotationalScanMatcher (rotational_scan_matcher.cc)
/// </summary>
public class RotationalScanMatcher(double[]? _histogram)
{
// Constants from C++ (rotational_scan_matcher.cc lines 31-33)
private const float kMinDistance = 0.2f;
private const float kMaxDistance = 0.9f;
private const float kSliceHeight = 0.2f;
/// <summary>
/// Rotates the given 'histogram' by the given 'angle'. This might lead to
/// rotations of a fractional bucket which is handled by linearly interpolating.
/// Match C++ RotateHistogram (rotational_scan_matcher.cc lines 141-162)
/// </summary>
public static double[] RotateHistogram(double[] histogram, double angle)
{
if (histogram == null || histogram.Length == 0)
return histogram ?? [];
var numBuckets = histogram.Length;
// C++: rotate_by_buckets = -angle * histogram.size() / M_PI
var rotateByBuckets = -angle * numBuckets / Math.PI;
var fullBuckets = (int)Math.Round(rotateByBuckets - 0.5);
var fraction = rotateByBuckets - fullBuckets;
// Normalize full_buckets to be non-negative
while (fullBuckets < 0)
{
fullBuckets += numBuckets;
}
// Create two rotated histograms for interpolation
var rotatedHistogram0 = new double[numBuckets];
var rotatedHistogram1 = new double[numBuckets];
for (int i = 0; i < numBuckets; i++)
{
rotatedHistogram0[i] = histogram[(i + fullBuckets) % numBuckets];
rotatedHistogram1[i] = histogram[(i + 1 + fullBuckets) % numBuckets];
}
// Linear interpolation: fraction * rotated_histogram_1 + (1 - fraction) * rotated_histogram_0
var result = new double[numBuckets];
for (int i = 0; i < numBuckets; i++)
{
result[i] = fraction * rotatedHistogram1[i] + (1.0 - fraction) * rotatedHistogram0[i];
}
return result;
}
/// <summary>
/// Computes the histogram for a gravity aligned 'point_cloud'.
/// Match C++ ComputeHistogram (rotational_scan_matcher.cc lines 164-176)
///
/// Algorithm:
/// 1. Divide points into horizontal slices by Z coordinate
/// 2. For each slice, compute centroid and sort points by angle around centroid
/// 3. Compute angle differences between consecutive points
/// 4. Weight values by orthogonality to centroid direction (reject ceiling/floor angles)
/// </summary>
public static double[] ComputeHistogram(PointCloud pointCloud, int histogramSize)
{
if (pointCloud == null || pointCloud.Count == 0)
return new double[histogramSize];
var histogram = new double[histogramSize];
// Step 1: Divide points into slices by Z (C++ lines 167-171)
var slices = new Dictionary<int, List<RangefinderPoint>>();
foreach (var point in pointCloud)
{
var sliceIndex = (int)Math.Round(point.Position.Z / kSliceHeight);
if (!slices.TryGetValue(sliceIndex, out var slice))
{
slice = [];
slices[sliceIndex] = slice;
}
slice.Add(point);
}
// Step 2: Process each slice (C++ lines 172-174)
foreach (var slice in slices.Values)
{
AddPointCloudSliceToHistogram(SortSlice(slice), histogram);
}
return histogram;
}
/// <summary>
/// Computes the centroid of a point cloud slice.
/// Match C++ ComputeCentroid (rotational_scan_matcher.cc lines 52-59)
/// </summary>
private static Vector3 ComputeCentroid(List<RangefinderPoint> slice)
{
if (slice.Count == 0)
return Vector3.Zero;
var sum = Vector3.Zero;
foreach (var point in slice)
{
sum += point.Position;
}
return sum / slice.Count;
}
/// <summary>
/// Sorts points in a slice by angle around the centroid.
/// Match C++ SortSlice (rotational_scan_matcher.cc lines 94-119)
/// </summary>
private static List<RangefinderPoint> SortSlice(List<RangefinderPoint> slice)
{
if (slice.Count == 0)
return [];
var centroid = ComputeCentroid(slice);
// Create list of (angle, point) pairs
var byAngle = new List<(double angle, RangefinderPoint point)>();
foreach (var point in slice)
{
var delta = new Vector2(
point.Position.X - centroid.X,
point.Position.Y - centroid.Y);
if (delta.Length() < kMinDistance)
continue;
var angle = Math.Atan2(delta.Y, delta.X);
byAngle.Add((angle, point));
}
// Sort by angle
byAngle.Sort((a, b) => a.angle.CompareTo(b.angle));
// Return sorted points
return byAngle.Select(p => p.point).ToList();
}
/// <summary>
/// Adds histogram values for a sorted point cloud slice.
/// Match C++ AddPointCloudSliceToHistogram (rotational_scan_matcher.cc lines 61-89)
/// </summary>
private static void AddPointCloudSliceToHistogram(List<RangefinderPoint> sortedSlice, double[] histogram)
{
if (sortedSlice.Count == 0)
return;
var centroid = ComputeCentroid(sortedSlice);
var lastPointPosition = sortedSlice[0].Position;
foreach (var point in sortedSlice)
{
// Compute delta between consecutive points (2D only, XY plane)
var delta = new Vector2(
point.Position.X - lastPointPosition.X,
point.Position.Y - lastPointPosition.Y);
// Direction from centroid to current point
var direction = new Vector2(
point.Position.X - centroid.X,
point.Position.Y - centroid.Y);
var distance = delta.Length();
if (distance < kMinDistance || direction.Length() < kMinDistance)
{
continue;
}
if (distance > kMaxDistance)
{
lastPointPosition = point.Position;
continue;
}
// Compute angle of the delta vector
var angle = (float)Math.Atan2(delta.Y, delta.X);
// Weight: orthogonality to centroid direction (reject ceiling/floor angles)
// Value is higher when delta is perpendicular to direction
var deltaNorm = Vector2.Normalize(delta);
var directionNorm = Vector2.Normalize(direction);
var dotProduct = Vector2.Dot(deltaNorm, directionNorm);
var value = Math.Max(0.0, 1.0 - Math.Abs(dotProduct));
AddValueToHistogram(angle, value, histogram);
}
}
/// <summary>
/// Adds a value to the histogram at the given angle.
/// Match C++ AddValueToHistogram (rotational_scan_matcher.cc lines 35-50)
/// </summary>
private static void AddValueToHistogram(float angle, double value, double[] histogram)
{
// Map the angle to [0, pi), i.e. a vector and its inverse are considered to
// represent the same angle.
while (angle > Math.PI)
{
angle -= (float)Math.PI;
}
while (angle < 0)
{
angle += (float)Math.PI;
}
var zeroToOne = angle / Math.PI;
var bucket = Math.Clamp(
(int)Math.Round(histogram.Length * zeroToOne - 0.5),
0,
histogram.Length - 1);
histogram[bucket] += value;
}
/// <summary>
/// Matches two histograms and returns a normalized score.
/// Match C++ MatchHistograms (rotational_scan_matcher.cc lines 121-132)
/// </summary>
private static double MatchHistograms(double[] submapHistogram, double[] scanHistogram)
{
// We compute the dot product of normalized histograms as a measure of similarity.
var scanNorm = ComputeNorm(scanHistogram);
var submapNorm = ComputeNorm(submapHistogram);
var normalization = scanNorm * submapNorm;
if (normalization < 1e-3)
{
return 1.0; // Both histograms are nearly zero, consider them similar
}
var dotProduct = 0.0;
for (int i = 0; i < scanHistogram.Length && i < submapHistogram.Length; i++)
{
dotProduct += scanHistogram[i] * submapHistogram[i];
}
return dotProduct / normalization;
}
/// <summary>
/// Computes the L2 norm of a histogram.
/// </summary>
private static double ComputeNorm(double[] histogram)
{
var sumSquares = 0.0;
foreach (var val in histogram)
{
sumSquares += val * val;
}
return Math.Sqrt(sumSquares);
}
/// <summary>
/// Scores how well 'histogram' rotated by 'initial_angle' can be understood as
/// further rotated by certain 'angles' relative to the 'nodes'. Each angle
/// results in a score between 0 (worst) and 1 (best).
/// Match C++ Match (rotational_scan_matcher.cc lines 178-189)
/// </summary>
public List<double> Match(double[] histogram, double initialAngle, List<double> angles)
{
if (_histogram == null || _histogram.Length == 0)
{
// Return zero scores if no reference histogram
return [.. angles.Select(_ => 0.0)];
}
if (histogram == null || histogram.Length != _histogram.Length)
{
return [.. angles.Select(_ => 0.0)];
}
var scores = new List<double>();
foreach (var angle in angles)
{
var totalAngle = initialAngle + angle;
var rotatedHistogram = RotateHistogram(histogram, totalAngle);
// Use MatchHistograms which normalizes by the product of norms
var score = MatchHistograms(_histogram, rotatedHistogram);
scores.Add(score);
}
return scores;
}
}

74
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ScanMatching/TranslationDeltaCostFunctor3D.cs Normal file
View File

@@ -0,0 +1,74 @@
/*
* 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 CeresSharp;
using RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
/// <summary>
/// Computes the cost of translating 'translation' to 'target_translation'.
/// Cost increases with the solution's distance from 'target_translation'.
/// </summary>
/// <remarks>
/// Creates a translation delta cost functor for 3D.
/// </remarks>
/// <param name="scalingFactor">Weight factor.</param>
/// <param name="targetTranslation">Target translation to match.</param>
public class TranslationDeltaCostFunctor3D(double scalingFactor, Vector3 targetTranslation)
{
private readonly double _targetX = targetTranslation.X;
private readonly double _targetY = targetTranslation.Y;
private readonly double _targetZ = targetTranslation.Z;
/// <summary>
/// Creates a DynamicAutoDiff cost function for translation delta.
/// </summary>
/// <param name="scalingFactor">Weight factor.</param>
/// <param name="targetTranslation">Target translation to match.</param>
/// <returns>DynamicAutoDiff cost function.</returns>
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
double scalingFactor,
Vector3 targetTranslation)
{
var functor = new TranslationDeltaCostFunctor3D(scalingFactor, targetTranslation);
return new DynamicAutoDiffCostFunction(
functor.Evaluate,
numResiduals: 3, // [x, y, z]
parameterBlockSizes: [3] // [x, y, z]
);
}
/// <summary>
/// Evaluates the cost function.
/// </summary>
/// <param name="parameters">Translation parameters [x, y, z].</param>
/// <param name="residuals">Output residuals [x, y, z].</param>
/// <returns>True on success.</returns>
private bool Evaluate(double[][] parameters, double[] residuals)
{
if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
return false;
if (residuals == null || residuals.Length < 3)
return false;
var translation = parameters[0];
residuals[0] = scalingFactor * (translation[0] - _targetX);
residuals[1] = scalingFactor * (translation[1] - _targetY);
residuals[2] = scalingFactor * (translation[2] - _targetZ);
return true;
}
}