smart-bow-infrared/Assets/InfraredProject/WebCamera/Script/ZIM/InfraredLocate/InfraredLocate.cs

using DbscanImplementation;
using o0;
using o0.Project;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.Serialization.Formatters;
using System.Threading.Tasks;
using UnityEngine;
using UnityEngine.UI;
using ZIM.Unity;
using Color = UnityEngine.Color;
using Debug = UnityEngine.Debug;

namespace ZIM
{
    //class DescendingComparer<T> : IComparer<T>
    //{
    //    public int Compare(T x, T y)
    //    {
    //        return Comparer<T>.Default.Compare(y, x);
    //    }
    //}

    public enum InfraredMatch
    {
        Nomatch = 0,
        Match0 = 1,
        Match1 = 2
    }

    // 支持2个红外点的识别
    public class InfraredLocate
    {
        public static object locker = new object();

        public static List<InfraredMatch> GetMatches(InfraredMatch im)
        {
            List<InfraredMatch> positions = new List<InfraredMatch>();
            InfraredMatch bit = InfraredMatch.Match0;
            while (bit <= im)
            {
                if ((im & bit) != 0)
                {
                    positions.Add(bit);
                }
                bit = (InfraredMatch)((int)bit << 1);
            }
            return positions;
        }

        readonly float[] spotBrightness = new float[] { 0.93f, 0.5f };      // 亮点阈值
        int samplingScale = 1;
        //const float circleVariance = 30f;
        //const int brightAreaRadius = 30; 
        //const int LeastBrightPoint = 10000;

        SLAMUVC.UVCManager.CameraInfo mCameraInfo;
        ScreenIdentification screenIdentification;
        InfraredSpotSettings infraredSpotSettings;
        //KMeans kMeans;
        PixelCheaker ScreenPixelCheaker;

        InfraredSpot[] InfraredSpots;
        public InfraredSpot GetSpot(InfraredMatch match)
        {
            switch (match)
            {
                case InfraredMatch.Match0:
                    return InfraredSpots[0];
                case InfraredMatch.Match1:
                    return InfraredSpots[1];
                default:
                    return null;
            }
        }

        // 参数是 红外灯的亮度阈值，阈值越小能够检测到的亮度就越低
        public void SetBrightnessThreshold(float brightnessThreshold = 0.93f)
        {
            spotBrightness[0] = brightnessThreshold; spotBrightness[1] = (float)Math.Min(Math.Exp(1.5 * brightnessThreshold - 1.8), brightnessThreshold);       // 周围泛光的亮度用指数函数直接算
        }

        public InfraredLocate(SLAMUVC.UVCManager.CameraInfo cameraInfo, ScreenIdentification infraredIdentification, InfraredSpotSettings infraredSpotSettings, PixelCheaker screenPixelCheaker)
        {
            this.mCameraInfo = cameraInfo;
            this.screenIdentification = infraredIdentification;
            this.infraredSpotSettings = infraredSpotSettings;
            this.ScreenPixelCheaker = screenPixelCheaker;

            //this.kMeans = new KMeans();
            //samplingScale = 2;
        }

        readonly int CheakFrame = 10;

        bool Infrared12Overlap = false;
        int cheakCounter = 0;

        public InfraredSpot[] UpdateSingle(Color[] cameraPixels)
        {
            // Debug.Log(cameraPixels.Length + ",  " + screenIdentification.Screen.QuadRect + " -----------------");
            var spotArea = LocateToScreen(cameraPixels, screenIdentification.Screen.QuadRect);
            return MatchInfraredRaySingle(spotArea);
        }

        // New, 通过透视映射计算红外点的相对位置, 返回的红外点根据半径 从大到小排序
        public InfraredSpot[] Update(Color[] cameraPixels)
        {
            var spotArea = LocateToScreen(cameraPixels, screenIdentification.Screen.QuadRect);
            return MatchInfraredRay(spotArea);
        }

