yichael_smartbow/Assets/InfraredProject/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 ZIM.Unity;
using o0.Num;
using o0.Geometry2D;

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

        // 返回四边形的四条边，List长度一定是4 (如果没有识别到就是null)，且线段顺序是: 下、右、上、左
        public static List<Line> ZIMIdentifyQuadLSD(this Matrix screenLocateMat, Matrix edgeMat, Matrix edgeDirMat,
            out Line[] oldLines, out List<Line> lightLines, ScreenMap screen, 
            float conSize, 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, LineCaptureSize: new Vector(0, 5));

            {
                // 将lines输出一张图片
                var LSDLineMap = new Matrix(new Geometry2D.Vector<int>((int)(edgeMat.Size.x + conSize - 1), (int)(edgeMat.Size.y + conSize - 1)), Tiling: true);
                foreach (var l in lines)
                {
                    if (l.Item1 != null)
                        o0Extension.DrawLine(LSDLineMap, l.Item1, (x, y) => 3, new Geometry2D.Float.Vector(0, 2), true); // 其他的备选线段
                }
                ScreenLocate.DebugTexture(1, LSDLineMap.ToTexRGBA(ScreenIdentification.FloatValueToColor));
            }

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

            // 沿直线计算平均梯度
            float averageGradient(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]);
                }
                return 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)>() };
            lightLines = 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)).ToArray();     // 当前定位的垂足，下、右、上、左

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