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

/*
 * 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);
    }
}