        // New, 返回由大到小排序的点
        public List<PixelSpotArea> LocateToScreen(Color[] pixels, Rect rect)
        {
            if (!screenIdentification.Screen.Active)
                rect = new Rect(0, 0, screenIdentification.Size.x, screenIdentification.Size.y);

            if (InfraredSpots == null)
            {
                InfraredSpots = new InfraredSpot[2] {
                new InfraredSpot(screenIdentification.Screen, InfraredMatch.Match0),
                new InfraredSpot(screenIdentification.Screen, InfraredMatch.Match1) };
            }

            //var watch = new System.Diagnostics.Stopwatch();
            //watch.Start();
            //var times = new List<double>() { 0.0 };

            (int x, int y) rectMin = ((int)rect.min.x / samplingScale, (int)rect.min.y / samplingScale);
            (int x, int y) rectMax = ((int)rect.max.x / samplingScale, (int)rect.max.y / samplingScale);

            var spotPoint = new List<Vector2>(200);     // 预估的初始容量
            var brightPoint = new List<Vector2>(1000);

            Parallel.For(rectMin.x, rectMax.x, (i) =>
            {
                var points0 = new List<Vector2>(rectMax.y - rectMin.y);
                var points1 = new List<Vector2>(rectMax.y - rectMin.y);
                for (int j = rectMin.y; j < rectMax.y; j++)
                {
                    int ip = i * samplingScale;
                    int jp = j * samplingScale;

                    if (!screenIdentification.Screen.Active && ScreenPixelCheaker.OutArea2D(new Vector2(ip, jp), screenIdentification.Size))
                        continue;

                    var index = mCameraInfo.CoordToIndex(ip, jp);
                    //var b = brightness[index];
                    //var b = (int)Math.Round(ConvBrightness(brightness, (ip, jp)));
                    var b = pixels[index].Brightness();

                    if (b > spotBrightness[0])
                    {
                        points0.Add(new Vector2(ip, jp));
                    }
                    else if (b > spotBrightness[1])
                    {
                        points1.Add(new Vector2(ip, jp));
                    }
                }
                lock (spotPoint)
                {
                    spotPoint.AddRange(points0);
                    brightPoint.AddRange(points1);
                }
            });

            if (ScreenLocate.Main.DebugOnZIMDemo)
            {
                if (spotPoint.Count > 400)     // 如果亮点太多，控制亮点数量在200左右
                {
                    samplingScale = (int)Math.Ceiling(samplingScale * Math.Sqrt(spotPoint.Count / 400.0));
                    return new List<PixelSpotArea>();
                }
                else if (samplingScale > 1 && spotPoint.Count < 100)
                {
                    samplingScale = Math.Max((int)Math.Ceiling(samplingScale * Math.Sqrt(spotPoint.Count / 200.0)), 1);
                    return new List<PixelSpotArea>();
                }
            }

            //times.Add(watch.ElapsedMilliseconds);
            //UnityEngine.Debug.Log("time1: " + (times[times.Count - 1] - times[times.Count - 2]));

            // 所有点映射到屏幕空间
            Parallel.For(0, spotPoint.Count, (i) => spotPoint[i] = screenIdentification.Screen.TransformToScreen(spotPoint[i]));
            Parallel.For(0, brightPoint.Count, (i) => brightPoint[i] = screenIdentification.Screen.TransformToScreen(brightPoint[i]));

            // 筛掉屏幕外的点
            var temp0 = new List<Vector2>(spotPoint.Count);
            var temp1 = new List<Vector2>(spotPoint.Count);
            foreach (var p in spotPoint)
            {
                if (screenIdentification.Screen.UVInScreen(p))
                    temp0.Add(p);
            }
            foreach (var p in brightPoint)
            {
                if (screenIdentification.Screen.UVInScreen(p))
                    temp1.Add(p);
            }
            spotPoint = temp0;
            brightPoint = temp1;

            if (spotPoint.Count > 0)
            {
                //times.Add(watch.ElapsedMilliseconds);
                //UnityEngine.Debug.Log("time2: " + (times[times.Count - 1] - times[times.Count - 2]));

                //var db = Dbscan.Run(spotPoint, ZIM.Unity.ZIMMath.LengthManhattan, samplingScale * 2, 3);
                //var spotArea = DbscanToSpotAreas(db, brightPoint);
                var spotArea = PixelSpotArea.Cluster(spotPoint, brightPoint, screenIdentification.Screen.UVInScreen);

                if (ScreenLocate.Main.DebugOnZIMDemo)
                    DebugAreas(spotArea);


                //Debug.Log("db: " + db.Clusters.Count);
                //Debug.Log("db noise: " + db.Noise.Count);
                //foreach (var i in spotArea)
                //{
                //    Debug.Log("i.Radius" + i.Radius);
                //}

                //times.Add(watch.ElapsedMilliseconds);
                //UnityEngine.Debug.Log("time3: " + (times[times.Count - 1] - times[times.Count - 2]));

                return spotArea;

                //半径再按透视修正一遍，降低一点常规角度下反射的影响
                foreach (var i in spotArea)
                {
                    var r0 = i.Radius / 2 / mCameraInfo.CurrentWidth;
                    var center = i.Centroid;
                    var offset1 = center + new Vector2(i.Radius / 2, 0);
                    var offset2 = center - new Vector2(i.Radius / 2, 0);
                    var offset3 = center + new Vector2(0, i.Radius / 2);
                    var offset4 = center - new Vector2(0, i.Radius / 2);
                    var transR = ((screenIdentification.Screen.TransformToScreen(offset1) - screenIdentification.Screen.TransformToScreen(offset2)).magnitude +
                        (screenIdentification.Screen.TransformToScreen(offset3) - screenIdentification.Screen.TransformToScreen(offset4)).magnitude) / 4;
                    var r1 = transR / screenIdentification.Screen.UVSize.x;
                    //Debug.Log(r1 / r0);
                    i.Radius *= (float)Math.Pow(r1 / r0, 2);      // 摄像机位置不同参数也可能不同
                }

                //if (spotArea.Count == 1)
                //Debug.Log($"{spotArea[0].MaxRadius}");

                return spotArea;

                //// 排序亮区
                //spotArea.Sort((a, b) => b.MaxRadius.CompareTo(a.MaxRadius));
                ////var areas = new SortedList<float, PixelSpotArea>(new DescendingComparer<float>());
                ////foreach (var i in spotArea)
                ////    areas.Add(i.MaxRadius, i);

                //var result = new Vector2[spotArea.Count];
                //foreach (var i in spotArea.Index())
                //    result[i] = spotArea[i].Center;

                //times.Add(watch.ElapsedMilliseconds);
                //UnityEngine.Debug.Log("time6: " + (times[times.Count - 1] - times[times.Count - 2]));

                //return result;
            }

            return new List<PixelSpotArea>();
        }

