Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Sensor/PointCloud.cs

@@ -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);
+    }
+}