smart-bow-infrared/Assets/InfraredProject/o0/zimIdentifyLineLSD.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using UnityEngine;
using o0.Geometry2D.Float;
using ZIM.Unity;
using o0.Num;
using o0InfraredLocate.ZIM;

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

        // Old, 弃用的 ------------
        public static List<Line> ZIMIdentifyQuadLSD(this Matrix screenLocateMat, Matrix edgeMat, Matrix edgeDirMat,
            out Line[] oldLines, out List<Line> possibleLines, out List<(Line, float, float)> allLines, ScreenMap screen, 
            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;

            allLines = edgeMat.IdentifyLineLSD(edgeDirMat, minLength, 20, LineCaptureSize: new Vector(0, 5));

            //Debug.Log("[IdentifyLineLSD] lines.Count: " + lines.Count);
            // LSD计算得到的矩阵尺寸较小（因为卷积），这里必须进行位移
            var offset = new Vector((screenLocateMat.Size.x - edgeMat.Size.x) / 2, (screenLocateMat.Size.y - edgeMat.Size.y) / 2);
            for (int i = 0; i < allLines.Count; i++)
                allLines[i] = (allLines[i].Item1 + offset, allLines[i].Item2, allLines[i].Item3);

            // 沿直线计算综合梯度（梯度乘以长度系数）
            float estimateGradient(Line line)
            {
                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]);
                }

                float e = (float)Math.Sqrt(Math.Max(1, line.Length / minLength / 3));       // 长度系数，筛选时梯度更大、长度更长的线段更优
                return e * Math.Abs(lg.Mean());
            }

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

            // 如果已有定位数据，根据现有数据筛选线条
            if (screen.QuadInCamera != null)
            {
                Debug.Log("[IdentifyLineLSD] 根据已有定位数据做筛选");
                screen.RefreshCameraSize(new Vector2(screenLocateMat.Size.x, screenLocateMat.Size.y));

                var calibration = ScreenLocate.Main.ReDoLocateCalibrationRatio * screenLocateMat.Size.y;
                oldLines = screen.QuadInCamera.GetLines();

                var pedals = oldLines.Select((i) => o0Extension.PointPedal(i, avgPoint, out _)).ToArray();     // 当前定位的垂足，下、右、上、左

                foreach (var i in allLines)
                {
                    float minDistance = float.MaxValue;
                    int index = -1;
                    foreach (var j in pedals.Index())
                    {
                        var d = (o0Extension.PointPedal(i.Item1, avgPoint, out _) - pedals[j]).Length;
                        if (d < minDistance)
                        {
                            minDistance = d;
                            index = j;
                        }
                    }
                    //Debug.Log(minDistance +", -----------"+ calibration);
                    if (minDistance < calibration)      // 垂足的距离足够近
                    {
                        quadLines[index].Add((estimateGradient(i.Item1), i.Item1));
                        possibleLines.Add(i.Item1);
                    }
                }
            }
            else
            {
                var avaAngleHalf = 75f;
                foreach (var (line, sum, gradient) in allLines)
                {
                    possibleLines.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 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((estimateGradient(line), line));
                    }
                }
            }

            var result = new Line[4];
            for (int i = 0; i < 4; i++)
            {
                if (quadLines[i].Count > 0)
                    result[i] = quadLines[i].Max((a, b) =>a.Item1.CompareTo(b.Item1)).Item2;
            }
            return result.ToList();
        }
    }
}