        private void DebugAreas(List<PixelSpotArea> areas)
        {
            Texture2D texture = new Texture2D(ScreenLocate.Main.CameraSize.x, ScreenLocate.Main.CameraSize.y);
            Color[] blackPixels = new Color[texture.width * texture.height];
            for (int i = 0; i < blackPixels.Length; i++)
                blackPixels[i] = Color.black;
            texture.SetPixels(blackPixels);

            Color[] colors = new Color[4] { Color.yellow, Color.white, Color.green, Color.blue };
            int index = 0;
            foreach (var i in areas)
            {
                foreach (var p in i.Pixels0)
                    texture.SetPixel((int)p.x, (int)p.y, colors[index]);
                index++;
                foreach (var p in i.Pixels1)
                    texture.SetPixel((int)p.x, (int)p.y, colors[index]);
                index++;
                if (index >= 4)
                    break;
            }

            texture.Apply();
            ScreenLocate.DebugTexture(6, texture);
        }

        private List<PixelSpotArea_DbScan> DbscanToSpotAreas(DbscanResult<Vector2> db, List<Vector2> brightPoint)
        {
            if (db.Clusters.Count == 0)
                return new List<PixelSpotArea_DbScan>();

            // 用dbscan的结果创建SpotArea，先用高亮区算一遍半径
            var clusters = new List<PixelSpotArea_DbScan>();
            var areaIncludeRadius = new List<(Vector2, float)>();
            foreach (var i in db.Clusters.Values)
            {
                var area = new PixelSpotArea_DbScan(i);
                areaIncludeRadius.Add((area.Centroid, area.Radius));
                clusters.Add(area);
            }
            foreach (var i in db.Noise)
                areaIncludeRadius.Add((i.Feature, 0));

            // 将泛光点添加到最近的area
            foreach (var i in brightPoint)
            {
                var index = FindNearestAreaIndex(i, areaIncludeRadius);
                if (index < clusters.Count)
                    clusters[index].Add1(i);
            }
            // 添加了泛光点后，重新计算半径
            foreach (var i in clusters)
                i.Radius = i.CalculateRadius();

            return clusters;
        }

