PointCloudXYZ.cs

Go to the documentation of this file.

using System.Runtime.InteropServices;
using IDSImaging.Peak.ICV.Algorithms.Calibration;
using IDSImaging.Peak.ICV.Backend;
using IDSImaging.Peak.ICV.Types.Detail;
using IDSImaging.Peak.ICV.Types.Geometry;
 
namespace IDSImaging.Peak.ICV.Types;
 
public class PointCloudXYZ : PointCloudT<PointXYZ>
{
    public PointCloudXYZ(XYZImage image)
        : base(CreateFromXYZImage(image))
    {
    }
 
    public PointCloudXYZ(UndistortedImage undistortedDepthMap)
        : base(CreateFromUndistortedImage(undistortedDepthMap))
    {
    }
 
    public PointCloudXYZ(string filePath)
        : base(LoadFromFile(filePath))
    {
    }
 
    public PointCloudXYZ(PointXYZ[] points)
        : base(CreateFromPoints(points))
    {
        _points = points;
    }
 
    private PointCloudXYZ()
    {
    }
 
    private PointCloudXYZ(Handle handle) : base(handle)
    {
    }
 
    public override PointXYZ[] Points
    {
        get
        {
            if (_points != null)
            {
                return _points;
            }
 
            nuint numPoints = 0;
            DllHelper.ExecuteAndMapReturnCodes(() =>
                CInterface.peak_icv_PointCloud_GetPoints_GetCount(_handle, out numPoints));
 
            if (numPoints == 0)
            {
                _points = Array.Empty<PointXYZ>();
                return _points;
            }
 
            var pointsC = new Backend.Types.peak_icv_point_xyz[numPoints];
            var handle = GCHandle.Alloc(pointsC, GCHandleType.Pinned);
 
            try
            {
                IntPtr ptr = handle.AddrOfPinnedObject();
 
                DllHelper.ExecuteAndMapReturnCodes(() =>
                    CInterface.peak_icv_PointCloud_GetPoints(_handle,
                        ptr,
                        (nuint) Marshal.SizeOf<Backend.Types.peak_icv_point_xyz>() * numPoints));
            }
            finally
            {
                handle.Free();
            }
 
 
            _points = pointsC
                .Select(np => new PointXYZ(np.x, np.y, np.z))
                .ToArray();
            return _points;
        }
    }
 
    public static IEqualityComparer<PointCloudXYZ> Comparer { get; } = new PointCloudEqualityComparer();
 
    private static Handle CreateFromUndistortedImage(UndistortedImage undistortedDepthMap)
    {
        using var xyz = new XYZImage(undistortedDepthMap);
        return CreateFromXYZImage(xyz);
    }
 
    public PointCloudXYZ Transform(TransformationMatrix3D matrix)
    {
        var pointCloud = new PointCloudXYZ();
        var cMatrix = matrix.ToCType();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_Transform(_handle, cMatrix, ref pointCloud._handle));
        return pointCloud;
    }
 
    public PointCloudXYZ TransformToWorkspace(ExtrinsicParameters extrinsicParameters)
    {
        var pointCloud = new PointCloudXYZ();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_TransformToWorkspace(_handle, extrinsicParameters.ToCType(),
                (nuint) Marshal.SizeOf<Backend.Types.peak_icv_extrinsic_parameters>(),
                ref pointCloud._handle));
        return pointCloud;
    }
 
    private static Handle CreateFromXYZImage(XYZImage image)
    {
        var handle = new Handle();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_CreateFromXYZImage(out handle, image._handle));
        return handle;
    }
 
    private static Handle LoadFromFile(string filePath)
    {
        var handle = new Handle();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_CreateFromFile(out handle,
                Backend.Types.peak_icv_point_type.PEAK_ICV_POINT_TYPE_XYZ, Utils.ToUtf8(filePath)));
        return handle;
    }
 
    private static Handle CreateFromPoints(PointXYZ[] points)
    {
        Backend.Types.peak_icv_point_xyz[] pointsC = points.Select(point =>
            new Backend.Types.peak_icv_point_xyz
            {
                x = point.X,
                y = point.Y,
                z = point.Z
            }).ToArray();
 
        var handle = new Handle();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_CreateFromPoints(
                out handle, pointsC, (nuint) pointsC.Length,
                Backend.Types.peak_icv_point_type.PEAK_ICV_POINT_TYPE_XYZ));
        return handle;
    }
 
    private sealed class PointCloudEqualityComparer : IEqualityComparer<PointCloudXYZ>
    {
        public bool Equals(PointCloudXYZ x, PointCloudXYZ y)
        {
            if (ReferenceEquals(x, y))
            {
                return true;
            }
 
            if (ReferenceEquals(x, null))
            {
                return false;
            }
 
            if (ReferenceEquals(y, null))
            {
                return false;
            }
 
            return x.Points.SequenceEqual(y.Points, PointXYZ.Comparer);
        }
 
        public int GetHashCode(PointCloudXYZ obj)
        {
            PointXYZ[] points = obj.Points;
            var count = points.Count();
 
            return (
                count,
                PointXYZ.Comparer.GetHashCode(points.First()),
                PointXYZ.Comparer.GetHashCode(points.ElementAt(count / 2)),
                PointXYZ.Comparer.GetHashCode(points.Last())
            ).GetHashCode();
        }
    }
}