lvjincheng_infraredlocategamepc/Assets/o0/zimHoughTransform.cs

using o0.Geometry2D;
using SixLabors.ImageSharp.PixelFormats;
using SixLabors.ImageSharp.Processing.Processors;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Unity.VisualScripting;
using UnityEngine;
using UnityEngine.UIElements;

namespace o0.Project
{
    public static partial class Extension
    {

    }


    public static class Hough
    {
        public static List<(float Ax, float Ay, float Bx, float By)> Transform(MatrixF2D mat, float LigntPointRate = 0.01f, int threshold = 100, double minLineLength = 300)
        {
            var lines = Lines(mat, LigntPointRate);
            var result = new List<(float, float, float, float)>();
            foreach (var l in lines)
            {
                if (l.PointsCount > threshold && l.Length > minLineLength)
                {
                    //UnityEngine.Debug.Log($"线段: {l.A}, {l.B}, {mat[(int)l.A.x, (int)l.A.y]}, {mat[(int)l.B.x, (int)l.B.y]}");
                    result.Add((l.A.x, l.A.y, l.B.x, l.B.y));
                }
            }
            return result;
        }

        static Line[,] Lines(MatrixF2D mat, float LigntPointRate, double rho = 1, double theta = Math.PI / 180, double maxLineGap = 10)
        {
            var TrigValues = GetTrigValues(theta);
            int maxR = (int)Math.Floor(Math.Sqrt(mat.Width * mat.Width + mat.Height * mat.Height) / rho);

            var Acumulator = new Line[maxR, TrigValues.Length];
            Parallel.For(0, maxR, (i) =>
            {
                for (int j = 0; j < TrigValues.Length; j++)
                    Acumulator[i, j] = new Line();
            });

            // 取亮度较高的点
            int LigntPointCount = (int)(mat.Element.Length * LigntPointRate);
            var Bright = GetBrightElements(mat, LigntPointCount);

            /**/
            var mm = new MatrixF2D(mat.Width, mat.Height);
            foreach (var i in Bright)
                mm[i.x, i.y] = 1;
            var tt = mm.ToTex();
            var bytes = tt.EncodeToPNG();
            File.WriteAllBytes($"output/Bright.png", bytes);
            /**/

            Parallel.For(0, TrigValues.Length, a =>
            {
                foreach (var i in Bright)
                {
                    var r = (int)Math.Floor((i.x * TrigValues[a].cos + i.y * TrigValues[a].sin) / rho);
                    //UnityEngine.Debug.Log($"{i.x}, {i.y}, {angle}, maxR {maxR}");
                    Acumulator[r, a].Add(new Vector<float>(i.x, i.y));
                }
            });

            return Acumulator;
        }

        // 0 - 180度， cos 取绝对值
        static (float cos, float sin)[] GetTrigValues(double theta)
        {
            (float, float)[] result = new (float, float)[(int)Math.Floor((Math.PI / theta))];
            Parallel.For(0, result.Length, i => result[i] = ((float)Math.Abs(Math.Cos(i * theta)), (float)Math.Sin(i * theta)));
            return result;
        }

        // 取不少于指定数量的亮点
        static List<(int x, int y)> GetBrightElements(MatrixF2D mat, int leastCount, int bit = 256)
        {
            bit -= 1;
            var p = mat.Element;
            float scale = 1 / Enumerable.Max(p);
            Parallel.For(0, p.Length, (i) => p[i] *= scale);        // 归一化
            int[] pint = new int[p.Length];
            Parallel.For(0, p.Length, i => pint[i] = (int)(p[i] * bit));        // 映射到256色彩

            // 得到色阶
            int[] H = new int[bit + 1];
            for (int i = 0; i < p.Length; i++)
                H[pint[i]]++;

            int b = bit;
            var sum = 0;
            for (; b >= 0; b--)
            {
                sum += H[b];
                if (sum >= leastCount)
                    break;
            }

            var result = new List<(int x, int y)>();
            for (int i = 0; i < p.Length; i++)
            {
                if (pint[i] >= b)
                    result.Add((i % mat.Width, i / mat.Width));
            }
            return result;
        }

        class Line
        {
            public int PointsCount => Points.Count;
            public float Length => (A - B).Length;

            public Vector<float> A { get;private set; }
            public Vector<float> B { get;private set; }


            List<Vector<float>> Points = new List<Vector<float>>();

            public Line()
            {

            }

            public void Add( Vector<float> p)
            {
                Points.Add(p);
                if (Points.Count == 1)
                    A = B = p;
                else
                {
                    A = A < p ? A : p;
                    B = B > p ? B : p;
                }
            }


        }

    }

}