Initial commit
This commit is contained in:
@@ -0,0 +1,301 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using System.Collections;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// A compressed representation of a point cloud consisting of a collection of
|
||||
/// points (Vector3) without time information.
|
||||
/// Internally, points are grouped by blocks. Each block encodes a bit of meta
|
||||
/// data (number of points in block, coordinates of the block) and encodes each
|
||||
/// point with a fixed bit rate in relation to the block.
|
||||
/// </summary>
|
||||
public class CompressedPointCloud : IEnumerable<RangefinderPoint>
|
||||
{
|
||||
private const double kPrecision = 0.001f; // in meters
|
||||
private const int kBitsPerCoordinate = 10;
|
||||
private const int kCoordinateMask = (1 << kBitsPerCoordinate) - 1;
|
||||
private const int kMaxBitsPerDirection = 23;
|
||||
|
||||
private readonly List<int> _pointData;
|
||||
private readonly int _numPoints;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an empty compressed point cloud.
|
||||
/// </summary>
|
||||
public CompressedPointCloud()
|
||||
{
|
||||
_pointData = [];
|
||||
_numPoints = 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a compressed point cloud from a point cloud.
|
||||
/// </summary>
|
||||
public CompressedPointCloud(PointCloud pointCloud)
|
||||
{
|
||||
_numPoints = pointCloud.Count;
|
||||
|
||||
// Distribute points into blocks.
|
||||
// Using Dictionary to simulate HybridGrid behavior
|
||||
var blocks = new Dictionary<(int x, int y, int z), List<(Vector3 rasterPoint, int index)>>();
|
||||
|
||||
for (int pointIndex = 0; pointIndex < pointCloud.Count; pointIndex++)
|
||||
{
|
||||
var point = pointCloud[pointIndex];
|
||||
var absMax = Math.Max(Math.Max(Math.Abs(point.Position.X), Math.Abs(point.Position.Y)), Math.Abs(point.Position.Z));
|
||||
|
||||
if (absMax / kPrecision >= (1 << kMaxBitsPerDirection))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(pointCloud),
|
||||
$"Point out of bounds: {point.Position}");
|
||||
}
|
||||
|
||||
var rasterPoint = new Vector3(
|
||||
Math.Round(point.Position.X / kPrecision),
|
||||
Math.Round(point.Position.Y / kPrecision),
|
||||
Math.Round(point.Position.Z / kPrecision)
|
||||
);
|
||||
|
||||
var blockCoordinate = (
|
||||
(int)rasterPoint.X >> kBitsPerCoordinate,
|
||||
(int)rasterPoint.Y >> kBitsPerCoordinate,
|
||||
(int)rasterPoint.Z >> kBitsPerCoordinate
|
||||
);
|
||||
|
||||
var relativePoint = new Vector3(
|
||||
(int)rasterPoint.X & kCoordinateMask,
|
||||
(int)rasterPoint.Y & kCoordinateMask,
|
||||
(int)rasterPoint.Z & kCoordinateMask
|
||||
);
|
||||
|
||||
if (!blocks.TryGetValue(blockCoordinate, out var block))
|
||||
{
|
||||
block = [];
|
||||
blocks[blockCoordinate] = block;
|
||||
}
|
||||
block.Add((relativePoint, pointIndex));
|
||||
}
|
||||
|
||||
// Encode blocks.
|
||||
_pointData = [];
|
||||
foreach (var (blockCoord, rasterPoints) in blocks)
|
||||
{
|
||||
if (rasterPoints.Count > int.MaxValue)
|
||||
{
|
||||
throw new ArgumentException("Block too large");
|
||||
}
|
||||
|
||||
_pointData.Add(rasterPoints.Count);
|
||||
_pointData.Add(blockCoord.x);
|
||||
_pointData.Add(blockCoord.y);
|
||||
_pointData.Add(blockCoord.z);
|
||||
|
||||
foreach (var (rasterPoint, _) in rasterPoints)
|
||||
{
|
||||
int encoded = (int)(((((int)rasterPoint.Z << kBitsPerCoordinate) + (int)rasterPoint.Y) << kBitsPerCoordinate) + (int)rasterPoint.X);
|
||||
_pointData.Add(encoded);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a compressed point cloud from a proto.
|
||||
/// </summary>
|
||||
public CompressedPointCloud(CompressedPointCloud proto)
|
||||
{
|
||||
_numPoints = proto.NumPoints;
|
||||
_pointData = [.. proto.PointData];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of points.
|
||||
/// </summary>
|
||||
public int NumPoints => _numPoints;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the point data.
|
||||
/// </summary>
|
||||
internal List<int> PointData => _pointData;
|
||||
|
||||
/// <summary>
|
||||
/// Checks if the point cloud is empty.
|
||||
/// </summary>
|
||||
public bool IsEmpty => _numPoints == 0;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of points.
|
||||
/// </summary>
|
||||
public int Count => _numPoints;
|
||||
|
||||
/// <summary>
|
||||
/// Returns decompressed point cloud.
|
||||
/// </summary>
|
||||
public PointCloud Decompress()
|
||||
{
|
||||
var decompressed = new PointCloud();
|
||||
foreach (var point in this)
|
||||
{
|
||||
decompressed.Add(point);
|
||||
}
|
||||
return decompressed;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets an enumerator for the points.
|
||||
/// </summary>
|
||||
public IEnumerator<RangefinderPoint> GetEnumerator()
|
||||
{
|
||||
return new ConstIterator(this);
|
||||
}
|
||||
|
||||
IEnumerator IEnumerable.GetEnumerator()
|
||||
{
|
||||
return GetEnumerator();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts to proto representation.
|
||||
/// </summary>
|
||||
public Models.Sensor.CompressedPointCloud ToProto()
|
||||
{
|
||||
return new Models.Sensor.CompressedPointCloud(_numPoints, [.. _pointData]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates from proto representation.
|
||||
/// </summary>
|
||||
public static CompressedPointCloud FromProto(Models.Sensor.CompressedPointCloud proto)
|
||||
{
|
||||
return new CompressedPointCloud(proto.NumPoints, proto.PointData ?? []);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a compressed point cloud from proto data.
|
||||
/// </summary>
|
||||
private CompressedPointCloud(int numPoints, List<int> pointData)
|
||||
{
|
||||
_numPoints = numPoints;
|
||||
_pointData = pointData ?? [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Forward iterator for compressed point clouds.
|
||||
/// </summary>
|
||||
private class ConstIterator : IEnumerator<RangefinderPoint>
|
||||
{
|
||||
private readonly CompressedPointCloud _compressedPointCloud;
|
||||
private int _remainingPoints;
|
||||
private int _remainingPointsInCurrentBlock;
|
||||
private int _inputIndex;
|
||||
private Vector3 _currentPoint;
|
||||
private Vector3 _currentBlockCoordinates;
|
||||
|
||||
public ConstIterator(CompressedPointCloud compressedPointCloud)
|
||||
{
|
||||
_compressedPointCloud = compressedPointCloud;
|
||||
_remainingPoints = compressedPointCloud._numPoints;
|
||||
_remainingPointsInCurrentBlock = 0;
|
||||
_inputIndex = 0;
|
||||
|
||||
if (_remainingPoints > 0)
|
||||
{
|
||||
ReadNextPoint();
|
||||
}
|
||||
}
|
||||
|
||||
public RangefinderPoint Current { get; private set; }
|
||||
|
||||
object IEnumerator.Current => Current;
|
||||
|
||||
public bool MoveNext()
|
||||
{
|
||||
if (_remainingPoints <= 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Current = new RangefinderPoint(_currentPoint);
|
||||
_remainingPoints--;
|
||||
|
||||
if (_remainingPoints > 0)
|
||||
{
|
||||
ReadNextPoint();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
_remainingPoints = _compressedPointCloud._numPoints;
|
||||
_remainingPointsInCurrentBlock = 0;
|
||||
_inputIndex = 0;
|
||||
if (_remainingPoints > 0)
|
||||
{
|
||||
ReadNextPoint();
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
// Nothing to dispose
|
||||
}
|
||||
|
||||
private void ReadNextPoint()
|
||||
{
|
||||
if (_remainingPointsInCurrentBlock == 0)
|
||||
{
|
||||
if (_inputIndex >= _compressedPointCloud._pointData.Count)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_remainingPointsInCurrentBlock = _compressedPointCloud._pointData[_inputIndex++];
|
||||
|
||||
if (_inputIndex + 3 > _compressedPointCloud._pointData.Count)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_currentBlockCoordinates = new Vector3(
|
||||
_compressedPointCloud._pointData[_inputIndex++] << kBitsPerCoordinate,
|
||||
_compressedPointCloud._pointData[_inputIndex++] << kBitsPerCoordinate,
|
||||
_compressedPointCloud._pointData[_inputIndex++] << kBitsPerCoordinate
|
||||
);
|
||||
}
|
||||
|
||||
_remainingPointsInCurrentBlock--;
|
||||
|
||||
if (_inputIndex >= _compressedPointCloud._pointData.Count)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
int point = _compressedPointCloud._pointData[_inputIndex++];
|
||||
const int kMask = (1 << kBitsPerCoordinate) - 1;
|
||||
|
||||
_currentPoint = new Vector3(
|
||||
((_currentBlockCoordinates.X + (point & kMask)) * kPrecision),
|
||||
((_currentBlockCoordinates.Y + ((point >> kBitsPerCoordinate) & kMask)) * kPrecision),
|
||||
((_currentBlockCoordinates.Z + (point >> (2 * kBitsPerCoordinate))) * kPrecision)
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* Copyright 2017 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.Transform;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// The fixed frame pose data (like GPS, pose, etc.) will be used in the optimization.
|
||||
/// </summary>
|
||||
public struct FixedFramePoseData(long time, Rigid3d? pose = null)
|
||||
{
|
||||
public long Time { get; set; } = time;
|
||||
public Rigid3d? Pose { get; set; } = pose;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Operations on FixedFramePoseData.
|
||||
/// </summary>
|
||||
public static class FixedFramePoseDataOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts 'pose_data' to a proto::FixedFramePoseData.
|
||||
/// </summary>
|
||||
public static Models.Sensor.FixedFramePoseData ToProto(FixedFramePoseData poseData)
|
||||
{
|
||||
return new Models.Sensor.FixedFramePoseData(
|
||||
poseData.Time,
|
||||
poseData.Pose.HasValue ? (Rigid3dProto)poseData.Pose.Value : default
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts 'proto' to an FixedFramePoseData.
|
||||
/// </summary>
|
||||
public static FixedFramePoseData FromProto(Models.Sensor.FixedFramePoseData proto)
|
||||
{
|
||||
// Check if pose is set (equivalent to proto.has_pose() in C++)
|
||||
// In C#, since Rigid3dProto is a struct, we check if rotation quaternion is normalized
|
||||
// (a valid quaternion should have norm close to 1, default would be all zeros)
|
||||
Rigid3d? pose = null;
|
||||
var rot = proto.Pose.Rotation;
|
||||
var quatNorm = Math.Sqrt(rot.W * rot.W + rot.X * rot.X + rot.Y * rot.Y + rot.Z * rot.Z);
|
||||
// If quaternion is normalized (or close to normalized), pose is set
|
||||
if (quatNorm > 0.1) // Threshold to distinguish from default (0,0,0,0)
|
||||
{
|
||||
pose = (Rigid3d)proto.Pose;
|
||||
}
|
||||
|
||||
return new FixedFramePoseData(
|
||||
proto.Timestamp,
|
||||
pose
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* Copyright 2017 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 RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// IMU data structure.
|
||||
/// </summary>
|
||||
public struct ImuData(long time, Vector3 linearAcceleration, Vector3 angularVelocity)
|
||||
{
|
||||
public long Time { get; set; } = time;
|
||||
public Vector3 LinearAcceleration { get; set; } = linearAcceleration;
|
||||
public Vector3 AngularVelocity { get; set; } = angularVelocity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Operations on ImuData.
|
||||
/// </summary>
|
||||
public static class ImuDataOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts 'imu_data' to a proto::ImuData.
|
||||
/// </summary>
|
||||
public static Models.Sensor.ImuData ToProto(ImuData imuData)
|
||||
{
|
||||
return new Models.Sensor.ImuData(
|
||||
imuData.Time,
|
||||
new Vector3d(imuData.LinearAcceleration.X, imuData.LinearAcceleration.Y, imuData.LinearAcceleration.Z),
|
||||
new Vector3d(imuData.AngularVelocity.X, imuData.AngularVelocity.Y, imuData.AngularVelocity.Z)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts 'proto' to an ImuData.
|
||||
/// </summary>
|
||||
public static ImuData FromProto(Models.Sensor.ImuData proto)
|
||||
{
|
||||
return new ImuData(
|
||||
proto.Timestamp,
|
||||
new Vector3(proto.LinearAcceleration.X, proto.LinearAcceleration.Y, proto.LinearAcceleration.Z),
|
||||
new Vector3(proto.AngularVelocity.X, proto.AngularVelocity.Y, proto.AngularVelocity.Z)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* Copyright 2017 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.Transform;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// Landmark observation structure.
|
||||
/// </summary>
|
||||
public struct LandmarkObservation(string id, Rigid3d landmarkToTrackingTransform, double translationWeight, double rotationWeight)
|
||||
{
|
||||
public string Id { get; set; } = id;
|
||||
public Rigid3d LandmarkToTrackingTransform { get; set; } = landmarkToTrackingTransform;
|
||||
public double TranslationWeight { get; set; } = translationWeight;
|
||||
public double RotationWeight { get; set; } = rotationWeight;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Landmark data structure.
|
||||
/// </summary>
|
||||
public struct LandmarkData(long time, List<LandmarkObservation>? landmarkObservations = null)
|
||||
{
|
||||
public long Time { get; set; } = time;
|
||||
public List<LandmarkObservation> LandmarkObservations { get; set; } = landmarkObservations ?? [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Operations on LandmarkData.
|
||||
/// </summary>
|
||||
public static class LandmarkDataOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts 'landmark_data' to a proto::LandmarkData.
|
||||
/// </summary>
|
||||
public static Models.Sensor.LandmarkData ToProto(LandmarkData landmarkData)
|
||||
{
|
||||
var proto = new Models.Sensor.LandmarkData(landmarkData.Time, []);
|
||||
|
||||
foreach (var observation in landmarkData.LandmarkObservations)
|
||||
{
|
||||
proto.LandmarkObservations.Add(new Models.Sensor.LandmarkData.LandmarkObservation(
|
||||
System.Text.Encoding.UTF8.GetBytes(observation.Id),
|
||||
(Rigid3dProto)observation.LandmarkToTrackingTransform,
|
||||
observation.TranslationWeight,
|
||||
observation.RotationWeight
|
||||
));
|
||||
}
|
||||
|
||||
return proto;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts 'proto' to an LandmarkData.
|
||||
/// </summary>
|
||||
public static LandmarkData FromProto(Models.Sensor.LandmarkData proto)
|
||||
{
|
||||
var observations = new List<LandmarkObservation>();
|
||||
foreach (var protoObservation in proto.LandmarkObservations)
|
||||
{
|
||||
observations.Add(new LandmarkObservation(
|
||||
System.Text.Encoding.UTF8.GetString(protoObservation.Id),
|
||||
(Rigid3d)protoObservation.LandmarkToTrackingTransform,
|
||||
protoObservation.TranslationWeight,
|
||||
protoObservation.RotationWeight
|
||||
));
|
||||
}
|
||||
|
||||
return new LandmarkData(proto.Timestamp, observations);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/*
|
||||
* Copyright 2017 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.Transform;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// Odometry data structure.
|
||||
/// </summary>
|
||||
public struct OdometryData(long time, Rigid3d pose)
|
||||
{
|
||||
public long Time { get; set; } = time;
|
||||
public Rigid3d Pose { get; set; } = pose;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Operations on OdometryData.
|
||||
/// </summary>
|
||||
public static class OdometryDataOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts 'odometry_data' to a proto::OdometryData.
|
||||
/// </summary>
|
||||
public static Models.Sensor.OdometryData ToProto(OdometryData odometryData)
|
||||
{
|
||||
return new Models.Sensor.OdometryData(
|
||||
odometryData.Time,
|
||||
(Rigid3dProto)odometryData.Pose
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts 'proto' to an OdometryData.
|
||||
/// </summary>
|
||||
public static OdometryData FromProto(Models.Sensor.OdometryData proto)
|
||||
{
|
||||
return new OdometryData(
|
||||
proto.Timestamp,
|
||||
(Rigid3d)proto.Pose
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,215 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
using CartographerSharp.Transform;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// Stores 3D positions of points together with some additional data, e.g. intensities.
|
||||
/// </summary>
|
||||
public class PointCloud
|
||||
{
|
||||
private readonly List<RangefinderPoint> _points;
|
||||
private readonly List<double> _intensities;
|
||||
|
||||
/// <summary>
|
||||
/// Creates an empty point cloud.
|
||||
/// </summary>
|
||||
public PointCloud()
|
||||
{
|
||||
_points = [];
|
||||
_intensities = [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a point cloud from points.
|
||||
/// </summary>
|
||||
public PointCloud(IEnumerable<RangefinderPoint> points)
|
||||
{
|
||||
_points = [.. points];
|
||||
_intensities = [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a point cloud from points and intensities.
|
||||
/// </summary>
|
||||
public PointCloud(IEnumerable<RangefinderPoint> points, IEnumerable<double> intensities)
|
||||
{
|
||||
_points = [.. points];
|
||||
_intensities = [.. intensities];
|
||||
|
||||
if (_intensities.Count > 0 && _intensities.Count != _points.Count)
|
||||
{
|
||||
throw new ArgumentException("Intensities must have the same size as points, or be empty.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns the number of points in the point cloud.
|
||||
/// </summary>
|
||||
public int Count => _points.Count;
|
||||
|
||||
/// <summary>
|
||||
/// Checks whether there are any points in the point cloud.
|
||||
/// </summary>
|
||||
public bool IsEmpty => _points.Count == 0;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the points in the point cloud.
|
||||
/// </summary>
|
||||
public IReadOnlyList<RangefinderPoint> Points => _points;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the intensities in the point cloud.
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Intensities => _intensities;
|
||||
|
||||
/// <summary>
|
||||
/// Gets a point at the specified index.
|
||||
/// </summary>
|
||||
public RangefinderPoint this[int index] => _points[index];
|
||||
|
||||
/// <summary>
|
||||
/// Adds a point to the point cloud.
|
||||
/// </summary>
|
||||
public void Add(RangefinderPoint point)
|
||||
{
|
||||
_points.Add(point);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a PointCloud consisting of all the points for which predicate returns true,
|
||||
/// together with the corresponding intensities.
|
||||
/// </summary>
|
||||
public PointCloud CopyIf(Func<RangefinderPoint, bool> predicate)
|
||||
{
|
||||
var points = new List<RangefinderPoint>();
|
||||
var intensities = new List<double>();
|
||||
|
||||
if (_intensities.Count == 0)
|
||||
{
|
||||
foreach (var point in _points)
|
||||
{
|
||||
if (predicate(point))
|
||||
{
|
||||
points.Add(point);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < _points.Count; i++)
|
||||
{
|
||||
var point = _points[i];
|
||||
if (predicate(point))
|
||||
{
|
||||
points.Add(point);
|
||||
intensities.Add(_intensities[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return new PointCloud(points, intensities);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets an enumerator for the points.
|
||||
/// </summary>
|
||||
public IEnumerator<RangefinderPoint> GetEnumerator()
|
||||
{
|
||||
return _points.GetEnumerator();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Stores 3D positions of points with their relative measurement time in the
|
||||
/// fourth entry. Time is in seconds, increasing and relative to the moment when
|
||||
/// the last point was acquired. So, the fourth entry for the last point is 0.f.
|
||||
/// If timing is not available, all fourth entries are 0.f. For 2D points, the
|
||||
/// third entry is 0.f (and the fourth entry is time).
|
||||
/// </summary>
|
||||
public class TimedPointCloud : List<TimedRangefinderPoint>
|
||||
{
|
||||
public TimedPointCloud() : base() { }
|
||||
public TimedPointCloud(int capacity) : base(capacity) { }
|
||||
public TimedPointCloud(IEnumerable<TimedRangefinderPoint> collection) : base(collection) { }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Retained for compatibility. Contains timed point cloud with intensities.
|
||||
/// </summary>
|
||||
public struct PointCloudWithIntensities(TimedPointCloud points, List<double> intensities)
|
||||
{
|
||||
public TimedPointCloud Points { get; set; } = points;
|
||||
public List<double> Intensities { get; set; } = intensities;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transforms a point cloud according to a transform.
|
||||
/// </summary>
|
||||
public static class PointCloudOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Transforms 'point_cloud' according to 'transform'.
|
||||
/// </summary>
|
||||
public static PointCloud Transform(PointCloud pointCloud, Rigid3f transform)
|
||||
{
|
||||
var points = new List<RangefinderPoint>(pointCloud.Count);
|
||||
foreach (var point in pointCloud.Points)
|
||||
{
|
||||
points.Add(transform * point);
|
||||
}
|
||||
return new PointCloud(points, pointCloud.Intensities);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transforms 'point_cloud' according to 'transform'.
|
||||
/// </summary>
|
||||
public static TimedPointCloud Transform(TimedPointCloud pointCloud, Rigid3f transform)
|
||||
{
|
||||
var result = new TimedPointCloud(pointCloud.Count);
|
||||
foreach (var point in pointCloud)
|
||||
{
|
||||
result.Add(transform * point);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Returns a new point cloud without points that fall outside the region defined
|
||||
/// by 'min_z' and 'max_z'.
|
||||
/// </summary>
|
||||
public static PointCloud Crop(PointCloud pointCloud, double minZ, double maxZ)
|
||||
{
|
||||
return pointCloud.CopyIf(point =>
|
||||
minZ <= point.Position.Z && point.Position.Z <= maxZ);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Translates all points in the point cloud by the specified offset.
|
||||
/// </summary>
|
||||
public static PointCloud Translate(PointCloud pointCloud, Vector3 offset)
|
||||
{
|
||||
var points = new List<RangefinderPoint>(pointCloud.Count);
|
||||
foreach (var point in pointCloud.Points)
|
||||
{
|
||||
points.Add(new RangefinderPoint { Position = point.Position + offset });
|
||||
}
|
||||
return new PointCloud(points, pointCloud.Intensities);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,140 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// Range data structure.
|
||||
/// Rays begin at 'origin'. 'returns' are the points where obstructions were
|
||||
/// detected. 'misses' are points in the direction of rays for which no return
|
||||
/// was detected, and were inserted at a configured distance. It is assumed that
|
||||
/// between the 'origin' and 'misses' is free space.
|
||||
/// </summary>
|
||||
public struct RangeData(Vector3 origin, PointCloud returns, PointCloud misses)
|
||||
{
|
||||
public Vector3 Origin { get; set; } = origin;
|
||||
public PointCloud Returns { get; set; } = returns;
|
||||
public PointCloud Misses { get; set; } = misses;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Operations on RangeData.
|
||||
/// </summary>
|
||||
public static class RangeDataOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Transforms range data according to a transform.
|
||||
/// </summary>
|
||||
public static RangeData Transform(RangeData rangeData, Rigid3f transform)
|
||||
{
|
||||
return new RangeData(
|
||||
transform.TransformPoint(rangeData.Origin),
|
||||
PointCloudOperations.Transform(rangeData.Returns, transform),
|
||||
PointCloudOperations.Transform(rangeData.Misses, transform)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Normalizes range data origin to a target origin by translating all points.
|
||||
/// This is used in 2D SLAM to ensure consistent origin for ray casting in submap.
|
||||
/// </summary>
|
||||
public static RangeData NormalizeOrigin(RangeData rangeData, Vector3 targetOrigin)
|
||||
{
|
||||
var originOffset = targetOrigin - rangeData.Origin;
|
||||
return new RangeData(
|
||||
targetOrigin,
|
||||
PointCloudOperations.Translate(rangeData.Returns, originOffset),
|
||||
PointCloudOperations.Translate(rangeData.Misses, originOffset)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
|
||||
/// </summary>
|
||||
public static RangeData Crop(RangeData rangeData, double minZ, double maxZ)
|
||||
{
|
||||
return new RangeData(
|
||||
rangeData.Origin,
|
||||
PointCloudOperations.Crop(rangeData.Returns, minZ, maxZ),
|
||||
PointCloudOperations.Crop(rangeData.Misses, minZ, maxZ)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts 'range_data' to a proto::RangeData.
|
||||
/// </summary>
|
||||
public static Models.Sensor.RangeData ToProto(RangeData rangeData)
|
||||
{
|
||||
var proto = new Models.Sensor.RangeData
|
||||
{
|
||||
Origin = new Vector3f(rangeData.Origin.X, rangeData.Origin.Y, rangeData.Origin.Z),
|
||||
Returns = [],
|
||||
Misses = []
|
||||
};
|
||||
|
||||
foreach (var point in rangeData.Returns.Points)
|
||||
{
|
||||
proto.Returns.Add(new Models.Sensor.RangefinderPoint
|
||||
{
|
||||
Position = new Vector3f(point.Position.X, point.Position.Y, point.Position.Z)
|
||||
});
|
||||
}
|
||||
|
||||
foreach (var point in rangeData.Misses.Points)
|
||||
{
|
||||
proto.Misses.Add(new Models.Sensor.RangefinderPoint
|
||||
{
|
||||
Position = new Vector3f(point.Position.X, point.Position.Y, point.Position.Z)
|
||||
});
|
||||
}
|
||||
|
||||
return proto;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts 'proto' to RangeData.
|
||||
/// </summary>
|
||||
public static RangeData FromProto(Models.Sensor.RangeData proto)
|
||||
{
|
||||
var returns = new List<Sensor.RangefinderPoint>();
|
||||
if (proto.Returns.Count > 0)
|
||||
{
|
||||
foreach (var protoPoint in proto.Returns)
|
||||
{
|
||||
returns.Add(new Sensor.RangefinderPoint(new Vector3(protoPoint.Position.X, protoPoint.Position.Y, protoPoint.Position.Z)));
|
||||
}
|
||||
}
|
||||
|
||||
var misses = new List<Sensor.RangefinderPoint>();
|
||||
if (proto.Misses.Count > 0)
|
||||
{
|
||||
foreach (var protoPoint in proto.Misses)
|
||||
{
|
||||
misses.Add(new Sensor.RangefinderPoint(new Vector3(protoPoint.Position.X, protoPoint.Position.Y, protoPoint.Position.Z)));
|
||||
}
|
||||
}
|
||||
|
||||
return new RangeData(
|
||||
new Vector3(proto.Origin.X, proto.Origin.Y, proto.Origin.Z),
|
||||
new PointCloud(returns),
|
||||
new PointCloud(misses)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,136 @@
|
||||
/*
|
||||
* 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.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// Stores 3D position of a point observed by a rangefinder sensor.
|
||||
/// </summary>
|
||||
public struct RangefinderPoint(Vector3 position)
|
||||
{
|
||||
public Vector3 Position { get; set; } = position;
|
||||
|
||||
public static RangefinderPoint operator *(Rigid3f transform, RangefinderPoint point)
|
||||
{
|
||||
return new RangefinderPoint(transform.TransformPoint(point.Position));
|
||||
}
|
||||
|
||||
public static bool operator ==(RangefinderPoint lhs, RangefinderPoint rhs)
|
||||
{
|
||||
return lhs.Position == rhs.Position;
|
||||
}
|
||||
|
||||
public static bool operator !=(RangefinderPoint lhs, RangefinderPoint rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
public override readonly bool Equals(object? obj)
|
||||
{
|
||||
return obj is RangefinderPoint other && this == other;
|
||||
}
|
||||
|
||||
public override readonly int GetHashCode()
|
||||
{
|
||||
return Position.GetHashCode();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts from proto representation.
|
||||
/// </summary>
|
||||
public static RangefinderPoint FromProto(RangefinderPoint proto)
|
||||
{
|
||||
return new RangefinderPoint(proto.Position);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts to proto representation.
|
||||
/// </summary>
|
||||
public readonly RangefinderPoint ToProto()
|
||||
{
|
||||
return new RangefinderPoint(Position);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Stores 3D position of a point with its relative measurement time.
|
||||
/// See PointCloud for more details.
|
||||
/// </summary>
|
||||
public struct TimedRangefinderPoint(Vector3 position, double time)
|
||||
{
|
||||
public Vector3 Position { get; set; } = position;
|
||||
public double Time { get; set; } = time;
|
||||
|
||||
public static TimedRangefinderPoint operator *(Rigid3f transform, TimedRangefinderPoint point)
|
||||
{
|
||||
return new TimedRangefinderPoint(transform.TransformPoint(point.Position), point.Time);
|
||||
}
|
||||
|
||||
public static bool operator ==(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
|
||||
{
|
||||
return lhs.Position == rhs.Position && lhs.Time == rhs.Time;
|
||||
}
|
||||
|
||||
public static bool operator !=(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
|
||||
{
|
||||
return !(lhs == rhs);
|
||||
}
|
||||
|
||||
public override readonly bool Equals(object? obj)
|
||||
{
|
||||
return obj is TimedRangefinderPoint other && this == other;
|
||||
}
|
||||
|
||||
public override readonly int GetHashCode()
|
||||
{
|
||||
return HashCode.Combine(Position, Time);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts from proto representation.
|
||||
/// </summary>
|
||||
public static TimedRangefinderPoint FromProto(TimedRangefinderPoint proto)
|
||||
{
|
||||
return new TimedRangefinderPoint(proto.Position, proto.Time);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts to proto representation.
|
||||
/// </summary>
|
||||
public readonly TimedRangefinderPoint ToProto()
|
||||
{
|
||||
return new TimedRangefinderPoint(Position, Time);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts to RangefinderPoint (drops time information).
|
||||
/// </summary>
|
||||
public readonly RangefinderPoint ToRangefinderPoint()
|
||||
{
|
||||
return new RangefinderPoint(Position);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates TimedRangefinderPoint from RangefinderPoint with time.
|
||||
/// </summary>
|
||||
public static TimedRangefinderPoint FromRangefinderPoint(RangefinderPoint point, double time)
|
||||
{
|
||||
return new TimedRangefinderPoint(point.Position, time);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,147 @@
|
||||
/*
|
||||
* Copyright 2017 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using CartographerSharp.Models.Sensor;
|
||||
using CartographerSharp.Models.Transform;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// Timed point cloud data structure.
|
||||
/// </summary>
|
||||
public struct TimedPointCloudData
|
||||
{
|
||||
public long Time { get; set; }
|
||||
public Vector3 Origin { get; set; }
|
||||
public TimedPointCloud Ranges { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 'intensities' has to be same size as 'ranges', or empty.
|
||||
/// </summary>
|
||||
public List<double> Intensities { get; set; }
|
||||
|
||||
public TimedPointCloudData(long time, Vector3 origin, TimedPointCloud ranges, List<double>? intensities = null)
|
||||
{
|
||||
Time = time;
|
||||
Origin = origin;
|
||||
Ranges = ranges;
|
||||
Intensities = intensities ?? [];
|
||||
|
||||
if (Intensities.Count > 0 && Intensities.Count != Ranges.Count)
|
||||
{
|
||||
throw new ArgumentException("Intensities must have the same size as ranges, or be empty.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Timed point cloud origin data structure.
|
||||
/// </summary>
|
||||
public struct TimedPointCloudOriginData(long time, List<Vector3> origins, List<TimedPointCloudOriginData.RangeMeasurement> ranges)
|
||||
{
|
||||
/// <summary>
|
||||
/// Range measurement with point time, intensity, and origin index.
|
||||
/// </summary>
|
||||
public struct RangeMeasurement(TimedRangefinderPoint pointTime, double intensity, int originIndex)
|
||||
{
|
||||
public TimedRangefinderPoint PointTime { get; set; } = pointTime;
|
||||
public double Intensity { get; set; } = intensity;
|
||||
public int OriginIndex { get; set; } = originIndex;
|
||||
}
|
||||
|
||||
public long Time { get; set; } = time;
|
||||
public List<Vector3> Origins { get; set; } = origins;
|
||||
public List<RangeMeasurement> Ranges { get; set; } = ranges;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Operations on TimedPointCloudData.
|
||||
/// </summary>
|
||||
public static class TimedPointCloudDataOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData.
|
||||
/// Note: Time is already in Universal Time Scale ticks (long), so no conversion needed.
|
||||
/// </summary>
|
||||
public static Models.Sensor.TimedPointCloudData ToProto(TimedPointCloudData timedPointCloudData)
|
||||
{
|
||||
var pointData = new List<Models.Sensor.TimedRangefinderPoint>(timedPointCloudData.Ranges.Count);
|
||||
foreach (var range in timedPointCloudData.Ranges)
|
||||
{
|
||||
pointData.Add(new Models.Sensor.TimedRangefinderPoint(
|
||||
new Vector3f(range.Position.X, range.Position.Y, range.Position.Z),
|
||||
range.Time
|
||||
));
|
||||
}
|
||||
|
||||
return new Models.Sensor.TimedPointCloudData(
|
||||
timedPointCloudData.Time, // Already in Universal Time Scale ticks
|
||||
new Vector3f(timedPointCloudData.Origin.X, timedPointCloudData.Origin.Y, timedPointCloudData.Origin.Z),
|
||||
pointDataLegacy: null,
|
||||
pointData: pointData,
|
||||
intensities: timedPointCloudData.Intensities
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts 'proto' to TimedPointCloudData.
|
||||
/// Note: Time is already in Universal Time Scale ticks (long), so no conversion needed.
|
||||
/// </summary>
|
||||
public static TimedPointCloudData FromProto(Models.Sensor.TimedPointCloudData proto)
|
||||
{
|
||||
var timedPointCloud = new TimedPointCloud();
|
||||
|
||||
// Use point_data if available, otherwise fall back to point_data_legacy
|
||||
if (proto.PointData != null && proto.PointData.Count > 0)
|
||||
{
|
||||
timedPointCloud.Capacity = proto.PointData.Count;
|
||||
foreach (var protoPoint in proto.PointData)
|
||||
{
|
||||
timedPointCloud.Add(new TimedRangefinderPoint(
|
||||
new Vector3(protoPoint.Position.X, protoPoint.Position.Y, protoPoint.Position.Z),
|
||||
protoPoint.Time
|
||||
));
|
||||
}
|
||||
}
|
||||
else if (proto.PointDataLegacy != null && proto.PointDataLegacy.Count > 0)
|
||||
{
|
||||
// Legacy format: Vector4f where T component is time
|
||||
timedPointCloud.Capacity = proto.PointDataLegacy.Count;
|
||||
foreach (var point4 in proto.PointDataLegacy)
|
||||
{
|
||||
timedPointCloud.Add(new TimedRangefinderPoint(
|
||||
new Vector3(point4.X, point4.Y, point4.Z),
|
||||
point4.T
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
var intensities = new List<double>(proto.Intensities ?? []);
|
||||
|
||||
if (intensities.Count > 0 && intensities.Count != timedPointCloud.Count)
|
||||
{
|
||||
throw new ArgumentException("Intensities size must match ranges size, or be empty.");
|
||||
}
|
||||
|
||||
return new TimedPointCloudData(
|
||||
proto.Timestamp, // Already in Universal Time Scale ticks
|
||||
new Vector3(proto.Origin.X, proto.Origin.Y, proto.Origin.Z),
|
||||
timedPointCloud,
|
||||
intensities
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,268 @@
|
||||
/*
|
||||
* 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.Sensor;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace CartographerSharp.Sensor;
|
||||
|
||||
/// <summary>
|
||||
/// Voxel filter operations for point clouds.
|
||||
/// Uses randomized voxel filtering with reservoir sampling.
|
||||
/// </summary>
|
||||
public static class VoxelFilter
|
||||
{
|
||||
private const int kBitsPerCoordinate = 10;
|
||||
private const int kCoordinateMask = (1 << kBitsPerCoordinate) - 1;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the voxel cell index for a point at a given resolution.
|
||||
/// </summary>
|
||||
private static ulong GetVoxelCellIndex(Vector3 point, double resolution)
|
||||
{
|
||||
// Offset to handle negative coordinates. With 21 bits per coordinate, max value is 2^21-1.
|
||||
// Using 2^20 (1,048,576) as offset covers ±1,048,576 range, sufficient for most SLAM scenarios.
|
||||
// With resolution=0.01, this covers ±10,485.76m range.
|
||||
const long offset = 1L << 20; // 1,048,576
|
||||
|
||||
// Round to long first, then add offset, then cast to ulong
|
||||
var x = (ulong)((long)Math.Round(1.0 * point.X / resolution) + offset);
|
||||
var y = (ulong)((long)Math.Round(1.0 * point.Y / resolution) + offset);
|
||||
var z = (ulong)((long)Math.Round(1.0 * point.Z / resolution) + offset);
|
||||
|
||||
// Pack into 64-bit integer: x in bits 42-62, y in bits 21-41, z in bits 0-20
|
||||
return (x << 42) + (y << 21) + z;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Randomized voxel filter indices using reservoir sampling.
|
||||
/// According to https://en.wikipedia.org/wiki/Reservoir_sampling
|
||||
/// </summary>
|
||||
private static List<bool> RandomizedVoxelFilterIndices<T>(
|
||||
IReadOnlyList<T> pointCloud,
|
||||
double resolution,
|
||||
Func<T, Vector3> pointFunction)
|
||||
{
|
||||
var random = new Random();
|
||||
var voxelCountAndPointIndex = new Dictionary<ulong, (int count, int pointIndex)>();
|
||||
|
||||
for (int i = 0; i < pointCloud.Count; i++)
|
||||
{
|
||||
var voxelKey = GetVoxelCellIndex(pointFunction(pointCloud[i]), resolution);
|
||||
|
||||
if (!voxelCountAndPointIndex.TryGetValue(voxelKey, out var voxel))
|
||||
{
|
||||
voxelCountAndPointIndex[voxelKey] = (1, i);
|
||||
}
|
||||
else
|
||||
{
|
||||
var newCount = voxel.count + 1;
|
||||
int selectedIndex = voxel.pointIndex;
|
||||
|
||||
if (newCount > 1)
|
||||
{
|
||||
// Reservoir sampling: replace with probability 1/newCount
|
||||
if (random.Next(1, newCount + 1) == newCount)
|
||||
{
|
||||
selectedIndex = i;
|
||||
}
|
||||
}
|
||||
|
||||
voxelCountAndPointIndex[voxelKey] = (newCount, selectedIndex);
|
||||
}
|
||||
}
|
||||
|
||||
var pointsUsed = new List<bool>(new bool[pointCloud.Count]);
|
||||
foreach (var (_, (_, pointIndex)) in voxelCountAndPointIndex)
|
||||
{
|
||||
pointsUsed[pointIndex] = true;
|
||||
}
|
||||
|
||||
return pointsUsed;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Filters a list of RangefinderPoints using voxel filtering.
|
||||
/// </summary>
|
||||
public static List<RangefinderPoint> Filter(
|
||||
IReadOnlyList<RangefinderPoint> points,
|
||||
double resolution)
|
||||
{
|
||||
var pointsUsed = RandomizedVoxelFilterIndices(
|
||||
points, resolution,
|
||||
point => point.Position);
|
||||
|
||||
var results = new List<RangefinderPoint>();
|
||||
for (int i = 0; i < points.Count; i++)
|
||||
{
|
||||
if (pointsUsed[i])
|
||||
{
|
||||
results.Add(points[i]);
|
||||
}
|
||||
}
|
||||
return results;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Filters a PointCloud using voxel filtering.
|
||||
/// </summary>
|
||||
public static PointCloud Filter(PointCloud pointCloud, double resolution)
|
||||
{
|
||||
var pointsUsed = RandomizedVoxelFilterIndices(
|
||||
pointCloud.Points, resolution,
|
||||
point => point.Position);
|
||||
|
||||
var filteredPoints = new List<RangefinderPoint>();
|
||||
var filteredIntensities = new List<double>();
|
||||
|
||||
for (int i = 0; i < pointCloud.Count; i++)
|
||||
{
|
||||
if (pointsUsed[i])
|
||||
{
|
||||
filteredPoints.Add(pointCloud[i]);
|
||||
if (i < pointCloud.Intensities.Count)
|
||||
{
|
||||
filteredIntensities.Add(pointCloud.Intensities[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return new PointCloud(filteredPoints, filteredIntensities);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Filters a TimedPointCloud using voxel filtering.
|
||||
/// </summary>
|
||||
public static TimedPointCloud Filter(TimedPointCloud timedPointCloud, double resolution)
|
||||
{
|
||||
var pointsUsed = RandomizedVoxelFilterIndices(
|
||||
timedPointCloud, resolution,
|
||||
point => point.Position);
|
||||
|
||||
var results = new TimedPointCloud();
|
||||
for (int i = 0; i < timedPointCloud.Count; i++)
|
||||
{
|
||||
if (pointsUsed[i])
|
||||
{
|
||||
results.Add(timedPointCloud[i]);
|
||||
}
|
||||
}
|
||||
return results;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Filters range measurements using voxel filtering.
|
||||
/// </summary>
|
||||
public static List<TimedPointCloudOriginData.RangeMeasurement> Filter(
|
||||
IReadOnlyList<TimedPointCloudOriginData.RangeMeasurement> rangeMeasurements,
|
||||
double resolution)
|
||||
{
|
||||
var pointsUsed = RandomizedVoxelFilterIndices(
|
||||
rangeMeasurements, resolution,
|
||||
measurement => measurement.PointTime.Position);
|
||||
|
||||
var results = new List<TimedPointCloudOriginData.RangeMeasurement>();
|
||||
for (int i = 0; i < rangeMeasurements.Count; i++)
|
||||
{
|
||||
if (pointsUsed[i])
|
||||
{
|
||||
results.Add(rangeMeasurements[i]);
|
||||
}
|
||||
}
|
||||
return results;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adaptive voxel filter operations.
|
||||
/// </summary>
|
||||
public static class AdaptiveVoxelFilter
|
||||
{
|
||||
/// <summary>
|
||||
/// Filters point cloud by maximum range.
|
||||
/// </summary>
|
||||
private static PointCloud FilterByMaxRange(PointCloud pointCloud, double maxRange)
|
||||
{
|
||||
return pointCloud.CopyIf(point => point.Position.Length() <= maxRange);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adaptively voxel filters a point cloud.
|
||||
/// Uses binary search to find the right resolution that results in at least min_num_points.
|
||||
/// </summary>
|
||||
private static PointCloud AdaptivelyVoxelFiltered(
|
||||
AdaptiveVoxelFilterOptions options,
|
||||
PointCloud pointCloud)
|
||||
{
|
||||
if (pointCloud.Count <= options.MinNumPoints)
|
||||
{
|
||||
// Point cloud is already sparse enough.
|
||||
return pointCloud;
|
||||
}
|
||||
|
||||
var result = VoxelFilter.Filter(pointCloud, options.MaxLength);
|
||||
if (result.Count >= options.MinNumPoints)
|
||||
{
|
||||
// Filtering with max_length resulted in a sufficiently dense point cloud.
|
||||
return result;
|
||||
}
|
||||
|
||||
// Search for a 'low_length' that is known to result in a sufficiently
|
||||
// dense point cloud. We give up and use the full 'point_cloud' if reducing
|
||||
// the edge length by a factor of 1e-2 is not enough.
|
||||
for (double highLength = options.MaxLength;
|
||||
highLength > 1e-2 * options.MaxLength;
|
||||
highLength /= 2.0)
|
||||
{
|
||||
double lowLength = highLength / 2.0;
|
||||
result = VoxelFilter.Filter(pointCloud, lowLength);
|
||||
if (result.Count >= options.MinNumPoints)
|
||||
{
|
||||
// Binary search to find the right amount of filtering. 'low_length' gave
|
||||
// a sufficiently dense 'result', 'high_length' did not. We stop when the
|
||||
// edge length is at most 10% off.
|
||||
while ((highLength - lowLength) / lowLength > 1e-1)
|
||||
{
|
||||
double midLength = (lowLength + highLength) / 2.0;
|
||||
var candidate = VoxelFilter.Filter(pointCloud, midLength);
|
||||
if (candidate.Count >= options.MinNumPoints)
|
||||
{
|
||||
lowLength = midLength;
|
||||
result = candidate;
|
||||
}
|
||||
else
|
||||
{
|
||||
highLength = midLength;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Applies adaptive voxel filtering to a point cloud.
|
||||
/// </summary>
|
||||
public static PointCloud Filter(
|
||||
PointCloud pointCloud,
|
||||
AdaptiveVoxelFilterOptions options)
|
||||
{
|
||||
return AdaptivelyVoxelFiltered(
|
||||
options,
|
||||
FilterByMaxRange(pointCloud, options.MaxRange));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user