        private int FindNearestAreaIndex(Vector2 a, List<(Vector2, float)> areas)
        {
            if (areas == null || areas.Count == 0)
                return -1;

            int nearestIndex = 0;
            float nearestDistance = Vector2.Distance(a, areas[0].Item1) - areas[0].Item2;

            for (int i = 1; i < areas.Count; i++)
            {
                float distance = Vector2.Distance(a, areas[i].Item1) - areas[i].Item2;
                if (distance < nearestDistance)
                {
                    nearestDistance = distance;
                    nearestIndex = i;
                }
            }
            return nearestIndex;
        }

        InfraredSpot[] MatchInfraredRaySingle(List<PixelSpotArea> spotArea)
        {
            var matchedArea = new Dictionary<InfraredMatch, PixelSpotArea>() { { InfraredMatch.Match0, null } };
            var v0 = InfraredSpots[0].Verify(spotArea, matchedArea);

            if (v0)
            {
                // 每隔20帧检查异常
                //if (++cheakCounter >= CheakFrame)
                //{
                //    var maxArea = spotArea.Max((a, b) => a.Radius.CompareTo(b.Radius));
                //    cheakCounter = 0;
                //    if (matchedArea[InfraredMatch.Match0] != maxArea)
                //    {
                //        InfraredSpots[0].Reset();       // 防止异常
                //    }
                //}
            }
            else
            {
                if (spotArea.Count > 0)
                {
                    matchedArea[InfraredMatch.Match0] = spotArea.Max((a, b) => a.Radius.CompareTo(b.Radius));
                }
            }

            foreach (var i in matchedArea)
                GetSpot(i.Key).Update(i.Value);

            return InfraredSpots;
        }

