Initial commit
This commit is contained in:
@@ -0,0 +1,162 @@
|
||||
/*
|
||||
* 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.Internal.D3D;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// Wires up local SLAM (LocalTrajectoryBuilder3D) with the PoseGraph for 3D mapping.
|
||||
/// Handles sensor data, triggers local SLAM, and adds results to the pose graph.
|
||||
/// </summary>
|
||||
public class GlobalTrajectoryBuilder3D(
|
||||
LocalTrajectoryBuilder3D? localTrajectoryBuilder,
|
||||
int trajectoryId,
|
||||
PoseGraph3D poseGraph,
|
||||
MotionFilter? poseGraphOdometryMotionFilter = null) : ITrajectoryBuilder
|
||||
{
|
||||
public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
|
||||
{
|
||||
if (localTrajectoryBuilder == null)
|
||||
{
|
||||
throw new InvalidOperationException("Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.");
|
||||
}
|
||||
|
||||
var matchingResult = localTrajectoryBuilder.AddRangeData(sensorId, timedPointCloudData);
|
||||
if (matchingResult == null)
|
||||
{
|
||||
// The range data has not been fully accumulated yet.
|
||||
return null;
|
||||
}
|
||||
|
||||
var result = matchingResult.Value;
|
||||
ITrajectoryBuilder.InsertionResult? insertionResult = null;
|
||||
|
||||
// If we have an insertion result, add node to pose graph
|
||||
if (result.InsertionResult.HasValue)
|
||||
{
|
||||
var insertionResultValue = result.InsertionResult.Value;
|
||||
if (insertionResultValue.ConstantData is null)
|
||||
throw new InvalidOperationException($"insertionResult.ConstantData of sensorId {sensorId} is null");
|
||||
|
||||
// Cast submaps to Submap3D for PoseGraph3D.AddNode
|
||||
var submaps3D = insertionResultValue.InsertionSubmaps.Cast<Mapping.D3D.Submap3D>().ToList();
|
||||
|
||||
var nodeId = poseGraph.AddNode(
|
||||
insertionResultValue.ConstantData,
|
||||
trajectoryId,
|
||||
submaps3D);
|
||||
|
||||
if (nodeId.TrajectoryId != trajectoryId)
|
||||
{
|
||||
throw new InvalidOperationException($"Node trajectory ID {nodeId.TrajectoryId} does not match expected {trajectoryId}");
|
||||
}
|
||||
|
||||
// Update insertionResult with NodeId
|
||||
insertionResult = new ITrajectoryBuilder.InsertionResult(
|
||||
nodeId,
|
||||
insertionResultValue.ConstantData,
|
||||
insertionResultValue.InsertionSubmaps);
|
||||
|
||||
// Update result with new insertionResult (including NodeId)
|
||||
result = new ITrajectoryBuilder.MatchingResult(
|
||||
trajectoryId,
|
||||
result.Time,
|
||||
result.LocalPose,
|
||||
result.RangeDataInLocal,
|
||||
insertionResult,
|
||||
result.PoseConfidence,
|
||||
result.CeresScore,
|
||||
result.SamplePointCloudGlobal
|
||||
);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, ImuData imuData)
|
||||
{
|
||||
// Add to local trajectory builder if available
|
||||
localTrajectoryBuilder?.AddImuData(imuData);
|
||||
|
||||
// Always add to pose graph for global optimization
|
||||
poseGraph.AddImuData(trajectoryId, imuData);
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, OdometryData odometryData)
|
||||
{
|
||||
if (!odometryData.Pose.IsValid())
|
||||
{
|
||||
throw new ArgumentException($"Invalid odometry pose: {odometryData.Pose}", nameof(odometryData));
|
||||
}
|
||||
|
||||
// Add to local trajectory builder if available
|
||||
localTrajectoryBuilder?.AddOdometryData(odometryData);
|
||||
|
||||
// Apply motion filter if configured
|
||||
if (poseGraphOdometryMotionFilter != null &&
|
||||
poseGraphOdometryMotionFilter.IsSimilar(odometryData.Time, odometryData.Pose))
|
||||
{
|
||||
return; // Filtered out due to similar motion
|
||||
}
|
||||
|
||||
// Add to pose graph
|
||||
poseGraph.AddOdometryData(trajectoryId, odometryData);
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
|
||||
{
|
||||
if (fixedFramePoseData.Pose.HasValue && !fixedFramePoseData.Pose.Value.IsValid())
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"Invalid fixed frame pose: {fixedFramePoseData.Pose.Value}",
|
||||
nameof(fixedFramePoseData));
|
||||
}
|
||||
|
||||
poseGraph.AddFixedFramePoseData(trajectoryId, fixedFramePoseData);
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, LandmarkData landmarkData)
|
||||
{
|
||||
poseGraph.AddLandmarkData(trajectoryId, landmarkData);
|
||||
}
|
||||
|
||||
public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
|
||||
{
|
||||
if (localTrajectoryBuilder != null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"Can't add LocalSlamResultData with local_trajectory_builder_ present.");
|
||||
}
|
||||
|
||||
// Add the local SLAM result directly to the pose graph
|
||||
localSlamResultData.AddToPoseGraph(trajectoryId, poseGraph);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public Rigid3d? TryGetExtrapolatedPose(long time)
|
||||
{
|
||||
return localTrajectoryBuilder?.TryGetExtrapolatedPose(time);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
|
||||
{
|
||||
return localTrajectoryBuilder?.TryGetExtrapolatedPoseFilter(time);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// Result of IMU integration.
|
||||
/// </summary>
|
||||
public struct IntegrateImuResult
|
||||
{
|
||||
public Vector3 DeltaVelocity { get; set; }
|
||||
public Vector3 DeltaTranslation { get; set; }
|
||||
public Quaternion DeltaRotation { get; set; }
|
||||
|
||||
public IntegrateImuResult(Vector3 deltaVelocity, Vector3 deltaTranslation, Quaternion deltaRotation)
|
||||
{
|
||||
DeltaVelocity = deltaVelocity;
|
||||
DeltaTranslation = deltaTranslation;
|
||||
DeltaRotation = deltaRotation;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// IMU integration utilities.
|
||||
/// </summary>
|
||||
public static class ImuIntegration
|
||||
{
|
||||
/// <summary>
|
||||
/// Integrates IMU data between start_time and end_time.
|
||||
/// Returns delta_velocity, delta_translation, and delta_rotation.
|
||||
/// </summary>
|
||||
public static IntegrateImuResult IntegrateImu(
|
||||
List<ImuData> imuData,
|
||||
long startTime,
|
||||
long endTime,
|
||||
ref int imuIndex)
|
||||
{
|
||||
if (startTime > endTime)
|
||||
throw new ArgumentException("startTime must be <= endTime");
|
||||
|
||||
if (imuIndex < 0 || imuIndex >= imuData.Count)
|
||||
throw new ArgumentOutOfRangeException(nameof(imuIndex));
|
||||
|
||||
if (imuData[imuIndex].Time > startTime)
|
||||
throw new ArgumentException("imuData[imuIndex].Time must be <= startTime");
|
||||
|
||||
if (imuIndex + 1 < imuData.Count && imuData[imuIndex + 1].Time <= startTime)
|
||||
throw new ArgumentException("imuData[imuIndex+1].Time must be > startTime");
|
||||
|
||||
var result = new IntegrateImuResult(
|
||||
Vector3.Zero,
|
||||
Vector3.Zero,
|
||||
Quaternion.Identity);
|
||||
|
||||
long currentTime = startTime;
|
||||
|
||||
while (currentTime < endTime)
|
||||
{
|
||||
long nextImuTime = long.MaxValue;
|
||||
if (imuIndex + 1 < imuData.Count)
|
||||
{
|
||||
nextImuTime = imuData[imuIndex + 1].Time;
|
||||
}
|
||||
|
||||
long nextTime = Math.Min(nextImuTime, endTime);
|
||||
double deltaT = (nextTime - currentTime) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)
|
||||
|
||||
var currentImu = imuData[imuIndex];
|
||||
|
||||
// Compute delta angle from angular velocity
|
||||
var deltaAngle = currentImu.AngularVelocity * deltaT;
|
||||
|
||||
// Convert angle-axis to quaternion (simplified - assumes small angles)
|
||||
// For small angles: q ≈ [1, 0.5*angle.x, 0.5*angle.y, 0.5*angle.z]
|
||||
var angleLength = deltaAngle.Length();
|
||||
Quaternion deltaRotation;
|
||||
if (angleLength < 1e-6)
|
||||
{
|
||||
deltaRotation = Quaternion.Identity;
|
||||
}
|
||||
else
|
||||
{
|
||||
var axis = Vector3.Normalize(deltaAngle);
|
||||
deltaRotation = Quaternion.CreateFromAxisAngle(axis, angleLength);
|
||||
}
|
||||
|
||||
// Update cumulative rotation
|
||||
result.DeltaRotation = Quaternion.Multiply(result.DeltaRotation, deltaRotation);
|
||||
|
||||
// Integrate linear acceleration
|
||||
// Rotate acceleration to current orientation frame
|
||||
var rotatedAcceleration = Vector3.Transform(currentImu.LinearAcceleration, result.DeltaRotation);
|
||||
var deltaVelocity = rotatedAcceleration * deltaT;
|
||||
result.DeltaVelocity += deltaVelocity;
|
||||
|
||||
// Integrate velocity to get translation
|
||||
result.DeltaTranslation += result.DeltaVelocity * deltaT;
|
||||
|
||||
currentTime = nextTime;
|
||||
if (currentTime == nextImuTime)
|
||||
{
|
||||
imuIndex++;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,713 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Mapping.D3D;
|
||||
using CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using LocalTrajectoryBuilderOptions3D = CartographerSharp.Models.Mapping.LocalTrajectoryBuilderOptions3D;
|
||||
using RangeDataOperations = CartographerSharp.Sensor.RangeDataOperations;
|
||||
using Submap3D = CartographerSharp.Mapping.D3D.Submap3D;
|
||||
using PointCloudOperations = CartographerSharp.Sensor.PointCloudOperations;
|
||||
using FastCorrelativeScanMatcherOptions3D = CartographerSharp.Models.Mapping.FastCorrelativeScanMatcherOptions3D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
|
||||
/// without loop closure for 3D.
|
||||
/// </summary>
|
||||
public class LocalTrajectoryBuilder3D : IDisposable
|
||||
{
|
||||
private bool _disposed;
|
||||
public struct InsertionResult(TrajectoryNode.Data? constantData, List<Submap3D> insertionSubmaps)
|
||||
{
|
||||
public TrajectoryNode.Data? ConstantData { get; set; } = constantData;
|
||||
public List<Submap3D> InsertionSubmaps { get; set; } = insertionSubmaps ?? [];
|
||||
}
|
||||
|
||||
private readonly LocalTrajectoryBuilderOptions3D _options;
|
||||
private readonly ActiveSubmaps3D _activeSubmaps;
|
||||
private readonly MotionFilter _motionFilter;
|
||||
private readonly CeresScanMatcher3D? _ceresScanMatcher;
|
||||
private PoseExtrapolator? _extrapolator;
|
||||
// Range data accumulation - these are used when NumAccumulatedRangeData > 1
|
||||
private int _numAccumulated = 0;
|
||||
private readonly List<TimedPointCloudOriginData> _accumulatedPointCloudOriginData = [];
|
||||
private long? _lastSensorTime;
|
||||
private readonly RangeDataCollator _rangeDataCollator;
|
||||
|
||||
public LocalTrajectoryBuilder3D(
|
||||
LocalTrajectoryBuilderOptions3D options,
|
||||
List<string> expectedRangeSensorIds)
|
||||
{
|
||||
_options = options;
|
||||
_activeSubmaps = new ActiveSubmaps3D(options.SubmapsOptions);
|
||||
_motionFilter = new MotionFilter(options.MotionFilterOptions);
|
||||
|
||||
// Initialize scan matchers from options
|
||||
// Note: RealTimeCorrelativeScanMatcher3D is created per-submap in ScanMatch()
|
||||
// because it needs HybridGrid which is submap-specific and not available at construction time
|
||||
|
||||
if (options.CeresScanMatcherOptions.HasValue)
|
||||
{
|
||||
_ceresScanMatcher = new CeresScanMatcher3D(options.CeresScanMatcherOptions.Value);
|
||||
}
|
||||
|
||||
_rangeDataCollator = new RangeDataCollator(expectedRangeSensorIds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds IMU data to the pose extrapolator.
|
||||
/// Match C++ (local_trajectory_builder_3d.cc line 111-127)
|
||||
/// </summary>
|
||||
public void AddImuData(ImuData imuData)
|
||||
{
|
||||
if (_extrapolator != null)
|
||||
{
|
||||
_extrapolator.AddImuData(imuData);
|
||||
return;
|
||||
}
|
||||
|
||||
// Initialize extrapolator with IMU data and initial poses/data from options
|
||||
var poseQueueDuration = TimeSpan.FromSeconds(_options.PoseExtrapolatorOptions.ConstantVelocity.PoseQueueDuration);
|
||||
|
||||
// Convert initial poses from proto
|
||||
var initialPoses = new List<(long time, Rigid3d transform)>();
|
||||
if (_options.InitialPoses != null)
|
||||
{
|
||||
foreach (var poseProto in _options.InitialPoses)
|
||||
{
|
||||
var transform = (Rigid3d)poseProto.Transform;
|
||||
initialPoses.Add((poseProto.Time, transform));
|
||||
}
|
||||
}
|
||||
|
||||
// Convert initial IMU data from proto
|
||||
var initialImuData = new List<ImuData>();
|
||||
if (_options.InitialImuData != null)
|
||||
{
|
||||
foreach (var imuProto in _options.InitialImuData)
|
||||
{
|
||||
initialImuData.Add(ImuDataOperations.FromProto(imuProto));
|
||||
}
|
||||
}
|
||||
// Add current IMU data to the list
|
||||
initialImuData.Add(imuData);
|
||||
|
||||
// CRITICAL FIX: Match C++ CreateWithImuData behavior
|
||||
// C++ passes ALL initial_imu_data and initial_poses to the extrapolator
|
||||
// Initialize with first IMU data
|
||||
_extrapolator = PoseExtrapolator.InitializeWithImu(
|
||||
poseQueueDuration.Ticks,
|
||||
_options.PoseExtrapolatorOptions.ConstantVelocity.ImuGravityTimeConstant,
|
||||
initialImuData[0] // Initialize with first IMU data
|
||||
);
|
||||
|
||||
// Add remaining IMU data (skip the first one which was used for initialization)
|
||||
for (int i = 1; i < initialImuData.Count; i++)
|
||||
{
|
||||
_extrapolator.AddImuData(initialImuData[i]);
|
||||
}
|
||||
|
||||
// Add initial poses if available (match C++ line 126: initial_poses parameter)
|
||||
foreach (var (time, transform) in initialPoses)
|
||||
{
|
||||
_extrapolator.AddPose(time, transform);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds odometry data to the pose extrapolator.
|
||||
/// </summary>
|
||||
public void AddOdometryData(OdometryData odometryData)
|
||||
{
|
||||
_extrapolator?.AddOdometryData(odometryData);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tries to get the current pose from the extrapolator at the given time.
|
||||
/// Returns null when extrapolator is not initialized or time is before the last pose time.
|
||||
/// Used by ITrajectoryBuilder.TryGetExtrapolatedPose so callers (e.g. CartographerService) can read a live pose.
|
||||
/// </summary>
|
||||
public Rigid3d? TryGetExtrapolatedPose(long time)
|
||||
{
|
||||
if (_extrapolator == null)
|
||||
return null;
|
||||
if (time < _extrapolator.GetLastPoseTime())
|
||||
return null;
|
||||
try
|
||||
{
|
||||
return _extrapolator.ExtrapolatePose(time);
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tries to get the current pose with low-pass filter to reduce jitter during direction changes.
|
||||
/// Match C++: ExtrapolatePose_filter - should be used for publishing pose to external systems.
|
||||
/// </summary>
|
||||
public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
|
||||
{
|
||||
if (_extrapolator == null)
|
||||
return null;
|
||||
if (time < _extrapolator.GetLastPoseTime())
|
||||
return null;
|
||||
try
|
||||
{
|
||||
return _extrapolator.ExtrapolatePoseFilter(time);
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns 'MatchingResult' when range data accumulation completed,
|
||||
/// otherwise 'null'.
|
||||
/// </summary>
|
||||
public ITrajectoryBuilder.MatchingResult? AddRangeData(string sensorId, TimedPointCloudData rangeData)
|
||||
{
|
||||
// Check intensities consistency if enabled
|
||||
if (_options.UseIntensities && rangeData.Intensities != null)
|
||||
{
|
||||
if (rangeData.Intensities.Count != rangeData.Ranges.Count)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"Passed point cloud has inconsistent number of intensities and ranges.");
|
||||
}
|
||||
}
|
||||
|
||||
var synchronizedData = _rangeDataCollator.AddRangeData(sensorId, rangeData);
|
||||
if (synchronizedData.Ranges.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var time = synchronizedData.Time;
|
||||
_lastSensorTime = time;
|
||||
|
||||
if (_extrapolator == null)
|
||||
{
|
||||
// Until we've initialized the extrapolator with our first IMU message, we
|
||||
// cannot compute the orientation of the rangefinder.
|
||||
return null;
|
||||
}
|
||||
|
||||
// Validate time of first point
|
||||
if (synchronizedData.Ranges.Count > 0)
|
||||
{
|
||||
// MEDIUM FIX: Use Math.Round for consistent precision with RangeDataCollator
|
||||
var firstRangeTime = time + (long)Math.Round(synchronizedData.Ranges[0].PointTime.Time * TimeSpan.TicksPerSecond);
|
||||
if (firstRangeTime < _extrapolator.GetLastPoseTime())
|
||||
{
|
||||
// Extrapolator is still initializing
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
// Apply voxel filter before accumulation (0.5 * voxel_filter_size)
|
||||
var filteredRanges = Sensor.VoxelFilter.Filter(
|
||||
synchronizedData.Ranges,
|
||||
0.5 * _options.VoxelFilterSize);
|
||||
|
||||
// Create filtered synchronized data
|
||||
var filteredSynchronizedData = new TimedPointCloudOriginData(
|
||||
synchronizedData.Time,
|
||||
synchronizedData.Origins,
|
||||
filteredRanges);
|
||||
|
||||
// Range data accumulation: accumulate multiple range data if configured
|
||||
if (_numAccumulated == 0)
|
||||
{
|
||||
_accumulatedPointCloudOriginData.Clear();
|
||||
}
|
||||
|
||||
_accumulatedPointCloudOriginData.Add(filteredSynchronizedData);
|
||||
_numAccumulated++;
|
||||
|
||||
if (_numAccumulated < _options.NumAccumulatedRangeData)
|
||||
{
|
||||
return null; // Need more accumulation
|
||||
}
|
||||
|
||||
_numAccumulated = 0;
|
||||
|
||||
// Process accumulated range data
|
||||
return ProcessAccumulatedRangeData(time);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Processes accumulated range data and performs scan matching.
|
||||
/// </summary>
|
||||
private ITrajectoryBuilder.MatchingResult? ProcessAccumulatedRangeData(long time)
|
||||
{
|
||||
if (_accumulatedPointCloudOriginData.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// Collect detailed hit times with validation (per point)
|
||||
bool warned = false;
|
||||
var hitTimes = new List<long>();
|
||||
long prevTimePoint = _extrapolator!.GetLastExtrapolatedTime();
|
||||
|
||||
foreach (var pointCloudOriginData in _accumulatedPointCloudOriginData)
|
||||
{
|
||||
foreach (var hit in pointCloudOriginData.Ranges)
|
||||
{
|
||||
// Calculate absolute time for this hit point
|
||||
// MEDIUM FIX: Use Math.Round for consistent precision with RangeDataCollator
|
||||
var timePoint = pointCloudOriginData.Time +
|
||||
(long)Math.Round(hit.PointTime.Time * TimeSpan.TicksPerSecond);
|
||||
|
||||
// Validate time doesn't jump backwards
|
||||
if (timePoint < prevTimePoint)
|
||||
{
|
||||
if (!warned)
|
||||
{
|
||||
// Log warning (could use proper logger here)
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"Warning: Timestamp of individual range data point jumps backwards " +
|
||||
$"from {prevTimePoint} to {timePoint}");
|
||||
warned = true;
|
||||
}
|
||||
timePoint = prevTimePoint;
|
||||
}
|
||||
|
||||
hitTimes.Add(timePoint);
|
||||
prevTimePoint = timePoint;
|
||||
}
|
||||
}
|
||||
// Add last sensor time
|
||||
if (_accumulatedPointCloudOriginData.Count > 0)
|
||||
{
|
||||
hitTimes.Add(_accumulatedPointCloudOriginData[^1].Time);
|
||||
}
|
||||
|
||||
// Extrapolate poses for all hit times
|
||||
var extrapolationResult = _extrapolator!.ExtrapolatePosesWithGravity(hitTimes);
|
||||
|
||||
// Build list of poses (one per hit time)
|
||||
var hitPoses = new List<Rigid3f>();
|
||||
foreach (var pose in extrapolationResult.PreviousPoses)
|
||||
{
|
||||
hitPoses.Add(pose);
|
||||
}
|
||||
hitPoses.Add(new Rigid3f(
|
||||
(Vector3)extrapolationResult.CurrentPose.Translation,
|
||||
extrapolationResult.CurrentPose.Rotation));
|
||||
|
||||
// Transform accumulated points using poses at their respective times
|
||||
var accumulatedPoints = new List<RangefinderPoint>();
|
||||
var accumulatedIntensities = _options.UseIntensities ? new List<double>() : null;
|
||||
var misses = new PointCloud();
|
||||
|
||||
int hitPoseIndex = 0;
|
||||
bool warnedPosesExhausted = false;
|
||||
bool warnedOriginsEmpty = false;
|
||||
foreach (var pointCloudOriginData in _accumulatedPointCloudOriginData)
|
||||
{
|
||||
foreach (var hit in pointCloudOriginData.Ranges)
|
||||
{
|
||||
// MEDIUM FIX: Add warning log when hitPoses is exhausted
|
||||
if (hitPoseIndex >= hitPoses.Count)
|
||||
{
|
||||
if (!warnedPosesExhausted)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"Warning: hitPoses exhausted at index {hitPoseIndex}, expected {hitPoses.Count} poses. " +
|
||||
"This may indicate a mismatch between hit count and pose count.");
|
||||
warnedPosesExhausted = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
var poseAtTime = hitPoses[hitPoseIndex];
|
||||
hitPoseIndex++;
|
||||
|
||||
// Transform hit point using pose at its time
|
||||
var hitInLocal = poseAtTime.TransformPoint(hit.PointTime.Position);
|
||||
|
||||
// Get origin for this range
|
||||
// MEDIUM FIX: Add warning log when origins collection is empty/insufficient
|
||||
var originIndex = hit.OriginIndex;
|
||||
Vector3 originInLocal;
|
||||
if (originIndex < pointCloudOriginData.Origins.Count)
|
||||
{
|
||||
originInLocal = poseAtTime.TransformPoint(pointCloudOriginData.Origins[originIndex]);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!warnedOriginsEmpty)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"Warning: Origin index {originIndex} out of bounds (Origins.Count={pointCloudOriginData.Origins.Count}). " +
|
||||
"Using poseAtTime.Translation as fallback origin.");
|
||||
warnedOriginsEmpty = true;
|
||||
}
|
||||
originInLocal = poseAtTime.Translation;
|
||||
}
|
||||
|
||||
var delta = hitInLocal - originInLocal;
|
||||
var rangeLength = delta.Length();
|
||||
|
||||
if (rangeLength >= _options.MinRange)
|
||||
{
|
||||
if (rangeLength <= _options.MaxRange)
|
||||
{
|
||||
accumulatedPoints.Add(new RangefinderPoint { Position = hitInLocal });
|
||||
if (_options.UseIntensities && accumulatedIntensities != null)
|
||||
{
|
||||
accumulatedIntensities.Add(hit.Intensity);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Miss beyond max range - insert ray cropped to max_range
|
||||
var missPoint = new RangefinderPoint
|
||||
{
|
||||
Position = originInLocal + delta / rangeLength * _options.MaxRange
|
||||
};
|
||||
misses.Add(missPoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Create PointCloud with intensities if enabled
|
||||
var accumulatedPointCloud = new PointCloud(
|
||||
accumulatedPoints,
|
||||
accumulatedIntensities ?? []);
|
||||
|
||||
var origin = extrapolationResult.CurrentPose.Translation;
|
||||
|
||||
// Reset accumulation
|
||||
_accumulatedPointCloudOriginData.Clear();
|
||||
|
||||
// Apply voxel filter to accumulated points and misses
|
||||
var filteredReturns = Sensor.VoxelFilter.Filter(
|
||||
accumulatedPointCloud,
|
||||
_options.VoxelFilterSize);
|
||||
var filteredMisses = Sensor.VoxelFilter.Filter(
|
||||
misses,
|
||||
_options.VoxelFilterSize);
|
||||
|
||||
// Create RangeData from accumulated and transformed points
|
||||
// C++ line 260-263: filtered_range_data has origin in tracking frame (current_pose.translation())
|
||||
// and points in local frame (hit_in_local from line 222-223)
|
||||
// C++ line 276-278: Transform to local frame using current_pose.inverse()
|
||||
var filteredRangeData = new RangeData(
|
||||
(Vector3)extrapolationResult.CurrentPose.Translation, // origin in tracking frame
|
||||
filteredReturns, // points in local frame
|
||||
filteredMisses); // misses in local frame
|
||||
|
||||
// Transform to local frame (C++ line 276-278: current_pose.inverse())
|
||||
var filteredRangeDataInLocal = RangeDataOperations.Transform(
|
||||
filteredRangeData,
|
||||
new Rigid3f(
|
||||
(Vector3)extrapolationResult.CurrentPose.Translation,
|
||||
extrapolationResult.CurrentPose.Rotation).Inverse());
|
||||
|
||||
// Filter range data by max range (use maxRange from options)
|
||||
var filteredRangeDataInTracking = Submap3D.FilterRangeDataByMaxRange(
|
||||
filteredRangeDataInLocal,
|
||||
_options.MaxRange
|
||||
);
|
||||
|
||||
// Apply adaptive voxel filter using options
|
||||
PointCloud highResolutionPointCloud;
|
||||
if (_options.HighResolutionAdaptiveVoxelFilterOptions.HasValue)
|
||||
{
|
||||
highResolutionPointCloud = Sensor.AdaptiveVoxelFilter.Filter(
|
||||
filteredRangeDataInTracking.Returns,
|
||||
_options.HighResolutionAdaptiveVoxelFilterOptions.Value);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Fallback to regular voxel filter
|
||||
highResolutionPointCloud = Sensor.VoxelFilter.Filter(
|
||||
filteredRangeDataInTracking.Returns,
|
||||
_options.VoxelFilterSize);
|
||||
}
|
||||
|
||||
if (highResolutionPointCloud.Count == 0)
|
||||
{
|
||||
return null; // Empty point cloud
|
||||
}
|
||||
|
||||
PointCloud lowResolutionPointCloud;
|
||||
if (_options.LowResolutionAdaptiveVoxelFilterOptions.HasValue)
|
||||
{
|
||||
lowResolutionPointCloud = Sensor.AdaptiveVoxelFilter.Filter(
|
||||
filteredRangeDataInTracking.Returns,
|
||||
_options.LowResolutionAdaptiveVoxelFilterOptions.Value);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Fallback to regular voxel filter (typically 3x the high resolution size)
|
||||
var lowResolutionVoxelFilterSize = _options.VoxelFilterSize * 3.0;
|
||||
lowResolutionPointCloud = Sensor.VoxelFilter.Filter(
|
||||
filteredRangeDataInTracking.Returns,
|
||||
lowResolutionVoxelFilterSize);
|
||||
}
|
||||
|
||||
if (lowResolutionPointCloud.Count == 0)
|
||||
{
|
||||
return null; // Empty point cloud
|
||||
}
|
||||
|
||||
// Get current pose and gravity alignment from extrapolation
|
||||
var currentPose = extrapolationResult.CurrentPose;
|
||||
var gravityAlignment = extrapolationResult.GravityFromTracking;
|
||||
|
||||
// Scan match
|
||||
var poseEstimate = ScanMatch(
|
||||
currentPose,
|
||||
lowResolutionPointCloud,
|
||||
highResolutionPointCloud
|
||||
);
|
||||
|
||||
if (poseEstimate == null)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// Update extrapolator (called before InsertIntoSubmap to match C++ order)
|
||||
_extrapolator!.AddPose(time, poseEstimate.Value);
|
||||
|
||||
// Transform range data to local frame
|
||||
// C++ line 332-333: TransformRangeData(filtered_range_data_in_tracking, pose_estimate->cast<double>())
|
||||
// pose_estimate is in tracking frame, so this transforms from tracking to local
|
||||
var rangeDataInLocal = RangeDataOperations.Transform(
|
||||
filteredRangeDataInTracking,
|
||||
new Rigid3f((Vector3)poseEstimate.Value.Translation, poseEstimate.Value.Rotation)
|
||||
);
|
||||
|
||||
// Insert into submap (motion filter is checked inside InsertIntoSubmap)
|
||||
var localInsertionResult = InsertIntoSubmap(
|
||||
time,
|
||||
rangeDataInLocal,
|
||||
filteredRangeDataInTracking,
|
||||
highResolutionPointCloud,
|
||||
lowResolutionPointCloud,
|
||||
poseEstimate.Value,
|
||||
gravityAlignment
|
||||
);
|
||||
|
||||
// Convert LocalTrajectoryBuilder3D.InsertionResult to ITrajectoryBuilder.InsertionResult
|
||||
ITrajectoryBuilder.InsertionResult? insertionResult = null;
|
||||
if (localInsertionResult.HasValue)
|
||||
{
|
||||
var localInsertion = localInsertionResult.Value;
|
||||
insertionResult = new ITrajectoryBuilder.InsertionResult(
|
||||
nodeId: default, // NodeId will be assigned by PoseGraph
|
||||
constantData: localInsertion.ConstantData,
|
||||
insertionSubmaps: localInsertion.InsertionSubmaps.Cast<Submap>().ToList()
|
||||
);
|
||||
}
|
||||
|
||||
return new ITrajectoryBuilder.MatchingResult(
|
||||
trajectoryId: 0,
|
||||
time: time,
|
||||
localPose: poseEstimate.Value,
|
||||
rangeDataInLocal: rangeDataInLocal,
|
||||
insertionResult: insertionResult,
|
||||
poseConfidence: -1.0,
|
||||
ceresScore: -1.0,
|
||||
samplePointCloudGlobal: null // 3D builder doesn't generate sample point cloud yet
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scan matches using the two point clouds and returns the observed pose, or
|
||||
/// null on failure.
|
||||
/// </summary>
|
||||
private Rigid3d? ScanMatch(
|
||||
Rigid3d posePrediction,
|
||||
PointCloud lowResolutionPointCloudInTracking,
|
||||
PointCloud highResolutionPointCloudInTracking)
|
||||
{
|
||||
var submaps = _activeSubmaps.Submaps();
|
||||
if (submaps.Count == 0)
|
||||
{
|
||||
return posePrediction;
|
||||
}
|
||||
|
||||
var matchingSubmap = submaps[0];
|
||||
var initialCeresPose = matchingSubmap.LocalPose.Inverse() * posePrediction;
|
||||
|
||||
// Step 1: Real-time correlative scan matching (if enabled)
|
||||
if (_options.UseOnlineCorrelativeScanMatching &&
|
||||
_options.RealTimeCorrelativeScanMatcherOptions.HasValue)
|
||||
{
|
||||
// Convert RealTimeCorrelativeScanMatcherOptions to FastCorrelativeScanMatcherOptions3D
|
||||
var rtOptions = _options.RealTimeCorrelativeScanMatcherOptions.Value;
|
||||
var fastOptions = new FastCorrelativeScanMatcherOptions3D(
|
||||
branchAndBoundDepth: 7, // Default depth
|
||||
fullResolutionDepth: 0, // Default
|
||||
minRotationalScore: 0.75f, // Default
|
||||
minLowResolutionScore: 0.7, // Default
|
||||
linearXySearchWindow: rtOptions.LinearSearchWindow,
|
||||
linearZSearchWindow: rtOptions.LinearSearchWindow, // Use same as XY
|
||||
angularSearchWindow: rtOptions.AngularSearchWindow
|
||||
);
|
||||
|
||||
// Create scan matcher per-submap (needs HybridGrid which is submap-specific)
|
||||
var realTimeMatcher = new RealTimeCorrelativeScanMatcher3D(
|
||||
matchingSubmap.HighResolutionHybridGrid,
|
||||
matchingSubmap.LowResolutionHybridGrid,
|
||||
null, // Rotational histogram not available here
|
||||
fastOptions);
|
||||
|
||||
// Create constant data for matching (simplified - only point clouds needed)
|
||||
var constantData = new TrajectoryNode.Data
|
||||
{
|
||||
HighResolutionPointCloud = highResolutionPointCloudInTracking,
|
||||
LowResolutionPointCloud = lowResolutionPointCloudInTracking,
|
||||
RotationalScanMatcherHistogram = null,
|
||||
GravityAlignment = Quaternion.Identity // Not critical for initial matching
|
||||
};
|
||||
|
||||
// Match with real-time correlative scan matcher
|
||||
var matchingResult = realTimeMatcher.Match(
|
||||
posePrediction,
|
||||
matchingSubmap.LocalPose,
|
||||
constantData,
|
||||
minScore: 0.1);
|
||||
|
||||
if (matchingResult.HasValue)
|
||||
{
|
||||
// Use matched pose as initial pose for Ceres
|
||||
initialCeresPose = matchingSubmap.LocalPose.Inverse() * matchingResult.Value.PoseEstimate;
|
||||
}
|
||||
}
|
||||
|
||||
// Step 2: Ceres scan matching
|
||||
if (_ceresScanMatcher == null)
|
||||
{
|
||||
return initialCeresPose;
|
||||
}
|
||||
|
||||
var pointCloudsAndGrids = new List<PointCloudAndHybridGridsPointers>
|
||||
{
|
||||
new() {
|
||||
PointCloud = highResolutionPointCloudInTracking,
|
||||
HybridGrid = matchingSubmap.HighResolutionHybridGrid,
|
||||
IntensityHybridGrid = _options.UseIntensities
|
||||
? matchingSubmap.HighResolutionIntensityHybridGrid
|
||||
: null
|
||||
},
|
||||
new() {
|
||||
PointCloud = lowResolutionPointCloudInTracking,
|
||||
HybridGrid = matchingSubmap.LowResolutionHybridGrid,
|
||||
IntensityHybridGrid = null
|
||||
}
|
||||
};
|
||||
|
||||
var targetTranslation = (matchingSubmap.LocalPose.Inverse() * posePrediction).Translation;
|
||||
|
||||
// FIX: SolverSummary holds unmanaged resources - must be disposed to prevent memory leak
|
||||
CeresSharp.SolverSummary? summary = null;
|
||||
try
|
||||
{
|
||||
_ceresScanMatcher.Match(
|
||||
targetTranslation,
|
||||
initialCeresPose,
|
||||
pointCloudsAndGrids,
|
||||
out var poseObservationInSubmap,
|
||||
out summary
|
||||
);
|
||||
|
||||
return matchingSubmap.LocalPose * poseObservationInSubmap;
|
||||
}
|
||||
finally
|
||||
{
|
||||
summary?.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Inserts range data into submaps.
|
||||
/// </summary>
|
||||
private InsertionResult? InsertIntoSubmap(
|
||||
long time,
|
||||
RangeData filteredRangeDataInLocal,
|
||||
RangeData filteredRangeDataInTracking,
|
||||
PointCloud highResolutionPointCloudInTracking,
|
||||
PointCloud lowResolutionPointCloudInTracking,
|
||||
Rigid3d poseEstimate,
|
||||
Quaternion gravityAlignment)
|
||||
{
|
||||
// Check motion filter - skip insertion if motion is too small
|
||||
if (_motionFilter.IsSimilar(time, poseEstimate))
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// Insert data into active submaps
|
||||
// Compute localFromGravityAligned transform
|
||||
var localFromGravityAligned = poseEstimate.Rotation * Quaternion.Inverse(gravityAlignment);
|
||||
|
||||
// Compute rotational scan matcher histogram from gravity-aligned point cloud
|
||||
var gravityAlignedPointCloud = PointCloudOperations.Transform(
|
||||
filteredRangeDataInTracking.Returns,
|
||||
new Rigid3f(Vector3.Zero, gravityAlignment));
|
||||
|
||||
var rotationalScanMatcherHistogram = RotationalScanMatcher.ComputeHistogram(
|
||||
gravityAlignedPointCloud,
|
||||
_options.RotationalHistogramSize).ToList();
|
||||
|
||||
_activeSubmaps.InsertData(
|
||||
filteredRangeDataInLocal,
|
||||
localFromGravityAligned,
|
||||
rotationalScanMatcherHistogram
|
||||
);
|
||||
|
||||
var submaps = _activeSubmaps.Submaps();
|
||||
if (submaps.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// Create constant data with rotational histogram
|
||||
var constantData = new TrajectoryNode.Data
|
||||
{
|
||||
Time = time,
|
||||
GravityAlignment = gravityAlignment,
|
||||
HighResolutionPointCloud = highResolutionPointCloudInTracking,
|
||||
LowResolutionPointCloud = lowResolutionPointCloudInTracking,
|
||||
RotationalScanMatcherHistogram = rotationalScanMatcherHistogram.ToArray(),
|
||||
LocalPose = poseEstimate
|
||||
};
|
||||
|
||||
return new InsertionResult(constantData, submaps);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_ceresScanMatcher?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Penalizes differences between IMU data and optimized accelerations.
|
||||
/// Based on acceleration_cost_function_3d.h
|
||||
/// </summary>
|
||||
public class AccelerationCostFunction3D
|
||||
{
|
||||
private readonly double _scalingFactor;
|
||||
private readonly Vector3 _deltaVelocityImuFrame;
|
||||
private readonly double _firstDeltaTimeSeconds;
|
||||
private readonly double _secondDeltaTimeSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for acceleration constraint.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Scaling factor for the cost.</param>
|
||||
/// <param name="deltaVelocityImuFrame">Delta velocity from IMU integration in IMU frame.</param>
|
||||
/// <param name="firstDeltaTimeSeconds">Time duration of first interval in seconds.</param>
|
||||
/// <param name="secondDeltaTimeSeconds">Time duration of second interval in seconds.</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
Vector3 deltaVelocityImuFrame,
|
||||
double firstDeltaTimeSeconds,
|
||||
double secondDeltaTimeSeconds)
|
||||
{
|
||||
var costFunction = new AccelerationCostFunction3D(
|
||||
scalingFactor,
|
||||
deltaVelocityImuFrame,
|
||||
firstDeltaTimeSeconds,
|
||||
secondDeltaTimeSeconds);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 3, // [dx, dy, dz] - velocity difference error
|
||||
parameterBlockSizes: [4, 3, 3, 3, 1, 4] // [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]]
|
||||
);
|
||||
}
|
||||
|
||||
private AccelerationCostFunction3D(
|
||||
double scalingFactor,
|
||||
Vector3 deltaVelocityImuFrame,
|
||||
double firstDeltaTimeSeconds,
|
||||
double secondDeltaTimeSeconds)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_deltaVelocityImuFrame = deltaVelocityImuFrame;
|
||||
_firstDeltaTimeSeconds = firstDeltaTimeSeconds;
|
||||
_secondDeltaTimeSeconds = secondDeltaTimeSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter blocks [middle_rotation[4], start_position[3], middle_position[3], end_position[3], gravity_constant[1], imu_calibration[4]].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy, dz] (velocity difference error).</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 6)
|
||||
return false;
|
||||
if (parameters[0].Length < 4 || parameters[1].Length < 3 || parameters[2].Length < 3 ||
|
||||
parameters[3].Length < 3 || parameters[4].Length < 1 || parameters[5].Length < 4)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 3)
|
||||
return false;
|
||||
|
||||
var middleRotation = parameters[0]; // [w, x, y, z]
|
||||
var startPosition = parameters[1]; // [x, y, z]
|
||||
var middlePosition = parameters[2]; // [x, y, z]
|
||||
var endPosition = parameters[3]; // [x, y, z]
|
||||
var gravityConstant = parameters[4][0]; // [g]
|
||||
var imuCalibration = parameters[5]; // [w, x, y, z]
|
||||
|
||||
// Convert to quaternions
|
||||
var middleRot = new Quaternion(
|
||||
middleRotation[1], middleRotation[2], middleRotation[3], middleRotation[0]);
|
||||
var imuCal = new Quaternion(
|
||||
imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
|
||||
|
||||
// Convert positions to Vector3
|
||||
var startPos = new Vector3(startPosition[0], startPosition[1], startPosition[2]);
|
||||
var middlePos = new Vector3(middlePosition[0], middlePosition[1], middlePosition[2]);
|
||||
var endPos = new Vector3(endPosition[0], endPosition[1], endPosition[2]);
|
||||
|
||||
// Compute IMU delta velocity in map frame
|
||||
// Formula from C++:
|
||||
// imu_delta_velocity = middle_rotation * imu_calibration * delta_velocity_imu_frame - gravity_term
|
||||
// where gravity_term = gravity_constant * 0.5 * (first_delta_time + second_delta_time) * UnitZ
|
||||
|
||||
// Transform delta_velocity_imu_frame from IMU frame to map frame
|
||||
// In Eigen: quaternion * vector rotates the vector
|
||||
// In System.Numerics: Vector3.Transform(vector, quaternion) rotates the vector
|
||||
// C++: middle_rotation * imu_calibration * delta_velocity
|
||||
// = middle_rotation * (imu_calibration * delta_velocity)
|
||||
// Apply IMU calibration first, then middle rotation
|
||||
var imuDeltaVelocityCalibrated = Vector3.Transform(_deltaVelocityImuFrame, imuCal);
|
||||
var imuDeltaVelocityInMapFrame = Vector3.Transform(imuDeltaVelocityCalibrated, middleRot);
|
||||
|
||||
// Subtract gravity contribution
|
||||
// Gravity acts in positive Z direction in map frame (upward)
|
||||
var gravityTerm = gravityConstant * 0.5 * (_firstDeltaTimeSeconds + _secondDeltaTimeSeconds) * Vector3.UnitZ;
|
||||
var imuDeltaVelocity = imuDeltaVelocityInMapFrame - gravityTerm;
|
||||
|
||||
// Compute velocities from positions
|
||||
// start_velocity = (middle_position - start_position) / first_delta_time
|
||||
var startVelocity = (middlePos - startPos) / _firstDeltaTimeSeconds;
|
||||
|
||||
// end_velocity = (end_position - middle_position) / second_delta_time
|
||||
var endVelocity = (endPos - middlePos) / _secondDeltaTimeSeconds;
|
||||
|
||||
// delta_velocity = end_velocity - start_velocity
|
||||
var deltaVelocity = endVelocity - startVelocity;
|
||||
|
||||
// Error = IMU delta velocity - computed delta velocity
|
||||
var error = imuDeltaVelocity - deltaVelocity;
|
||||
|
||||
// Scale error
|
||||
residuals[0] = _scalingFactor * error.X;
|
||||
residuals[1] = _scalingFactor * error.Y;
|
||||
residuals[2] = _scalingFactor * error.Z;
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Penalizes differences between IMU data and optimized orientations.
|
||||
/// Based on rotation_cost_function_3d.h
|
||||
/// </summary>
|
||||
public class RotationCostFunction3D
|
||||
{
|
||||
private readonly double _scalingFactor;
|
||||
private readonly Quaternion _deltaRotationImuFrame;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for rotation constraint.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Scaling factor for the cost.</param>
|
||||
/// <param name="deltaRotationImuFrame">Delta rotation from IMU integration in IMU frame.</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
Quaternion deltaRotationImuFrame)
|
||||
{
|
||||
var costFunction = new RotationCostFunction3D(scalingFactor, deltaRotationImuFrame);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 3, // [dx, dy, dz] - rotation error as angle-axis vector
|
||||
parameterBlockSizes: [4, 4, 4] // [start_rotation[4], end_rotation[4], imu_calibration[4]]
|
||||
);
|
||||
}
|
||||
|
||||
private RotationCostFunction3D(double scalingFactor, Quaternion deltaRotationImuFrame)
|
||||
{
|
||||
_scalingFactor = scalingFactor;
|
||||
_deltaRotationImuFrame = deltaRotationImuFrame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter blocks [start_rotation[4], end_rotation[4], imu_calibration[4]].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy, dz] (angle-axis error).</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 3)
|
||||
return false;
|
||||
if (parameters[0].Length < 4 || parameters[1].Length < 4 || parameters[2].Length < 4)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 3)
|
||||
return false;
|
||||
|
||||
var startRotation = parameters[0]; // [w, x, y, z] from Ceres
|
||||
var endRotation = parameters[1]; // [w, x, y, z] from Ceres
|
||||
var imuCalibration = parameters[2]; // [w, x, y, z] from Ceres
|
||||
|
||||
// Convert to quaternions
|
||||
// C++ line 42-48: Eigen::Quaternion<T>(w, x, y, z)
|
||||
// System.Numerics.Quaternion constructor is (x, y, z, w)
|
||||
// So we need to convert [w, x, y, z] to (x, y, z, w)
|
||||
var start = new Quaternion(
|
||||
startRotation[1], startRotation[2], startRotation[3], startRotation[0]);
|
||||
var end = new Quaternion(
|
||||
endRotation[1], endRotation[2], endRotation[3], endRotation[0]);
|
||||
var imuCal = new Quaternion(
|
||||
imuCalibration[1], imuCalibration[2], imuCalibration[3], imuCalibration[0]);
|
||||
|
||||
// Compute error: end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
|
||||
// C++ line 49-51: error = end.conjugate() * start * imu_calibration * delta_rotation_imu_frame * imu_calibration.conjugate()
|
||||
// C++ line 52-54: residual = scaling_factor * error.vector() (x, y, z components of quaternion, not angle-axis)
|
||||
var endConj = Quaternion.Conjugate(end);
|
||||
var imuCalConj = Quaternion.Conjugate(imuCal);
|
||||
var error = Quaternion.Multiply(
|
||||
Quaternion.Multiply(
|
||||
Quaternion.Multiply(
|
||||
Quaternion.Multiply(endConj, start),
|
||||
imuCal),
|
||||
_deltaRotationImuFrame),
|
||||
imuCalConj);
|
||||
|
||||
// C++ uses error.x(), error.y(), error.z() which are the vector (imaginary) parts of the quaternion
|
||||
// NOT angle-axis representation. For small rotations, these are approximately the same, but we should match C++ exactly.
|
||||
// Scale error using vector part of quaternion (x, y, z components)
|
||||
residuals[0] = _scalingFactor * error.X;
|
||||
residuals[1] = _scalingFactor * error.Y;
|
||||
residuals[2] = _scalingFactor * error.Z;
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,190 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.Optimization;
|
||||
|
||||
/// <summary>
|
||||
/// Sparse Pose Adjustment (SPA) cost function for 3D pose graph optimization.
|
||||
/// Computes the error between observed relative pose and computed relative pose.
|
||||
/// </summary>
|
||||
public class SpaCostFunction3D
|
||||
{
|
||||
private readonly IPoseGraph.Constraint.Pose _observedRelativePose;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function for SPA 3D.
|
||||
/// </summary>
|
||||
/// <param name="observedRelativePose">The observed relative pose constraint.</param>
|
||||
/// <returns>AutoDiff cost function.</returns>
|
||||
public static AutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
IPoseGraph.Constraint.Pose observedRelativePose)
|
||||
{
|
||||
var costFunction = new SpaCostFunction3D(observedRelativePose);
|
||||
return new AutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: 6, // [dx, dy, dz, dqx, dqy, dqz] (quaternion rotation error as 3D vector)
|
||||
parameterBlockSizes: [4, 3, 4, 3] // [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]]
|
||||
);
|
||||
}
|
||||
|
||||
private SpaCostFunction3D(IPoseGraph.Constraint.Pose observedRelativePose)
|
||||
{
|
||||
_observedRelativePose = observedRelativePose;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Parameter blocks [submap_rotation[4], submap_translation[3], node_rotation[4], node_translation[3]].</param>
|
||||
/// <param name="residuals">Output residuals [dx, dy, dz, dqx, dqy, dqz].</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 4)
|
||||
return false;
|
||||
if (parameters[0].Length < 4 || parameters[1].Length < 3 ||
|
||||
parameters[2].Length < 4 || parameters[3].Length < 3)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 6)
|
||||
return false;
|
||||
|
||||
var submapRotation = parameters[0];
|
||||
var submapTranslation = parameters[1];
|
||||
var nodeRotation = parameters[2];
|
||||
var nodeTranslation = parameters[3];
|
||||
|
||||
// Compute unscaled error
|
||||
var unscaledError = ComputeUnscaledError(
|
||||
_observedRelativePose.ZbarIj,
|
||||
submapRotation,
|
||||
submapTranslation,
|
||||
nodeRotation,
|
||||
nodeTranslation
|
||||
);
|
||||
|
||||
// Scale error with weights
|
||||
var scaledError = ScaleError(
|
||||
unscaledError,
|
||||
_observedRelativePose.TranslationWeight,
|
||||
_observedRelativePose.RotationWeight
|
||||
);
|
||||
|
||||
residuals[0] = scaledError[0];
|
||||
residuals[1] = scaledError[1];
|
||||
residuals[2] = scaledError[2];
|
||||
residuals[3] = scaledError[3];
|
||||
residuals[4] = scaledError[4];
|
||||
residuals[5] = scaledError[5];
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes unscaled error between observed and computed relative pose.
|
||||
/// Based on cost_helpers_impl.h ComputeUnscaledError for 3D.
|
||||
/// </summary>
|
||||
private static double[] ComputeUnscaledError(
|
||||
Rigid3d observedRelativePose,
|
||||
double[] submapRotation,
|
||||
double[] submapTranslation,
|
||||
double[] nodeRotation,
|
||||
double[] nodeTranslation)
|
||||
{
|
||||
// submapRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
|
||||
// submapTranslation = [x, y, z]
|
||||
// nodeRotation = [w, x, y, z] (quaternion) - Eigen/Ceres order
|
||||
// nodeTranslation = [x, y, z]
|
||||
|
||||
// IMPORTANT: System.Numerics.Quaternion constructor is (x, y, z, w), NOT (w, x, y, z)!
|
||||
// Eigen::Quaternion uses (w, x, y, z), so we must reorder when creating System.Numerics.Quaternion.
|
||||
|
||||
// Compute R_i_inverse (inverse of submap rotation)
|
||||
// C++: Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1], -start_rotation[2], -start_rotation[3])
|
||||
var submapQuatInv = new Quaternion(
|
||||
-submapRotation[1], // -x
|
||||
-submapRotation[2], // -y
|
||||
-submapRotation[3], // -z
|
||||
submapRotation[0] // w
|
||||
);
|
||||
|
||||
// Compute delta = node_translation - submap_translation
|
||||
var delta = new Vector3(
|
||||
(nodeTranslation[0] - submapTranslation[0]),
|
||||
(nodeTranslation[1] - submapTranslation[1]),
|
||||
(nodeTranslation[2] - submapTranslation[2])
|
||||
);
|
||||
|
||||
// h_translation = R_i_inverse * delta
|
||||
var hTranslation = Vector3.Transform(delta, submapQuatInv);
|
||||
|
||||
// Compute h_rotation_inverse = node_rotation_inverse * submap_rotation
|
||||
// C++: Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2], -end_rotation[3]) *
|
||||
// Eigen::Quaternion<T>(start_rotation[0], start_rotation[1], start_rotation[2], start_rotation[3])
|
||||
var nodeQuatInv = new Quaternion(
|
||||
-nodeRotation[1], // -x
|
||||
-nodeRotation[2], // -y
|
||||
-nodeRotation[3], // -z
|
||||
nodeRotation[0] // w
|
||||
);
|
||||
var submapQuat = new Quaternion(
|
||||
submapRotation[1], // x
|
||||
submapRotation[2], // y
|
||||
submapRotation[3], // z
|
||||
submapRotation[0] // w
|
||||
);
|
||||
var hRotationInverse = nodeQuatInv * submapQuat;
|
||||
|
||||
// Compute angle-axis difference: RotationQuaternionToAngleAxisVector(h_rotation_inverse * observed_rotation)
|
||||
var observedQuat = observedRelativePose.Rotation;
|
||||
var angleAxisDifference = TransformOperations.RotationQuaternionToAngleAxisVector(
|
||||
hRotationInverse * observedQuat
|
||||
);
|
||||
|
||||
// Error = observed - computed
|
||||
return
|
||||
[
|
||||
observedRelativePose.Translation.X - hTranslation.X,
|
||||
observedRelativePose.Translation.Y - hTranslation.Y,
|
||||
observedRelativePose.Translation.Z - hTranslation.Z,
|
||||
angleAxisDifference.X,
|
||||
angleAxisDifference.Y,
|
||||
angleAxisDifference.Z
|
||||
];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scales error with translation and rotation weights.
|
||||
/// </summary>
|
||||
private static double[] ScaleError(
|
||||
double[] unscaledError,
|
||||
double translationWeight,
|
||||
double rotationWeight)
|
||||
{
|
||||
return
|
||||
[
|
||||
translationWeight * unscaledError[0],
|
||||
translationWeight * unscaledError[1],
|
||||
translationWeight * unscaledError[2],
|
||||
rotationWeight * unscaledError[3],
|
||||
rotationWeight * unscaledError[4],
|
||||
rotationWeight * unscaledError[5]
|
||||
];
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,218 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using CeresSharp;
|
||||
using CeresSharp.Enums;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Point cloud and hybrid grids pointers structure.
|
||||
/// </summary>
|
||||
public struct PointCloudAndHybridGridsPointers
|
||||
{
|
||||
public PointCloud? PointCloud { get; set; }
|
||||
public Mapping.D3D.HybridGrid? HybridGrid { get; set; }
|
||||
public Mapping.D3D.IntensityHybridGrid? IntensityHybridGrid { get; set; } // optional
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// This scan matcher uses Ceres to align scans with an existing 3D map.
|
||||
/// </summary>
|
||||
public class CeresScanMatcher3D : IDisposable
|
||||
{
|
||||
private readonly CeresScanMatcherOptions3D _options;
|
||||
private readonly SolverOptions _solverOptions;
|
||||
private bool _disposed;
|
||||
|
||||
public CeresScanMatcher3D(CeresScanMatcherOptions3D options)
|
||||
{
|
||||
_options = options;
|
||||
|
||||
// Initialize CeresSharp solver options
|
||||
_solverOptions = new SolverOptions
|
||||
{
|
||||
// Set linear solver type to DENSE_QR for 3D scan matching
|
||||
LinearSolverType = LinearSolverType.DenseQr,
|
||||
|
||||
// Configure from CeresSolverOptions if available, otherwise use defaults
|
||||
MaxNumIterations = options.CeresSolverOptions?.MaxNumIterations ?? 20, // Default for scan matching
|
||||
NumThreads = options.CeresSolverOptions?.NumThreads ?? 1, // Single thread for scan matching
|
||||
UseNonmonotonicSteps = options.CeresSolverOptions?.UseNonmonotonicSteps ?? false
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aligns 'point_clouds' within the 'hybrid_grids' given an
|
||||
/// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
|
||||
/// 'summary'.
|
||||
/// </summary>
|
||||
public void Match(
|
||||
Vector3 targetTranslation,
|
||||
Rigid3d initialPoseEstimate,
|
||||
List<PointCloudAndHybridGridsPointers> pointCloudsAndHybridGrids,
|
||||
out Rigid3d poseEstimate,
|
||||
out SolverSummary summary)
|
||||
{
|
||||
if (pointCloudsAndHybridGrids == null || pointCloudsAndHybridGrids.Count == 0)
|
||||
{
|
||||
poseEstimate = initialPoseEstimate;
|
||||
using var emptyProblem = new Problem();
|
||||
using var emptyOptions = new SolverOptions();
|
||||
summary = emptyProblem.Solve(emptyOptions);
|
||||
return;
|
||||
}
|
||||
|
||||
// Validate weights
|
||||
if (_options.OccupiedSpaceWeight.Count != pointCloudsAndHybridGrids.Count)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"OccupiedSpaceWeight count ({_options.OccupiedSpaceWeight.Count}) must match pointCloudsAndHybridGrids count ({pointCloudsAndHybridGrids.Count})",
|
||||
nameof(pointCloudsAndHybridGrids));
|
||||
}
|
||||
|
||||
for (int i = 0; i < _options.OccupiedSpaceWeight.Count; i++)
|
||||
{
|
||||
if (_options.OccupiedSpaceWeight[i] <= 0.0)
|
||||
{
|
||||
throw new ArgumentException($"OccupiedSpaceWeight[{i}] must be positive", nameof(_options));
|
||||
}
|
||||
}
|
||||
|
||||
if (_options.TranslationWeight <= 0.0)
|
||||
throw new ArgumentException("TranslationWeight must be positive", nameof(_options));
|
||||
if (_options.RotationWeight <= 0.0)
|
||||
throw new ArgumentException("RotationWeight must be positive", nameof(_options));
|
||||
|
||||
// Initialize pose parameters
|
||||
// For 3D: [translation[3], rotation[4]]
|
||||
var translationParams = new double[3]
|
||||
{
|
||||
initialPoseEstimate.Translation.X,
|
||||
initialPoseEstimate.Translation.Y,
|
||||
initialPoseEstimate.Translation.Z
|
||||
};
|
||||
|
||||
var rotationParams = new double[4]
|
||||
{
|
||||
initialPoseEstimate.Rotation.W,
|
||||
initialPoseEstimate.Rotation.X,
|
||||
initialPoseEstimate.Rotation.Y,
|
||||
initialPoseEstimate.Rotation.Z
|
||||
};
|
||||
|
||||
// Create Ceres problem
|
||||
using var problem = new Problem();
|
||||
|
||||
// Add parameter blocks
|
||||
problem.AddParameterBlock(translationParams, 3);
|
||||
problem.AddParameterBlock(rotationParams, 4);
|
||||
|
||||
// Set quaternion manifold (Ceres 2.2.0 uses Manifold instead of Parameterization)
|
||||
// TODO: When OnlyOptimizeYaw is true, use a YawOnlyQuaternionManifold instead
|
||||
// (C++ uses YawOnlyQuaternionPlus local parameterization for this case)
|
||||
using var quaternionManifold = new QuaternionManifold();
|
||||
problem.SetManifold(rotationParams, quaternionManifold);
|
||||
|
||||
// Add occupied space cost functions for each point cloud/grid pair
|
||||
for (int i = 0; i < pointCloudsAndHybridGrids.Count; i++)
|
||||
{
|
||||
var pcAndGrid = pointCloudsAndHybridGrids[i];
|
||||
if (pcAndGrid.PointCloud == null || pcAndGrid.HybridGrid == null)
|
||||
continue;
|
||||
|
||||
if (pcAndGrid.PointCloud.Count == 0)
|
||||
continue;
|
||||
|
||||
var occupiedSpaceCost = OccupiedSpaceCostFunction3D.CreateAutoDiffCostFunction(
|
||||
_options.OccupiedSpaceWeight[i] / Math.Sqrt(pcAndGrid.PointCloud.Count),
|
||||
pcAndGrid.PointCloud,
|
||||
pcAndGrid.HybridGrid
|
||||
);
|
||||
problem.AddResidualBlock(occupiedSpaceCost, null, [translationParams, rotationParams]);
|
||||
|
||||
// Add intensity cost function if intensity grid is available
|
||||
if (pcAndGrid.IntensityHybridGrid != null &&
|
||||
_options.IntensityCostFunctionOptions != null &&
|
||||
_options.IntensityCostFunctionOptions.Count > i)
|
||||
{
|
||||
var intensityOptions = _options.IntensityCostFunctionOptions[i];
|
||||
var intensityCost = IntensityCostFunction3D.CreateAutoDiffCostFunction(
|
||||
intensityOptions.Weight / Math.Sqrt(pcAndGrid.PointCloud.Count),
|
||||
intensityOptions.IntensityThreshold,
|
||||
pcAndGrid.PointCloud,
|
||||
pcAndGrid.IntensityHybridGrid
|
||||
);
|
||||
// Do NOT use 'using' here - Problem takes ownership of the loss function
|
||||
// via MarkOwnedByProblem() and will manage its lifetime
|
||||
var huberLoss = new HuberLoss(intensityOptions.HuberScale);
|
||||
problem.AddResidualBlock(intensityCost, huberLoss, [translationParams, rotationParams]);
|
||||
}
|
||||
}
|
||||
|
||||
// Add translation delta cost function
|
||||
var translationCost = TranslationDeltaCostFunctor3D.CreateAutoDiffCostFunction(
|
||||
_options.TranslationWeight,
|
||||
targetTranslation
|
||||
);
|
||||
problem.AddResidualBlock(translationCost, null, [translationParams]);
|
||||
|
||||
// Add rotation delta cost function
|
||||
var rotationCost = RotationDeltaCostFunctor3D.CreateAutoDiffCostFunction(
|
||||
_options.RotationWeight,
|
||||
initialPoseEstimate.Rotation
|
||||
);
|
||||
problem.AddResidualBlock(rotationCost, null, [rotationParams]);
|
||||
|
||||
// Solve
|
||||
summary = problem.Solve(_solverOptions);
|
||||
|
||||
// Extract result
|
||||
var newTranslation = new Vector3(
|
||||
translationParams[0],
|
||||
translationParams[1],
|
||||
translationParams[2]
|
||||
);
|
||||
|
||||
// rotationParams = [w, x, y, z] from Ceres
|
||||
// System.Numerics.Quaternion constructor is (x, y, z, w)
|
||||
var newRotation = new Quaternion(
|
||||
rotationParams[1], // x
|
||||
rotationParams[2], // y
|
||||
rotationParams[3], // z
|
||||
rotationParams[0] // w
|
||||
);
|
||||
// Normalize to ensure unit quaternion after Ceres optimization
|
||||
// C++ uses EigenQuaternionParameterization which maintains unit norm,
|
||||
// but CeresSharp may not have the same guarantee
|
||||
newRotation = Quaternion.Normalize(newRotation);
|
||||
|
||||
poseEstimate = new Rigid3d(newTranslation, newRotation);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_solverOptions?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
/*
|
||||
* Copyright 2019 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Mapping.D3D;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
|
||||
/// 'translation' and 'rotation'. The cost increases when points fall into space
|
||||
/// for which different intensity has been observed, i.e. at voxels with different
|
||||
/// values. Only points up to a certain threshold are evaluated which is intended
|
||||
/// to ignore data from retroreflections.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates an intensity cost function for 3D scan matching.
|
||||
/// </remarks>
|
||||
/// <param name="scalingFactor">Weight factor (typically intensity_weight / sqrt(point_cloud.size())).</param>
|
||||
/// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
|
||||
/// <param name="pointCloud">Point cloud to match (must have intensities).</param>
|
||||
/// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
|
||||
public class IntensityCostFunction3D(
|
||||
double scalingFactor,
|
||||
double intensityThreshold,
|
||||
PointCloud pointCloud,
|
||||
IntensityHybridGrid hybridGrid) : IDisposable
|
||||
{
|
||||
private readonly PointCloud _pointCloud = pointCloud ?? throw new ArgumentNullException(nameof(pointCloud));
|
||||
private readonly InterpolatedIntensityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
|
||||
private static readonly int[] parameterBlockSizes = [3, 4];
|
||||
|
||||
/// <summary>
|
||||
/// Creates a DynamicAutoDiff cost function for intensity matching.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight factor.</param>
|
||||
/// <param name="intensityThreshold">Points with intensity above this threshold are ignored.</param>
|
||||
/// <param name="pointCloud">Point cloud to match.</param>
|
||||
/// <param name="hybridGrid">Intensity hybrid grid to match against.</param>
|
||||
/// <returns>DynamicAutoDiff cost function.</returns>
|
||||
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
double intensityThreshold,
|
||||
PointCloud pointCloud,
|
||||
IntensityHybridGrid hybridGrid)
|
||||
{
|
||||
var costFunction = new IntensityCostFunction3D(scalingFactor, intensityThreshold, pointCloud, hybridGrid);
|
||||
return new DynamicAutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: parameterBlockSizes);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
|
||||
/// <param name="residuals">Output residuals (one per point).</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 2)
|
||||
return false;
|
||||
if (parameters[0].Length < 3 || parameters[1].Length < 4)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < _pointCloud.Count)
|
||||
return false;
|
||||
|
||||
var translation = parameters[0];
|
||||
var rotation = parameters[1]; // [w, x, y, z] from Ceres
|
||||
|
||||
// Create transform from translation and rotation
|
||||
// C++ line 48-50: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
|
||||
// where rotation = [w, x, y, z]
|
||||
// System.Numerics.Quaternion constructor is (x, y, z, w)
|
||||
var transform = new Rigid3d(
|
||||
new Vector3(translation[0], translation[1], translation[2]),
|
||||
new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
|
||||
);
|
||||
|
||||
// Transform each point and compute residual
|
||||
for (int i = 0; i < _pointCloud.Count; i++)
|
||||
{
|
||||
var point = _pointCloud[i];
|
||||
|
||||
// Get intensity from point cloud if available, otherwise use 0
|
||||
double intensity = 0.0;
|
||||
if (_pointCloud.Intensities.Count > 0 && i < _pointCloud.Intensities.Count)
|
||||
{
|
||||
intensity = _pointCloud.Intensities[i];
|
||||
}
|
||||
|
||||
// Ignore points with intensity above threshold (retroreflections)
|
||||
if (intensity > intensityThreshold)
|
||||
{
|
||||
residuals[i] = 0.0;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Transform point from local frame to world frame
|
||||
var worldPoint = transform * point.Position;
|
||||
|
||||
// Get interpolated intensity value at world point
|
||||
var interpolatedIntensity = _interpolatedGrid.GetInterpolatedValue(
|
||||
worldPoint.X,
|
||||
worldPoint.Y,
|
||||
worldPoint.Z
|
||||
);
|
||||
|
||||
// Residual = scaling_factor * (interpolated_intensity - intensity)
|
||||
residuals[i] = scalingFactor * (interpolatedIntensity - intensity);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disposes managed resources.
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
// InterpolatedIntensityGrid doesn't need disposal, but we implement IDisposable for consistency
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Common.Math;
|
||||
using CartographerSharp.Mapping.D3D;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Interpolates between HybridGrid voxels using tricubic interpolation.
|
||||
/// This class is designed to work with Ceres autodiff, so the interpolation
|
||||
/// scheme must be continuously differentiable.
|
||||
/// </summary>
|
||||
public class InterpolatedProbabilityGrid(HybridGrid _hybridGrid)
|
||||
{
|
||||
/// <summary>
|
||||
/// Returns the interpolated value at (x, y, z) of the HybridGrid.
|
||||
/// Uses tricubic interpolation (piecewise cubic polynomials).
|
||||
/// </summary>
|
||||
public double GetInterpolatedValue(double x, double y, double z)
|
||||
{
|
||||
ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
|
||||
|
||||
var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
|
||||
var q111 = GetValue(index1);
|
||||
var q112 = GetValue(index1 + new Array3i(0, 0, 1));
|
||||
var q121 = GetValue(index1 + new Array3i(0, 1, 0));
|
||||
var q122 = GetValue(index1 + new Array3i(0, 1, 1));
|
||||
var q211 = GetValue(index1 + new Array3i(1, 0, 0));
|
||||
var q212 = GetValue(index1 + new Array3i(1, 0, 1));
|
||||
var q221 = GetValue(index1 + new Array3i(1, 1, 0));
|
||||
var q222 = GetValue(index1 + new Array3i(1, 1, 1));
|
||||
|
||||
var normalizedX = (x - x1) / (x2 - x1);
|
||||
var normalizedY = (y - y1) / (y2 - y1);
|
||||
var normalizedZ = (z - z1) / (z2 - z1);
|
||||
|
||||
// Compute powers: t^2 and t^3
|
||||
var normalizedXx = normalizedX * normalizedX;
|
||||
var normalizedXxx = normalizedX * normalizedXx;
|
||||
var normalizedYy = normalizedY * normalizedY;
|
||||
var normalizedYyy = normalizedY * normalizedYy;
|
||||
var normalizedZz = normalizedZ * normalizedZ;
|
||||
var normalizedZzz = normalizedZ * normalizedZz;
|
||||
|
||||
// Interpolate in z, then y, then x
|
||||
// Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
|
||||
var q11 = (q111 - q112) * normalizedZzz * 2.0 +
|
||||
(q112 - q111) * normalizedZz * 3.0 + q111;
|
||||
var q12 = (q121 - q122) * normalizedZzz * 2.0 +
|
||||
(q122 - q121) * normalizedZz * 3.0 + q121;
|
||||
var q21 = (q211 - q212) * normalizedZzz * 2.0 +
|
||||
(q212 - q211) * normalizedZz * 3.0 + q211;
|
||||
var q22 = (q221 - q222) * normalizedZzz * 2.0 +
|
||||
(q222 - q221) * normalizedZz * 3.0 + q221;
|
||||
var q1 = (q11 - q12) * normalizedYyy * 2.0 +
|
||||
(q12 - q11) * normalizedYy * 3.0 + q11;
|
||||
var q2 = (q21 - q22) * normalizedYyy * 2.0 +
|
||||
(q22 - q21) * normalizedYy * 3.0 + q21;
|
||||
return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes interpolation data points (corners of the voxel containing the point).
|
||||
/// </summary>
|
||||
private void ComputeInterpolationDataPoints(
|
||||
double x, double y, double z,
|
||||
out double x1, out double y1, out double z1,
|
||||
out double x2, out double y2, out double z2)
|
||||
{
|
||||
var lower = CenterOfLowerVoxel(x, y, z);
|
||||
x1 = lower.X;
|
||||
y1 = lower.Y;
|
||||
z1 = lower.Z;
|
||||
x2 = lower.X + _hybridGrid.Resolution;
|
||||
y2 = lower.Y + _hybridGrid.Resolution;
|
||||
z2 = lower.Z + _hybridGrid.Resolution;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
|
||||
/// For each dimension, the largest voxel index so that the corresponding center
|
||||
/// is at most the given coordinate.
|
||||
/// </summary>
|
||||
private Vector3 CenterOfLowerVoxel(double x, double y, double z)
|
||||
{
|
||||
// Center of the cell containing (x, y, z)
|
||||
var center = _hybridGrid.GetCenterOfCell(
|
||||
_hybridGrid.GetCellIndex(new Vector3(x, y, z))
|
||||
);
|
||||
|
||||
// Move to the next lower voxel center
|
||||
var resolution = _hybridGrid.Resolution;
|
||||
if (center.X > x)
|
||||
{
|
||||
center.X -= resolution;
|
||||
}
|
||||
if (center.Y > y)
|
||||
{
|
||||
center.Y -= resolution;
|
||||
}
|
||||
if (center.Z > z)
|
||||
{
|
||||
center.Z -= resolution;
|
||||
}
|
||||
return center;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the probability value at the given cell index.
|
||||
/// </summary>
|
||||
private double GetValue(Array3i index)
|
||||
{
|
||||
// HybridGrid.GetProbability already returns probability in range [0, 1]
|
||||
// It internally calls ProbabilityValues.ValueToProbability which does the conversion
|
||||
// DO NOT divide by ushort.MaxValue - that was a bug!
|
||||
return _hybridGrid.GetProbability(index);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Interpolates between IntensityHybridGrid voxels using tricubic interpolation.
|
||||
/// </summary>
|
||||
public class InterpolatedIntensityGrid(IntensityHybridGrid _hybridGrid)
|
||||
{
|
||||
/// <summary>
|
||||
/// Returns the interpolated value at (x, y, z) of the IntensityHybridGrid.
|
||||
/// Uses tricubic interpolation (piecewise cubic polynomials).
|
||||
/// </summary>
|
||||
public double GetInterpolatedValue(double x, double y, double z)
|
||||
{
|
||||
ComputeInterpolationDataPoints(x, y, z, out var x1, out var y1, out var z1, out var x2, out var y2, out var z2);
|
||||
|
||||
var index1 = _hybridGrid.GetCellIndex(new Vector3(x1, y1, z1));
|
||||
var q111 = GetValue(index1);
|
||||
var q112 = GetValue(index1 + new Array3i(0, 0, 1));
|
||||
var q121 = GetValue(index1 + new Array3i(0, 1, 0));
|
||||
var q122 = GetValue(index1 + new Array3i(0, 1, 1));
|
||||
var q211 = GetValue(index1 + new Array3i(1, 0, 0));
|
||||
var q212 = GetValue(index1 + new Array3i(1, 0, 1));
|
||||
var q221 = GetValue(index1 + new Array3i(1, 1, 0));
|
||||
var q222 = GetValue(index1 + new Array3i(1, 1, 1));
|
||||
|
||||
var normalizedX = (x - x1) / (x2 - x1);
|
||||
var normalizedY = (y - y1) / (y2 - y1);
|
||||
var normalizedZ = (z - z1) / (z2 - z1);
|
||||
|
||||
// Compute powers: t^2 and t^3
|
||||
var normalizedXx = normalizedX * normalizedX;
|
||||
var normalizedXxx = normalizedX * normalizedXx;
|
||||
var normalizedYy = normalizedY * normalizedY;
|
||||
var normalizedYyy = normalizedY * normalizedYy;
|
||||
var normalizedZz = normalizedZ * normalizedZ;
|
||||
var normalizedZzz = normalizedZ * normalizedZz;
|
||||
|
||||
// Interpolate in z, then y, then x
|
||||
// Scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2)
|
||||
var q11 = (q111 - q112) * normalizedZzz * 2.0 +
|
||||
(q112 - q111) * normalizedZz * 3.0 + q111;
|
||||
var q12 = (q121 - q122) * normalizedZzz * 2.0 +
|
||||
(q122 - q121) * normalizedZz * 3.0 + q121;
|
||||
var q21 = (q211 - q212) * normalizedZzz * 2.0 +
|
||||
(q212 - q211) * normalizedZz * 3.0 + q211;
|
||||
var q22 = (q221 - q222) * normalizedZzz * 2.0 +
|
||||
(q222 - q221) * normalizedZz * 3.0 + q221;
|
||||
var q1 = (q11 - q12) * normalizedYyy * 2.0 +
|
||||
(q12 - q11) * normalizedYy * 3.0 + q11;
|
||||
var q2 = (q21 - q22) * normalizedYyy * 2.0 +
|
||||
(q22 - q21) * normalizedYy * 3.0 + q21;
|
||||
return (q1 - q2) * normalizedXxx * 2.0 + (q2 - q1) * normalizedXx * 3.0 + q1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes interpolation data points (corners of the voxel containing the point).
|
||||
/// </summary>
|
||||
private void ComputeInterpolationDataPoints(
|
||||
double x, double y, double z,
|
||||
out double x1, out double y1, out double z1,
|
||||
out double x2, out double y2, out double z2)
|
||||
{
|
||||
var lower = CenterOfLowerVoxel(x, y, z);
|
||||
x1 = lower.X;
|
||||
y1 = lower.Y;
|
||||
z1 = lower.Z;
|
||||
x2 = lower.X + _hybridGrid.Resolution;
|
||||
y2 = lower.Y + _hybridGrid.Resolution;
|
||||
z2 = lower.Z + _hybridGrid.Resolution;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Center of the next lower voxel (not necessarily the voxel containing (x, y, z)).
|
||||
/// </summary>
|
||||
private Vector3 CenterOfLowerVoxel(double x, double y, double z)
|
||||
{
|
||||
// Center of the cell containing (x, y, z)
|
||||
var center = _hybridGrid.GetCenterOfCell(
|
||||
_hybridGrid.GetCellIndex(new Vector3(x, y, z))
|
||||
);
|
||||
|
||||
// Move to the next lower voxel center
|
||||
var resolution = _hybridGrid.Resolution;
|
||||
if (center.X > x)
|
||||
{
|
||||
center.X -= resolution;
|
||||
}
|
||||
if (center.Y > y)
|
||||
{
|
||||
center.Y -= resolution;
|
||||
}
|
||||
if (center.Z > z)
|
||||
{
|
||||
center.Z -= resolution;
|
||||
}
|
||||
return center;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the intensity value at the given cell index.
|
||||
/// </summary>
|
||||
private double GetValue(Array3i index)
|
||||
{
|
||||
return _hybridGrid.GetIntensity(index);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Mapping.D3D;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
|
||||
/// 'translation' and 'rotation'. The cost increases when points fall into less
|
||||
/// occupied space, i.e. at voxels with lower values.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates an occupied space cost function for 3D scan matching.
|
||||
/// </remarks>
|
||||
/// <param name="scalingFactor">Weight factor (typically occupied_space_weight / sqrt(point_cloud.size())).</param>
|
||||
/// <param name="pointCloud">Point cloud to match.</param>
|
||||
/// <param name="hybridGrid">Hybrid grid to match against.</param>
|
||||
public class OccupiedSpaceCostFunction3D(
|
||||
double scalingFactor,
|
||||
PointCloud _pointCloud,
|
||||
HybridGrid hybridGrid) : IDisposable
|
||||
{
|
||||
private readonly InterpolatedProbabilityGrid _interpolatedGrid = new(hybridGrid ?? throw new ArgumentNullException(nameof(hybridGrid)));
|
||||
|
||||
/// <summary>
|
||||
/// Creates a DynamicAutoDiff cost function for occupied space matching.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight factor.</param>
|
||||
/// <param name="pointCloud">Point cloud to match.</param>
|
||||
/// <param name="hybridGrid">Hybrid grid to match against.</param>
|
||||
/// <returns>DynamicAutoDiff cost function.</returns>
|
||||
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
PointCloud pointCloud,
|
||||
HybridGrid hybridGrid)
|
||||
{
|
||||
var costFunction = new OccupiedSpaceCostFunction3D(scalingFactor, pointCloud, hybridGrid);
|
||||
return new DynamicAutoDiffCostFunction(
|
||||
costFunction.Evaluate,
|
||||
numResiduals: pointCloud.Count,
|
||||
parameterBlockSizes: [3, 4] // [translation[3], rotation[4]]
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Pose parameters [translation[3], rotation[4]].</param>
|
||||
/// <param name="residuals">Output residuals (one per point).</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 2)
|
||||
return false;
|
||||
if (parameters[0].Length < 3 || parameters[1].Length < 4)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < _pointCloud.Count)
|
||||
return false;
|
||||
|
||||
var translation = parameters[0];
|
||||
var rotation = parameters[1]; // [w, x, y, z] from Ceres
|
||||
|
||||
// Create transform from translation and rotation
|
||||
// C++ line 52-53: Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2], rotation[3])
|
||||
// where rotation = [w, x, y, z]
|
||||
// System.Numerics.Quaternion constructor is (x, y, z, w)
|
||||
var transform = new Rigid3d(
|
||||
new Vector3(translation[0], translation[1], translation[2]),
|
||||
new Quaternion(rotation[1], rotation[2], rotation[3], rotation[0])
|
||||
);
|
||||
|
||||
// Transform each point and compute residual
|
||||
for (int i = 0; i < _pointCloud.Count; i++)
|
||||
{
|
||||
var point = _pointCloud[i];
|
||||
|
||||
// Transform point from local frame to world frame
|
||||
var worldPoint = transform * point.Position;
|
||||
|
||||
// Get interpolated probability value at world point
|
||||
var probability = _interpolatedGrid.GetInterpolatedValue(
|
||||
worldPoint.X,
|
||||
worldPoint.Y,
|
||||
worldPoint.Z
|
||||
);
|
||||
|
||||
// Residual = scaling_factor * (1 - probability)
|
||||
// Higher probability (occupied space) = lower residual = better match
|
||||
residuals[i] = scalingFactor * (1.0 - probability);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Disposes managed resources.
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
// InterpolatedProbabilityGrid doesn't need disposal, but we implement IDisposable for consistency
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,152 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Common.Math;
|
||||
using CartographerSharp.Mapping.D3D;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Precomputation grid for 3D scan matching using 8-bit values instead of 16-bit.
|
||||
/// This is used for branch-and-bound algorithm in Fast Correlative Scan Matcher.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a new PrecomputationGrid3D with the specified resolution.
|
||||
/// </remarks>
|
||||
public class PrecomputationGrid3D(double resolution) : HybridGridBase<byte>(resolution)
|
||||
{
|
||||
/// <summary>
|
||||
/// Minimum probability value.
|
||||
/// </summary>
|
||||
public const double kMinProbability = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// Maximum probability value.
|
||||
/// </summary>
|
||||
public const double kMaxProbability = 0.9;
|
||||
|
||||
/// <summary>
|
||||
/// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
|
||||
/// </summary>
|
||||
public static double ToProbability(double value)
|
||||
{
|
||||
return kMinProbability +
|
||||
value * ((kMaxProbability - kMinProbability) / 255.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the value at the given cell index.
|
||||
/// </summary>
|
||||
public new byte GetValue(Array3i index)
|
||||
{
|
||||
return base.GetValue(index);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the value at the given cell index.
|
||||
/// </summary>
|
||||
public void SetValue(Array3i index, byte value)
|
||||
{
|
||||
ref var cell = ref GetMutableValue(index);
|
||||
cell = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts a HybridGrid to a PrecomputationGrid3D representing the same data,
|
||||
/// but only using 8 bit instead of 2 x 16 bit.
|
||||
/// </summary>
|
||||
public static class PrecomputationGrid3DOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts a HybridGrid to a PrecomputationGrid3D.
|
||||
/// </summary>
|
||||
public static PrecomputationGrid3D ConvertToPrecomputationGrid(Mapping.D3D.HybridGrid hybridGrid)
|
||||
{
|
||||
var result = new PrecomputationGrid3D(hybridGrid.Resolution);
|
||||
|
||||
// Iterate through all cells in the hybrid grid
|
||||
foreach (var (index, value) in hybridGrid)
|
||||
{
|
||||
// Convert probability (ushort) to byte [0, 255]
|
||||
var probability = ProbabilityValues.ValueToProbability(value);
|
||||
var cellValue = (int)Math.Round(
|
||||
(probability - PrecomputationGrid3D.kMinProbability) *
|
||||
(255.0 / (PrecomputationGrid3D.kMaxProbability - PrecomputationGrid3D.kMinProbability))
|
||||
);
|
||||
cellValue = Math.Max(0, Math.Min(255, cellValue));
|
||||
result.SetValue(index, (byte)cellValue);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns a grid of the same resolution containing the maximum value of
|
||||
/// original voxels in 'grid'. This maximum is over the 8 voxels that have
|
||||
/// any combination of index components optionally increased by 'shift'.
|
||||
/// </summary>
|
||||
public static PrecomputationGrid3D PrecomputeGrid(
|
||||
PrecomputationGrid3D grid,
|
||||
bool halfResolution,
|
||||
Array3i shift)
|
||||
{
|
||||
var result = new PrecomputationGrid3D(grid.Resolution);
|
||||
|
||||
// Iterate through all cells in the input grid
|
||||
foreach (var (index, value) in grid)
|
||||
{
|
||||
// Update 8 values in the resulting grid
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
var octant = HybridGridBase<byte>.GetOctant(i);
|
||||
// Element-wise multiplication: shift * octant
|
||||
var shiftOctant = new Array3i(
|
||||
shift.X * octant.X,
|
||||
shift.Y * octant.Y,
|
||||
shift.Z * octant.Z
|
||||
);
|
||||
var cellIndex = index - shiftOctant;
|
||||
|
||||
if (halfResolution)
|
||||
{
|
||||
// Convert to half resolution index
|
||||
cellIndex = CellIndexAtHalfResolution(cellIndex);
|
||||
}
|
||||
|
||||
// Take maximum value
|
||||
var currentValue = result.GetValue(cellIndex);
|
||||
var newValue = (byte)Math.Max(value, currentValue);
|
||||
result.SetValue(cellIndex, newValue);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the half resolution index corresponding to the full resolution
|
||||
/// 'cell_index'. Uses bit shift to round towards negative infinity.
|
||||
/// </summary>
|
||||
private static Array3i CellIndexAtHalfResolution(Array3i cellIndex)
|
||||
{
|
||||
return new Array3i(
|
||||
cellIndex.X >> 1, // Divide by 2, rounding towards negative infinity
|
||||
cellIndex.Y >> 1,
|
||||
cellIndex.Z >> 1
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Common.Math;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Stack of precomputation grids for branch-and-bound algorithm.
|
||||
/// </summary>
|
||||
public class PrecomputationGridStack3D
|
||||
{
|
||||
private readonly List<PrecomputationGrid3D> _precomputationGrids;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a precomputation grid stack from a hybrid grid.
|
||||
/// </summary>
|
||||
public PrecomputationGridStack3D(
|
||||
Mapping.D3D.HybridGrid hybridGrid,
|
||||
FastCorrelativeScanMatcherOptions3D options)
|
||||
{
|
||||
if (options.BranchAndBoundDepth < 1)
|
||||
throw new ArgumentException("branch_and_bound_depth must be >= 1", nameof(options));
|
||||
if (options.FullResolutionDepth < 1)
|
||||
throw new ArgumentException("full_resolution_depth must be >= 1", nameof(options));
|
||||
|
||||
_precomputationGrids = new List<PrecomputationGrid3D>(options.BranchAndBoundDepth)
|
||||
{
|
||||
// First grid: convert from hybrid grid
|
||||
PrecomputationGrid3DOperations.ConvertToPrecomputationGrid(hybridGrid)
|
||||
};
|
||||
|
||||
var lastWidth = new Array3i(1, 1, 1);
|
||||
|
||||
// Create grids for each depth
|
||||
for (int depth = 1; depth < options.BranchAndBoundDepth; depth++)
|
||||
{
|
||||
var halfResolution = depth >= options.FullResolutionDepth;
|
||||
var nextWidth = new Array3i(1 << depth, 1 << depth, 1 << depth);
|
||||
|
||||
var fullVoxelsPerHighResolutionVoxel = 1 << Math.Max(0, depth - options.FullResolutionDepth);
|
||||
// Element-wise division: (nextWidth - lastWidth + (fullVoxelsPerHighResolutionVoxel - 1)) / fullVoxelsPerHighResolutionVoxel
|
||||
var numerator = nextWidth - lastWidth + new Array3i(fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1, fullVoxelsPerHighResolutionVoxel - 1);
|
||||
var shift = new Array3i(
|
||||
numerator.X / fullVoxelsPerHighResolutionVoxel,
|
||||
numerator.Y / fullVoxelsPerHighResolutionVoxel,
|
||||
numerator.Z / fullVoxelsPerHighResolutionVoxel
|
||||
);
|
||||
|
||||
_precomputationGrids.Add(
|
||||
PrecomputationGrid3DOperations.PrecomputeGrid(
|
||||
_precomputationGrids[^1],
|
||||
halfResolution,
|
||||
shift
|
||||
)
|
||||
);
|
||||
|
||||
lastWidth = nextWidth;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the precomputation grid at the specified depth.
|
||||
/// </summary>
|
||||
public PrecomputationGrid3D Get(int depth)
|
||||
{
|
||||
if (depth < 0 || depth >= _precomputationGrids.Count)
|
||||
throw new ArgumentOutOfRangeException(nameof(depth));
|
||||
return _precomputationGrids[depth];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the maximum depth (0-based).
|
||||
/// </summary>
|
||||
public int MaxDepth => _precomputationGrids.Count - 1;
|
||||
}
|
||||
@@ -0,0 +1,632 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Common.Math;
|
||||
using CartographerSharp.Models.Mapping;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using InterpolatedProbabilityGrid = CartographerSharp.Mapping.Internal.D3D.ScanMatching.InterpolatedProbabilityGrid;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Result of fast correlative scan matching for 3D.
|
||||
/// </summary>
|
||||
public struct FastCorrelativeScanMatcher3DResult(double score, Rigid3d poseEstimate, double rotationalScore, double lowResolutionScore)
|
||||
{
|
||||
public double Score { get; set; } = score;
|
||||
public Rigid3d PoseEstimate { get; set; } = poseEstimate;
|
||||
public double RotationalScore { get; set; } = rotationalScore;
|
||||
public double LowResolutionScore { get; set; } = lowResolutionScore;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Discrete scan structure for 3D scan matching.
|
||||
/// </summary>
|
||||
internal struct DiscreteScan3D
|
||||
{
|
||||
public Rigid3f Pose { get; set; }
|
||||
public List<List<Array3i>> CellIndicesPerDepth { get; set; }
|
||||
public double RotationalScore { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Candidate structure for branch-and-bound algorithm.
|
||||
/// </summary>
|
||||
internal struct Candidate3D(int scanIndex, Array3i offset) : IComparable<Candidate3D>
|
||||
{
|
||||
public int ScanIndex { get; set; } = scanIndex;
|
||||
public Array3i Offset { get; set; } = offset;
|
||||
public double Score { get; set; } = double.NegativeInfinity;
|
||||
public double LowResolutionScore { get; set; } = 0.0;
|
||||
|
||||
public static Candidate3D Unsuccessful()
|
||||
{
|
||||
return new Candidate3D(0, Array3i.Zero);
|
||||
}
|
||||
|
||||
public readonly int CompareTo(Candidate3D other)
|
||||
{
|
||||
return Score.CompareTo(other.Score);
|
||||
}
|
||||
|
||||
public static bool operator <(Candidate3D left, Candidate3D right)
|
||||
{
|
||||
return left.Score < right.Score;
|
||||
}
|
||||
|
||||
public static bool operator >(Candidate3D left, Candidate3D right)
|
||||
{
|
||||
return left.Score > right.Score;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// WARNING: NAMING MISMATCH WITH C++
|
||||
///
|
||||
/// This class is actually an implementation of FastCorrelativeScanMatcher3D (branch-and-bound algorithm),
|
||||
/// NOT RealTimeCorrelativeScanMatcher3D (exhaustive search).
|
||||
///
|
||||
/// C++ differences:
|
||||
/// - real_time_correlative_scan_matcher_3d.cc: Uses exhaustive search with 6 nested loops over
|
||||
/// a SMALL search window (linear and angular). Simple O(n^6) brute force.
|
||||
/// - fast_correlative_scan_matcher_3d.cc: Uses branch-and-bound optimization with precomputation
|
||||
/// grids for efficient search over LARGE windows. This is what this class implements.
|
||||
///
|
||||
/// The class name was incorrectly chosen. For constraint building (loop closure), this branch-and-bound
|
||||
/// implementation is actually correct since it can search over large windows efficiently.
|
||||
/// For real-time scan matching in LocalTrajectoryBuilder3D, the exhaustive search version should be
|
||||
/// used (smaller window, simpler, more predictable performance).
|
||||
///
|
||||
/// TODO: Consider renaming to FastCorrelativeScanMatcher3D and implementing a proper
|
||||
/// RealTimeCorrelativeScanMatcher3D for local SLAM if needed.
|
||||
/// </summary>
|
||||
public class RealTimeCorrelativeScanMatcher3D(
|
||||
Mapping.D3D.HybridGrid _hybridGrid,
|
||||
Mapping.D3D.HybridGrid? lowResolutionHybridGrid,
|
||||
double[]? rotationalScanMatcherHistogram,
|
||||
FastCorrelativeScanMatcherOptions3D options)
|
||||
{
|
||||
private readonly double _resolution = _hybridGrid.Resolution;
|
||||
private readonly int _widthInVoxels = 256;
|
||||
private readonly PrecomputationGridStack3D _precomputationGridStack = new(_hybridGrid, options);
|
||||
private readonly RotationalScanMatcher _rotationalScanMatcher = new(rotationalScanMatcherHistogram);
|
||||
|
||||
/// <summary>
|
||||
/// Search parameters for branch-and-bound algorithm.
|
||||
/// </summary>
|
||||
private struct SearchParameters
|
||||
{
|
||||
public int LinearXyWindowSize { get; set; } // voxels
|
||||
public int LinearZWindowSize { get; set; } // voxels
|
||||
public double AngularSearchWindow { get; set; } // radians
|
||||
public Func<Rigid3f, double>? LowResolutionMatcher { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a low resolution matcher function.
|
||||
/// </summary>
|
||||
private static Func<Rigid3f, double>? CreateLowResolutionMatcher(
|
||||
Mapping.D3D.HybridGrid? lowResolutionGrid,
|
||||
PointCloud? points)
|
||||
{
|
||||
if (lowResolutionGrid == null || points == null || points.Count == 0)
|
||||
return null;
|
||||
|
||||
return pose =>
|
||||
{
|
||||
double score = 0.0;
|
||||
var transformedPoints = PointCloudOperations.Transform(points, pose);
|
||||
var interpolatedGrid = new InterpolatedProbabilityGrid(lowResolutionGrid);
|
||||
|
||||
foreach (var point in transformedPoints)
|
||||
{
|
||||
// Use interpolated grid for better score
|
||||
var probability = interpolatedGrid.GetInterpolatedValue(
|
||||
point.Position.X,
|
||||
point.Position.Y,
|
||||
point.Position.Z);
|
||||
score += probability;
|
||||
}
|
||||
return score / points.Count;
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
|
||||
/// given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
|
||||
/// returned if a score above 'min_score' (excluding equality) is possible.
|
||||
/// </summary>
|
||||
public FastCorrelativeScanMatcher3DResult? Match(
|
||||
Rigid3d globalNodePose,
|
||||
Rigid3d globalSubmapPose,
|
||||
TrajectoryNode.Data constantData,
|
||||
double minScore)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(constantData);
|
||||
|
||||
var pointCloud = constantData.HighResolutionPointCloud;
|
||||
if (pointCloud == null || pointCloud.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var lowResolutionMatcher = CreateLowResolutionMatcher(
|
||||
lowResolutionHybridGrid,
|
||||
constantData.LowResolutionPointCloud);
|
||||
|
||||
var searchParameters = new SearchParameters
|
||||
{
|
||||
LinearXyWindowSize = (int)Math.Round(options.LinearXySearchWindow / _resolution),
|
||||
LinearZWindowSize = (int)Math.Round(options.LinearZSearchWindow / _resolution),
|
||||
AngularSearchWindow = options.AngularSearchWindow,
|
||||
LowResolutionMatcher = lowResolutionMatcher
|
||||
};
|
||||
|
||||
return MatchWithSearchParameters(
|
||||
searchParameters,
|
||||
new Rigid3f(globalNodePose.Translation, globalNodePose.Rotation),
|
||||
new Rigid3f(globalSubmapPose.Translation, globalSubmapPose.Rotation),
|
||||
pointCloud,
|
||||
constantData.RotationalScanMatcherHistogram?.ToArray(),
|
||||
constantData.GravityAlignment,
|
||||
minScore);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
|
||||
/// given rotations which are expected to be approximately gravity aligned.
|
||||
/// 'Result' is only returned if a score above 'min_score' (excluding equality)
|
||||
/// is possible.
|
||||
/// </summary>
|
||||
public FastCorrelativeScanMatcher3DResult? MatchFullSubmap(
|
||||
Quaternion globalNodeRotation,
|
||||
Quaternion globalSubmapRotation,
|
||||
TrajectoryNode.Data constantData,
|
||||
double minScore)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(constantData);
|
||||
|
||||
var pointCloud = constantData.HighResolutionPointCloud;
|
||||
if (pointCloud == null || pointCloud.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// Compute max point distance to determine search window
|
||||
double maxPointDistance = 0.0;
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
maxPointDistance = Math.Max(maxPointDistance, point.Position.Length());
|
||||
}
|
||||
|
||||
var linearWindowSize = (_widthInVoxels + 1) / 2 +
|
||||
(int)Math.Round(maxPointDistance / _resolution + 0.5);
|
||||
|
||||
var lowResolutionMatcher = CreateLowResolutionMatcher(
|
||||
lowResolutionHybridGrid,
|
||||
constantData.LowResolutionPointCloud);
|
||||
|
||||
var searchParameters = new SearchParameters
|
||||
{
|
||||
LinearXyWindowSize = linearWindowSize,
|
||||
LinearZWindowSize = linearWindowSize,
|
||||
AngularSearchWindow = Math.PI,
|
||||
LowResolutionMatcher = lowResolutionMatcher
|
||||
};
|
||||
|
||||
var globalNodePose = Rigid3f.FromRotation(globalNodeRotation);
|
||||
var globalSubmapPose = Rigid3f.FromRotation(globalSubmapRotation);
|
||||
|
||||
return MatchWithSearchParameters(
|
||||
searchParameters,
|
||||
globalNodePose,
|
||||
globalSubmapPose,
|
||||
pointCloud,
|
||||
constantData.RotationalScanMatcherHistogram?.ToArray(),
|
||||
constantData.GravityAlignment,
|
||||
minScore);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matches with given search parameters.
|
||||
/// </summary>
|
||||
private FastCorrelativeScanMatcher3DResult? MatchWithSearchParameters(
|
||||
SearchParameters searchParameters,
|
||||
Rigid3f globalNodePose,
|
||||
Rigid3f globalSubmapPose,
|
||||
PointCloud pointCloud,
|
||||
double[]? rotationalScanMatcherHistogram,
|
||||
Quaternion gravityAlignment,
|
||||
double minScore)
|
||||
{
|
||||
var discreteScans = GenerateDiscreteScans(
|
||||
searchParameters,
|
||||
pointCloud,
|
||||
rotationalScanMatcherHistogram,
|
||||
gravityAlignment,
|
||||
globalNodePose,
|
||||
globalSubmapPose);
|
||||
|
||||
var lowestResolutionCandidates = ComputeLowestResolutionCandidates(
|
||||
searchParameters,
|
||||
discreteScans);
|
||||
|
||||
var bestCandidate = BranchAndBound(
|
||||
searchParameters,
|
||||
discreteScans,
|
||||
lowestResolutionCandidates,
|
||||
_precomputationGridStack.MaxDepth,
|
||||
minScore);
|
||||
|
||||
if (bestCandidate.Score > minScore)
|
||||
{
|
||||
var pose = GetPoseFromCandidate(discreteScans, bestCandidate);
|
||||
return new FastCorrelativeScanMatcher3DResult(
|
||||
bestCandidate.Score,
|
||||
new Rigid3d(pose.Translation, pose.Rotation),
|
||||
discreteScans[bestCandidate.ScanIndex].RotationalScore,
|
||||
bestCandidate.LowResolutionScore);
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Discretizes a scan at different resolutions for branch-and-bound.
|
||||
/// </summary>
|
||||
private DiscreteScan3D DiscretizeScan(
|
||||
SearchParameters searchParameters,
|
||||
PointCloud pointCloud,
|
||||
Rigid3f pose,
|
||||
double rotationalScore)
|
||||
{
|
||||
var cellIndicesPerDepth = new List<List<Array3i>>();
|
||||
var originalGrid = _precomputationGridStack.Get(0);
|
||||
|
||||
// Transform point cloud
|
||||
var transformedPoints = PointCloudOperations.Transform(pointCloud, pose);
|
||||
|
||||
// Get full resolution cell indices
|
||||
var fullResolutionCellIndices = new List<Array3i>();
|
||||
foreach (var point in transformedPoints)
|
||||
{
|
||||
fullResolutionCellIndices.Add(originalGrid.GetCellIndex(point.Position));
|
||||
}
|
||||
|
||||
var fullResolutionDepth = Math.Min(
|
||||
options.FullResolutionDepth,
|
||||
options.BranchAndBoundDepth);
|
||||
|
||||
if (fullResolutionDepth < 1)
|
||||
fullResolutionDepth = 1;
|
||||
|
||||
// Add full resolution indices for each depth up to full_resolution_depth
|
||||
for (int i = 0; i < fullResolutionDepth; i++)
|
||||
{
|
||||
cellIndicesPerDepth.Add([.. fullResolutionCellIndices]);
|
||||
}
|
||||
|
||||
var lowResolutionDepth = options.BranchAndBoundDepth - fullResolutionDepth;
|
||||
if (lowResolutionDepth < 0)
|
||||
lowResolutionDepth = 0;
|
||||
|
||||
var searchWindowStart = new Array3i(
|
||||
-searchParameters.LinearXyWindowSize,
|
||||
-searchParameters.LinearXyWindowSize,
|
||||
-searchParameters.LinearZWindowSize);
|
||||
|
||||
// Add low resolution indices
|
||||
for (int i = 0; i < lowResolutionDepth; i++)
|
||||
{
|
||||
var reductionExponent = i + 1;
|
||||
var lowResolutionSearchWindowStart = new Array3i(
|
||||
searchWindowStart.X >> reductionExponent,
|
||||
searchWindowStart.Y >> reductionExponent,
|
||||
searchWindowStart.Z >> reductionExponent);
|
||||
|
||||
var lowResolutionIndices = new List<Array3i>();
|
||||
foreach (var cellIndex in fullResolutionCellIndices)
|
||||
{
|
||||
var cellAtStart = cellIndex + searchWindowStart;
|
||||
var lowResolutionCellAtStart = new Array3i(
|
||||
cellAtStart.X >> reductionExponent,
|
||||
cellAtStart.Y >> reductionExponent,
|
||||
cellAtStart.Z >> reductionExponent);
|
||||
lowResolutionIndices.Add(
|
||||
lowResolutionCellAtStart - lowResolutionSearchWindowStart);
|
||||
}
|
||||
cellIndicesPerDepth.Add(lowResolutionIndices);
|
||||
}
|
||||
|
||||
return new DiscreteScan3D
|
||||
{
|
||||
Pose = pose,
|
||||
CellIndicesPerDepth = cellIndicesPerDepth,
|
||||
RotationalScore = rotationalScore
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Generates discrete scans for different rotation angles.
|
||||
/// </summary>
|
||||
private List<DiscreteScan3D> GenerateDiscreteScans(
|
||||
SearchParameters searchParameters,
|
||||
PointCloud pointCloud,
|
||||
double[]? rotationalScanMatcherHistogram,
|
||||
Quaternion gravityAlignment,
|
||||
Rigid3f globalNodePose,
|
||||
Rigid3f globalSubmapPose)
|
||||
{
|
||||
var result = new List<DiscreteScan3D>();
|
||||
|
||||
// Compute max scan range
|
||||
double maxScanRange = 3.0 * _resolution;
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
var range = point.Position.Length();
|
||||
maxScanRange = Math.Max(range, maxScanRange);
|
||||
}
|
||||
|
||||
const double kSafetyMargin = 1.0 - 1e-2;
|
||||
var angularStepSize = kSafetyMargin * Math.Acos(
|
||||
1.0 - MathUtils.Pow2(_resolution) / (2.0 * MathUtils.Pow2(maxScanRange)));
|
||||
|
||||
var angularWindowSize = (int)Math.Round(
|
||||
searchParameters.AngularSearchWindow / angularStepSize);
|
||||
|
||||
var angles = new List<double>();
|
||||
for (int rz = -angularWindowSize; rz <= angularWindowSize; rz++)
|
||||
{
|
||||
angles.Add(rz * angularStepSize);
|
||||
}
|
||||
|
||||
var nodeToSubmap = globalSubmapPose.Inverse() * globalNodePose;
|
||||
var initialAngle = TransformOperations.GetYaw(
|
||||
nodeToSubmap.Rotation * Quaternion.Inverse(gravityAlignment));
|
||||
|
||||
var scores = _rotationalScanMatcher.Match(
|
||||
rotationalScanMatcherHistogram ?? [],
|
||||
initialAngle,
|
||||
angles);
|
||||
|
||||
for (int i = 0; i < angles.Count; i++)
|
||||
{
|
||||
if (scores[i] < options.MinRotationalScore)
|
||||
continue;
|
||||
|
||||
var angleAxis = new Vector3(0.0f, 0.0f, angles[i]);
|
||||
// Apply rotation between translation and rotation of initial_pose
|
||||
var pose = new Rigid3f(
|
||||
nodeToSubmap.Translation,
|
||||
Quaternion.Inverse(globalSubmapPose.Rotation) *
|
||||
TransformOperations.AngleAxisVectorToRotationQuaternion(angleAxis) *
|
||||
globalNodePose.Rotation);
|
||||
|
||||
result.Add(DiscretizeScan(searchParameters, pointCloud, pose, scores[i]));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Generates candidates at the lowest resolution.
|
||||
/// </summary>
|
||||
private List<Candidate3D> GenerateLowestResolutionCandidates(
|
||||
SearchParameters searchParameters,
|
||||
int numDiscreteScans)
|
||||
{
|
||||
var linearStepSize = 1 << _precomputationGridStack.MaxDepth;
|
||||
var numLowestResolutionLinearXyCandidates =
|
||||
(2 * searchParameters.LinearXyWindowSize + linearStepSize) / linearStepSize;
|
||||
var numLowestResolutionLinearZCandidates =
|
||||
(2 * searchParameters.LinearZWindowSize + linearStepSize) / linearStepSize;
|
||||
var numCandidates = numDiscreteScans *
|
||||
MathUtils.Power(numLowestResolutionLinearXyCandidates, 2) *
|
||||
numLowestResolutionLinearZCandidates;
|
||||
|
||||
var candidates = new List<Candidate3D>((int)numCandidates);
|
||||
for (int scanIndex = 0; scanIndex < numDiscreteScans; scanIndex++)
|
||||
{
|
||||
for (int z = -searchParameters.LinearZWindowSize;
|
||||
z <= searchParameters.LinearZWindowSize;
|
||||
z += linearStepSize)
|
||||
{
|
||||
for (int y = -searchParameters.LinearXyWindowSize;
|
||||
y <= searchParameters.LinearXyWindowSize;
|
||||
y += linearStepSize)
|
||||
{
|
||||
for (int x = -searchParameters.LinearXyWindowSize;
|
||||
x <= searchParameters.LinearXyWindowSize;
|
||||
x += linearStepSize)
|
||||
{
|
||||
candidates.Add(new Candidate3D(scanIndex, new Array3i(x, y, z)));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return candidates;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scores candidates at a given depth.
|
||||
/// </summary>
|
||||
private void ScoreCandidates(
|
||||
int depth,
|
||||
List<DiscreteScan3D> discreteScans,
|
||||
List<Candidate3D> candidates)
|
||||
{
|
||||
var reductionExponent = Math.Max(0, depth - options.FullResolutionDepth + 1);
|
||||
|
||||
for (int i = 0; i < candidates.Count; i++)
|
||||
{
|
||||
var candidate = candidates[i];
|
||||
int sum = 0;
|
||||
var discreteScan = discreteScans[candidate.ScanIndex];
|
||||
var offset = new Array3i(
|
||||
candidate.Offset.X >> reductionExponent,
|
||||
candidate.Offset.Y >> reductionExponent,
|
||||
candidate.Offset.Z >> reductionExponent);
|
||||
|
||||
if (depth >= discreteScan.CellIndicesPerDepth.Count)
|
||||
continue;
|
||||
|
||||
var grid = _precomputationGridStack.Get(depth);
|
||||
foreach (var cellIndex in discreteScan.CellIndicesPerDepth[depth])
|
||||
{
|
||||
var proposedCellIndex = cellIndex + offset;
|
||||
sum += grid.GetValue(proposedCellIndex);
|
||||
}
|
||||
|
||||
var newScore = PrecomputationGrid3D.ToProbability(
|
||||
sum / discreteScan.CellIndicesPerDepth[depth].Count);
|
||||
// Create new candidate with updated score
|
||||
var updatedCandidate = new Candidate3D(candidate.ScanIndex, candidate.Offset)
|
||||
{
|
||||
Score = newScore,
|
||||
LowResolutionScore = candidate.LowResolutionScore
|
||||
};
|
||||
candidates[i] = updatedCandidate;
|
||||
}
|
||||
|
||||
// Sort candidates by score (descending)
|
||||
candidates.Sort((a, b) => b.Score.CompareTo(a.Score));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes candidates at the lowest resolution.
|
||||
/// </summary>
|
||||
private List<Candidate3D> ComputeLowestResolutionCandidates(
|
||||
SearchParameters searchParameters,
|
||||
List<DiscreteScan3D> discreteScans)
|
||||
{
|
||||
var lowestResolutionCandidates = GenerateLowestResolutionCandidates(
|
||||
searchParameters,
|
||||
discreteScans.Count);
|
||||
|
||||
ScoreCandidates(
|
||||
_precomputationGridStack.MaxDepth,
|
||||
discreteScans,
|
||||
lowestResolutionCandidates);
|
||||
|
||||
return lowestResolutionCandidates;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets pose from candidate.
|
||||
/// </summary>
|
||||
private Rigid3f GetPoseFromCandidate(
|
||||
List<DiscreteScan3D> discreteScans,
|
||||
Candidate3D candidate)
|
||||
{
|
||||
var translation = (_resolution) * candidate.Offset.ToVector3();
|
||||
return Rigid3f.FromTranslation(translation) * discreteScans[candidate.ScanIndex].Pose;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Branch-and-bound algorithm to find best candidate.
|
||||
/// </summary>
|
||||
private Candidate3D BranchAndBound(
|
||||
SearchParameters searchParameters,
|
||||
List<DiscreteScan3D> discreteScans,
|
||||
List<Candidate3D> candidates,
|
||||
int candidateDepth,
|
||||
double minScore)
|
||||
{
|
||||
if (candidateDepth == 0)
|
||||
{
|
||||
foreach (var candidate in candidates)
|
||||
{
|
||||
if (candidate.Score <= minScore)
|
||||
{
|
||||
// Return if candidate is bad because following candidates won't be better
|
||||
return Candidate3D.Unsuccessful();
|
||||
}
|
||||
|
||||
if (searchParameters.LowResolutionMatcher == null)
|
||||
continue;
|
||||
|
||||
var lowResolutionScore = searchParameters.LowResolutionMatcher(
|
||||
GetPoseFromCandidate(discreteScans, candidate));
|
||||
|
||||
if (lowResolutionScore >= options.MinLowResolutionScore)
|
||||
{
|
||||
// Found best candidate that passes matching function
|
||||
var bestCandidate = candidate;
|
||||
bestCandidate.LowResolutionScore = lowResolutionScore;
|
||||
return bestCandidate;
|
||||
}
|
||||
}
|
||||
// All candidates have good scores but none passes matching function
|
||||
return Candidate3D.Unsuccessful();
|
||||
}
|
||||
|
||||
var bestHighResolutionCandidate = Candidate3D.Unsuccessful();
|
||||
bestHighResolutionCandidate.Score = minScore;
|
||||
|
||||
foreach (var candidate in candidates)
|
||||
{
|
||||
if (candidate.Score <= minScore)
|
||||
break;
|
||||
|
||||
var higherResolutionCandidates = new List<Candidate3D>();
|
||||
var halfWidth = 1 << (candidateDepth - 1);
|
||||
|
||||
for (int z = 0; z <= halfWidth; z += halfWidth)
|
||||
{
|
||||
if (candidate.Offset.Z + z > searchParameters.LinearZWindowSize)
|
||||
break;
|
||||
|
||||
for (int y = 0; y <= halfWidth; y += halfWidth)
|
||||
{
|
||||
if (candidate.Offset.Y + y > searchParameters.LinearXyWindowSize)
|
||||
break;
|
||||
|
||||
for (int x = 0; x <= halfWidth; x += halfWidth)
|
||||
{
|
||||
if (candidate.Offset.X + x > searchParameters.LinearXyWindowSize)
|
||||
break;
|
||||
|
||||
higherResolutionCandidates.Add(new Candidate3D(
|
||||
candidate.ScanIndex,
|
||||
candidate.Offset + new Array3i(x, y, z)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ScoreCandidates(candidateDepth - 1, discreteScans, higherResolutionCandidates);
|
||||
|
||||
// C++ line 433-437: std::max(best_high_resolution_candidate, BranchAndBound(...))
|
||||
// This ensures we always get the candidate with the highest score (or equal)
|
||||
var bestCandidate = BranchAndBound(
|
||||
searchParameters,
|
||||
discreteScans,
|
||||
higherResolutionCandidates,
|
||||
candidateDepth - 1,
|
||||
bestHighResolutionCandidate.Score);
|
||||
|
||||
// Use >= to match std::max behavior (prefer new candidate if score is equal or greater)
|
||||
if (bestCandidate.Score >= bestHighResolutionCandidate.Score)
|
||||
{
|
||||
bestHighResolutionCandidate = bestCandidate;
|
||||
}
|
||||
}
|
||||
|
||||
return bestHighResolutionCandidate;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
|
||||
/// Cost increases with the solution's distance from 'target_rotation'.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a rotation delta cost functor for 3D.
|
||||
/// </remarks>
|
||||
/// <param name="scalingFactor">Weight factor.</param>
|
||||
/// <param name="targetRotation">Target rotation to match.</param>
|
||||
public class RotationDeltaCostFunctor3D(double scalingFactor, Quaternion targetRotation)
|
||||
{
|
||||
private readonly double[] _targetRotationInverse =
|
||||
[
|
||||
targetRotation.W,
|
||||
-targetRotation.X,
|
||||
-targetRotation.Y,
|
||||
-targetRotation.Z
|
||||
]; // [w, x, y, z]
|
||||
|
||||
/// <summary>
|
||||
/// Creates a DynamicAutoDiff cost function for rotation delta.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight factor.</param>
|
||||
/// <param name="targetRotation">Target rotation to match.</param>
|
||||
/// <returns>DynamicAutoDiff cost function.</returns>
|
||||
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
Quaternion targetRotation)
|
||||
{
|
||||
var functor = new RotationDeltaCostFunctor3D(scalingFactor, targetRotation);
|
||||
return new DynamicAutoDiffCostFunction(
|
||||
functor.Evaluate,
|
||||
numResiduals: 3, // [x, y, z] - imaginary part of delta quaternion
|
||||
parameterBlockSizes: [4] // [w, x, y, z] - quaternion
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// Computes delta = target_rotation_inverse * rotation_quaternion
|
||||
/// Returns the imaginary part (x, y, z) of the delta quaternion.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Rotation quaternion [w, x, y, z].</param>
|
||||
/// <param name="residuals">Output residuals [x, y, z] - imaginary part of delta.</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 1 || parameters[0].Length < 4)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 3)
|
||||
return false;
|
||||
|
||||
var rotation = parameters[0];
|
||||
|
||||
// Compute quaternion product: target_rotation_inverse * rotation
|
||||
// delta = q_inv * q = [w1, x1, y1, z1] * [w2, x2, y2, z2]
|
||||
// delta.w = w1*w2 - x1*x2 - y1*y2 - z1*z2
|
||||
// delta.x = w1*x2 + x1*w2 + y1*z2 - z1*y2
|
||||
// delta.y = w1*y2 - x1*z2 + y1*w2 + z1*x2
|
||||
// delta.z = w1*z2 + x1*y2 - y1*x2 + z1*w2
|
||||
var w1 = _targetRotationInverse[0];
|
||||
var x1 = _targetRotationInverse[1];
|
||||
var y1 = _targetRotationInverse[2];
|
||||
var z1 = _targetRotationInverse[3];
|
||||
|
||||
var w2 = rotation[0];
|
||||
var x2 = rotation[1];
|
||||
var y2 = rotation[2];
|
||||
var z2 = rotation[3];
|
||||
|
||||
// Compute delta quaternion (only need imaginary part for residual)
|
||||
// The squared norm of the imaginary component is sin(phi/2)^2
|
||||
residuals[0] = scalingFactor * (w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2);
|
||||
residuals[1] = scalingFactor * (w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2);
|
||||
residuals[2] = scalingFactor * (w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,312 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Sensor;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Rotational scan matcher for 3D scan matching.
|
||||
/// Computes histogram-based rotational matching scores.
|
||||
/// Match C++ RotationalScanMatcher (rotational_scan_matcher.cc)
|
||||
/// </summary>
|
||||
public class RotationalScanMatcher(double[]? _histogram)
|
||||
{
|
||||
// Constants from C++ (rotational_scan_matcher.cc lines 31-33)
|
||||
private const float kMinDistance = 0.2f;
|
||||
private const float kMaxDistance = 0.9f;
|
||||
private const float kSliceHeight = 0.2f;
|
||||
|
||||
/// <summary>
|
||||
/// Rotates the given 'histogram' by the given 'angle'. This might lead to
|
||||
/// rotations of a fractional bucket which is handled by linearly interpolating.
|
||||
/// Match C++ RotateHistogram (rotational_scan_matcher.cc lines 141-162)
|
||||
/// </summary>
|
||||
public static double[] RotateHistogram(double[] histogram, double angle)
|
||||
{
|
||||
if (histogram == null || histogram.Length == 0)
|
||||
return histogram ?? [];
|
||||
|
||||
var numBuckets = histogram.Length;
|
||||
// C++: rotate_by_buckets = -angle * histogram.size() / M_PI
|
||||
var rotateByBuckets = -angle * numBuckets / Math.PI;
|
||||
var fullBuckets = (int)Math.Round(rotateByBuckets - 0.5);
|
||||
var fraction = rotateByBuckets - fullBuckets;
|
||||
|
||||
// Normalize full_buckets to be non-negative
|
||||
while (fullBuckets < 0)
|
||||
{
|
||||
fullBuckets += numBuckets;
|
||||
}
|
||||
|
||||
// Create two rotated histograms for interpolation
|
||||
var rotatedHistogram0 = new double[numBuckets];
|
||||
var rotatedHistogram1 = new double[numBuckets];
|
||||
|
||||
for (int i = 0; i < numBuckets; i++)
|
||||
{
|
||||
rotatedHistogram0[i] = histogram[(i + fullBuckets) % numBuckets];
|
||||
rotatedHistogram1[i] = histogram[(i + 1 + fullBuckets) % numBuckets];
|
||||
}
|
||||
|
||||
// Linear interpolation: fraction * rotated_histogram_1 + (1 - fraction) * rotated_histogram_0
|
||||
var result = new double[numBuckets];
|
||||
for (int i = 0; i < numBuckets; i++)
|
||||
{
|
||||
result[i] = fraction * rotatedHistogram1[i] + (1.0 - fraction) * rotatedHistogram0[i];
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the histogram for a gravity aligned 'point_cloud'.
|
||||
/// Match C++ ComputeHistogram (rotational_scan_matcher.cc lines 164-176)
|
||||
///
|
||||
/// Algorithm:
|
||||
/// 1. Divide points into horizontal slices by Z coordinate
|
||||
/// 2. For each slice, compute centroid and sort points by angle around centroid
|
||||
/// 3. Compute angle differences between consecutive points
|
||||
/// 4. Weight values by orthogonality to centroid direction (reject ceiling/floor angles)
|
||||
/// </summary>
|
||||
public static double[] ComputeHistogram(PointCloud pointCloud, int histogramSize)
|
||||
{
|
||||
if (pointCloud == null || pointCloud.Count == 0)
|
||||
return new double[histogramSize];
|
||||
|
||||
var histogram = new double[histogramSize];
|
||||
|
||||
// Step 1: Divide points into slices by Z (C++ lines 167-171)
|
||||
var slices = new Dictionary<int, List<RangefinderPoint>>();
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
var sliceIndex = (int)Math.Round(point.Position.Z / kSliceHeight);
|
||||
if (!slices.TryGetValue(sliceIndex, out var slice))
|
||||
{
|
||||
slice = [];
|
||||
slices[sliceIndex] = slice;
|
||||
}
|
||||
slice.Add(point);
|
||||
}
|
||||
|
||||
// Step 2: Process each slice (C++ lines 172-174)
|
||||
foreach (var slice in slices.Values)
|
||||
{
|
||||
AddPointCloudSliceToHistogram(SortSlice(slice), histogram);
|
||||
}
|
||||
|
||||
return histogram;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the centroid of a point cloud slice.
|
||||
/// Match C++ ComputeCentroid (rotational_scan_matcher.cc lines 52-59)
|
||||
/// </summary>
|
||||
private static Vector3 ComputeCentroid(List<RangefinderPoint> slice)
|
||||
{
|
||||
if (slice.Count == 0)
|
||||
return Vector3.Zero;
|
||||
|
||||
var sum = Vector3.Zero;
|
||||
foreach (var point in slice)
|
||||
{
|
||||
sum += point.Position;
|
||||
}
|
||||
return sum / slice.Count;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sorts points in a slice by angle around the centroid.
|
||||
/// Match C++ SortSlice (rotational_scan_matcher.cc lines 94-119)
|
||||
/// </summary>
|
||||
private static List<RangefinderPoint> SortSlice(List<RangefinderPoint> slice)
|
||||
{
|
||||
if (slice.Count == 0)
|
||||
return [];
|
||||
|
||||
var centroid = ComputeCentroid(slice);
|
||||
|
||||
// Create list of (angle, point) pairs
|
||||
var byAngle = new List<(double angle, RangefinderPoint point)>();
|
||||
foreach (var point in slice)
|
||||
{
|
||||
var delta = new Vector2(
|
||||
point.Position.X - centroid.X,
|
||||
point.Position.Y - centroid.Y);
|
||||
|
||||
if (delta.Length() < kMinDistance)
|
||||
continue;
|
||||
|
||||
var angle = Math.Atan2(delta.Y, delta.X);
|
||||
byAngle.Add((angle, point));
|
||||
}
|
||||
|
||||
// Sort by angle
|
||||
byAngle.Sort((a, b) => a.angle.CompareTo(b.angle));
|
||||
|
||||
// Return sorted points
|
||||
return byAngle.Select(p => p.point).ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds histogram values for a sorted point cloud slice.
|
||||
/// Match C++ AddPointCloudSliceToHistogram (rotational_scan_matcher.cc lines 61-89)
|
||||
/// </summary>
|
||||
private static void AddPointCloudSliceToHistogram(List<RangefinderPoint> sortedSlice, double[] histogram)
|
||||
{
|
||||
if (sortedSlice.Count == 0)
|
||||
return;
|
||||
|
||||
var centroid = ComputeCentroid(sortedSlice);
|
||||
var lastPointPosition = sortedSlice[0].Position;
|
||||
|
||||
foreach (var point in sortedSlice)
|
||||
{
|
||||
// Compute delta between consecutive points (2D only, XY plane)
|
||||
var delta = new Vector2(
|
||||
point.Position.X - lastPointPosition.X,
|
||||
point.Position.Y - lastPointPosition.Y);
|
||||
|
||||
// Direction from centroid to current point
|
||||
var direction = new Vector2(
|
||||
point.Position.X - centroid.X,
|
||||
point.Position.Y - centroid.Y);
|
||||
|
||||
var distance = delta.Length();
|
||||
if (distance < kMinDistance || direction.Length() < kMinDistance)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (distance > kMaxDistance)
|
||||
{
|
||||
lastPointPosition = point.Position;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Compute angle of the delta vector
|
||||
var angle = (float)Math.Atan2(delta.Y, delta.X);
|
||||
|
||||
// Weight: orthogonality to centroid direction (reject ceiling/floor angles)
|
||||
// Value is higher when delta is perpendicular to direction
|
||||
var deltaNorm = Vector2.Normalize(delta);
|
||||
var directionNorm = Vector2.Normalize(direction);
|
||||
var dotProduct = Vector2.Dot(deltaNorm, directionNorm);
|
||||
var value = Math.Max(0.0, 1.0 - Math.Abs(dotProduct));
|
||||
|
||||
AddValueToHistogram(angle, value, histogram);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds a value to the histogram at the given angle.
|
||||
/// Match C++ AddValueToHistogram (rotational_scan_matcher.cc lines 35-50)
|
||||
/// </summary>
|
||||
private static void AddValueToHistogram(float angle, double value, double[] histogram)
|
||||
{
|
||||
// Map the angle to [0, pi), i.e. a vector and its inverse are considered to
|
||||
// represent the same angle.
|
||||
while (angle > Math.PI)
|
||||
{
|
||||
angle -= (float)Math.PI;
|
||||
}
|
||||
while (angle < 0)
|
||||
{
|
||||
angle += (float)Math.PI;
|
||||
}
|
||||
|
||||
var zeroToOne = angle / Math.PI;
|
||||
var bucket = Math.Clamp(
|
||||
(int)Math.Round(histogram.Length * zeroToOne - 0.5),
|
||||
0,
|
||||
histogram.Length - 1);
|
||||
|
||||
histogram[bucket] += value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Matches two histograms and returns a normalized score.
|
||||
/// Match C++ MatchHistograms (rotational_scan_matcher.cc lines 121-132)
|
||||
/// </summary>
|
||||
private static double MatchHistograms(double[] submapHistogram, double[] scanHistogram)
|
||||
{
|
||||
// We compute the dot product of normalized histograms as a measure of similarity.
|
||||
var scanNorm = ComputeNorm(scanHistogram);
|
||||
var submapNorm = ComputeNorm(submapHistogram);
|
||||
var normalization = scanNorm * submapNorm;
|
||||
|
||||
if (normalization < 1e-3)
|
||||
{
|
||||
return 1.0; // Both histograms are nearly zero, consider them similar
|
||||
}
|
||||
|
||||
var dotProduct = 0.0;
|
||||
for (int i = 0; i < scanHistogram.Length && i < submapHistogram.Length; i++)
|
||||
{
|
||||
dotProduct += scanHistogram[i] * submapHistogram[i];
|
||||
}
|
||||
|
||||
return dotProduct / normalization;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Computes the L2 norm of a histogram.
|
||||
/// </summary>
|
||||
private static double ComputeNorm(double[] histogram)
|
||||
{
|
||||
var sumSquares = 0.0;
|
||||
foreach (var val in histogram)
|
||||
{
|
||||
sumSquares += val * val;
|
||||
}
|
||||
return Math.Sqrt(sumSquares);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scores how well 'histogram' rotated by 'initial_angle' can be understood as
|
||||
/// further rotated by certain 'angles' relative to the 'nodes'. Each angle
|
||||
/// results in a score between 0 (worst) and 1 (best).
|
||||
/// Match C++ Match (rotational_scan_matcher.cc lines 178-189)
|
||||
/// </summary>
|
||||
public List<double> Match(double[] histogram, double initialAngle, List<double> angles)
|
||||
{
|
||||
if (_histogram == null || _histogram.Length == 0)
|
||||
{
|
||||
// Return zero scores if no reference histogram
|
||||
return [.. angles.Select(_ => 0.0)];
|
||||
}
|
||||
|
||||
if (histogram == null || histogram.Length != _histogram.Length)
|
||||
{
|
||||
return [.. angles.Select(_ => 0.0)];
|
||||
}
|
||||
|
||||
var scores = new List<double>();
|
||||
|
||||
foreach (var angle in angles)
|
||||
{
|
||||
var totalAngle = initialAngle + angle;
|
||||
var rotatedHistogram = RotateHistogram(histogram, totalAngle);
|
||||
|
||||
// Use MatchHistograms which normalizes by the product of norms
|
||||
var score = MatchHistograms(_histogram, rotatedHistogram);
|
||||
scores.Add(score);
|
||||
}
|
||||
|
||||
return scores;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CeresSharp;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D.ScanMatching;
|
||||
|
||||
/// <summary>
|
||||
/// Computes the cost of translating 'translation' to 'target_translation'.
|
||||
/// Cost increases with the solution's distance from 'target_translation'.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// Creates a translation delta cost functor for 3D.
|
||||
/// </remarks>
|
||||
/// <param name="scalingFactor">Weight factor.</param>
|
||||
/// <param name="targetTranslation">Target translation to match.</param>
|
||||
public class TranslationDeltaCostFunctor3D(double scalingFactor, Vector3 targetTranslation)
|
||||
{
|
||||
private readonly double _targetX = targetTranslation.X;
|
||||
private readonly double _targetY = targetTranslation.Y;
|
||||
private readonly double _targetZ = targetTranslation.Z;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a DynamicAutoDiff cost function for translation delta.
|
||||
/// </summary>
|
||||
/// <param name="scalingFactor">Weight factor.</param>
|
||||
/// <param name="targetTranslation">Target translation to match.</param>
|
||||
/// <returns>DynamicAutoDiff cost function.</returns>
|
||||
public static DynamicAutoDiffCostFunction CreateAutoDiffCostFunction(
|
||||
double scalingFactor,
|
||||
Vector3 targetTranslation)
|
||||
{
|
||||
var functor = new TranslationDeltaCostFunctor3D(scalingFactor, targetTranslation);
|
||||
return new DynamicAutoDiffCostFunction(
|
||||
functor.Evaluate,
|
||||
numResiduals: 3, // [x, y, z]
|
||||
parameterBlockSizes: [3] // [x, y, z]
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the cost function.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Translation parameters [x, y, z].</param>
|
||||
/// <param name="residuals">Output residuals [x, y, z].</param>
|
||||
/// <returns>True on success.</returns>
|
||||
private bool Evaluate(double[][] parameters, double[] residuals)
|
||||
{
|
||||
if (parameters == null || parameters.Length < 1 || parameters[0].Length < 3)
|
||||
return false;
|
||||
if (residuals == null || residuals.Length < 3)
|
||||
return false;
|
||||
|
||||
var translation = parameters[0];
|
||||
residuals[0] = scalingFactor * (translation[0] - _targetX);
|
||||
residuals[1] = scalingFactor * (translation[1] - _targetY);
|
||||
residuals[2] = scalingFactor * (translation[2] - _targetZ);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// Adapter to make LocalTrajectoryBuilder3D implement TrajectoryBuilderInterface.
|
||||
/// </summary>
|
||||
internal class TrajectoryBuilder3DAdapter(LocalTrajectoryBuilder3D localBuilder) : ITrajectoryBuilder
|
||||
{
|
||||
private readonly LocalTrajectoryBuilder3D _localBuilder = localBuilder ?? throw new ArgumentNullException(nameof(localBuilder));
|
||||
|
||||
public ITrajectoryBuilder.MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData)
|
||||
{
|
||||
// LocalTrajectoryBuilder3D now returns ITrajectoryBuilder.MatchingResult directly
|
||||
return _localBuilder.AddRangeData(sensorId, timedPointCloudData);
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, ImuData imuData)
|
||||
{
|
||||
_localBuilder.AddImuData(imuData);
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, OdometryData odometryData)
|
||||
{
|
||||
_localBuilder.AddOdometryData(odometryData);
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData)
|
||||
{
|
||||
// Fixed frame pose data is typically used for external localization sources
|
||||
// Forward to the wrapped trajectory builder if it supports it
|
||||
// In this adapter, we assume LocalTrajectoryBuilder3D does not directly handle this,
|
||||
// so we do nothing or could potentially pass it to a different component if available.
|
||||
// For now, it remains unimplemented for _localBuilder.
|
||||
}
|
||||
|
||||
public void AddSensorData(string sensorId, LandmarkData landmarkData)
|
||||
{
|
||||
// Landmark data is used for landmark-based SLAM
|
||||
// Forward to the wrapped trajectory builder if it supports it
|
||||
// In this adapter, we assume LocalTrajectoryBuilder3D does not directly handle this,
|
||||
// so we do nothing or could potentially pass it to a different component if available.
|
||||
// For now, it remains unimplemented for _localBuilder.
|
||||
}
|
||||
|
||||
public void AddLocalSlamResultData(LocalSlamResultData localSlamResultData)
|
||||
{
|
||||
// LocalTrajectoryBuilder3D doesn't use this method
|
||||
// Results are returned directly from AddRangeData
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public Rigid3d? TryGetExtrapolatedPose(long time)
|
||||
{
|
||||
return _localBuilder.TryGetExtrapolatedPose(time);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public Rigid3d? TryGetExtrapolatedPoseFilter(long time)
|
||||
{
|
||||
return _localBuilder.TryGetExtrapolatedPoseFilter(time);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user