Initial commit
This commit is contained in:
@@ -0,0 +1,476 @@
|
||||
/*
|
||||
* 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.D2D;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using CeresSharp;
|
||||
using CeresSharp.Enums;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
|
||||
using TSDF2DGrid = CartographerSharp.Mapping.D2D.TSDF2D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Align scans with an existing map using Ceres.
|
||||
/// </summary>
|
||||
public class CeresScanMatcher2D : IDisposable
|
||||
{
|
||||
private readonly CeresScanMatcherOptions2D _options;
|
||||
private readonly SolverOptions _solverOptions;
|
||||
private bool _disposed;
|
||||
|
||||
// Static counters for tracking scan matching statistics
|
||||
private static int _totalMatchAttempts = 0;
|
||||
private static int _successfulMatches = 0;
|
||||
|
||||
// Thread-safe counter for tracking active scan matching operations
|
||||
private static int _activeScanMatchingCount = 0;
|
||||
|
||||
// Static cache for BiCubicInterpolator resources to avoid expensive re-computation
|
||||
// PrecomputeGridData() takes ~1000ms per call, caching reduces this to near-zero
|
||||
private static readonly GridInterpolatorCache _interpolatorCache = new(maxCacheSize: 10);
|
||||
|
||||
public CeresScanMatcher2D(CeresScanMatcherOptions2D options)
|
||||
{
|
||||
_options = options;
|
||||
|
||||
// Initialize CeresSharp solver options
|
||||
// Match C++ ceres_scan_matcher_2d.cc line 66-68: CreateCeresSolverOptions(options.ceres_solver_options())
|
||||
// C++ Ceres defaults: function_tolerance=1e-6, gradient_tolerance=1e-10, parameter_tolerance=1e-8
|
||||
// These are NOT explicitly set in C++, so they use Ceres library defaults.
|
||||
_solverOptions = new SolverOptions
|
||||
{
|
||||
// Set linear solver type to DENSE_QR for 2D scan matching (match C++ line 68)
|
||||
LinearSolverType = LinearSolverType.DenseQr,
|
||||
|
||||
// Configure from CeresSolverOptions if available, otherwise use C++ Ceres defaults
|
||||
MaxNumIterations = options.CeresSolverOptions?.MaxNumIterations ?? 50, // C++ Ceres default is 50
|
||||
NumThreads = options.CeresSolverOptions?.NumThreads ?? 1, // Single thread for scan matching
|
||||
UseNonmonotonicSteps = options.CeresSolverOptions?.UseNonmonotonicSteps ?? false,
|
||||
|
||||
// Tolerance settings - use C++ Ceres defaults unless explicitly configured
|
||||
// C++ Ceres defaults: function_tolerance=1e-6, gradient_tolerance=1e-10, parameter_tolerance=1e-8
|
||||
// Note: If convergence issues occur with small costs (~0.1), consider relaxing these:
|
||||
// FunctionTolerance: 1e-3 (allows 0.1% cost reduction)
|
||||
// GradientTolerance: 1e-6
|
||||
// ParameterTolerance: 1e-6
|
||||
FunctionTolerance = options.CeresSolverOptions?.FunctionTolerance ?? 1e-6, // C++ Ceres default
|
||||
GradientTolerance = options.CeresSolverOptions?.GradientTolerance ?? 1e-10, // C++ Ceres default
|
||||
ParameterTolerance = options.CeresSolverOptions?.ParameterTolerance ?? 1e-8 // C++ Ceres default
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aligns 'point_cloud' within the 'grid' given an
|
||||
/// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
|
||||
/// 'summary'.
|
||||
/// </summary>
|
||||
/// <param name="targetTranslation">Target translation to match</param>
|
||||
/// <param name="initialPoseEstimate">Initial pose estimate</param>
|
||||
/// <param name="pointCloud">Point cloud to match</param>
|
||||
/// <param name="grid">Grid to match against</param>
|
||||
/// <param name="poseEstimate">Output pose estimate</param>
|
||||
/// <param name="summary">Output solver summary</param>
|
||||
/// <param name="highResGrid">Optional high resolution grid for matching</param>
|
||||
public void Match(
|
||||
Vector2 targetTranslation,
|
||||
Rigid2d initialPoseEstimate,
|
||||
PointCloud pointCloud,
|
||||
Grid2D grid,
|
||||
out Rigid2d poseEstimate,
|
||||
out SolverSummary summary,
|
||||
Grid2D? highResGrid = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pointCloud);
|
||||
ArgumentNullException.ThrowIfNull(grid);
|
||||
if (pointCloud.Count == 0)
|
||||
{
|
||||
poseEstimate = initialPoseEstimate;
|
||||
// Create empty summary for empty point cloud case
|
||||
// NOTE: summary is an 'out' parameter, so caller is responsible for disposing it
|
||||
// CRITICAL: Create minimal problem to avoid memory leak
|
||||
using var emptyProblem = new Problem();
|
||||
// CRITICAL: Create new SolverOptions for each solve to avoid any state leakage
|
||||
using var emptyOptions = new SolverOptions
|
||||
{
|
||||
LinearSolverType = LinearSolverType.DenseQr,
|
||||
MaxNumIterations = 1 // Minimal iterations for empty case
|
||||
};
|
||||
summary = emptyProblem.Solve(emptyOptions);
|
||||
return;
|
||||
}
|
||||
|
||||
// Validate weights
|
||||
if (_options.OccupiedSpaceWeight <= 0.0)
|
||||
throw new ArgumentException("OccupiedSpaceWeight 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 [x, y, theta]
|
||||
var poseParams = new double[3]
|
||||
{
|
||||
initialPoseEstimate.Translation.X,
|
||||
initialPoseEstimate.Translation.Y,
|
||||
initialPoseEstimate.Rotation
|
||||
};
|
||||
|
||||
// Increment active scan matching counter (thread-safe)
|
||||
Interlocked.Increment(ref _activeScanMatchingCount);
|
||||
|
||||
// List to store cost function instances that need explicit disposal
|
||||
// These instances must be disposed after Problem.Solve completes:
|
||||
// - OccupiedSpaceCostFunction2D: holds unmanaged resources (BiCubicInterpolator) that MUST be disposed
|
||||
// - TSDFMatchCostFunction2D: implements IDisposable pattern (good practice to dispose)
|
||||
// Note: DynamicAutoDiffCostFunction objects added to Problem are owned by Problem and will
|
||||
// be disposed when Problem is disposed. However, the underlying cost function instances
|
||||
// (OccupiedSpaceCostFunction2D, TSDFMatchCostFunction2D) are NOT owned by Problem and must
|
||||
// be explicitly disposed.
|
||||
var costFunctionInstances = new List<IDisposable>();
|
||||
|
||||
try
|
||||
{
|
||||
var swTotal = Stopwatch.StartNew();
|
||||
var swStep = new Stopwatch();
|
||||
|
||||
// Create Ceres problem
|
||||
// Problem will own all CostFunction objects added via AddResidualBlock
|
||||
// and dispose them when Problem is disposed (via 'using' statement)
|
||||
using var problem = new Problem();
|
||||
|
||||
// Add parameter block
|
||||
problem.AddParameterBlock(poseParams, 3);
|
||||
|
||||
// Reuse parameter blocks array to avoid creating new arrays for each AddResidualBlock call
|
||||
// This reduces allocation overhead when Match is called frequently
|
||||
var parameterBlocks = new double[][] { poseParams };
|
||||
|
||||
// Match C++: Always use standard weights, regardless of high res grid
|
||||
// C++: options_.occupied_space_weight(), options_.translation_weight(), options_.rotation_weight()
|
||||
var occupiedSpaceWeight = _options.OccupiedSpaceWeight;
|
||||
var translationWeight = _options.TranslationWeight;
|
||||
var rotationWeight = _options.RotationWeight;
|
||||
|
||||
// Add occupied space cost function for main grid
|
||||
swStep.Restart();
|
||||
switch (grid.GetGridType())
|
||||
{
|
||||
case GridType.ProbabilityGrid:
|
||||
{
|
||||
// Use cached interpolator to avoid expensive PrecomputeGridData() (~1000ms savings)
|
||||
var cachedInterpolator = _interpolatorCache.GetOrCreate(grid);
|
||||
|
||||
// Create the underlying cost function instance with cached interpolator
|
||||
var occupiedSpaceCostFunctionInstance = new OccupiedSpaceCostFunction2D(
|
||||
occupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
|
||||
pointCloud,
|
||||
grid,
|
||||
cachedInterpolator);
|
||||
costFunctionInstances.Add(occupiedSpaceCostFunctionInstance);
|
||||
|
||||
var occupiedSpaceCost = new DynamicAutoDiffCostFunction(
|
||||
occupiedSpaceCostFunctionInstance.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
problem.AddResidualBlock(occupiedSpaceCost, null, parameterBlocks);
|
||||
}
|
||||
break;
|
||||
case GridType.TSDF:
|
||||
if (grid is TSDF2DGrid tsdfGrid)
|
||||
{
|
||||
// Create the underlying cost function instance explicitly to track it
|
||||
var tsdfMatchCostFunctionInstance = new TSDFMatchCostFunction2D(
|
||||
occupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
|
||||
pointCloud,
|
||||
tsdfGrid);
|
||||
costFunctionInstances.Add(tsdfMatchCostFunctionInstance);
|
||||
|
||||
var tsdfMatchCost = new DynamicAutoDiffCostFunction(
|
||||
tsdfMatchCostFunctionInstance.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
// residualBlockId is just an identifier, doesn't need to be stored or freed
|
||||
_ = problem.AddResidualBlock(tsdfMatchCost, null, parameterBlocks);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
throw new ArgumentException($"Unsupported grid type: {grid.GetGridType()}", nameof(grid));
|
||||
}
|
||||
swStep.Stop();
|
||||
var mainGridCostMs = swStep.Elapsed.TotalMilliseconds;
|
||||
|
||||
// Add high resolution grid cost function if provided
|
||||
swStep.Restart();
|
||||
double highResCacheMs = 0, highResCostFuncMs = 0, highResAddBlockMs = 0;
|
||||
if (highResGrid != null)
|
||||
{
|
||||
switch (highResGrid.GetGridType())
|
||||
{
|
||||
case GridType.ProbabilityGrid:
|
||||
{
|
||||
var highResOccupiedSpaceWeight = _options.HighResOccupiedSpaceWeight ?? _options.OccupiedSpaceWeight;
|
||||
|
||||
// Use cached interpolator to avoid expensive PrecomputeGridData() (~1000ms savings)
|
||||
var swHrSub = Stopwatch.StartNew();
|
||||
var cachedHighResInterpolator = _interpolatorCache.GetOrCreate(highResGrid);
|
||||
swHrSub.Stop();
|
||||
highResCacheMs = swHrSub.Elapsed.TotalMilliseconds;
|
||||
|
||||
// Create the underlying cost function instance with cached interpolator
|
||||
swHrSub.Restart();
|
||||
var highResOccupiedSpaceCostFunctionInstance = new OccupiedSpaceCostFunction2D(
|
||||
highResOccupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
|
||||
pointCloud,
|
||||
highResGrid,
|
||||
cachedHighResInterpolator);
|
||||
costFunctionInstances.Add(highResOccupiedSpaceCostFunctionInstance);
|
||||
|
||||
var highResOccupiedSpaceCost = new DynamicAutoDiffCostFunction(
|
||||
highResOccupiedSpaceCostFunctionInstance.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
swHrSub.Stop();
|
||||
highResCostFuncMs = swHrSub.Elapsed.TotalMilliseconds;
|
||||
|
||||
// residualBlockId is just an identifier, doesn't need to be stored or freed
|
||||
swHrSub.Restart();
|
||||
_ = problem.AddResidualBlock(highResOccupiedSpaceCost, null, parameterBlocks);
|
||||
swHrSub.Stop();
|
||||
highResAddBlockMs = swHrSub.Elapsed.TotalMilliseconds;
|
||||
}
|
||||
break;
|
||||
case GridType.TSDF:
|
||||
if (highResGrid is TSDF2DGrid highResTsdfGrid)
|
||||
{
|
||||
var highResOccupiedSpaceWeight = _options.HighResOccupiedSpaceWeight ?? _options.OccupiedSpaceWeight;
|
||||
// Create the underlying cost function instance explicitly to track it
|
||||
var swHrSub = Stopwatch.StartNew();
|
||||
var highResTsdfMatchCostFunctionInstance = new TSDFMatchCostFunction2D(
|
||||
highResOccupiedSpaceWeight / Math.Sqrt(pointCloud.Count),
|
||||
pointCloud,
|
||||
highResTsdfGrid);
|
||||
costFunctionInstances.Add(highResTsdfMatchCostFunctionInstance);
|
||||
|
||||
var highResTsdfMatchCost = new DynamicAutoDiffCostFunction(
|
||||
highResTsdfMatchCostFunctionInstance.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
swHrSub.Stop();
|
||||
highResCostFuncMs = swHrSub.Elapsed.TotalMilliseconds;
|
||||
|
||||
// residualBlockId is just an identifier, doesn't need to be stored or freed
|
||||
swHrSub.Restart();
|
||||
_ = problem.AddResidualBlock(highResTsdfMatchCost, null, parameterBlocks);
|
||||
swHrSub.Stop();
|
||||
highResAddBlockMs = swHrSub.Elapsed.TotalMilliseconds;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
throw new ArgumentException($"Unsupported high resolution grid type: {highResGrid.GetGridType()}", nameof(highResGrid));
|
||||
}
|
||||
}
|
||||
swStep.Stop();
|
||||
var highResGridCostMs = swStep.Elapsed.TotalMilliseconds;
|
||||
|
||||
// Add translation delta cost function
|
||||
var translationCost = TranslationDeltaCostFunctor2D.CreateAutoDiffCostFunction(
|
||||
translationWeight,
|
||||
targetTranslation
|
||||
);
|
||||
// residualBlockId is just an identifier, doesn't need to be stored or freed
|
||||
_ = problem.AddResidualBlock(translationCost, null, parameterBlocks);
|
||||
var rotationCost = RotationDeltaCostFunctor2D.CreateAutoDiffCostFunction(
|
||||
rotationWeight,
|
||||
poseParams[2]
|
||||
);
|
||||
// residualBlockId is just an identifier, doesn't need to be stored or freed
|
||||
_ = problem.AddResidualBlock(rotationCost, null, parameterBlocks);
|
||||
|
||||
// Solve the optimization problem
|
||||
// CRITICAL: SolverSummary holds unmanaged resources (SolverSummaryHandle) that MUST be disposed
|
||||
// by the caller. Since summary is an 'out' parameter and may be used by caller after Match returns,
|
||||
// we cannot dispose it here. The caller MUST dispose summary after use to prevent memory leaks.
|
||||
swStep.Restart();
|
||||
summary = problem.Solve(_solverOptions);
|
||||
swStep.Stop();
|
||||
var solveMs = swStep.Elapsed.TotalMilliseconds;
|
||||
|
||||
try
|
||||
{
|
||||
swTotal.Stop();
|
||||
var totalMs = swTotal.Elapsed.TotalMilliseconds;
|
||||
|
||||
// Log timing if any step takes significant time (> 10ms)
|
||||
if (totalMs > 100.0)
|
||||
{
|
||||
Console.WriteLine($"[CeresScanMatcher2D] Match: total={totalMs:F1}ms, mainGridCost={mainGridCostMs:F1}ms, highResCost={highResGridCostMs:F1}ms (cache={highResCacheMs:F1}ms, costFunc={highResCostFuncMs:F1}ms, addBlock={highResAddBlockMs:F1}ms), solve={solveMs:F1}ms, points={pointCloud.Count}, iterations={summary?.Iterations ?? 0}, cache={GetInterpolatorCacheStats()}");
|
||||
}
|
||||
|
||||
// Update statistics (thread-safe)
|
||||
Interlocked.Increment(ref _totalMatchAttempts);
|
||||
bool isSuccess = summary != null &&
|
||||
summary.InitialCost != -1.0 &&
|
||||
summary.TerminationType == TerminationType.Convergence;
|
||||
|
||||
if (isSuccess)
|
||||
{
|
||||
Interlocked.Increment(ref _successfulMatches);
|
||||
}
|
||||
|
||||
// Extract result
|
||||
poseEstimate = new Rigid2d(
|
||||
new Vector2(poseParams[0], poseParams[1]),
|
||||
poseParams[2]
|
||||
);
|
||||
}
|
||||
catch
|
||||
{
|
||||
// If exception occurs after Solve() but before return, dispose summary
|
||||
// to prevent native handle leak (caller won't receive the out parameter)
|
||||
summary?.Dispose();
|
||||
summary = null!;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// Dispose cost function instances after Problem.Solve completes (or on exception)
|
||||
// CRITICAL: Problem.Solve() has completed, so native code is no longer using callbacks.
|
||||
// Problem will be disposed by 'using' statement, which will dispose DynamicAutoDiffCostFunction
|
||||
// objects. However, the underlying cost function instances (OccupiedSpaceCostFunction2D,
|
||||
// TSDFMatchCostFunction2D) are NOT owned by Problem and must be explicitly disposed:
|
||||
// - OccupiedSpaceCostFunction2D: holds unmanaged resources (BiCubicInterpolator) that MUST be disposed
|
||||
// - TSDFMatchCostFunction2D: implements IDisposable pattern (should be disposed for consistency)
|
||||
// It is safe to dispose these instances here because:
|
||||
// 1. Problem.Solve() has completed, so callbacks are no longer called
|
||||
// 2. Problem will be disposed immediately after this finally block (via 'using' statement)
|
||||
// Dispose all cost function instances to free unmanaged resources
|
||||
foreach (var instance in costFunctionInstances)
|
||||
{
|
||||
try
|
||||
{
|
||||
instance?.Dispose();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore disposal errors - instance may already be disposed or may have been disposed
|
||||
// by finalizer in case of exception
|
||||
}
|
||||
}
|
||||
// Note: costFunctionInstances will go out of scope after method returns, allowing GC collection.
|
||||
// No need to explicitly call Clear().
|
||||
|
||||
// Note: We do NOT call GC.Collect here because:
|
||||
// 1. Unmanaged resources are explicitly disposed above
|
||||
// 2. Forced GC can cause performance issues and is generally not recommended
|
||||
// 3. The GC will run automatically when needed
|
||||
|
||||
// Decrement active scan matching counter (thread-safe)
|
||||
Interlocked.Decrement(ref _activeScanMatchingCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of active scan matching operations currently running.
|
||||
/// </summary>
|
||||
public static int GetActiveScanMatchingCount()
|
||||
{
|
||||
return _activeScanMatchingCount;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Waits for all active scan matching operations to complete.
|
||||
/// </summary>
|
||||
/// <param name="maxWaitTime">Maximum time to wait</param>
|
||||
/// <param name="checkInterval">Interval between checks</param>
|
||||
/// <returns>True if all scan matching completed, false if timeout</returns>
|
||||
public static bool WaitForAllScanMatchingToComplete(TimeSpan maxWaitTime, TimeSpan checkInterval)
|
||||
{
|
||||
var startTime = DateTime.UtcNow;
|
||||
|
||||
while (DateTime.UtcNow - startTime < maxWaitTime)
|
||||
{
|
||||
var activeCount = _activeScanMatchingCount;
|
||||
if (activeCount == 0)
|
||||
{
|
||||
return true; // All scan matching completed
|
||||
}
|
||||
|
||||
Thread.Sleep(checkInterval);
|
||||
}
|
||||
|
||||
// Timeout - check one more time
|
||||
return _activeScanMatchingCount == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of cached interpolators.
|
||||
/// </summary>
|
||||
public static int GetInterpolatorCacheSize()
|
||||
{
|
||||
return _interpolatorCache.Count;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets cache statistics as a formatted string.
|
||||
/// </summary>
|
||||
public static string GetInterpolatorCacheStats()
|
||||
{
|
||||
return $"size={_interpolatorCache.Count}, hits={_interpolatorCache.CacheHits}, misses={_interpolatorCache.CacheMisses}, hitRate={_interpolatorCache.HitRate:P1}";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Invalidates cached interpolator for a specific grid.
|
||||
/// Call this when a grid is modified to force re-computation on next scan match.
|
||||
/// </summary>
|
||||
/// <param name="grid">The grid whose cache entry should be invalidated.</param>
|
||||
public static void InvalidateGridCache(Grid2D grid)
|
||||
{
|
||||
_interpolatorCache.Invalidate(grid);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clears all cached interpolators.
|
||||
/// </summary>
|
||||
public static void ClearInterpolatorCache()
|
||||
{
|
||||
_interpolatorCache.Clear();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disposes the solver options and other managed resources.
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_solverOptions?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,278 @@
|
||||
/*
|
||||
* 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.D2D;
|
||||
using CartographerSharp.Sensor;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Discrete scan representation as a list of integer cell indices.
|
||||
/// </summary>
|
||||
public class DiscreteScan2D : List<Array2i>
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Describes the search space for scan matching.
|
||||
/// </summary>
|
||||
public class SearchParameters
|
||||
{
|
||||
/// <summary>
|
||||
/// Linear search window in pixel offsets; bounds are inclusive.
|
||||
/// </summary>
|
||||
public struct LinearBounds(int minX, int maxX, int minY, int maxY)
|
||||
{
|
||||
public int MinX { get; set; } = minX;
|
||||
public int MaxX { get; set; } = maxX;
|
||||
public int MinY { get; set; } = minY;
|
||||
public int MaxY { get; set; } = maxY;
|
||||
}
|
||||
|
||||
public int NumAngularPerturbations { get; set; }
|
||||
public double AngularPerturbationStepSize { get; set; }
|
||||
public double Resolution { get; set; }
|
||||
public int NumScans { get; set; }
|
||||
public List<LinearBounds> LinearBoundsList { get; set; } // Per rotated scans
|
||||
|
||||
// === MEMORY OPTIMIZATION: Cap maximum NumScans to prevent excessive allocations ===
|
||||
// Each scan creates a rotated point cloud copy + discretized version
|
||||
// With 500 points per scan, 500 scans = ~18MB. 3000 scans = ~108MB per MatchFullSubmap call.
|
||||
// Multiple concurrent calls can cause GB-level memory spikes.
|
||||
private const int MaxNumScans = 500;
|
||||
|
||||
public SearchParameters(
|
||||
double linearSearchWindow,
|
||||
double angularSearchWindow,
|
||||
PointCloud pointCloud,
|
||||
double resolution)
|
||||
{
|
||||
Resolution = resolution;
|
||||
|
||||
// Compute max scan range
|
||||
double maxScanRange = 3.0 * resolution;
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
var range = new Vector2(point.Position.X, point.Position.Y).Length();
|
||||
maxScanRange = Math.Max(range, maxScanRange);
|
||||
}
|
||||
|
||||
// Compute angular perturbation step size
|
||||
const double kSafetyMargin = 1.0 - 1e-3;
|
||||
var resolutionSquared = resolution * resolution;
|
||||
var maxScanRangeSquared = maxScanRange * maxScanRange;
|
||||
// FIX: Clamp argument to valid Acos range [-1, 1] to prevent NaN
|
||||
// This can occur with extreme resolution/maxScanRange ratios
|
||||
var acosArg = Math.Clamp(1.0 - resolutionSquared / (2.0 * maxScanRangeSquared), -1.0, 1.0);
|
||||
AngularPerturbationStepSize = kSafetyMargin * Math.Acos(acosArg);
|
||||
|
||||
NumAngularPerturbations = (int)Math.Ceiling(angularSearchWindow / AngularPerturbationStepSize);
|
||||
NumScans = 2 * NumAngularPerturbations + 1;
|
||||
|
||||
// === MEMORY OPTIMIZATION: Cap NumScans to prevent memory exhaustion ===
|
||||
// If NumScans exceeds limit, increase angular step size to reduce scan count
|
||||
if (NumScans > MaxNumScans)
|
||||
{
|
||||
var originalNumScans = NumScans;
|
||||
var originalStepSize = AngularPerturbationStepSize;
|
||||
|
||||
// Recalculate with capped scans
|
||||
NumAngularPerturbations = (MaxNumScans - 1) / 2;
|
||||
NumScans = 2 * NumAngularPerturbations + 1;
|
||||
AngularPerturbationStepSize = angularSearchWindow / NumAngularPerturbations;
|
||||
|
||||
/*Console.WriteLine($"[SearchParameters] CAPPED NumScans: {originalNumScans} -> {NumScans}, " +
|
||||
$"AngularStep: {originalStepSize * 180 / Math.PI:F4}° -> {AngularPerturbationStepSize * 180 / Math.PI:F4}°, " +
|
||||
$"AngularSearchWindow={angularSearchWindow * 180 / Math.PI:F1}°");*/
|
||||
}
|
||||
|
||||
// Compute linear bounds for each rotated scan
|
||||
var numLinearPerturbations = (int)Math.Ceiling(linearSearchWindow / resolution);
|
||||
LinearBoundsList = [];
|
||||
for (int i = 0; i < NumScans; i++)
|
||||
{
|
||||
LinearBoundsList.Add(new LinearBounds(
|
||||
-numLinearPerturbations,
|
||||
numLinearPerturbations,
|
||||
-numLinearPerturbations,
|
||||
numLinearPerturbations
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
public SearchParameters(
|
||||
int numLinearPerturbations,
|
||||
int numAngularPerturbations,
|
||||
double angularPerturbationStepSize,
|
||||
double resolution)
|
||||
{
|
||||
NumAngularPerturbations = numAngularPerturbations;
|
||||
AngularPerturbationStepSize = angularPerturbationStepSize;
|
||||
Resolution = resolution;
|
||||
NumScans = 2 * numAngularPerturbations + 1;
|
||||
|
||||
//var linearSearchWindow = numLinearPerturbations * resolution;
|
||||
LinearBoundsList = [];
|
||||
for (int i = 0; i < NumScans; i++)
|
||||
{
|
||||
LinearBoundsList.Add(new LinearBounds(
|
||||
-numLinearPerturbations,
|
||||
numLinearPerturbations,
|
||||
-numLinearPerturbations,
|
||||
numLinearPerturbations
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tightens the search window as much as possible.
|
||||
/// </summary>
|
||||
public void ShrinkToFit(List<DiscreteScan2D> scans, CellLimits cellLimits)
|
||||
{
|
||||
if (scans.Count != NumScans)
|
||||
throw new ArgumentException($"scans.Count ({scans.Count}) must equal NumScans ({NumScans})", nameof(scans));
|
||||
if (LinearBoundsList.Count != NumScans)
|
||||
throw new ArgumentException($"LinearBoundsList.Count ({LinearBoundsList.Count}) must equal NumScans ({NumScans})", nameof(LinearBoundsList));
|
||||
|
||||
for (int i = 0; i < NumScans; i++)
|
||||
{
|
||||
var scan = scans[i];
|
||||
|
||||
// Compute min_bound and max_bound like C++: min_bound.min(-xy_index) and max_bound.max(cell_limits - xy_index)
|
||||
var minBound = Array2i.Zero;
|
||||
var maxBound = Array2i.Zero;
|
||||
|
||||
foreach (var xyIndex in scan)
|
||||
{
|
||||
// min_bound = min_bound.min(-xy_index)
|
||||
minBound = new Array2i(
|
||||
Math.Min(minBound.X, -xyIndex.X),
|
||||
Math.Min(minBound.Y, -xyIndex.Y)
|
||||
);
|
||||
|
||||
// max_bound = max_bound.max(cell_limits - xy_index)
|
||||
var cellLimitMinusXY = new Array2i(
|
||||
cellLimits.NumXCells - 1 - xyIndex.X,
|
||||
cellLimits.NumYCells - 1 - xyIndex.Y
|
||||
);
|
||||
maxBound = new Array2i(
|
||||
Math.Max(maxBound.X, cellLimitMinusXY.X),
|
||||
Math.Max(maxBound.Y, cellLimitMinusXY.Y)
|
||||
);
|
||||
}
|
||||
|
||||
var bounds = LinearBoundsList[i];
|
||||
bounds.MinX = Math.Max(bounds.MinX, minBound.X);
|
||||
bounds.MaxX = Math.Min(bounds.MaxX, maxBound.X);
|
||||
bounds.MinY = Math.Max(bounds.MinY, minBound.Y);
|
||||
bounds.MaxY = Math.Min(bounds.MaxY, maxBound.Y);
|
||||
|
||||
LinearBoundsList[i] = bounds;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// A possible solution for scan matching.
|
||||
/// </summary>
|
||||
public struct Candidate2D(int scanIndex, int xIndexOffset, int yIndexOffset, SearchParameters searchParameters) : IComparable<Candidate2D>
|
||||
{
|
||||
public int ScanIndex { get; set; } = scanIndex;
|
||||
public int XIndexOffset { get; set; } = xIndexOffset;
|
||||
public int YIndexOffset { get; set; } = yIndexOffset;
|
||||
public double X { get; set; } = -yIndexOffset * searchParameters.Resolution;
|
||||
public double Y { get; set; } = -xIndexOffset * searchParameters.Resolution;
|
||||
public double Orientation { get; set; } = (scanIndex - searchParameters.NumAngularPerturbations) * searchParameters.AngularPerturbationStepSize;
|
||||
public double Score { get; set; } = 0.0;
|
||||
|
||||
public readonly int CompareTo(Candidate2D other)
|
||||
{
|
||||
return Score.CompareTo(other.Score);
|
||||
}
|
||||
|
||||
public static bool operator <(Candidate2D left, Candidate2D right)
|
||||
{
|
||||
return left.Score < right.Score;
|
||||
}
|
||||
|
||||
public static bool operator >(Candidate2D left, Candidate2D right)
|
||||
{
|
||||
return left.Score > right.Score;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Generates a collection of rotated scans.
|
||||
/// </summary>
|
||||
public static class ScanMatchingUtilities
|
||||
{
|
||||
public static List<PointCloud> GenerateRotatedScans(
|
||||
PointCloud pointCloud,
|
||||
SearchParameters searchParameters)
|
||||
{
|
||||
var rotatedScans = new List<PointCloud>();
|
||||
|
||||
for (int i = 0; i < searchParameters.NumScans; i++)
|
||||
{
|
||||
var angle = (i - searchParameters.NumAngularPerturbations) * searchParameters.AngularPerturbationStepSize;
|
||||
var rotation = Matrix3x2.CreateRotation(angle);
|
||||
|
||||
var rotatedScan = new PointCloud();
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotation);
|
||||
rotatedScan.Add(new RangefinderPoint
|
||||
{
|
||||
Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
|
||||
});
|
||||
}
|
||||
|
||||
rotatedScans.Add(rotatedScan);
|
||||
}
|
||||
|
||||
return rotatedScans;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Translates and discretizes the rotated scans into a vector of integer indices.
|
||||
/// </summary>
|
||||
public static List<DiscreteScan2D> DiscretizeScans(
|
||||
MapLimits mapLimits,
|
||||
List<PointCloud> scans,
|
||||
Vector2 initialTranslation)
|
||||
{
|
||||
var discreteScans = new List<DiscreteScan2D>();
|
||||
|
||||
foreach (var scan in scans)
|
||||
{
|
||||
var discreteScan = new DiscreteScan2D();
|
||||
foreach (var point in scan)
|
||||
{
|
||||
var translatedPoint = new Vector2(
|
||||
point.Position.X + initialTranslation.X,
|
||||
point.Position.Y + initialTranslation.Y
|
||||
);
|
||||
var cellIndex = mapLimits.GetCellIndex(translatedPoint);
|
||||
discreteScan.Add(cellIndex);
|
||||
}
|
||||
discreteScans.Add(discreteScan);
|
||||
}
|
||||
|
||||
return discreteScans;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,435 @@
|
||||
/*
|
||||
* 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 System.Buffers;
|
||||
using CartographerSharp.Common.Math;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
|
||||
using MapLimits2D = CartographerSharp.Mapping.D2D.MapLimits;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// An implementation of "Real-Time Correlative Scan Matching" by Olson.
|
||||
/// It is similar to the RealTimeCorrelativeScanMatcher but has a different
|
||||
/// trade-off: Scan matching is faster because more effort is put into the
|
||||
/// precomputation done for a given map. However, this map is immutable after
|
||||
/// construction.
|
||||
/// </summary>
|
||||
public class FastCorrelativeScanMatcher2D(Grid2D grid, FastCorrelativeScanMatcherOptions2D _options)
|
||||
{
|
||||
private readonly MapLimits2D _limits = grid.Limits;
|
||||
private readonly PrecomputationGridStack2D _precomputationGridStack = new(grid, _options);
|
||||
|
||||
/// <summary>
|
||||
/// Aligns 'pointCloud' within the 'grid' given an
|
||||
/// 'initialPoseEstimate'. If a score above 'minScore' (excluding equality)
|
||||
/// is possible, true is returned, and 'score' and 'poseEstimate' are updated
|
||||
/// with the result.
|
||||
/// </summary>
|
||||
public bool Match(
|
||||
Rigid2d initialPoseEstimate,
|
||||
PointCloud pointCloud,
|
||||
double minScore,
|
||||
out double score,
|
||||
out Rigid2d poseEstimate)
|
||||
{
|
||||
var searchParameters = new SearchParameters(
|
||||
_options.LinearSearchWindow,
|
||||
_options.AngularSearchWindow,
|
||||
pointCloud,
|
||||
_limits.Resolution);
|
||||
|
||||
return MatchWithSearchParameters(
|
||||
searchParameters,
|
||||
initialPoseEstimate,
|
||||
pointCloud,
|
||||
minScore,
|
||||
out score,
|
||||
out poseEstimate);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aligns 'pointCloud' within the 'grid' using localization search windows.
|
||||
/// Match C++: LocalizationMatch() with localization_linear_search_window and localization_angular_search_window.
|
||||
/// </summary>
|
||||
public bool LocalizationMatch(
|
||||
Rigid2d initialPoseEstimate,
|
||||
PointCloud pointCloud,
|
||||
double minScore,
|
||||
out double score,
|
||||
out Rigid2d poseEstimate)
|
||||
{
|
||||
if (!_options.LocalizationLinearSearchWindow.HasValue || !_options.LocalizationAngularSearchWindow.HasValue)
|
||||
{
|
||||
score = 0.0;
|
||||
poseEstimate = initialPoseEstimate;
|
||||
return false;
|
||||
}
|
||||
|
||||
var searchParameters = new SearchParameters(
|
||||
_options.LocalizationLinearSearchWindow.Value,
|
||||
_options.LocalizationAngularSearchWindow.Value,
|
||||
pointCloud,
|
||||
_limits.Resolution);
|
||||
|
||||
return MatchWithSearchParameters(
|
||||
searchParameters,
|
||||
initialPoseEstimate,
|
||||
pointCloud,
|
||||
minScore,
|
||||
out score,
|
||||
out poseEstimate);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aligns 'pointCloud' with custom search windows and optional resolution.
|
||||
/// Match C++: MatchWithCustomizeParameters().
|
||||
/// </summary>
|
||||
/// <param name="resolution">Resolution for search; use -1.0 to use grid resolution.</param>
|
||||
public bool MatchWithCustomizeParameters(
|
||||
double linearSearchWindow,
|
||||
double angularSearchWindow,
|
||||
double resolution,
|
||||
Rigid2d initialPoseEstimate,
|
||||
PointCloud pointCloud,
|
||||
double minScore,
|
||||
out double score,
|
||||
out Rigid2d poseEstimate)
|
||||
{
|
||||
var res = resolution >= 0.0 ? resolution : _limits.Resolution;
|
||||
var searchParameters = new SearchParameters(
|
||||
linearSearchWindow,
|
||||
angularSearchWindow,
|
||||
pointCloud,
|
||||
res);
|
||||
|
||||
return MatchWithSearchParameters(
|
||||
searchParameters,
|
||||
initialPoseEstimate,
|
||||
pointCloud,
|
||||
minScore,
|
||||
out score,
|
||||
out poseEstimate);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aligns 'pointCloud' within the full 'grid', i.e., not
|
||||
/// restricted to the configured search window. If a score above 'minScore'
|
||||
/// (excluding equality) is possible, true is returned, and 'score' and
|
||||
/// 'poseEstimate' are updated with the result.
|
||||
/// Match C++: Always uses full submap search with 1e3 * resolution and PI.
|
||||
/// </summary>
|
||||
public bool MatchFullSubmap(
|
||||
PointCloud pointCloud,
|
||||
double minScore,
|
||||
out double score,
|
||||
out Rigid2d poseEstimate)
|
||||
{
|
||||
// Match C++ exactly: Always use full submap search (1e3 cells/direction, 180 degrees)
|
||||
// C++: SearchParameters(1e3 * limits_.resolution(), M_PI, point_cloud, limits_.resolution())
|
||||
var linearSearchWindow = 1e3 * _limits.Resolution;
|
||||
var angularSearchWindow = Math.PI;
|
||||
|
||||
var searchParameters = new SearchParameters(
|
||||
linearSearchWindow, // Linear search window, 1e3 cells/direction
|
||||
angularSearchWindow, // Angular search window, 180 degrees in both directions
|
||||
pointCloud,
|
||||
_limits.Resolution);
|
||||
|
||||
// Match C++: center = Rigid2d::Translation(limits_.max() - 0.5 * resolution * Vector2d(num_y_cells, num_x_cells))
|
||||
var centerTranslation = _limits.Max -
|
||||
(0.5 * _limits.Resolution) *
|
||||
new Vector2(_limits.CellLimits.NumYCells, _limits.CellLimits.NumXCells);
|
||||
var center = new Rigid2d(centerTranslation, 0.0);
|
||||
|
||||
return MatchWithSearchParameters(
|
||||
searchParameters,
|
||||
center,
|
||||
pointCloud,
|
||||
minScore,
|
||||
out score,
|
||||
out poseEstimate);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// The actual implementation of the scan matcher, called by Match() and
|
||||
/// MatchFullSubmap() with appropriate 'initialPoseEstimate' and 'searchParameters'.
|
||||
/// </summary>
|
||||
private bool MatchWithSearchParameters(
|
||||
SearchParameters searchParameters,
|
||||
Rigid2d initialPoseEstimate,
|
||||
PointCloud pointCloud,
|
||||
double minScore,
|
||||
out double score,
|
||||
out Rigid2d poseEstimate)
|
||||
{
|
||||
score = 0.0;
|
||||
poseEstimate = initialPoseEstimate;
|
||||
|
||||
var initialAngle = initialPoseEstimate.Rotation;
|
||||
|
||||
// Rotate point cloud to align with initial rotation
|
||||
var rotationMatrix = Matrix3x2.CreateRotation(initialAngle);
|
||||
var rotatedPointCloud = new PointCloud();
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotationMatrix);
|
||||
rotatedPointCloud.Add(new RangefinderPoint
|
||||
{
|
||||
Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
|
||||
});
|
||||
}
|
||||
|
||||
// Generate rotated scans
|
||||
var rotatedScans = ScanMatchingUtilities.GenerateRotatedScans(rotatedPointCloud, searchParameters);
|
||||
|
||||
// Discretize scans
|
||||
var initialTranslation = new Vector2(initialPoseEstimate.Translation.X, initialPoseEstimate.Translation.Y);
|
||||
var discreteScans = ScanMatchingUtilities.DiscretizeScans(_limits, rotatedScans, initialTranslation);
|
||||
|
||||
// Shrink search parameters to fit
|
||||
searchParameters.ShrinkToFit(discreteScans, _limits.CellLimits);
|
||||
|
||||
// Compute lowest resolution candidates
|
||||
var lowestResolutionCandidates = ComputeLowestResolutionCandidates(discreteScans, searchParameters);
|
||||
|
||||
// Branch and bound search
|
||||
var bestCandidate = BranchAndBound(
|
||||
discreteScans,
|
||||
searchParameters,
|
||||
lowestResolutionCandidates,
|
||||
_precomputationGridStack.MaxDepth,
|
||||
minScore);
|
||||
|
||||
// === MEMORY CLEANUP: Clear large lists to help GC ===
|
||||
// These lists are on LOH (>85KB) and won't be collected until Gen2 GC
|
||||
// Clearing them allows GC to reclaim memory sooner
|
||||
// Note: PointCloud doesn't have Clear(), so we just let GC handle it
|
||||
rotatedScans.Clear();
|
||||
|
||||
foreach (var scan in discreteScans)
|
||||
{
|
||||
scan.Clear();
|
||||
}
|
||||
discreteScans.Clear();
|
||||
|
||||
lowestResolutionCandidates.Clear();
|
||||
|
||||
// Force Gen2 GC every N calls to prevent LOH fragmentation
|
||||
// Gen2 GC is expensive but necessary to reclaim LOH memory
|
||||
if (Interlocked.Increment(ref _matchCallCount) % 10 == 0)
|
||||
{
|
||||
GC.Collect(2, GCCollectionMode.Optimized, false);
|
||||
}
|
||||
|
||||
if (bestCandidate.Score > minScore)
|
||||
{
|
||||
score = bestCandidate.Score;
|
||||
poseEstimate = new Rigid2d(
|
||||
new Vector2(
|
||||
(initialPoseEstimate.Translation.X + bestCandidate.X),
|
||||
(initialPoseEstimate.Translation.Y + bestCandidate.Y)),
|
||||
initialAngle + bestCandidate.Orientation);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Counter for periodic GC
|
||||
private static int _matchCallCount = 0;
|
||||
|
||||
/// <summary>
|
||||
/// Computes lowest resolution candidates for branch-and-bound search.
|
||||
/// </summary>
|
||||
private List<Candidate2D> ComputeLowestResolutionCandidates(
|
||||
List<DiscreteScan2D> discreteScans,
|
||||
SearchParameters searchParameters)
|
||||
{
|
||||
var lowestResolutionCandidates = GenerateLowestResolutionCandidates(searchParameters);
|
||||
ScoreCandidates(
|
||||
_precomputationGridStack.Get(_precomputationGridStack.MaxDepth),
|
||||
discreteScans,
|
||||
lowestResolutionCandidates);
|
||||
return lowestResolutionCandidates;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Generates candidates at the lowest resolution for branch-and-bound search.
|
||||
/// </summary>
|
||||
private List<Candidate2D> GenerateLowestResolutionCandidates(SearchParameters searchParameters)
|
||||
{
|
||||
var linearStepSize = 1 << _precomputationGridStack.MaxDepth;
|
||||
var candidates = new List<Candidate2D>();
|
||||
|
||||
// === DEBUG: Estimate candidate count before generation ===
|
||||
long estimatedCandidates = 0;
|
||||
for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
|
||||
{
|
||||
var bounds = searchParameters.LinearBoundsList[scanIndex];
|
||||
var xSteps = (bounds.MaxX - bounds.MinX) / linearStepSize + 1;
|
||||
var ySteps = (bounds.MaxY - bounds.MinY) / linearStepSize + 1;
|
||||
estimatedCandidates += (long)xSteps * ySteps;
|
||||
}
|
||||
for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
|
||||
{
|
||||
var bounds = searchParameters.LinearBoundsList[scanIndex];
|
||||
for (int xIndexOffset = bounds.MinX; xIndexOffset <= bounds.MaxX; xIndexOffset += linearStepSize)
|
||||
{
|
||||
for (int yIndexOffset = bounds.MinY; yIndexOffset <= bounds.MaxY; yIndexOffset += linearStepSize)
|
||||
{
|
||||
candidates.Add(new Candidate2D(scanIndex, xIndexOffset, yIndexOffset, searchParameters));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return candidates;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scores candidates using the precomputation grid.
|
||||
/// </summary>
|
||||
private static void ScoreCandidates(PrecomputationGrid2D precomputationGrid, List<DiscreteScan2D> discreteScans, List<Candidate2D> candidates)
|
||||
{
|
||||
for (int i = 0; i < candidates.Count; i++)
|
||||
{
|
||||
var candidate = candidates[i];
|
||||
if (candidate.ScanIndex >= discreteScans.Count)
|
||||
{
|
||||
candidate.Score = 0.0;
|
||||
candidates[i] = candidate;
|
||||
continue;
|
||||
}
|
||||
|
||||
var discreteScan = discreteScans[candidate.ScanIndex];
|
||||
if (discreteScan.Count == 0)
|
||||
{
|
||||
candidate.Score = 0.0;
|
||||
candidates[i] = candidate;
|
||||
continue;
|
||||
}
|
||||
|
||||
int sum = 0;
|
||||
foreach (var xyIndex in discreteScan)
|
||||
{
|
||||
var proposedXYIndex = new Array2i(
|
||||
xyIndex.X + candidate.XIndexOffset,
|
||||
xyIndex.Y + candidate.YIndexOffset);
|
||||
sum += precomputationGrid.GetValue(proposedXYIndex);
|
||||
}
|
||||
|
||||
// CRITICAL FIX: Use floating-point division to match C++ behavior
|
||||
// C++ uses: static_cast<float>(sum) / static_cast<float>(discrete_scan.size())
|
||||
candidate.Score = precomputationGrid.ToScore((double)sum / discreteScan.Count);
|
||||
candidates[i] = candidate;
|
||||
}
|
||||
|
||||
// Sort candidates by score (descending)
|
||||
candidates.Sort((a, b) => b.Score.CompareTo(a.Score));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Branch-and-bound search for best candidate.
|
||||
/// </summary>
|
||||
private Candidate2D BranchAndBound(
|
||||
List<DiscreteScan2D> discreteScans,
|
||||
SearchParameters searchParameters,
|
||||
List<Candidate2D> candidates,
|
||||
int candidateDepth,
|
||||
double minScore)
|
||||
{
|
||||
if (candidateDepth == 0)
|
||||
{
|
||||
// Return the best candidate (first element after sorting by ScoreCandidates)
|
||||
if (candidates.Count == 0)
|
||||
{
|
||||
return new Candidate2D(0, 0, 0, searchParameters);
|
||||
}
|
||||
return candidates[0];
|
||||
}
|
||||
|
||||
var bestHighResolutionCandidate = new Candidate2D(0, 0, 0, searchParameters)
|
||||
{
|
||||
Score = minScore
|
||||
};
|
||||
|
||||
foreach (var candidate in candidates)
|
||||
{
|
||||
if (candidate.Score <= minScore)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// Generate higher resolution candidates
|
||||
var higherResolutionCandidates = new List<Candidate2D>();
|
||||
var halfWidth = 1 << (candidateDepth - 1);
|
||||
var bounds = searchParameters.LinearBoundsList[candidate.ScanIndex];
|
||||
|
||||
foreach (var xOffset in new[] { 0, halfWidth })
|
||||
{
|
||||
if (candidate.XIndexOffset + xOffset > bounds.MaxX)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
foreach (var yOffset in new[] { 0, halfWidth })
|
||||
{
|
||||
if (candidate.YIndexOffset + yOffset > bounds.MaxY)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
higherResolutionCandidates.Add(new Candidate2D(
|
||||
candidate.ScanIndex,
|
||||
candidate.XIndexOffset + xOffset,
|
||||
candidate.YIndexOffset + yOffset,
|
||||
searchParameters));
|
||||
}
|
||||
}
|
||||
|
||||
// Score higher resolution candidates
|
||||
ScoreCandidates(
|
||||
_precomputationGridStack.Get(candidateDepth - 1),
|
||||
discreteScans,
|
||||
higherResolutionCandidates);
|
||||
|
||||
// Recursively search higher resolution
|
||||
var bestCandidate = BranchAndBound(
|
||||
discreteScans,
|
||||
searchParameters,
|
||||
higherResolutionCandidates,
|
||||
candidateDepth - 1,
|
||||
bestHighResolutionCandidate.Score);
|
||||
|
||||
// Clear to help GC - these lists accumulate in deep recursion
|
||||
higherResolutionCandidates.Clear();
|
||||
|
||||
// Match C++: std::max(best_high_resolution_candidate, BranchAndBound(...))
|
||||
// std::max uses operator> which compares scores, and returns FIRST element if equal
|
||||
// Therefore, we should only update if strictly greater (not >=)
|
||||
if (bestCandidate.Score > bestHighResolutionCandidate.Score)
|
||||
{
|
||||
bestHighResolutionCandidate = bestCandidate;
|
||||
}
|
||||
}
|
||||
|
||||
return bestHighResolutionCandidate;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,355 @@
|
||||
/*
|
||||
* 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.Mapping.D2D;
|
||||
using CeresSharp;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Runtime.CompilerServices;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Cached interpolator resources for a grid.
|
||||
/// Contains the pre-computed ProbabilityGridAdapter and BiCubicInterpolator.
|
||||
/// </summary>
|
||||
internal sealed class CachedGridInterpolator : IDisposable
|
||||
{
|
||||
public ProbabilityGridAdapter Adapter { get; }
|
||||
public BiCubicInterpolator Interpolator { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Grid identity hash at cache time (for validation).
|
||||
/// </summary>
|
||||
public int GridHashCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Grid cell limits at cache time (for validation).
|
||||
/// Only invalidate cache when grid SIZE changes (GrowLimits), not when cells are updated.
|
||||
/// Using slightly stale interpolation data is acceptable for scan matching.
|
||||
/// </summary>
|
||||
public (int NumXCells, int NumYCells) CellLimits { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Grid resolution at cache time (for validation).
|
||||
/// </summary>
|
||||
public double Resolution { get; }
|
||||
|
||||
private bool _disposed;
|
||||
|
||||
public CachedGridInterpolator(
|
||||
Grid2D grid,
|
||||
ProbabilityGridAdapter adapter,
|
||||
BiCubicInterpolator interpolator)
|
||||
{
|
||||
Adapter = adapter ?? throw new ArgumentNullException(nameof(adapter));
|
||||
Interpolator = interpolator ?? throw new ArgumentNullException(nameof(interpolator));
|
||||
|
||||
// Store grid state for validation
|
||||
// Only track size and resolution, NOT cell contents (KnownCellsBox)
|
||||
// Reason: KnownCellsBox changes on every scan insertion, causing excessive cache invalidation
|
||||
// Using slightly stale data is acceptable for scan matching optimization
|
||||
GridHashCode = RuntimeHelpers.GetHashCode(grid);
|
||||
var limits = grid.Limits.CellLimits;
|
||||
CellLimits = (limits.NumXCells, limits.NumYCells);
|
||||
Resolution = grid.Limits.Resolution;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
Interpolator?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Thread-safe cache for BiCubicInterpolator resources.
|
||||
/// Caches ProbabilityGridAdapter and BiCubicInterpolator per Grid2D to avoid
|
||||
/// expensive re-computation of grid data on every scan match.
|
||||
///
|
||||
/// Performance: Creating BiCubicInterpolator requires PrecomputeGridData() which
|
||||
/// iterates all grid cells (~1000ms for 1000x1000 grid). Caching reduces this
|
||||
/// to near-zero for subsequent scan matches on the same grid.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a new GridInterpolatorCache.
|
||||
/// </remarks>
|
||||
/// <param name="maxCacheSize">Maximum number of cached interpolators (default: 10).</param>
|
||||
internal sealed class GridInterpolatorCache(int maxCacheSize = 10) : IDisposable
|
||||
{
|
||||
/// <summary>
|
||||
/// Cache entry with weak reference to grid and strong reference to cached resources.
|
||||
/// </summary>
|
||||
private class CacheEntry(Grid2D grid, CachedGridInterpolator cachedInterpolator)
|
||||
{
|
||||
public WeakReference<Grid2D> GridRef { get; } = new WeakReference<Grid2D>(grid);
|
||||
public CachedGridInterpolator CachedInterpolator { get; } = cachedInterpolator;
|
||||
public DateTime LastAccessTime { get; set; } = DateTime.UtcNow;
|
||||
}
|
||||
|
||||
// Cache keyed by grid identity hash code
|
||||
private readonly ConcurrentDictionary<int, CacheEntry> _cache = new();
|
||||
|
||||
// Maximum cache size to prevent unbounded memory growth
|
||||
private readonly int _maxCacheSize = maxCacheSize;
|
||||
|
||||
// Lock for cache cleanup and creation operations
|
||||
private readonly Lock _cleanupLock = new();
|
||||
|
||||
// Statistics for debugging
|
||||
private long _cacheHits;
|
||||
private long _cacheMisses;
|
||||
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Gets or creates cached interpolator resources for the given grid.
|
||||
/// Thread-safe: multiple threads can call this concurrently.
|
||||
/// </summary>
|
||||
/// <param name="grid">The grid to get interpolator for.</param>
|
||||
/// <returns>Cached interpolator resources (do NOT dispose - owned by cache).</returns>
|
||||
public CachedGridInterpolator GetOrCreate(Grid2D grid)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(grid);
|
||||
|
||||
var gridHash = RuntimeHelpers.GetHashCode(grid);
|
||||
|
||||
// Fast path: try to get from cache
|
||||
if (TryGetValidEntry(gridHash, grid, out var cachedInterpolator))
|
||||
{
|
||||
Interlocked.Increment(ref _cacheHits);
|
||||
return cachedInterpolator;
|
||||
}
|
||||
|
||||
// Slow path: need to create new interpolator
|
||||
// Use lock to prevent multiple threads from creating interpolators for the same grid
|
||||
lock (_cleanupLock)
|
||||
{
|
||||
// Double-check after acquiring lock
|
||||
if (TryGetValidEntry(gridHash, grid, out cachedInterpolator))
|
||||
{
|
||||
Interlocked.Increment(ref _cacheHits);
|
||||
return cachedInterpolator;
|
||||
}
|
||||
|
||||
Interlocked.Increment(ref _cacheMisses);
|
||||
|
||||
// Remove invalid entry if exists
|
||||
if (_cache.TryRemove(gridHash, out var removedEntry))
|
||||
{
|
||||
_ = removedEntry.CachedInterpolator?.CellLimits;
|
||||
removedEntry.CachedInterpolator?.Dispose();
|
||||
}
|
||||
|
||||
// Create new cached interpolator
|
||||
var newInterpolator = CreateCachedInterpolator(grid);
|
||||
var newEntry = new CacheEntry(grid, newInterpolator);
|
||||
|
||||
// Add to cache (should succeed since we removed invalid entry)
|
||||
_cache[gridHash] = newEntry;
|
||||
|
||||
// Cleanup if needed
|
||||
if (_cache.Count > _maxCacheSize)
|
||||
{
|
||||
CleanupOldEntriesLocked();
|
||||
}
|
||||
|
||||
return newInterpolator;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tries to get a valid cached entry for the given grid.
|
||||
/// </summary>
|
||||
private bool TryGetValidEntry(int gridHash, Grid2D grid, out CachedGridInterpolator cachedInterpolator)
|
||||
{
|
||||
if (_cache.TryGetValue(gridHash, out var entry))
|
||||
{
|
||||
// Validate cached entry is still valid
|
||||
if (entry.GridRef.TryGetTarget(out var cachedGrid) &&
|
||||
ReferenceEquals(cachedGrid, grid) &&
|
||||
IsValid(grid, entry.CachedInterpolator))
|
||||
{
|
||||
entry.LastAccessTime = DateTime.UtcNow;
|
||||
cachedInterpolator = entry.CachedInterpolator;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
cachedInterpolator = null!;
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Invalidates cache entry for the given grid.
|
||||
/// Call this when the grid is modified.
|
||||
/// </summary>
|
||||
public void Invalidate(Grid2D grid)
|
||||
{
|
||||
if (grid == null) return;
|
||||
|
||||
var gridHash = RuntimeHelpers.GetHashCode(grid);
|
||||
if (_cache.TryRemove(gridHash, out var entry))
|
||||
{
|
||||
entry.CachedInterpolator?.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clears all cached entries.
|
||||
/// </summary>
|
||||
public void Clear()
|
||||
{
|
||||
lock (_cleanupLock)
|
||||
{
|
||||
foreach (var kvp in _cache)
|
||||
{
|
||||
kvp.Value.CachedInterpolator?.Dispose();
|
||||
}
|
||||
_cache.Clear();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the current cache size.
|
||||
/// </summary>
|
||||
public int Count => _cache.Count;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of cache hits.
|
||||
/// </summary>
|
||||
public long CacheHits => Interlocked.Read(ref _cacheHits);
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of cache misses.
|
||||
/// </summary>
|
||||
public long CacheMisses => Interlocked.Read(ref _cacheMisses);
|
||||
|
||||
/// <summary>
|
||||
/// Gets cache hit rate (0.0 to 1.0).
|
||||
/// </summary>
|
||||
public double HitRate
|
||||
{
|
||||
get
|
||||
{
|
||||
var hits = CacheHits;
|
||||
var total = hits + CacheMisses;
|
||||
return total > 0 ? (double)hits / total : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
private static CachedGridInterpolator CreateCachedInterpolator(Grid2D grid)
|
||||
{
|
||||
var adapter = new ProbabilityGridAdapter(grid);
|
||||
|
||||
var interpolator = new BiCubicInterpolator(
|
||||
adapter.Data,
|
||||
adapter.NumRows,
|
||||
adapter.NumCols);
|
||||
|
||||
return new CachedGridInterpolator(grid, adapter, interpolator);
|
||||
}
|
||||
|
||||
private static bool IsValid(Grid2D grid, CachedGridInterpolator cached)
|
||||
{
|
||||
// Check if grid hash matches (same grid instance)
|
||||
if (RuntimeHelpers.GetHashCode(grid) != cached.GridHashCode)
|
||||
return false;
|
||||
|
||||
// Check if grid SIZE has changed (GrowLimits was called)
|
||||
// Only invalidate when grid grows - this is the critical structural change
|
||||
var limits = grid.Limits.CellLimits;
|
||||
if (limits.NumXCells != cached.CellLimits.NumXCells ||
|
||||
limits.NumYCells != cached.CellLimits.NumYCells)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check if resolution changed (shouldn't happen normally)
|
||||
if (Math.Abs(grid.Limits.Resolution - cached.Resolution) > 1e-9)
|
||||
return false;
|
||||
|
||||
// NOTE: We intentionally do NOT check KnownCellsBox here
|
||||
// KnownCellsBox changes on every scan insertion, causing excessive cache invalidation
|
||||
// Using slightly stale interpolation data is acceptable for scan matching:
|
||||
// - Existing cells: correspondence costs are similar
|
||||
// - New cells: will return max correspondence cost via bounds check in Evaluate()
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private void CleanupOldEntries()
|
||||
{
|
||||
lock (_cleanupLock)
|
||||
{
|
||||
CleanupOldEntriesLocked();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Cleanup old entries. Caller must hold _cleanupLock.
|
||||
/// </summary>
|
||||
private void CleanupOldEntriesLocked()
|
||||
{
|
||||
if (_cache.Count <= _maxCacheSize)
|
||||
return;
|
||||
|
||||
// Find entries to remove (oldest and entries with dead references)
|
||||
var entriesToRemove = new List<int>();
|
||||
|
||||
foreach (var kvp in _cache)
|
||||
{
|
||||
// Remove entries with dead grid references
|
||||
if (!kvp.Value.GridRef.TryGetTarget(out _))
|
||||
{
|
||||
entriesToRemove.Add(kvp.Key);
|
||||
}
|
||||
}
|
||||
|
||||
// If still need to remove more, remove oldest entries
|
||||
if (_cache.Count - entriesToRemove.Count > _maxCacheSize)
|
||||
{
|
||||
var oldestEntries = _cache
|
||||
.Where(kvp => !entriesToRemove.Contains(kvp.Key))
|
||||
.OrderBy(kvp => kvp.Value.LastAccessTime)
|
||||
.Take(_cache.Count - _maxCacheSize)
|
||||
.Select(kvp => kvp.Key)
|
||||
.ToList();
|
||||
|
||||
entriesToRemove.AddRange(oldestEntries);
|
||||
}
|
||||
|
||||
// Remove entries
|
||||
foreach (var key in entriesToRemove)
|
||||
{
|
||||
if (_cache.TryRemove(key, out var entry))
|
||||
{
|
||||
entry.CachedInterpolator?.Dispose();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
Clear();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,138 @@
|
||||
/*
|
||||
* 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.D2D;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Interpolates between TSDF2D pixels with bilinear interpolation.
|
||||
/// This class works with Ceres autodiff by using double for interpolation.
|
||||
/// </summary>
|
||||
public class InterpolatedTSDF2D(TSDF2D tsdf)
|
||||
{
|
||||
private readonly TSDF2D _tsdf = tsdf ?? throw new ArgumentNullException(nameof(tsdf));
|
||||
|
||||
/// <summary>
|
||||
/// Returns the interpolated correspondence cost at (x,y).
|
||||
/// Cells with at least one 'unknown' interpolation point result in
|
||||
/// "MaxCorrespondenceCost()" with zero gradient.
|
||||
/// </summary>
|
||||
public double GetCorrespondenceCost(double x, double y)
|
||||
{
|
||||
ComputeInterpolationDataPoints(x, y, out double x1, out double y1, out double x2, out double y2);
|
||||
|
||||
// Match C++: tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1))
|
||||
// Use x1, y1 (center of lower pixel) instead of x, y (input coordinates)
|
||||
var index1 = _tsdf.Limits.GetCellIndex(new Vector2(x1, y1));
|
||||
|
||||
var w11 = GetWeightAt(index1);
|
||||
var w12 = GetWeightAt(index1 + new Array2i(-1, 0));
|
||||
var w21 = GetWeightAt(index1 + new Array2i(0, -1));
|
||||
var w22 = GetWeightAt(index1 + new Array2i(-1, -1));
|
||||
|
||||
if (w11 == 0.0 || w12 == 0.0 || w21 == 0.0 || w22 == 0.0)
|
||||
{
|
||||
return _tsdf.MaxCorrespondenceCost;
|
||||
}
|
||||
|
||||
var q11 = _tsdf.GetCorrespondenceCost(index1);
|
||||
var q12 = _tsdf.GetCorrespondenceCost(index1 + new Array2i(-1, 0));
|
||||
var q21 = _tsdf.GetCorrespondenceCost(index1 + new Array2i(0, -1));
|
||||
var q22 = _tsdf.GetCorrespondenceCost(index1 + new Array2i(-1, -1));
|
||||
|
||||
return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the interpolated weight at (x,y).
|
||||
/// </summary>
|
||||
public double GetWeight(double x, double y)
|
||||
{
|
||||
ComputeInterpolationDataPoints(x, y, out double x1, out double y1, out double x2, out double y2);
|
||||
|
||||
// Match C++: tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1))
|
||||
// Use x1, y1 (center of lower pixel) instead of x, y (input coordinates)
|
||||
var index1 = _tsdf.Limits.GetCellIndex(new Vector2(x1, y1));
|
||||
var q11 = GetWeightAt(index1);
|
||||
var q12 = GetWeightAt(index1 + new Array2i(-1, 0));
|
||||
var q21 = GetWeightAt(index1 + new Array2i(0, -1));
|
||||
var q22 = GetWeightAt(index1 + new Array2i(-1, -1));
|
||||
|
||||
return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22);
|
||||
}
|
||||
|
||||
private double GetWeightAt(Array2i index)
|
||||
{
|
||||
if (_tsdf.Limits.Contains(index))
|
||||
{
|
||||
return _tsdf.GetWeight(index);
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
private void ComputeInterpolationDataPoints(double x, double y, out double x1, out double y1, out double x2, out double y2)
|
||||
{
|
||||
var lower = CenterOfLowerPixel(x, y);
|
||||
x1 = lower.X;
|
||||
y1 = lower.Y;
|
||||
x2 = lower.X + _tsdf.Limits.Resolution;
|
||||
y2 = lower.Y + _tsdf.Limits.Resolution;
|
||||
}
|
||||
|
||||
private Vector2 CenterOfLowerPixel(double x, double y)
|
||||
{
|
||||
// Center of the cell containing (x, y)
|
||||
var cellIndex = _tsdf.Limits.GetCellIndex(new Vector2(x, y));
|
||||
var center = _tsdf.Limits.GetCellCenter(cellIndex);
|
||||
|
||||
// Move to the next lower pixel center
|
||||
if (center.X > x)
|
||||
{
|
||||
center.X -= _tsdf.Limits.Resolution;
|
||||
}
|
||||
if (center.Y > y)
|
||||
{
|
||||
center.Y -= _tsdf.Limits.Resolution;
|
||||
}
|
||||
|
||||
return center;
|
||||
}
|
||||
|
||||
private static double InterpolateBilinear(double x, double y, double x1, double y1, double x2, double y2,
|
||||
double q11, double q12, double q21, double q22)
|
||||
{
|
||||
// FIX: Guard against division by zero due to degenerate cell bounds
|
||||
var dx = x2 - x1;
|
||||
var dy = y2 - y1;
|
||||
const double kEpsilon = 1e-10;
|
||||
if (Math.Abs(dx) < kEpsilon || Math.Abs(dy) < kEpsilon)
|
||||
{
|
||||
// Degenerate case: return average of corner values
|
||||
return (q11 + q12 + q21 + q22) * 0.25;
|
||||
}
|
||||
|
||||
var normalizedX = (x - x1) / dx;
|
||||
var normalizedY = (y - y1) / dy;
|
||||
|
||||
var q1 = (q12 - q11) * normalizedY + q11;
|
||||
var q2 = (q22 - q21) * normalizedY + q21;
|
||||
return (q2 - q1) * normalizedX + q1;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,285 @@
|
||||
/*
|
||||
* 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.Mapping.D2D;
|
||||
using CartographerSharp.Sensor;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a cost function for matching the 'point_cloud' to the 'grid' with
|
||||
/// a 'pose'. The cost increases with poorer correspondence of the grid and the
|
||||
/// point observation (e.g. points falling into less occupied space).
|
||||
/// Match C++: cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc
|
||||
/// </summary>
|
||||
public class OccupiedSpaceCostFunction2D : IDisposable
|
||||
{
|
||||
private readonly double _scalingFactor;
|
||||
private readonly PointCloud _pointCloud;
|
||||
private readonly Grid2D _grid;
|
||||
private readonly MapLimits _limits;
|
||||
private readonly BiCubicInterpolator _interpolator;
|
||||
private readonly ProbabilityGridAdapter _adapter;
|
||||
|
||||
// Flag to track if we own the interpolator (should dispose) or borrowed from cache (should not dispose)
|
||||
private readonly bool _ownsInterpolator;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an occupied space cost function for 2D scan matching.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
|
||||
/// <param name="pointCloud">Point cloud to match.</param>
|
||||
/// <param name="grid">Grid to match against.</param>
|
||||
public OccupiedSpaceCostFunction2D(
|
||||
double scalingFactor,
|
||||
PointCloud pointCloud,
|
||||
Grid2D grid)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
|
||||
_grid = grid ?? throw new ArgumentNullException(nameof(grid));
|
||||
_limits = grid.Limits;
|
||||
|
||||
// Create adapter and interpolator
|
||||
_adapter = new ProbabilityGridAdapter(grid);
|
||||
|
||||
_interpolator = new BiCubicInterpolator(
|
||||
_adapter.Data,
|
||||
_adapter.NumRows,
|
||||
_adapter.NumCols
|
||||
);
|
||||
|
||||
_ownsInterpolator = true; // We created it, we own it
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates an occupied space cost function using cached interpolator resources.
|
||||
/// This constructor is much faster as it avoids PrecomputeGridData() (~1000ms savings).
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
|
||||
/// <param name="pointCloud">Point cloud to match.</param>
|
||||
/// <param name="grid">Grid to match against.</param>
|
||||
/// <param name="cachedInterpolator">Cached interpolator resources (owned by cache, NOT disposed by this class).</param>
|
||||
internal OccupiedSpaceCostFunction2D(
|
||||
double scalingFactor,
|
||||
PointCloud pointCloud,
|
||||
Grid2D grid,
|
||||
CachedGridInterpolator cachedInterpolator)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
|
||||
_grid = grid ?? throw new ArgumentNullException(nameof(grid));
|
||||
_limits = grid.Limits;
|
||||
|
||||
ArgumentNullException.ThrowIfNull(cachedInterpolator);
|
||||
|
||||
// Use cached adapter and interpolator
|
||||
_adapter = cachedInterpolator.Adapter;
|
||||
_interpolator = cachedInterpolator.Interpolator;
|
||||
|
||||
_ownsInterpolator = false; // Borrowed from cache, do NOT dispose
|
||||
}
|
||||
|
||||
/// <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="grid">Grid to match against.</param>
|
||||
/// <returns>DynamicAutoDiff cost function.</returns>
|
||||
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
PointCloud pointCloud,
|
||||
Grid2D grid)
|
||||
{
|
||||
var costFunction = new OccupiedSpaceCostFunction2D(scalingFactor, pointCloud, grid);
|
||||
var dynamicCostFunction = new DynamicAutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
return dynamicCostFunction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// Match C++: OccupiedSpaceCostFunction2D::operator() in occupied_space_cost_function_2d.cc
|
||||
/// </summary>
|
||||
/// <param name="parameters">Pose parameters [x, y, theta].</param>
|
||||
/// <param name="residuals">Output residuals (one per point).</param>
|
||||
/// <returns>True on success (always returns true to match C++ behavior).</returns>
|
||||
internal bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Match C++ behavior - validate inputs but don't return false for invalid inputs
|
||||
if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
|
||||
{
|
||||
FillWithMaxCost(residuals);
|
||||
return true;
|
||||
}
|
||||
if (residuals == null || residuals.Length < _pointCloud.Count)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
var pose = parameters[0];
|
||||
var translation = new Vector2(pose[0], pose[1]);
|
||||
var rotation = pose[2];
|
||||
|
||||
// Create rotation matrix
|
||||
// Match C++: Eigen::Rotation2D<T> rotation(pose[2]); rotation_matrix = rotation.toRotationMatrix();
|
||||
var rotationMatrix = Matrix3x2.CreateRotation(rotation);
|
||||
|
||||
// Get grid parameters
|
||||
var resolution = _limits.Resolution;
|
||||
var max = _limits.Max;
|
||||
var numRows = _adapter.NumRows;
|
||||
var numCols = _adapter.NumCols;
|
||||
|
||||
// Check if grid is too small
|
||||
if (numRows <= 0 || numCols <= 0)
|
||||
{
|
||||
FillWithMaxCost(residuals);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Use max correspondence cost for out-of-bounds points (matching C++ behavior)
|
||||
var kMaxCorrespondenceCost = ProbabilityValues.kMaxCorrespondenceCost;
|
||||
|
||||
// Match C++: for (size_t i = 0; i < point_cloud_.size(); ++i)
|
||||
for (int i = 0; i < _pointCloud.Count; i++)
|
||||
{
|
||||
var point = _pointCloud[i];
|
||||
|
||||
// Match C++: const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())), ...);
|
||||
// const Eigen::Matrix<T, 3, 1> world = transform * point;
|
||||
var localPoint = new Vector2(point.Position.X, point.Position.Y);
|
||||
var worldPoint = Vector2.Transform(localPoint, rotationMatrix) + translation;
|
||||
|
||||
// COORDINATE SYSTEM MAPPING (verified correct):
|
||||
// =============================================
|
||||
// C++ code (occupied_space_cost_function_2d.cc lines 57-62):
|
||||
// interpolator.Evaluate(
|
||||
// (limits.max().x() - world[0]) / limits.resolution() - 0.5 + kPadding, // row (1st arg)
|
||||
// (limits.max().y() - world[1]) / limits.resolution() - 0.5 + kPadding, // col (2nd arg)
|
||||
// &residual[i]);
|
||||
//
|
||||
// C++ Ceres BiCubicInterpolator::Evaluate(row, col, value):
|
||||
// - First arg = row index
|
||||
// - Second arg = column index
|
||||
//
|
||||
// C# BiCubicInterpolator::Evaluate(x, y):
|
||||
// - x = "X coordinate in grid space (0 <= x < cols)" = column index
|
||||
// - y = "Y coordinate in grid space (0 <= y < rows)" = row index
|
||||
//
|
||||
// Therefore, to match C++ Evaluate(row, col), C# must call Evaluate(col, row) = Evaluate(x, y)
|
||||
//
|
||||
// actualRow = (max.X - worldPoint.X) / resolution - 0.5 // matches C++ row formula
|
||||
// actualColumn = (max.Y - worldPoint.Y) / resolution - 0.5 // matches C++ col formula
|
||||
//
|
||||
// C# call: Evaluate(actualColumn, actualRow) = Evaluate(col, row) ✓ CORRECT
|
||||
|
||||
double actualRow = (max.X - worldPoint.X) / resolution - 0.5;
|
||||
double actualColumn = (max.Y - worldPoint.Y) / resolution - 0.5;
|
||||
|
||||
// Check for NaN/Infinity
|
||||
double correspondenceCost;
|
||||
if (double.IsNaN(actualColumn) || double.IsInfinity(actualColumn) ||
|
||||
double.IsNaN(actualRow) || double.IsInfinity(actualRow))
|
||||
{
|
||||
correspondenceCost = kMaxCorrespondenceCost;
|
||||
}
|
||||
else
|
||||
{
|
||||
// FIX: Simplified bounds checking to match C++ behavior more closely
|
||||
// C++ uses kPadding (INT_MAX/4) virtually - GetValue returns kMaxCorrespondenceCost
|
||||
// for anything outside actual grid cells.
|
||||
// C# uses actual array without virtual padding, so we check bounds explicitly.
|
||||
// BiCubicInterpolator needs 4x4 grid neighborhood (row-1 to row+2, col-1 to col+2)
|
||||
int minRow = (int)Math.Floor(actualRow - 1);
|
||||
int maxRow = (int)Math.Ceiling(actualRow + 2);
|
||||
int minCol = (int)Math.Floor(actualColumn - 1);
|
||||
int maxCol = (int)Math.Ceiling(actualColumn + 2);
|
||||
|
||||
bool isOutOfBounds = minRow < 0 || maxRow >= numRows ||
|
||||
minCol < 0 || maxCol >= numCols;
|
||||
|
||||
if (isOutOfBounds)
|
||||
{
|
||||
// Out of bounds - return max cost (matches C++ GetValue behavior when
|
||||
// coordinates are outside kPadding range)
|
||||
correspondenceCost = kMaxCorrespondenceCost;
|
||||
}
|
||||
else
|
||||
{
|
||||
// In bounds - perform interpolation
|
||||
// Call Evaluate(x=column, y=row) to match C++ Evaluate(row, col)
|
||||
correspondenceCost = _interpolator.Evaluate(actualColumn, actualRow);
|
||||
|
||||
// Validate interpolated value
|
||||
if (double.IsNaN(correspondenceCost) || double.IsInfinity(correspondenceCost))
|
||||
{
|
||||
correspondenceCost = kMaxCorrespondenceCost;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Match C++: residual[i] = scaling_factor_ * residual[i];
|
||||
residuals[i] = _scalingFactor * correspondenceCost;
|
||||
}
|
||||
|
||||
// Match C++ behavior - always return true
|
||||
return true;
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
FillWithMaxCost(residuals);
|
||||
return true; // Match C++ behavior - always return true
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fills residuals array with max correspondence cost.
|
||||
/// </summary>
|
||||
private void FillWithMaxCost(double[]? residuals)
|
||||
{
|
||||
if (residuals == null) return;
|
||||
|
||||
var maxCorrespondenceCost = _scalingFactor * ProbabilityValues.kMaxCorrespondenceCost;
|
||||
int count = Math.Min(residuals.Length, _pointCloud.Count);
|
||||
for (int i = 0; i < count; i++)
|
||||
{
|
||||
residuals[i] = maxCorrespondenceCost;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disposes managed resources.
|
||||
/// Only disposes interpolator if we own it (not borrowed from cache).
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
// Only dispose if we created the interpolator (not borrowed from cache)
|
||||
if (_ownsInterpolator)
|
||||
{
|
||||
_interpolator?.Dispose();
|
||||
}
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,217 @@
|
||||
/*
|
||||
* 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.D2D;
|
||||
using Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// A precomputed grid that contains in each cell (x0, y0) the maximum
|
||||
/// probability in the width x width area defined by x0 <= x < x0 + width and
|
||||
/// y0 <= y < y0 + width.
|
||||
/// </summary>
|
||||
internal class PrecomputationGrid2D
|
||||
{
|
||||
private readonly Array2i _offset;
|
||||
private readonly CellLimits _wideLimits;
|
||||
private readonly double _minScore;
|
||||
private readonly double _maxScore;
|
||||
private readonly byte[] _cells;
|
||||
|
||||
/// <summary>
|
||||
/// A collection of values which can be added and later removed, and the maximum
|
||||
/// of the current values in the collection can be retrieved. All in O(1).
|
||||
/// </summary>
|
||||
private class SlidingWindowMaximum
|
||||
{
|
||||
private readonly LinkedList<double> _nonAscendingMaxima = new();
|
||||
|
||||
public void AddValue(double value)
|
||||
{
|
||||
while (_nonAscendingMaxima.Count > 0 && value > _nonAscendingMaxima.Last!.Value)
|
||||
{
|
||||
_nonAscendingMaxima.RemoveLast();
|
||||
}
|
||||
_nonAscendingMaxima.AddLast(value);
|
||||
}
|
||||
|
||||
public void RemoveValue(double value)
|
||||
{
|
||||
// FIX: Match C++ DCHECK behavior - assert preconditions instead of silently returning
|
||||
// C++ uses DCHECK (debug assertions) for performance:
|
||||
// DCHECK(!non_ascending_maxima_.empty());
|
||||
// DCHECK_LE(value, non_ascending_maxima_.front());
|
||||
// Silently returning could hide bugs in the algorithm
|
||||
System.Diagnostics.Debug.Assert(_nonAscendingMaxima.Count > 0,
|
||||
"SlidingWindowMaximum.RemoveValue: list should not be empty");
|
||||
System.Diagnostics.Debug.Assert(value <= _nonAscendingMaxima.First!.Value,
|
||||
$"SlidingWindowMaximum.RemoveValue: value ({value}) should be <= front ({_nonAscendingMaxima.First.Value})");
|
||||
|
||||
if (value == _nonAscendingMaxima.First.Value)
|
||||
{
|
||||
_nonAscendingMaxima.RemoveFirst();
|
||||
}
|
||||
}
|
||||
|
||||
public double GetMaximum()
|
||||
{
|
||||
if (_nonAscendingMaxima.Count == 0)
|
||||
throw new InvalidOperationException("SlidingWindowMaximum is empty");
|
||||
return _nonAscendingMaxima.First!.Value;
|
||||
}
|
||||
|
||||
public void CheckIsEmpty()
|
||||
{
|
||||
if (_nonAscendingMaxima.Count != 0)
|
||||
throw new InvalidOperationException("SlidingWindowMaximum is not empty");
|
||||
}
|
||||
}
|
||||
|
||||
public PrecomputationGrid2D(
|
||||
Grid2D grid,
|
||||
CellLimits limits,
|
||||
int width,
|
||||
List<double> reusableIntermediateGrid)
|
||||
{
|
||||
if (width < 1)
|
||||
throw new ArgumentException("width must be >= 1", nameof(width));
|
||||
if (limits.NumXCells < 1 || limits.NumYCells < 1)
|
||||
throw new ArgumentException("limits must have at least 1 cell in each dimension", nameof(limits));
|
||||
|
||||
_offset = new Array2i(-width + 1, -width + 1);
|
||||
_wideLimits = new CellLimits(
|
||||
limits.NumXCells + width - 1,
|
||||
limits.NumYCells + width - 1);
|
||||
_minScore = 1.0 - grid.MaxCorrespondenceCost;
|
||||
_maxScore = 1.0 - grid.MinCorrespondenceCost;
|
||||
_cells = new byte[_wideLimits.NumXCells * _wideLimits.NumYCells];
|
||||
|
||||
var stride = _wideLimits.NumXCells;
|
||||
|
||||
// First we compute the maximum probability for each (x0, y) achieved in the
|
||||
// span defined by x0 <= x < x0 + width.
|
||||
reusableIntermediateGrid.Clear();
|
||||
reusableIntermediateGrid.Capacity = _wideLimits.NumXCells * limits.NumYCells;
|
||||
for (int i = 0; i < reusableIntermediateGrid.Capacity; i++)
|
||||
{
|
||||
reusableIntermediateGrid.Add(0.0);
|
||||
}
|
||||
|
||||
for (int y = 0; y < limits.NumYCells; y++)
|
||||
{
|
||||
var currentValues = new SlidingWindowMaximum();
|
||||
currentValues.AddValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(0, y))));
|
||||
|
||||
for (int x = -width + 1; x < 0; x++)
|
||||
{
|
||||
reusableIntermediateGrid[x + width - 1 + y * stride] = currentValues.GetMaximum();
|
||||
if (x + width < limits.NumXCells)
|
||||
{
|
||||
currentValues.AddValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x + width, y))));
|
||||
}
|
||||
}
|
||||
|
||||
for (int x = 0; x < limits.NumXCells - width; x++)
|
||||
{
|
||||
reusableIntermediateGrid[x + width - 1 + y * stride] = currentValues.GetMaximum();
|
||||
currentValues.RemoveValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x, y))));
|
||||
currentValues.AddValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x + width, y))));
|
||||
}
|
||||
|
||||
for (int x = Math.Max(limits.NumXCells - width, 0); x < limits.NumXCells; x++)
|
||||
{
|
||||
reusableIntermediateGrid[x + width - 1 + y * stride] = currentValues.GetMaximum();
|
||||
currentValues.RemoveValue(1.0 - Math.Abs(grid.GetCorrespondenceCost(new Array2i(x, y))));
|
||||
}
|
||||
|
||||
currentValues.CheckIsEmpty();
|
||||
}
|
||||
|
||||
// For each (x, y), we compute the maximum probability in the width x width
|
||||
// region starting at each (x, y) and precompute the resulting bound on the
|
||||
// score.
|
||||
for (int x = 0; x < _wideLimits.NumXCells; x++)
|
||||
{
|
||||
var currentValues = new SlidingWindowMaximum();
|
||||
currentValues.AddValue(reusableIntermediateGrid[x]);
|
||||
|
||||
for (int y = -width + 1; y < 0; y++)
|
||||
{
|
||||
_cells[x + (y + width - 1) * stride] = ComputeCellValue(currentValues.GetMaximum());
|
||||
if (y + width < limits.NumYCells)
|
||||
{
|
||||
currentValues.AddValue(reusableIntermediateGrid[x + (y + width) * stride]);
|
||||
}
|
||||
}
|
||||
|
||||
for (int y = 0; y < limits.NumYCells - width; y++)
|
||||
{
|
||||
_cells[x + (y + width - 1) * stride] = ComputeCellValue(currentValues.GetMaximum());
|
||||
currentValues.RemoveValue(reusableIntermediateGrid[x + y * stride]);
|
||||
currentValues.AddValue(reusableIntermediateGrid[x + (y + width) * stride]);
|
||||
}
|
||||
|
||||
for (int y = Math.Max(limits.NumYCells - width, 0); y < limits.NumYCells; y++)
|
||||
{
|
||||
_cells[x + (y + width - 1) * stride] = ComputeCellValue(currentValues.GetMaximum());
|
||||
currentValues.RemoveValue(reusableIntermediateGrid[x + y * stride]);
|
||||
}
|
||||
|
||||
currentValues.CheckIsEmpty();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns a value between 0 and 255 to represent probabilities between
|
||||
/// min_score and max_score.
|
||||
/// </summary>
|
||||
public int GetValue(Array2i xyIndex)
|
||||
{
|
||||
var localXYIndex = xyIndex - _offset;
|
||||
|
||||
// Check bounds (similar to C++ unsigned cast trick)
|
||||
if (localXYIndex.X < 0 || localXYIndex.Y < 0 ||
|
||||
localXYIndex.X >= _wideLimits.NumXCells ||
|
||||
localXYIndex.Y >= _wideLimits.NumYCells)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
var stride = _wideLimits.NumXCells;
|
||||
return _cells[localXYIndex.X + localXYIndex.Y * stride];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Maps values from [0, 255] to [min_score, max_score].
|
||||
/// </summary>
|
||||
public double ToScore(double value)
|
||||
{
|
||||
return _minScore + value * ((_maxScore - _minScore) / 255.0);
|
||||
}
|
||||
|
||||
private byte ComputeCellValue(double probability)
|
||||
{
|
||||
// Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
|
||||
// (away from zero for .5), equivalent to MidpointRounding.AwayFromZero
|
||||
var cellValue = (int)Math.Round((probability - _minScore) * (255.0 / (_maxScore - _minScore)), MidpointRounding.AwayFromZero);
|
||||
// Match C++: CHECK_GE(cell_value, 0) and CHECK_LE(cell_value, 255)
|
||||
cellValue = Math.Clamp(cellValue, 0, 255);
|
||||
return (byte)cellValue;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* 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 Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Stack of precomputation grids at different resolutions for fast scan matching.
|
||||
/// </summary>
|
||||
internal class PrecomputationGridStack2D
|
||||
{
|
||||
private readonly List<PrecomputationGrid2D> _precomputationGrids = [];
|
||||
private readonly List<double> _reusableIntermediateGrid = [];
|
||||
|
||||
public PrecomputationGridStack2D(
|
||||
Grid2D grid,
|
||||
FastCorrelativeScanMatcherOptions2D options)
|
||||
{
|
||||
if (options.BranchAndBoundDepth < 1)
|
||||
throw new ArgumentException("branch_and_bound_depth must be >= 1", nameof(options));
|
||||
|
||||
var maxWidth = 1 << (options.BranchAndBoundDepth - 1);
|
||||
var limits = grid.Limits.CellLimits;
|
||||
// Match C++: reserve capacity for precomputation_grids_
|
||||
_precomputationGrids.Capacity = options.BranchAndBoundDepth;
|
||||
// Match C++: reserve capacity for reusable_intermediate_grid
|
||||
_reusableIntermediateGrid.Capacity = (limits.NumXCells + maxWidth - 1) * limits.NumYCells;
|
||||
|
||||
for (int i = 0; i < options.BranchAndBoundDepth; i++)
|
||||
{
|
||||
var width = 1 << i;
|
||||
_precomputationGrids.Add(new PrecomputationGrid2D(
|
||||
grid, limits, width, _reusableIntermediateGrid));
|
||||
}
|
||||
}
|
||||
|
||||
public PrecomputationGrid2D Get(int index)
|
||||
{
|
||||
if (index < 0 || index >= _precomputationGrids.Count)
|
||||
throw new ArgumentOutOfRangeException(nameof(index));
|
||||
|
||||
return _precomputationGrids[index];
|
||||
}
|
||||
|
||||
public int MaxDepth => _precomputationGrids.Count - 1;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,132 @@
|
||||
/*
|
||||
* 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.D2D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Adapter to convert ProbabilityGrid to format suitable for BiCubicInterpolator.
|
||||
/// Provides grid data as 2D array with padding for boundary handling.
|
||||
/// </summary>
|
||||
internal class ProbabilityGridAdapter
|
||||
{
|
||||
// CRITICAL: Match C++ behavior - use virtual padding like C++ (INT_MAX / 4)
|
||||
// C++ uses: static constexpr int kPadding = INT_MAX / 4; (~536,870,912)
|
||||
// This creates a virtual padding that doesn't require creating a real array for padding region
|
||||
// The padding is used to offset grid coordinates, and GetValue handles out-of-bounds
|
||||
public const int kPadding = int.MaxValue / 4; // ~536,870,912 - matches C++ exactly
|
||||
private readonly Grid2D _grid;
|
||||
private readonly MapLimits _limits;
|
||||
private readonly int _numRows; // Virtual size: num_cells + 2 * kPadding
|
||||
private readonly int _numCols; // Virtual size: num_cells + 2 * kPadding
|
||||
private readonly int _actualNumRows; // Actual grid cells
|
||||
private readonly int _actualNumCols; // Actual grid cells
|
||||
private readonly double[] _data; // Only stores actual grid cells, not padding
|
||||
|
||||
public ProbabilityGridAdapter(Grid2D grid)
|
||||
{
|
||||
_grid = grid ?? throw new ArgumentNullException(nameof(grid));
|
||||
_limits = grid.Limits;
|
||||
|
||||
var cellLimits = _limits.CellLimits;
|
||||
|
||||
// CRITICAL: Match C++ behavior - use virtual padding (INT_MAX / 4)
|
||||
// C++: NumRows() = num_y_cells + 2 * kPadding (virtual, not real array)
|
||||
// We need to create a virtual array for BiCubicInterpolator, but we can optimize
|
||||
// by only storing actual grid cells and using GetValue for padding region
|
||||
|
||||
_actualNumRows = cellLimits.NumYCells;
|
||||
_actualNumCols = cellLimits.NumXCells;
|
||||
|
||||
// Virtual size (matches C++): num_cells + 2 * kPadding
|
||||
// Note: This can be very large, but we only create array for actual cells
|
||||
// BiCubicInterpolator needs the virtual size, but we'll handle padding in GetValue
|
||||
long numRowsLong = (long)_actualNumRows + 2L * kPadding;
|
||||
long numColsLong = (long)_actualNumCols + 2L * kPadding;
|
||||
|
||||
// Check for overflow (shouldn't happen with kPadding = INT_MAX/4)
|
||||
if (numRowsLong > int.MaxValue || numColsLong > int.MaxValue)
|
||||
{
|
||||
var errorMsg = $"ProbabilityGridAdapter: Integer overflow detected! NumRows would be {numRowsLong}, NumCols would be {numColsLong}, but max int is {int.MaxValue}";
|
||||
throw new ArgumentOutOfRangeException(nameof(grid), errorMsg);
|
||||
}
|
||||
|
||||
_numRows = (int)numRowsLong;
|
||||
_numCols = (int)numColsLong;
|
||||
|
||||
// Create array for actual grid cells (not including virtual padding)
|
||||
long arraySizeLong = (long)_actualNumRows * _actualNumCols;
|
||||
if (arraySizeLong > int.MaxValue)
|
||||
{
|
||||
var errorMsg = $"ProbabilityGridAdapter: Array size overflow! _actualNumRows={_actualNumRows}, _actualNumCols={_actualNumCols}, array size would be {arraySizeLong}, but max int is {int.MaxValue}";
|
||||
throw new ArgumentOutOfRangeException(nameof(grid), errorMsg);
|
||||
}
|
||||
|
||||
_data = new double[_actualNumRows * _actualNumCols];
|
||||
_grid.CopyCorrespondenceCostData(_data);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of rows for BiCubicInterpolator (actual size, not virtual).
|
||||
/// BiCubicInterpolator needs actual array size, not virtual size with padding.
|
||||
/// </summary>
|
||||
public int NumRows => _actualNumRows;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of columns for BiCubicInterpolator (actual size, not virtual).
|
||||
/// BiCubicInterpolator needs actual array size, not virtual size with padding.
|
||||
/// </summary>
|
||||
public int NumCols => _actualNumCols;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the virtual number of rows (including padding) - for coordinate calculation only.
|
||||
/// </summary>
|
||||
public int VirtualNumRows => _numRows;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the virtual number of columns (including padding) - for coordinate calculation only.
|
||||
/// </summary>
|
||||
public int VirtualNumCols => _numCols;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the grid data array (row-major order).
|
||||
/// </summary>
|
||||
public double[] Data => _data;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the correspondence cost value at (row, col).
|
||||
/// Returns kMaxCorrespondenceCost for out-of-bounds or padding regions.
|
||||
/// </summary>
|
||||
public double GetValue(int row, int col)
|
||||
{
|
||||
// CRITICAL: Match C++ behavior exactly
|
||||
// C++: if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || column >= NumCols() - kPadding)
|
||||
if (row < kPadding || col < kPadding ||
|
||||
row >= _numRows - kPadding || col >= _numCols - kPadding)
|
||||
{
|
||||
// Out of bounds or padding region - return max correspondence cost
|
||||
return ProbabilityValues.kMaxCorrespondenceCost;
|
||||
}
|
||||
|
||||
// Convert from virtual coordinate space to actual grid cell coordinates
|
||||
// C++: Eigen::Array2i(column - kPadding, row - kPadding)
|
||||
var cellIndex = new Array2i(col - kPadding, row - kPadding);
|
||||
|
||||
return _grid.GetCorrespondenceCost(cellIndex);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,540 @@
|
||||
/*
|
||||
* 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 Grid2D = CartographerSharp.Mapping.D2D.Grid2D;
|
||||
using ProbabilityGrid = CartographerSharp.Mapping.D2D.ProbabilityGrid;
|
||||
using TSDF2D = CartographerSharp.Mapping.D2D.TSDF2D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// An implementation of "Real-Time Correlative Scan Matching" by Olson.
|
||||
/// The correlative scan matching algorithm is exhaustively evaluating the scan
|
||||
/// matching search space.
|
||||
/// </summary>
|
||||
public class RealTimeCorrelativeScanMatcher2D(RealTimeCorrelativeScanMatcherOptions options)
|
||||
{
|
||||
private readonly int _numThreads = Math.Max(1, options.NumThreads);
|
||||
|
||||
/// <summary>
|
||||
/// Aligns 'point_cloud' within the 'grid' given an
|
||||
/// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
|
||||
/// returns the score.
|
||||
/// </summary>
|
||||
public double Match(
|
||||
Rigid2d initialPoseEstimate,
|
||||
PointCloud pointCloud,
|
||||
Grid2D grid,
|
||||
out Rigid2d poseEstimate)
|
||||
{
|
||||
var initialAngle = initialPoseEstimate.Rotation; // Rotation is already the angle in radians
|
||||
|
||||
// Rotate point cloud to align with initial rotation
|
||||
var rotationMatrix = Matrix3x2.CreateRotation(initialAngle);
|
||||
var rotatedPointCloud = new PointCloud();
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotationMatrix);
|
||||
rotatedPointCloud.Add(new RangefinderPoint
|
||||
{
|
||||
Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
|
||||
});
|
||||
}
|
||||
|
||||
var searchParameters = new SearchParameters(
|
||||
options.LinearSearchWindow,
|
||||
options.AngularSearchWindow,
|
||||
rotatedPointCloud,
|
||||
grid.Limits.Resolution
|
||||
);
|
||||
|
||||
var rotatedScans = ScanMatchingUtilities.GenerateRotatedScans(rotatedPointCloud, searchParameters);
|
||||
|
||||
var initialTranslation = new Vector2(initialPoseEstimate.Translation.X, initialPoseEstimate.Translation.Y);
|
||||
var discreteScans = ScanMatchingUtilities.DiscretizeScans(grid.Limits, rotatedScans, initialTranslation);
|
||||
|
||||
var candidates = GenerateExhaustiveSearchCandidates(searchParameters);
|
||||
|
||||
ScoreCandidates(grid, discreteScans, candidates);
|
||||
|
||||
// Match C++: Find best candidate using std::max_element
|
||||
var bestCandidate = candidates[0];
|
||||
for (int i = 1; i < candidates.Count; i++)
|
||||
{
|
||||
if (candidates[i].Score > bestCandidate.Score)
|
||||
{
|
||||
bestCandidate = candidates[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Match C++: Calculate final pose
|
||||
var finalTranslation = new Vector2(
|
||||
(initialPoseEstimate.Translation.X + bestCandidate.X),
|
||||
(initialPoseEstimate.Translation.Y + bestCandidate.Y)
|
||||
);
|
||||
var finalAngle = initialAngle + bestCandidate.Orientation;
|
||||
poseEstimate = new Rigid2d(finalTranslation, finalAngle);
|
||||
|
||||
return bestCandidate.Score;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the pose confidence by evaluating candidates around the estimated pose.
|
||||
/// Match C++: LocalPose_Confidence method in RealTimeCorrelativeScanMatcher2D.
|
||||
/// Returns confidence score as percentage (0-100).
|
||||
/// </summary>
|
||||
public double LocalPose_Confidence(
|
||||
Rigid2d poseEstimated,
|
||||
PointCloud pointCloud,
|
||||
Grid2D grid)
|
||||
{
|
||||
var initialAngle = poseEstimated.Rotation;
|
||||
|
||||
// Rotate point cloud to align with estimated rotation
|
||||
var rotationMatrix = Matrix3x2.CreateRotation(initialAngle);
|
||||
var rotatedPointCloud = new PointCloud();
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
var rotatedPoint = Vector2.Transform(new Vector2(point.Position.X, point.Position.Y), rotationMatrix);
|
||||
rotatedPointCloud.Add(new RangefinderPoint
|
||||
{
|
||||
Position = new Vector3(rotatedPoint.X, rotatedPoint.Y, point.Position.Z)
|
||||
});
|
||||
}
|
||||
|
||||
// Match C++: fixed parameters for confidence calculation
|
||||
const int fixNumLinearPerturbations = 5;
|
||||
const int fixNumAngularPerturbations = 25;
|
||||
const double fixAngularPerturbationStepSize = 0.007;
|
||||
const double fixResolution = 0.04;
|
||||
|
||||
var searchParameters = new SearchParameters(
|
||||
fixNumLinearPerturbations,
|
||||
fixNumAngularPerturbations,
|
||||
fixAngularPerturbationStepSize,
|
||||
fixResolution);
|
||||
|
||||
var rotatedScans = ScanMatchingUtilities.GenerateRotatedScans(rotatedPointCloud, searchParameters);
|
||||
var initialTranslation = new Vector2(poseEstimated.Translation.X, poseEstimated.Translation.Y);
|
||||
var discreteScans = ScanMatchingUtilities.DiscretizeScans(grid.Limits, rotatedScans, initialTranslation);
|
||||
|
||||
var candidates = GenerateExhaustiveSearchCandidatesForConfidence(searchParameters);
|
||||
ScoreCandidates_Confidence(grid, discreteScans, candidates);
|
||||
|
||||
// Evaluate confidence from the set of candidates
|
||||
double limitAngle = searchParameters.AngularPerturbationStepSize * (fixNumAngularPerturbations + 1);
|
||||
double limitDist = searchParameters.Resolution * (fixNumLinearPerturbations + 1);
|
||||
|
||||
const double kMinScoreThreshold = 1e-10;
|
||||
double maxOutCandidateScore = kMinScoreThreshold;
|
||||
double maxInCandidateScore = kMinScoreThreshold;
|
||||
|
||||
foreach (var candidate in candidates)
|
||||
{
|
||||
if (Math.Abs(candidate.Orientation) <= limitAngle / 2.0 &&
|
||||
Math.Abs(candidate.X) <= limitDist / 2.0 &&
|
||||
Math.Abs(candidate.Y) <= limitDist / 2.0)
|
||||
{
|
||||
if (candidate.Score > maxInCandidateScore)
|
||||
{
|
||||
maxInCandidateScore = candidate.Score;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (candidate.Score > maxOutCandidateScore)
|
||||
{
|
||||
maxOutCandidateScore = candidate.Score;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Validate scores before division to avoid edge cases
|
||||
if (maxInCandidateScore <= kMinScoreThreshold)
|
||||
{
|
||||
// No valid "in" candidates found - return neutral confidence
|
||||
return 50.0;
|
||||
}
|
||||
|
||||
double confidence = 1.0 - Math.Pow(maxOutCandidateScore / maxInCandidateScore, 10);
|
||||
return confidence * 100.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scores candidates without applying cost weights (for confidence calculation).
|
||||
/// Match C++: ScoreCandidates_Confidence method.
|
||||
/// </summary>
|
||||
private void ScoreCandidates_Confidence(Grid2D grid, List<DiscreteScan2D> discreteScans, List<Candidate2D> candidates)
|
||||
{
|
||||
for (int i = 0; i < candidates.Count; i++)
|
||||
{
|
||||
var candidate = candidates[i];
|
||||
|
||||
if (candidate.ScanIndex >= discreteScans.Count)
|
||||
{
|
||||
candidate.Score = 0.0;
|
||||
candidates[i] = candidate;
|
||||
continue;
|
||||
}
|
||||
|
||||
var discreteScan = discreteScans[candidate.ScanIndex];
|
||||
double candidateScore = 0.0;
|
||||
|
||||
if (grid is ProbabilityGrid probabilityGrid)
|
||||
{
|
||||
foreach (var xyIndex in discreteScan)
|
||||
{
|
||||
var proposedXYIndex = new Array2i(
|
||||
xyIndex.X + candidate.XIndexOffset,
|
||||
xyIndex.Y + candidate.YIndexOffset
|
||||
);
|
||||
var probability = probabilityGrid.GetProbability(proposedXYIndex);
|
||||
candidateScore += probability;
|
||||
}
|
||||
if (discreteScan.Count > 0)
|
||||
{
|
||||
candidateScore /= discreteScan.Count;
|
||||
}
|
||||
}
|
||||
else if (grid.GetGridType() == CartographerSharp.Mapping.D2D.GridType.TSDF && grid is TSDF2D tsdfGrid)
|
||||
{
|
||||
double summedWeight = 0.0;
|
||||
foreach (var xyIndex in discreteScan)
|
||||
{
|
||||
var proposedXYIndex = new Array2i(
|
||||
xyIndex.X + candidate.XIndexOffset,
|
||||
xyIndex.Y + candidate.YIndexOffset
|
||||
);
|
||||
var (tsd, weight) = tsdfGrid.GetTSDAndWeight(proposedXYIndex);
|
||||
double maxCorrespondenceCost = tsdfGrid.MaxCorrespondenceCost;
|
||||
double normalizedTsdScore = (maxCorrespondenceCost - Math.Abs(tsd)) / maxCorrespondenceCost;
|
||||
candidateScore += normalizedTsdScore * weight;
|
||||
summedWeight += weight;
|
||||
}
|
||||
if (summedWeight == 0.0)
|
||||
{
|
||||
candidateScore = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
candidateScore /= summedWeight;
|
||||
}
|
||||
}
|
||||
|
||||
// NOTE: No cost weight penalty applied for confidence calculation (matches C++)
|
||||
candidate.Score = candidateScore;
|
||||
candidates[i] = candidate;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Generates candidates for confidence calculation (simpler than ScoreCandidates).
|
||||
/// </summary>
|
||||
private static List<Candidate2D> GenerateExhaustiveSearchCandidatesForConfidence(SearchParameters searchParameters)
|
||||
{
|
||||
int numCandidates = 0;
|
||||
for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
|
||||
{
|
||||
var bounds = searchParameters.LinearBoundsList[scanIndex];
|
||||
int numLinearXCandidates = bounds.MaxX - bounds.MinX + 1;
|
||||
int numLinearYCandidates = bounds.MaxY - bounds.MinY + 1;
|
||||
numCandidates += numLinearXCandidates * numLinearYCandidates;
|
||||
}
|
||||
|
||||
var candidates = new List<Candidate2D>(numCandidates);
|
||||
|
||||
for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
|
||||
{
|
||||
var bounds = searchParameters.LinearBoundsList[scanIndex];
|
||||
for (int xIndexOffset = bounds.MinX; xIndexOffset <= bounds.MaxX; xIndexOffset++)
|
||||
{
|
||||
for (int yIndexOffset = bounds.MinY; yIndexOffset <= bounds.MaxY; yIndexOffset++)
|
||||
{
|
||||
candidates.Add(new Candidate2D(scanIndex, xIndexOffset, yIndexOffset, searchParameters));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return candidates;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the score for each Candidate2D in a collection. The cost is
|
||||
/// computed as the sum of probabilities or normalized TSD values.
|
||||
/// </summary>
|
||||
public void ScoreCandidates(Grid2D grid, List<DiscreteScan2D> discreteScans, List<Candidate2D> candidates)
|
||||
{
|
||||
int totalCandidates = candidates.Count;
|
||||
|
||||
// Use sequential processing if NumThreads <= 1 or too few candidates
|
||||
if (_numThreads <= 1 || totalCandidates < _numThreads * 10)
|
||||
{
|
||||
int candidatesWithKnownCells = 0;
|
||||
double maxScore = double.MinValue;
|
||||
Candidate2D? bestCandidateWithKnownCells = null;
|
||||
double maxScoreWithKnownCells = double.MinValue;
|
||||
|
||||
for (int i = 0; i < candidates.Count; i++)
|
||||
{
|
||||
ScoreSingleCandidate(grid, discreteScans, candidates, i,
|
||||
ref candidatesWithKnownCells, ref maxScore, ref bestCandidateWithKnownCells, ref maxScoreWithKnownCells);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Parallel processing using Thread with high priority
|
||||
ScoreCandidatesParallel(grid, discreteScans, candidates, totalCandidates);
|
||||
}
|
||||
|
||||
private void ScoreCandidatesParallel(
|
||||
Grid2D grid,
|
||||
List<DiscreteScan2D> discreteScans,
|
||||
List<Candidate2D> candidates,
|
||||
int totalCandidates)
|
||||
{
|
||||
// THREAD SAFETY NOTE:
|
||||
// This uses partitioned writes pattern where each thread writes to non-overlapping indices.
|
||||
// List<T> internally uses an array, and concurrent writes to different indices of an array
|
||||
// are thread-safe as long as no reallocation occurs (no Add/Remove operations).
|
||||
// Each thread processes a distinct chunk [startIndex, endIndex) with no overlap.
|
||||
|
||||
// Thread-safe shared state
|
||||
int candidatesWithKnownCells = 0;
|
||||
double maxScore = double.MinValue;
|
||||
Candidate2D? bestCandidateWithKnownCells = null;
|
||||
double maxScoreWithKnownCells = double.MinValue;
|
||||
Lock lockObject = new();
|
||||
|
||||
// Calculate chunk size
|
||||
int chunkSize = Math.Max(1, totalCandidates / _numThreads);
|
||||
int numThreads = Math.Min(_numThreads, totalCandidates);
|
||||
|
||||
// Create and start threads with high priority
|
||||
Thread[] threads = new Thread[numThreads];
|
||||
|
||||
// Use CountdownEvent with using statement to ensure proper disposal
|
||||
using CountdownEvent countdown = new(numThreads);
|
||||
// Capture variables for thread closure to avoid closure issues
|
||||
int capturedNumThreads = numThreads;
|
||||
int capturedChunkSize = chunkSize;
|
||||
int capturedTotalCandidates = totalCandidates;
|
||||
|
||||
for (int threadIndex = 0; threadIndex < numThreads; threadIndex++)
|
||||
{
|
||||
// Capture loop variables to avoid closure issues
|
||||
int capturedThreadIndex = threadIndex;
|
||||
int capturedStartIndex = capturedThreadIndex * capturedChunkSize;
|
||||
int capturedEndIndex = (capturedThreadIndex == capturedNumThreads - 1)
|
||||
? capturedTotalCandidates
|
||||
: (capturedThreadIndex + 1) * capturedChunkSize;
|
||||
|
||||
threads[capturedThreadIndex] = new Thread(() =>
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
try
|
||||
{
|
||||
int localCandidatesWithKnownCells = 0;
|
||||
double localMaxScore = double.MinValue;
|
||||
Candidate2D? localBestCandidateWithKnownCells = null;
|
||||
double localMaxScoreWithKnownCells = double.MinValue;
|
||||
|
||||
// Process candidates in this thread's chunk
|
||||
for (int i = capturedStartIndex; i < capturedEndIndex; i++)
|
||||
{
|
||||
ScoreSingleCandidate(grid, discreteScans, candidates, i,
|
||||
ref localCandidatesWithKnownCells, ref localMaxScore,
|
||||
ref localBestCandidateWithKnownCells, ref localMaxScoreWithKnownCells);
|
||||
}
|
||||
|
||||
// Merge thread-local results with shared state (thread-safe)
|
||||
lock (lockObject)
|
||||
{
|
||||
candidatesWithKnownCells += localCandidatesWithKnownCells;
|
||||
if (localMaxScore > maxScore)
|
||||
{
|
||||
maxScore = localMaxScore;
|
||||
}
|
||||
if (localBestCandidateWithKnownCells.HasValue &&
|
||||
localMaxScoreWithKnownCells > maxScoreWithKnownCells)
|
||||
{
|
||||
maxScoreWithKnownCells = localMaxScoreWithKnownCells;
|
||||
bestCandidateWithKnownCells = localBestCandidateWithKnownCells;
|
||||
}
|
||||
}
|
||||
countdown.Signal();
|
||||
}
|
||||
finally
|
||||
{
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
})
|
||||
{
|
||||
IsBackground = false, // Foreground thread for high priority
|
||||
Priority = ThreadPriority.Highest // Set thread priority to highest
|
||||
};
|
||||
|
||||
threads[capturedThreadIndex].Start();
|
||||
}
|
||||
|
||||
// Ensure all threads have finished (additional safety check)
|
||||
foreach (var thread in threads)
|
||||
{
|
||||
if (thread.IsAlive)
|
||||
{
|
||||
thread.Join();
|
||||
}
|
||||
}
|
||||
countdown.Wait();
|
||||
}
|
||||
|
||||
private void ScoreSingleCandidate(
|
||||
Grid2D grid,
|
||||
List<DiscreteScan2D> discreteScans,
|
||||
List<Candidate2D> candidates,
|
||||
int index,
|
||||
ref int candidatesWithKnownCells,
|
||||
ref double maxScore,
|
||||
ref Candidate2D? bestCandidateWithKnownCells,
|
||||
ref double maxScoreWithKnownCells)
|
||||
{
|
||||
var candidate = candidates[index];
|
||||
|
||||
if (candidate.ScanIndex >= discreteScans.Count)
|
||||
{
|
||||
candidate.Score = 0.0;
|
||||
candidates[index] = candidate;
|
||||
return;
|
||||
}
|
||||
|
||||
var discreteScan = discreteScans[candidate.ScanIndex];
|
||||
double candidateScore = 0.0;
|
||||
|
||||
if (grid is ProbabilityGrid probabilityGrid)
|
||||
{
|
||||
// FIX: Match C++ behavior - no explicit bounds check needed
|
||||
// ProbabilityGrid.GetProbability already returns kMinProbability for out-of-bounds cells
|
||||
// (C++ probability_grid.cc line 79: if (!limits().Contains(cell_index)) return kMinProbability;)
|
||||
foreach (var xyIndex in discreteScan)
|
||||
{
|
||||
var proposedXYIndex = new Array2i(
|
||||
xyIndex.X + candidate.XIndexOffset,
|
||||
xyIndex.Y + candidate.YIndexOffset
|
||||
);
|
||||
|
||||
// Get probability - out-of-bounds/unknown cells will return kMinProbability (0.1)
|
||||
var probability = probabilityGrid.GetProbability(proposedXYIndex);
|
||||
candidateScore += probability;
|
||||
}
|
||||
candidateScore /= discreteScan.Count;
|
||||
// Match C++ CHECK_GT(candidate_score, 0.f) - validate score is positive
|
||||
// For ProbabilityGrid, scores should always be > 0 since probabilities are >= kMinProbability
|
||||
System.Diagnostics.Debug.Assert(candidateScore > 0.0,
|
||||
$"Candidate score must be positive for ProbabilityGrid, got {candidateScore}");
|
||||
}
|
||||
else if (grid.GetGridType() == CartographerSharp.Mapping.D2D.GridType.TSDF)
|
||||
{
|
||||
if (grid is TSDF2D tsdfGrid)
|
||||
{
|
||||
// Match C++: Use GetTSDAndWeight and compute normalized score with weighted average
|
||||
double summedWeight = 0.0;
|
||||
foreach (var xyIndex in discreteScan)
|
||||
{
|
||||
var proposedXYIndex = new Array2i(
|
||||
xyIndex.X + candidate.XIndexOffset,
|
||||
xyIndex.Y + candidate.YIndexOffset
|
||||
);
|
||||
var (tsd, weight) = tsdfGrid.GetTSDAndWeight(proposedXYIndex);
|
||||
// Match C++: normalized_tsd_score = (max_correspondence_cost - abs(tsd)) / max_correspondence_cost
|
||||
double maxCorrespondenceCost = tsdfGrid.MaxCorrespondenceCost;
|
||||
double normalizedTsdScore = (maxCorrespondenceCost - Math.Abs(tsd)) / maxCorrespondenceCost;
|
||||
candidateScore += normalizedTsdScore * weight;
|
||||
summedWeight += weight;
|
||||
}
|
||||
// Match C++: if (summed_weight == 0.f) return 0.f; candidate_score /= summed_weight;
|
||||
if (summedWeight == 0.0)
|
||||
{
|
||||
candidateScore = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
candidateScore /= summedWeight;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Apply exponential penalty based on translation and rotation delta cost weights
|
||||
var translationDistance = Math.Sqrt(candidate.X * candidate.X + candidate.Y * candidate.Y);
|
||||
var rotationDelta = Math.Abs(candidate.Orientation);
|
||||
var cost = translationDistance * options.TranslationDeltaCostWeight +
|
||||
rotationDelta * options.RotationDeltaCostWeight;
|
||||
candidateScore *= Math.Exp(-cost * cost);
|
||||
|
||||
candidate.Score = candidateScore;
|
||||
candidates[index] = candidate;
|
||||
|
||||
// Track candidates with known cells AFTER Score is set
|
||||
// Unknown cells all return kMinProbability = 0.1, so scores > 0.1 indicate known cells
|
||||
if (candidateScore > 0.1 + 1e-5)
|
||||
{
|
||||
candidatesWithKnownCells++;
|
||||
if (candidateScore > maxScoreWithKnownCells)
|
||||
{
|
||||
maxScoreWithKnownCells = candidateScore;
|
||||
bestCandidateWithKnownCells = candidate;
|
||||
}
|
||||
}
|
||||
|
||||
// Track best candidate overall
|
||||
if (candidateScore > maxScore)
|
||||
{
|
||||
maxScore = candidateScore;
|
||||
}
|
||||
}
|
||||
|
||||
private static List<Candidate2D> GenerateExhaustiveSearchCandidates(SearchParameters searchParameters)
|
||||
{
|
||||
// Match C++: Calculate total number of candidates and reserve capacity
|
||||
int numCandidates = 0;
|
||||
for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
|
||||
{
|
||||
var bounds = searchParameters.LinearBoundsList[scanIndex];
|
||||
int numLinearXCandidates = bounds.MaxX - bounds.MinX + 1;
|
||||
int numLinearYCandidates = bounds.MaxY - bounds.MinY + 1;
|
||||
numCandidates += numLinearXCandidates * numLinearYCandidates;
|
||||
}
|
||||
|
||||
var candidates = new List<Candidate2D>(numCandidates); // Reserve capacity
|
||||
|
||||
for (int scanIndex = 0; scanIndex < searchParameters.NumScans; scanIndex++)
|
||||
{
|
||||
var bounds = searchParameters.LinearBoundsList[scanIndex];
|
||||
for (int xIndexOffset = bounds.MinX; xIndexOffset <= bounds.MaxX; xIndexOffset++)
|
||||
{
|
||||
for (int yIndexOffset = bounds.MinY; yIndexOffset <= bounds.MaxY; yIndexOffset++)
|
||||
{
|
||||
candidates.Add(new Candidate2D(scanIndex, xIndexOffset, yIndexOffset, searchParameters));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return candidates;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
/*
|
||||
* 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;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
|
||||
/// the solution's distance from 'target_angle'.
|
||||
/// </summary>
|
||||
public class RotationDeltaCostFunctor2D
|
||||
{
|
||||
private readonly double _scalingFactor;
|
||||
private readonly double _targetAngle;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for rotation delta.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight for the rotation cost.</param>
|
||||
/// <param name="targetAngle">Target rotation angle in radians.</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor, double targetAngle)
|
||||
{
|
||||
var functor = new RotationDeltaCostFunctor2D(scalingFactor, targetAngle);
|
||||
return new AutoDiffCostFunction(
|
||||
functor.Evaluate,
|
||||
numResiduals: 1,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
}
|
||||
|
||||
private RotationDeltaCostFunctor2D(double scalingFactor, double targetAngle)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_targetAngle = targetAngle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Pose parameters [x, y, theta].</param>
|
||||
/// <param name="residuals">Output residual [dtheta].</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 < 1)
|
||||
return false;
|
||||
|
||||
var pose = parameters[0];
|
||||
var theta = pose[2]; // rotation angle
|
||||
|
||||
// Match C++: residual[0] = scaling_factor_ * (pose[2] - angle_);
|
||||
// C++ does NOT normalize angle difference - Ceres autodiff handles it
|
||||
residuals[0] = _scalingFactor * (theta - _targetAngle);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* 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.Mapping.D2D;
|
||||
using CartographerSharp.Sensor;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a cost function for matching the 'point_cloud' in the 'grid' at a 'pose'.
|
||||
/// The cost increases with the signed distance of the matched point location in the 'grid'.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a TSDF match cost function for 2D scan matching.
|
||||
/// </remarks>
|
||||
/// <param name="residualScalingFactor">Scaling factor for residuals.</param>
|
||||
/// <param name="pointCloud">Point cloud to match.</param>
|
||||
/// <param name="tsdf">TSDF grid to match against.</param>
|
||||
public class TSDFMatchCostFunction2D(
|
||||
double residualScalingFactor,
|
||||
PointCloud _pointCloud,
|
||||
TSDF2D tsdf) : IDisposable
|
||||
{
|
||||
private readonly InterpolatedTSDF2D _interpolatedTSDF = new(tsdf);
|
||||
// Cache tempResiduals array to avoid allocation on every Evaluate call
|
||||
// Evaluate() is called many times during Ceres optimization (function + Jacobian)
|
||||
private double[]? _tempResiduals;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a DynamicAutoDiff cost function for TSDF matching.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Scaling factor.</param>
|
||||
/// <param name="pointCloud">Point cloud to match.</param>
|
||||
/// <param name="tsdf">TSDF grid to match against.</param>
|
||||
/// <returns>DynamicAutoDiff cost function.</returns>
|
||||
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
PointCloud pointCloud,
|
||||
TSDF2D tsdf)
|
||||
{
|
||||
var costFunction = new TSDFMatchCostFunction2D(scalingFactor, pointCloud, tsdf);
|
||||
return new DynamicAutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Pose parameters [x, y, theta].</param>
|
||||
/// <param name="residuals">Output residuals (one per point).</param>
|
||||
/// <returns>True on success.</returns>
|
||||
internal bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
// Return true with zero residuals for invalid inputs (consistent with OccupiedSpaceCostFunction2D)
|
||||
// Returning false would tell Ceres the evaluation failed, causing it to reject the step
|
||||
if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3 ||
|
||||
residuals == null || residuals.Length < _pointCloud.Count)
|
||||
{
|
||||
if (residuals != null)
|
||||
Array.Clear(residuals, 0, residuals.Length);
|
||||
return true;
|
||||
}
|
||||
|
||||
var pose = parameters[0];
|
||||
var translation = new Vector2(pose[0], pose[1]);
|
||||
var rotation = pose[2];
|
||||
|
||||
// Create rotation matrix
|
||||
var rotationMatrix = Matrix3x2.CreateRotation(rotation);
|
||||
|
||||
// Reuse cached array to avoid allocation per Evaluate call
|
||||
if (_tempResiduals == null || _tempResiduals.Length < _pointCloud.Count)
|
||||
_tempResiduals = new double[_pointCloud.Count];
|
||||
|
||||
double summedWeight = 0.0;
|
||||
|
||||
for (int i = 0; i < _pointCloud.Count; i++)
|
||||
{
|
||||
var point = _pointCloud[i];
|
||||
|
||||
// Transform point from local frame to world frame
|
||||
var localPoint = new Vector2(point.Position.X, point.Position.Y);
|
||||
var worldPoint = Vector2.Transform(localPoint, rotationMatrix) + translation;
|
||||
|
||||
var pointWeight = _interpolatedTSDF.GetWeight(worldPoint.X, worldPoint.Y);
|
||||
summedWeight += pointWeight;
|
||||
|
||||
_tempResiduals[i] = _pointCloud.Count * residualScalingFactor *
|
||||
_interpolatedTSDF.GetCorrespondenceCost(worldPoint.X, worldPoint.Y) *
|
||||
pointWeight;
|
||||
}
|
||||
|
||||
if (summedWeight == 0.0)
|
||||
{
|
||||
// All weights are zero - return zero residuals (consistent with OccupiedSpaceCostFunction2D)
|
||||
Array.Clear(residuals, 0, _pointCloud.Count);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Normalize residuals by summed weight
|
||||
for (int i = 0; i < _pointCloud.Count; i++)
|
||||
{
|
||||
residuals[i] = _tempResiduals[i] / summedWeight;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disposes managed resources.
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* 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 RobotNet10.Shared.Numbers;
|
||||
using CeresSharp;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D2D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Computes the cost of translating 'pose' to 'target_translation'.
|
||||
/// Cost increases with the solution's distance from 'target_translation'.
|
||||
/// </summary>
|
||||
public class TranslationDeltaCostFunctor2D
|
||||
{
|
||||
private readonly double _scalingFactor;
|
||||
private readonly double _targetX;
|
||||
private readonly double _targetY;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for translation delta.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight for the translation cost.</param>
|
||||
/// <param name="targetTranslation">Target translation (x, y).</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor, Vector2 targetTranslation)
|
||||
{
|
||||
var functor = new TranslationDeltaCostFunctor2D(scalingFactor, targetTranslation);
|
||||
return new AutoDiffCostFunction(
|
||||
functor.Evaluate,
|
||||
numResiduals: 2,
|
||||
parameterBlockSizes: [3] // [x, y, theta]
|
||||
);
|
||||
}
|
||||
|
||||
private TranslationDeltaCostFunctor2D(double scalingFactor, Vector2 targetTranslation)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_targetX = targetTranslation.X;
|
||||
_targetY = targetTranslation.Y;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Pose parameters [x, y, theta].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy].</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 < 2)
|
||||
return false;
|
||||
|
||||
var pose = parameters[0];
|
||||
var x = pose[0];
|
||||
var y = pose[1];
|
||||
// theta (pose[2]) is not used for translation delta
|
||||
|
||||
residuals[0] = _scalingFactor * (x - _targetX);
|
||||
residuals[1] = _scalingFactor * (y - _targetY);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user