        InfraredSpot[] MatchInfraredRay(List<PixelSpotArea> spotArea)
        {
            var matchedArea = new Dictionary<InfraredMatch, PixelSpotArea>() { { InfraredMatch.Match0, null }, { InfraredMatch.Match1, null } };
            var v0 = InfraredSpots[0].Verify(spotArea, matchedArea);
            var v1 = InfraredSpots[1].Verify(spotArea, matchedArea);

            if (!Infrared12Overlap)
            {
                if (!v0 && !v1)
                {
                    //Application.targetFrameRate = 1;
                    //Debug.Log($"{Time.time}全失败 spotArea {spotArea.Count}");
                    if (spotArea.Count == 0)
                    {
                        //InfraredSpots[0].UpdateByPredict();
                        //InfraredSpots[1].UpdateByPredict();
                    }
                    else if (spotArea.Count == 1)
                    {
                        if (InfraredSpots[0].Predict != null && InfraredSpots[1].Predict == null)
                        {
                            matchedArea[InfraredMatch.Match0] = spotArea[0];
                            //InfraredSpots[0].Update(spotArea[0]);
                            //InfraredSpots[1].UpdateByPredict();
                        }
                        else if (InfraredSpots[1].Predict != null && InfraredSpots[0].Predict == null)
                        {
                            matchedArea[InfraredMatch.Match1] = spotArea[0];
                        }
                        else
                        {
                            if (spotArea[0].Radius > infraredSpotSettings.RadiusThreshold)
                            {
                                matchedArea[InfraredMatch.Match0] = spotArea[0];
                            }
                            else
                            {
                                matchedArea[InfraredMatch.Match1] = spotArea[0];
                            }
                        }
                    }
                    else
                    {
                        spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius));                    // 排序亮区
                        matchedArea[InfraredMatch.Match0] = spotArea[0];
                        matchedArea[InfraredMatch.Match1] = spotArea[1];
                    }
                }
                else
                {
                    PixelSpotArea select = null;
                    PixelSpotArea selectOther = null;
                    InfraredMatch oneFailed = InfraredMatch.Nomatch;
                    if (!v0)
                    {
                        select = matchedArea[InfraredMatch.Match1];
                        //InfraredSpots[1].Update(select);
                        oneFailed = InfraredMatch.Match0;
                    }
                    else if (!v1)
                    {
                        select = matchedArea[InfraredMatch.Match0];
                        //InfraredSpots[0].Update(select);
                        oneFailed = InfraredMatch.Match1;
                    }

                    if (oneFailed != InfraredMatch.Nomatch)
                    {
                        //Debug.LogWarning($"{Time.time}成功1个 {oneFailed}");
                        spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius));                        // 排序亮区
                        foreach (var i in spotArea)
                        {
                            if (i != select)
                            {
                                selectOther = i;
                                break;
                            }
                        }
                        matchedArea[oneFailed] = selectOther;
                        //if (selectOther == null)
                        //{
                        //    GetSpot(oneFailed).UpdateByPredict();
                        //}
                        //else
                        //{
                        //    GetSpot(oneFailed).Update(selectOther);
                        //}
                    }
                    else
                    {
                        //Debug.LogWarning($"{Time.time}成功2个");
                        if (matchedArea[InfraredMatch.Match0] == matchedArea[InfraredMatch.Match1])
                        {
                            // 重叠
                            Infrared12Overlap = true;
                            //if (spotArea.Count == 1)
                            //{
                            //    Infrared12Overlap = true;

                            //    InfraredSpots[0].Update(spotArea[0]);
                            //    InfraredSpots[1].UpdateByPredict();
                            //    return InfraredSpots;
                            //}
                            //else
                            //{
                            //    Infrared12Overlap = false;
                            //    spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius));                    // 排序亮区
                            //    InfraredSpots[0].Update(spotArea[0]);
                            //    InfraredSpots[1].Update(spotArea[1]);
                            //}
                        }
                        else //if (matchedArea[InfraredMatch.Match1] != matchedArea[InfraredMatch.Match2])
                        {
                            //Infrared12Overlap = false;
                            //InfraredSpots[0].Update(matchedArea[InfraredMatch.Match0]);
                            //InfraredSpots[1].Update(matchedArea[InfraredMatch.Match1]);

                            // 每隔20帧更新阈值
                            if (++cheakCounter >= CheakFrame)
                            {
                                //Debug.Log($"{Time.time}更新阈值2个");
                                var middle = (matchedArea[InfraredMatch.Match0].Radius + matchedArea[InfraredMatch.Match1].Radius) / 2;
                                infraredSpotSettings.UpdateThreshold(middle);
                                cheakCounter = 0;
                                if (matchedArea[InfraredMatch.Match0].Radius < middle)
                                {
                                    //Debug.Log($"{Time.time}大小异常");
                                    InfraredSpots[0].Reset();       // 防止异常
                                    InfraredSpots[1].Reset();
                                }
                            }
                        }
                    }

                }
            }

            if (Infrared12Overlap)
            {
                if (v0)
                {
                    var overlapAera = matchedArea[InfraredMatch.Match0];
                    (PixelSpotArea, float) split = (null, float.MaxValue);
                    foreach (var i in spotArea)
                    {
                        if (i != overlapAera)
                        {
                            var distance = (i.Centroid - overlapAera.Centroid).magnitude;
                            if (distance < overlapAera.Radius * 2 && distance < split.Item2)
                            {
                                split = (i, distance);
                            }
                        }
                    }
                    if (split.Item1 != null)
                    {
                        //InfraredSpots[0].Update(overlapAera);
                        //InfraredSpots[1].Update(split.Item1);
                        matchedArea[InfraredMatch.Match1] = split.Item1;
                        InfraredSpots[1].Disappear = false;
                        Infrared12Overlap = false;
                    }
                    else
                    {
                        if ((InfraredSpots[1].Predict.Value - overlapAera.Centroid).magnitude > overlapAera.Radius / 2)
                            InfraredSpots[1].Disappear = true;

                        //InfraredSpots[0].Update(overlapAera);
                        if (InfraredSpots[1].Predict == null || InfraredSpots[1].Disappear)
                        {
                            //InfraredSpots[1].Update(overlapAera);
                            matchedArea[InfraredMatch.Match1] = overlapAera;
                            InfraredSpots[1].Predict = null;
                        }
                        else
                        {
                            matchedArea[InfraredMatch.Match1] = null;
                            //InfraredSpots[1].UpdateByPredict();
                        }

                        //if (++cheakCounter >= 20 * cheakFrame)
                        //{
                        //    cheakCounter = 0;
                        //    InfraredSpots[1].Reset();
                        //    Infrared12Overlap = false;
                        //}
                    }
                }
                else
                {
                    //InfraredSpots[0].UpdateByPredict();
                    //InfraredSpots[1].UpdateByPredict();
                    matchedArea[InfraredMatch.Match1] = null;
                }
            }

            foreach (var i in matchedArea)
                GetSpot(i.Key).Update(i.Value);

            return InfraredSpots;
        }

        // 亮度图是64位整形，返回卷积后的均值是float
        float ConvBrightness(int[] pixels, (int x, int y) point, int kernel_size = 3)
        {
            var sum = 0f;
            for (int i = point.x - kernel_size / 2; i <= point.x + kernel_size / 2; i++)
            {
                for (int j = point.y - kernel_size / 2; j <= point.y + kernel_size / 2; j++)
                {
                    var index = mCameraInfo.CoordToIndex(i, j);
                    if (index > 0 && index < pixels.Length)       // 超出边缘不计算
                        sum += pixels[index];
                }
            }
            return sum / (kernel_size * kernel_size);
        }

        public List<Vector2> GetOld(int[] brightness)        // 取整后的亮度图
        {
            return LocateOld(brightness, new Rect(0, 0, mCameraInfo.CurrentWidth, mCameraInfo.CurrentHeight));
        }
        public List<Vector2> LocateOld(int[] brightness, Rect rect)
        {
            var brightPixelDic = new Dictionary<float, List<Vector2>>();
            (int x, int y) origin = ((int)rect.min.x + 1, (int)rect.min.y + 1);
            Parallel.For(origin.x / samplingScale, (origin.x + (int)rect.width) / samplingScale, (i) =>
            {
                for (int j = origin.y / samplingScale; j < (origin.y + rect.height) / samplingScale; j++)
                {
                    int ip = i * samplingScale;
                    int jp = j * samplingScale;
                    var index = mCameraInfo.CoordToIndex(ip, jp);
                    //var b = brightness[index];
                    //var b = (int)Math.Round(ConvBrightness(brightness, (ip, jp)));
                    var b = ConvBrightness(brightness, (ip, jp), 3);

                    if (b > 32)
                    {
                        lock (brightPixelDic)
                        {
                            if (brightPixelDic.TryGetValue(b, out List<Vector2> list))
                                list.Add(new Vector2(ip, jp));
                            else
                                brightPixelDic.Add(b, new List<Vector2> { new Vector2(ip, jp) });
                        }
                    }
                }
            });

            //Parallel.For(0, pixels.Length / 7, (i) =>
            //{
            //    i = i * 7;
            //    var b = getBrightness(pixels[i]);
            //    if (b >= maxBrightness)
            //    {
            //        lock (brightSpots)
            //        {
            //            if (b != maxBrightness)
            //                brightSpots.Clear();
            //            brightSpots.Add(indexToVector2(i));
            //        }
            //        maxBrightness = b;
            //    }
            //});

            if (brightPixelDic.Count > 0)
            {
                // 取亮度最高的像素
                var keys = brightPixelDic.Keys.ToList();
                var maxIndex = o0.o0.MaxIndex(keys);
                //keys.Sort((a, b) => -a.CompareTo(b));
                var brightPixels = new List<Vector2>();
                for (int i = 0; i < Math.Min(1, keys.Count); i++)
                    brightPixels.AddRange(brightPixelDic[keys[maxIndex]]);

                //Debug.Log("max brightness: " + keys[maxIndex]);
                //Debug.Log("brightPixels.Count: " + brightPixels.Count);

                //var testTexture = new Texture2D((int)_textureSize.x, (int)_textureSize.y);
                //testTexture.wrapMode = TextureWrapMode.Clamp;
                //testTexture.filterMode = FilterMode.Point;
                //foreach (var i in brightPixels)
                //    testTexture.SetPixel((int)i.x, (int)i.y, Color.black);
                //testTexture.Apply();
                //testImage.texture = testTexture;

                var brightArea = new List<PixelCircleArea>() { new PixelCircleArea(brightPixels.First()) };
                for (int i = 1; i < brightPixels.Count; i++)
                {
                    var p = brightPixels[i];
                    var grid = PixelArea.GetGrid(p);
                    var join = new SortedList<int, PixelCircleArea>();
                    for (int j = 0; j < brightArea.Count; j++)
                    {
                        var area = brightArea[j];
                        if (area.Include(grid))
                            join.Add(j, area);
                    }
                    if (join.Count == 0)
                        brightArea.Add(new PixelCircleArea(p));
                    else if (join.Count == 1)
                        join.First().Value.Add(p, grid);
                    else
                    {
                        var combine = new PixelCircleArea(join.Values);
                        combine.Add(p, grid);
                        brightArea.Add(combine);
                        for (int j = join.Count - 1; j >= 0; j--)
                            brightArea.RemoveAt(join.ElementAt(j).Key);
                    }

                    //foreach (var j in brightArea)
                    //{
                    //    var offset = (p - j.Center);
                    //    if (offset.magnitude < brightAreaRadius)     // 距离近的并入该区域
                    //    {
                    //        j.Pixels.Add(p);
                    //        j.Center += offset / j.Pixels.Count;
                    //        join = true;
                    //        break;
                    //    }
                    //}
                    //if (!join)
                    //    brightArea.Add(new PixelArea(p));
                }

                //var sum = new Vector2(0, 0);
                //foreach (var i in brightPixels)
                //{
                //    sum += i;
                //}
                //var middle = sum/brightPixels.Count;
                //Debug.Log("brightArea.Count: " + brightArea.Count);

                //if (brightArea.Count <= 1)
                //    return brightArea.FirstOrDefault().Center;

                //PixelArea maxBrightArea = brightArea.First();
                //float maxCircleBrightness = 10000f;
                var CircleAreas = new List<PixelCircleArea>();

                for (int i = 0; i < brightArea.Count; i++)
                {
                    var area = brightArea[i];
                    var value = area.CircleBrightness(out float variance, brightness, ConvBrightness, 3);
                    //Debug.Log(counter + "Variance: " + variance);
                    //Debug.Log(counter + "CircleBrightness: " + value);
                    //Debug.Log(counter + "CircleRadius: " + area.MaxRadius);

                    if (variance < 30)
                    {
                        CircleAreas.Add(area);
                    }

                    //if (maxCircleBrightness == 10000f) 
                    //{
                    //    maxCircleBrightness = value;
                    //    maxBrightArea = area;
                    //}
                    //else if (maxCircleBrightness < value || (maxCircleBrightness == value && area.Pixels.Count > maxBrightArea.Pixels.Count)) 
                    //{
                    //    maxCircleBrightness = value;
                    //    maxBrightArea = area;
                    //}
                }
                //Debug.Log("CricleBrightness: " + maxCircleBrightness);

                //Debug.Log(counter + "CircleAreas: " + CircleAreas.Count);
                //counter++;
                if (CircleAreas.Count == 0)
                    return null;
                if (CircleAreas.Count == 1)
                    return new List<Vector2> { CircleAreas[0].Center };

                CircleAreas.Sort((a, b) => b.MaxRadius.CompareTo(a.MaxRadius));
                return new List<Vector2> { CircleAreas[0].Center, CircleAreas[1].Center };
            }

            return null;
        }

        //(int max, int second) MaxAreaIndex(Color[] pixels, List<PixelArea> list)
        //{
        //    var maxValue = float.MinValue;
        //    var secondValue = float.MinValue;
        //    var max = 0;
        //    var second = 0;
        //    foreach (var i in list.Index())
        //    {
        //        var value = list[i].TotalBrightness(pixels, camera.Vector2ToIndex, samplingScale * 2);
        //        if (value > maxValue)
        //        {
        //            maxValue = value;
        //            max = i;
        //        }
        //        else if (maxValue > secondValue)
        //        {
        //            secondValue = value;
        //            second = i;
        //        }
        //    }
        //    return (max, second);
        //}

        //float CricleVariance(int[] pixels, PixelArea area, int kernel_size = 3)
        //{
        //    var hash = new HashSet<(int, int)>();
        //    foreach (var (cos,sin) in angleMathList)
        //    {
        //        var x = (int)Math.Round(area.Center.x + (area.MaxRadius + PixelArea.gridLength * 4) * cos);
        //        var y = (int)Math.Round(area.Center.y + (area.MaxRadius + PixelArea.gridLength * 4) * sin);
        //        hash.Add((x, y));
        //    }
        //    var avg = 0f;
        //    foreach (var p in hash)
        //    {
        //        var value = ConvBrightness(pixels, p, kernel_size);
        //        avg += value;
        //    }
        //    return avg / hash.Count;
        //}

        //class PixelArea
        //{
        //    public static int gridLength;
        //    public static (int x, int y) gridSize;
        //    public static (int x, int y) GetGrid(Vector2 v)
        //    {
        //        var m = (int)(v.x / gridLength);
        //        var n = (int)(v.y / gridLength);
        //        return (m, n);
        //    }

        //    public Vector2 Center;
        //    public List<Vector2> Pixels;

        //    public PixelArea(Vector2 location)
        //    {
        //        this.Center = location;
        //        this.Pixels = new List<Vector2> { location };
        //    }
        //}
    }
}