slambb_smartbow-infrared/Assets/o0/zimIdentifyLineLSD.cs

using o0.Geometry;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;
using UnityEngine.UI;
using o0.Geometry2D.Float;
using ZIM;
using o0.Num;

namespace o0.Project
{
    public static partial class Extension
    {                                                                                      // (垂直方向,水平方向)
        public static (Vector, float) ZIMLineSegGradient(Matrix mat, Matrix dir, float gradient, Vector start, Vector end, (int, int) range, bool reverseXY)
        {
            int startX, endX;
            if (start.x < end.x)
            {
                startX = Mathf.RoundToInt(start.x) + 1;
                endX = Mathf.RoundToInt(end.x) + 1;
            }
            else
            {
                startX = Mathf.RoundToInt(start.x) - 1;
                endX = Mathf.RoundToInt(end.x) - 1;
            }
            if (startX == endX)
                return (new Vector(start.x, start.y), 0);

            var xDiff = end.x - start.x;
            var xToYRate = (float)(end.y - start.y) / xDiff;
            Func<float, float> xToYFunc = (x) => (x - start.x) * xToYRate + start.y;

            var B1 = Mathf.RoundToInt(start.x);
            var B1V = mat[B1, Mathf.RoundToInt(start.y)];
            var Sum1 = B1V;

            foreach (var Bx in startX.Range(endX))
            {
                int[,] CIArray = new int[range.Item1, range.Item2];
                float[] CAArray = new float[range.Item1 * range.Item2];
                float[] CMArray = new float[range.Item1 * range.Item2];
                for (int j = 0; j < range.Item2; j++)
                {
                    var x = xDiff > 0 ? Bx + j : Bx - j;
                    var y = xToYFunc(x);
                    var m1 = range.Item1 / 2 - 1;
                    var m2 = range.Item1 / 2;
                    var y1 = Mathf.FloorToInt(y);
                    var y2 = Mathf.CeilToInt(y);
                    for (int i = 0; i < range.Item1 / 2; i++) 
                    {
                        if (reverseXY)
                        CIArray[i, j] = mat.Index(y1 + i - m1, x);
                        else
                        CIArray[i, j] = mat.Index(x, y1 + i - m1);
                        CAArray[i + j * range.Item1] = dir.Element[CIArray[i, j]];
                        CMArray[i + j * range.Item1] = mat.Element[CIArray[i, j]];
                    }
                    for (int i = range.Item1 / 2; i < range.Item1; i++)
                    {
                        if (reverseXY)
                        CIArray[i, j] = mat.Index(y2 + i - m2, x);
                        else
                        CIArray[i, j] = mat.Index(x, y2 + i - m2);
                        CAArray[i + j * range.Item1] = dir.Element[CIArray[i, j]];
                        CMArray[i + j * range.Item1] = mat.Element[CIArray[i, j]];
                    }
                }

                var CV = o0.Max(CMArray);
                var avaAngle = 60;
                var halfAvaAngle = avaAngle / 2;

                var breakFlag = true;
                foreach (var ca in CAArray)
                {
                    if (MathF.Abs(ca - gradient) <= halfAvaAngle || MathF.Abs(ca - 360 - gradient) <= halfAvaAngle || MathF.Abs(ca + 360 - gradient) <= halfAvaAngle)
                    {
                        breakFlag = false;
                        break;
                    }
                }
                if (breakFlag)
                    break;

                B1 = Bx;
                Sum1 += CV;
            }
            return (new Vector(B1, xToYFunc(B1)), Sum1);
        }

