PointCloudXYZI.cs

Go to the documentation of this file.

using System.Runtime.InteropServices;
using IDSImaging.Peak.Common.Types;
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 PointCloudXYZI : PointCloudT<PointXYZI>
{
    public PointCloudXYZI(XYZImage image, Image intensityImage)
        : base(CreateFromXYZImage(image, intensityImage))
    {
    }
 
    public PointCloudXYZI(UndistortedImage undistortedDepthMap, Image intensityImage)
        : base(CreateFromUndistortedImage(undistortedDepthMap, intensityImage))
    {
    }
 
    public PointCloudXYZI(string filePath, PixelFormat pixelFormat)
        : base(LoadFromFile(filePath, pixelFormat))
    {
    }
 
    public PointCloudXYZI(PointXYZI[] points, PixelFormat pixelFormat)
        : base(CreateFromPoints(points, pixelFormat))
    {
        _points = points;
    }
 
    private PointCloudXYZI()
    {
    }
 
    private PointCloudXYZI(Handle handle) : base(handle)
    {
    }
 
    public override PointXYZI[] 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<PointXYZI>();
                return _points;
            }
 
            var pointsC = new Backend.Types.peak_icv_point_xyzi[numPoints];
            var size = Marshal.SizeOf<Backend.Types.peak_icv_point_xyzi>();
 
            var handle = GCHandle.Alloc(pointsC, GCHandleType.Pinned);
 
            try
            {
                IntPtr ptr = handle.AddrOfPinnedObject();
 
                DllHelper.ExecuteAndMapReturnCodes(() =>
                    CInterface.peak_icv_PointCloud_GetPoints(
                        _handle,
                        ptr,
                        (nuint) (size * (int) numPoints)));
 
                _points = pointsC
                    .Select(np => new PointXYZI(np.x, np.y, np.z, np.intensity))
                    .ToArray();
            }
            finally
            {
                handle.Free();
            }
 
            return _points;
        }
    }
 
    public static IEqualityComparer<PointCloudXYZI> Comparer { get; } = new PointCloudEqualityComparer();
 
 
    public PointCloudXYZI Transform(TransformationMatrix3D matrix)
    {
        var pointCloud = new PointCloudXYZI();
        var cMatrix = matrix.ToCType();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_Transform(_handle, cMatrix,
                ref pointCloud._handle));
        return pointCloud;
    }
 
    public PointCloudXYZI TransformToWorkspace(ExtrinsicParameters extrinsicParameters)
    {
        var pointCloud = new PointCloudXYZI();
        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, Image intensityImage)
    {
        var handle = new Handle();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_CreateFromXYZImageAndIntensityImage(out handle, image._handle,
                intensityImage._handle));
        return handle;
    }
 
    private static Handle CreateFromUndistortedImage(UndistortedImage undistortedDepthMap, Image intensityImage)
    {
        using var xyz = new XYZImage(undistortedDepthMap);
        return CreateFromXYZImage(xyz, intensityImage);
    }
 
    private static Handle LoadFromFile(string filePath, PixelFormat pixelFormat)
    {
        var handle = new Handle();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_CreateFromFile(out handle,
                PixelFormatToPointType(pixelFormat), Utils.ToUtf8(filePath)));
        return handle;
    }
 
    private static Handle CreateFromPoints(PointXYZI[] points, PixelFormat pixelFormat)
    {
        Backend.Types.peak_icv_point_xyzi[] pointsC = points.Select(point =>
            new Backend.Types.peak_icv_point_xyzi
            {
                x = point.X,
                y = point.Y,
                z = point.Z,
                intensity = point.Intensity
            }).ToArray();
 
        var handle = new Handle();
        DllHelper.ExecuteAndMapReturnCodes(() =>
            CInterface.peak_icv_PointCloud_CreateFromPoints(
                out handle, pointsC, (nuint) pointsC.Length,
                PixelFormatToPointType(pixelFormat)));
 
        return handle;
    }
 
    private static Backend.Types.peak_icv_point_type PixelFormatToPointType(PixelFormat pixelFormat)
    {
        return pixelFormat switch
        {
            PixelFormat.Mono8 => Backend.Types.peak_icv_point_type.PEAK_ICV_POINT_TYPE_XYZ_I8,
            PixelFormat.Mono10 => Backend.Types.peak_icv_point_type.PEAK_ICV_POINT_TYPE_XYZ_I10,
            PixelFormat.Mono12 => Backend.Types.peak_icv_point_type.PEAK_ICV_POINT_TYPE_XYZ_I12,
            _ => throw new IcvNotSupportedException(
                "Unsupported pixel format. Supported formats are Mono8, Mono10, and Mono12.")
        };
    }
 
    private sealed class PointCloudEqualityComparer : IEqualityComparer<PointCloudXYZI>
    {
        public bool Equals(PointCloudXYZI x, PointCloudXYZI 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, PointXYZI.Comparer);
        }
 
        public int GetHashCode(PointCloudXYZI obj)
        {
            PointXYZI[] points = obj.Points;
            var count = points.Count();
 
            return (
                count,
                PointXYZI.Comparer.GetHashCode(points.First()),
                PointXYZI.Comparer.GetHashCode(points.ElementAt(count / 2)),
                PointXYZI.Comparer.GetHashCode(points.Last())
            ).GetHashCode();
        }
    }
}