Initial commit
This commit is contained in:
@@ -0,0 +1,263 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using CartographerSharp.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using RotationalScanMatcher = CartographerSharp.Mapping.Internal.D3D.ScanMatching.RotationalScanMatcher;
|
||||
|
||||
namespace CartographerSharp.Mapping.D3D;
|
||||
|
||||
/// <summary>
|
||||
/// 3D Submap implementation.
|
||||
/// </summary>
|
||||
public class Submap3D : Submap
|
||||
{
|
||||
private HybridGrid _highResolutionHybridGrid;
|
||||
private HybridGrid _lowResolutionHybridGrid;
|
||||
private IntensityHybridGrid? _highResolutionIntensityHybridGrid;
|
||||
private List<double> _rotationalScanMatcherHistogram;
|
||||
|
||||
public Submap3D(
|
||||
double highResolution,
|
||||
double lowResolution,
|
||||
Rigid3d localSubmapPose,
|
||||
List<double> rotationalScanMatcherHistogram)
|
||||
: base(localSubmapPose)
|
||||
{
|
||||
_highResolutionHybridGrid = new HybridGrid(highResolution);
|
||||
_lowResolutionHybridGrid = new HybridGrid(lowResolution);
|
||||
_highResolutionIntensityHybridGrid = new IntensityHybridGrid(highResolution);
|
||||
_rotationalScanMatcherHistogram = [.. rotationalScanMatcherHistogram];
|
||||
}
|
||||
|
||||
public Submap3D(Models.Mapping.Submap3D proto)
|
||||
: base((Rigid3d)proto.LocalPose)
|
||||
{
|
||||
// Initialize with default values first
|
||||
_highResolutionHybridGrid = new HybridGrid(0.05f); // Default resolution
|
||||
_lowResolutionHybridGrid = new HybridGrid(0.05f);
|
||||
_rotationalScanMatcherHistogram = [];
|
||||
UpdateFromProto(proto);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the high resolution hybrid grid.
|
||||
/// </summary>
|
||||
public HybridGrid HighResolutionHybridGrid => _highResolutionHybridGrid;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the low resolution hybrid grid.
|
||||
/// </summary>
|
||||
public HybridGrid LowResolutionHybridGrid => _lowResolutionHybridGrid;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the high resolution intensity hybrid grid.
|
||||
/// </summary>
|
||||
public IntensityHybridGrid? HighResolutionIntensityHybridGrid => _highResolutionIntensityHybridGrid;
|
||||
|
||||
/// <summary>
|
||||
/// Forgets the intensity hybrid grid to reduce memory usage.
|
||||
/// </summary>
|
||||
public void ForgetIntensityHybridGrid()
|
||||
{
|
||||
_highResolutionIntensityHybridGrid = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the rotational scan matcher histogram.
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> RotationalScanMatcherHistogram => _rotationalScanMatcherHistogram;
|
||||
|
||||
/// <summary>
|
||||
/// Insert 'range_data' into this submap using 'range_data_inserter'. The
|
||||
/// submap must not be finished yet.
|
||||
/// </summary>
|
||||
public void InsertData(
|
||||
RangeData rangeDataInLocal,
|
||||
RangeDataInserter3D rangeDataInserter,
|
||||
double highResolutionMaxRange,
|
||||
Quaternion localFromGravityAligned,
|
||||
List<double> scanHistogramInGravity)
|
||||
{
|
||||
if (InsertionFinished)
|
||||
{
|
||||
throw new InvalidOperationException("Cannot insert data into finished submap");
|
||||
}
|
||||
|
||||
// Transform range data into submap frame
|
||||
var submapInverse = LocalPose.Inverse();
|
||||
var submapInverseFloat = new Rigid3f(
|
||||
(Vector3)submapInverse.Translation,
|
||||
submapInverse.Rotation);
|
||||
var transformedRangeData = RangeDataOperations.Transform(rangeDataInLocal, submapInverseFloat);
|
||||
|
||||
// Filter range data by max range for high resolution grid
|
||||
var filteredRangeData = FilterRangeDataByMaxRange(transformedRangeData, highResolutionMaxRange);
|
||||
|
||||
// Insert into high resolution grid with intensity
|
||||
rangeDataInserter.Insert(
|
||||
filteredRangeData,
|
||||
_highResolutionHybridGrid,
|
||||
_highResolutionIntensityHybridGrid);
|
||||
|
||||
// Insert into low resolution grid without intensity
|
||||
rangeDataInserter.Insert(
|
||||
transformedRangeData,
|
||||
_lowResolutionHybridGrid,
|
||||
null);
|
||||
|
||||
NumRangeData++;
|
||||
|
||||
// Update rotational scan matcher histogram
|
||||
// C++: yaw_in_submap_from_gravity = GetYaw(local_pose().inverse().rotation() * local_from_gravity_aligned)
|
||||
// rotational_scan_matcher_histogram_ += RotationalScanMatcher::RotateHistogram(scan_histogram_in_gravity, yaw_in_submap_from_gravity)
|
||||
var yawInSubmapFromGravity = TransformOperations.GetYaw(submapInverse.Rotation * localFromGravityAligned);
|
||||
|
||||
if (_rotationalScanMatcherHistogram.Count == scanHistogramInGravity.Count)
|
||||
{
|
||||
var rotatedHistogram = RotationalScanMatcher.RotateHistogram(
|
||||
scanHistogramInGravity.ToArray(),
|
||||
yawInSubmapFromGravity);
|
||||
|
||||
for (int i = 0; i < rotatedHistogram.Length; i++)
|
||||
{
|
||||
_rotationalScanMatcherHistogram[i] += rotatedHistogram[i];
|
||||
}
|
||||
}
|
||||
else if (scanHistogramInGravity.Count > 0)
|
||||
{
|
||||
// Log warning for histogram size mismatch - this can cause rotational matching failures
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"Warning: Histogram size mismatch in Submap3D.InsertData: " +
|
||||
$"expected {_rotationalScanMatcherHistogram.Count}, got {scanHistogramInGravity.Count}. " +
|
||||
"Rotational scan matching may not work correctly.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Finishes the submap.
|
||||
/// </summary>
|
||||
public void Finish()
|
||||
{
|
||||
if (InsertionFinished)
|
||||
{
|
||||
throw new InvalidOperationException("Submap is already finished");
|
||||
}
|
||||
InsertionFinished = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts to proto representation.
|
||||
/// </summary>
|
||||
public override Models.Mapping.Submap ToProto(bool includeGridData)
|
||||
{
|
||||
Models.Mapping.HybridGrid highResGrid;
|
||||
Models.Mapping.HybridGrid lowResGrid;
|
||||
|
||||
if (includeGridData)
|
||||
{
|
||||
highResGrid = _highResolutionHybridGrid.ToProto();
|
||||
lowResGrid = _lowResolutionHybridGrid.ToProto();
|
||||
}
|
||||
else
|
||||
{
|
||||
// Create empty grids with just resolution
|
||||
highResGrid = new Models.Mapping.HybridGrid
|
||||
{
|
||||
Resolution = _highResolutionHybridGrid.Resolution,
|
||||
XIndices = [],
|
||||
YIndices = [],
|
||||
ZIndices = [],
|
||||
Values = []
|
||||
};
|
||||
lowResGrid = new Models.Mapping.HybridGrid
|
||||
{
|
||||
Resolution = _lowResolutionHybridGrid.Resolution,
|
||||
XIndices = [],
|
||||
YIndices = [],
|
||||
ZIndices = [],
|
||||
Values = []
|
||||
};
|
||||
}
|
||||
|
||||
var submap3D = new Models.Mapping.Submap3D(
|
||||
(Rigid3dProto)LocalPose,
|
||||
NumRangeData,
|
||||
InsertionFinished,
|
||||
highResGrid,
|
||||
lowResGrid,
|
||||
[.. _rotationalScanMatcherHistogram]);
|
||||
|
||||
// Note: SubmapId will be set by caller
|
||||
return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), null, submap3D);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Updates from proto representation.
|
||||
/// </summary>
|
||||
public override void UpdateFromProto(Models.Mapping.Submap proto)
|
||||
{
|
||||
if (!proto.Submap3D.HasValue)
|
||||
{
|
||||
throw new ArgumentException("Proto must contain Submap3D", nameof(proto));
|
||||
}
|
||||
|
||||
UpdateFromProto(proto.Submap3D.Value);
|
||||
}
|
||||
|
||||
private void UpdateFromProto(Models.Mapping.Submap3D submap3D)
|
||||
{
|
||||
NumRangeData = submap3D.NumRangeData;
|
||||
InsertionFinished = submap3D.Finished;
|
||||
|
||||
if (submap3D.HighResolutionHybridGrid.Values != null && submap3D.HighResolutionHybridGrid.Values.Count > 0)
|
||||
{
|
||||
_highResolutionHybridGrid = new HybridGrid(submap3D.HighResolutionHybridGrid);
|
||||
}
|
||||
|
||||
if (submap3D.LowResolutionHybridGrid.Values != null && submap3D.LowResolutionHybridGrid.Values.Count > 0)
|
||||
{
|
||||
_lowResolutionHybridGrid = new HybridGrid(submap3D.LowResolutionHybridGrid);
|
||||
}
|
||||
|
||||
_rotationalScanMatcherHistogram = [.. submap3D.RotationalScanMatcherHistogram ?? []];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Filters 'range_data', retaining only the returns that have no more than
|
||||
/// 'max_range' distance from the origin. Removes misses.
|
||||
/// </summary>
|
||||
public static RangeData FilterRangeDataByMaxRange(RangeData rangeData, double maxRange)
|
||||
{
|
||||
var filteredReturns = new List<RangefinderPoint>();
|
||||
|
||||
foreach (var point in rangeData.Returns.Points)
|
||||
{
|
||||
var distance = Vector3.Distance(point.Position, rangeData.Origin);
|
||||
if (distance <= maxRange)
|
||||
{
|
||||
filteredReturns.Add(point);
|
||||
}
|
||||
}
|
||||
|
||||
return new RangeData(
|
||||
rangeData.Origin,
|
||||
new PointCloud(filteredReturns),
|
||||
new PointCloud()); // Misses are removed
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user