        // 线段顺序: 下、右、上、左
        public static List<Line> ZIMIdentifyQuadLSD(this Matrix screenLocateMat, Matrix edgeMat, Matrix edgeDirMat,out List<Line> lightLines, 
            float gradientLength, float minLength = 100)
        {
            // 加权平均
            Vector[] avgPointsColumn = new Vector[screenLocateMat.Size.x];
            float[] valueSumsColumn = new float[screenLocateMat.Size.x];
            Parallel.For(0, screenLocateMat.Size.x, i =>
            {
                for (int j = 0; j < screenLocateMat.Size.y; j++)
                {
                    var value = screenLocateMat[i, j];
                    valueSumsColumn[i] += value;
                    avgPointsColumn[i] += new Vector(i, j) * value;
                }
            });

            Vector avgPoint = Vector.Zero;
            var valueSum = 0f;
            for (int i = 0; i < screenLocateMat.Size.x; i++)
            {
                avgPoint += avgPointsColumn[i];
                valueSum += valueSumsColumn[i];
            }
            avgPoint /= valueSum;

            var lines = edgeMat.IdentifyLineLSD(edgeDirMat, minLength, 20);
            //Debug.Log("[IdentifyLineLSD] lines.Count: " + lines.Count);

            // 下、右、上、左
            var quadLines = new List<(float, Line)>[4] {new List<(float, Line)>(), new List<(float, Line)>(), new List<(float, Line)>(), new List<(float, Line)>() };        
            var avaAngleHalf = 75f;

            lightLines = new List<Line>();
            foreach (var (line, sum, gradient) in lines)
            {
                lightLines.Add(line);
                var a = (avgPoint - (line.A + line.B) / 2).DegreeToXAxis();
                //Debug.Log(a + ", " + gradient + ", " + sum);
                int index = -1;
                if (Math.Abs(a - gradient) < avaAngleHalf || Math.Abs(a - 360 - gradient) < avaAngleHalf || Math.Abs(a + 360 - gradient) < avaAngleHalf)
                {
                    if (gradient > 45 && gradient < 135)     // 下
                        index = 0;
                    else if (gradient > 135 && gradient < 225) // 右
                        index = 1;
                    else if (gradient > 225 && gradient < 315)  // 上
                        index = 2;
                    else
                        index = 3;

                    // 沿直线计算平均梯度
                    var dir = (line.B - line.A).Normalized;
                    var vertical = new Vector(-dir.y, dir.x) * (gradientLength / 2);
                    var step = 2;
                    var ll = line.Length;
                    var lg = new List<float>();
                    for (int i = 0; i <= ll; i += step) 
                    {
                        var point = line.A + dir * i;
                        var ga = point + vertical;
                        var gb = point - vertical;
                        lg.Add(screenLocateMat[(int)ga.x, (int)ga.y] - screenLocateMat[(int)gb.x, (int)gb.y]);
                        //Debug.Log(lg.Last());
                    }

                    //var g = Math.Abs(lg.Mean());
                    //Debug.Log(gradient + ", " + g);

                    //List<float> lp1 = new List<float>(), lp2 = new List<float>();    // 线两侧的值
                    //for (float i = 0; i <= ll; i += step)
                    //{
                    //    var point = line.A + dir * i;
                    //    var ga = point + vertical;
                    //    var gb = point - vertical;
                    //    lp1.Add(screenLocateMat[(int)ga.x, (int)ga.y]);
                    //    lp2.Add(screenLocateMat[(int)gb.x, (int)gb.y]);
                    //}

                    //var avg1 = lp1.Mean();
                    //var avg2 = lp2.Mean();
                    //var v1 = lp1.Variance();
                    //var v2 = lp2.Variance();

                    //var lineGradient = Math.Abs(avg1 - avg2) / (v1 + v2 + 0.2f);       // 方差越小，梯度的价值越高
                    ////var g = Math.Abs(lg.Mean());
                    ////Debug.Log(gradient + ", " + g);
                    //Debug.Log(v1 + ", " + v2 + ", " + lineGradient);

                    //quadLines[index].Add((lineGradient, line));

                    quadLines[index].Add((Math.Abs(lg.Mean()), line));
                }
            }

            var result = new List<Line>();
            foreach (var list in quadLines)
                result.Add(list.Max((a, b) => a.Item1.CompareTo(b.Item1)).Item2);
            return result;
        }
    }
}