|
|
@@ -1,3 +1,4 @@
|
|
|
+using DbscanImplementation;
|
|
|
using o0;
|
|
|
using o0.Project;
|
|
|
using SixLabors.ImageSharp;
|
|
|
@@ -7,6 +8,7 @@ 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;
|
|
|
@@ -49,7 +51,7 @@ namespace ZIM
|
|
|
}
|
|
|
|
|
|
readonly float[] spotBrightness = new float[] { 0.93f, 0.5f }; // 亮点阈值
|
|
|
- readonly int samplingScale = 1;
|
|
|
+ int samplingScale = 1;
|
|
|
//const float circleVariance = 30f;
|
|
|
//const int brightAreaRadius = 30;
|
|
|
//const int LeastBrightPoint = 10000;
|
|
|
@@ -57,6 +59,8 @@ namespace ZIM
|
|
|
SLAMUVC.UVCManager.CameraInfo mCameraInfo;
|
|
|
ScreenIdentification screenIdentification;
|
|
|
InfraredSpotSettings infraredSpotSettings;
|
|
|
+ //KMeans kMeans;
|
|
|
+ PixelCheaker ScreenPixelCheaker;
|
|
|
|
|
|
InfraredSpot[] InfraredSpots;
|
|
|
public InfraredSpot GetSpot(InfraredMatch match)
|
|
|
@@ -78,12 +82,14 @@ namespace ZIM
|
|
|
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)
|
|
|
+ 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;
|
|
|
}
|
|
|
|
|
|
@@ -109,6 +115,9 @@ namespace ZIM
|
|
|
// 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] {
|
|
|
@@ -116,44 +125,17 @@ namespace ZIM
|
|
|
new InfraredSpot(screenIdentification.Screen, InfraredMatch.Match1) };
|
|
|
}
|
|
|
|
|
|
+ ScreenPixelCheaker.ImageSize = new Vector2(screenIdentification.Size.x, screenIdentification.Size.y);
|
|
|
+
|
|
|
//var watch = new System.Diagnostics.Stopwatch();
|
|
|
//watch.Start();
|
|
|
//var times = new List<double>() { 0.0 };
|
|
|
|
|
|
- /* 根据亮点情况调整samplingScale
|
|
|
-
|
|
|
- (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);
|
|
|
- int brightCount = 0;
|
|
|
- Parallel.For(rectMin.x, rectMax.x, (i) =>
|
|
|
- {
|
|
|
- for (int j = rectMin.y; j < rectMax.y; j++)
|
|
|
- {
|
|
|
- int ip = i * samplingScale;
|
|
|
- int jp = j * samplingScale;
|
|
|
- var index = camera.Vector2ToIndex(ip, jp);
|
|
|
- var b = pixels[index].Brightness(64);
|
|
|
-
|
|
|
- if (b > minBrightness)
|
|
|
- {
|
|
|
- lock (locker)
|
|
|
- {
|
|
|
- brightCount++;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- });
|
|
|
- if (brightCount > 1000)
|
|
|
- {
|
|
|
- samplingScale = (int)Math.Ceiling(samplingScale * Math.Sqrt(brightCount / 1000.0)); // 如果亮点太多,控制亮点数量在1000左右
|
|
|
- }
|
|
|
- /**/
|
|
|
-
|
|
|
(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>((int)rect.width * (int)rect.height / 256); // 预估的初始容量
|
|
|
- var brightPoint = new List<Vector2>((int)rect.width * (int)rect.height / 64);
|
|
|
+ var spotPoint = new List<Vector2>(200); // 预估的初始容量
|
|
|
+ var brightPoint = new List<Vector2>(1000);
|
|
|
|
|
|
Parallel.For(rectMin.x, rectMax.x, (i) =>
|
|
|
{
|
|
|
@@ -163,6 +145,10 @@ namespace ZIM
|
|
|
{
|
|
|
int ip = i * samplingScale;
|
|
|
int jp = j * samplingScale;
|
|
|
+
|
|
|
+ if (ScreenPixelCheaker.OutCollider2D(new Vector2(ip, jp)))
|
|
|
+ continue;
|
|
|
+
|
|
|
var index = mCameraInfo.CoordToIndex(ip, jp);
|
|
|
//var b = brightness[index];
|
|
|
//var b = (int)Math.Round(ConvBrightness(brightness, (ip, jp)));
|
|
|
@@ -183,6 +169,20 @@ namespace ZIM
|
|
|
brightPoint.AddRange(points1);
|
|
|
}
|
|
|
});
|
|
|
+
|
|
|
+ //Debug.Log("---spotPoint.Count: " + spotPoint.Count);
|
|
|
+ //Debug.Log(samplingScale);
|
|
|
+ if (spotPoint.Count > 200) // 如果亮点太多,控制亮点数量在200左右
|
|
|
+ {
|
|
|
+ samplingScale = (int)Math.Ceiling(samplingScale * Math.Sqrt(spotPoint.Count / 200.0));
|
|
|
+ return new List<PixelSpotArea>();
|
|
|
+ }
|
|
|
+ else if (samplingScale > 1 && spotPoint.Count < 20)
|
|
|
+ {
|
|
|
+ samplingScale = Math.Max((int)Math.Ceiling(samplingScale * Math.Sqrt(spotPoint.Count / 80.0)), 1);
|
|
|
+ return new List<PixelSpotArea>();
|
|
|
+ }
|
|
|
+
|
|
|
//Debug.Log("spotPoint Count: " + spotPoint.Count + ", brightPoint Count: " + brightPoint.Count);
|
|
|
|
|
|
//times.Add(watch.ElapsedMilliseconds);
|
|
|
@@ -192,60 +192,43 @@ namespace ZIM
|
|
|
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 texure = new Texture2D((int)screenIdentification.Screen.UVSize.x + 1, (int)screenIdentification.Screen.UVSize.y + 1);
|
|
|
- // var pixels2 = new Color[texure.width * texure.height];
|
|
|
- // foreach (var i in pixels2.Index())
|
|
|
- // pixels2[i] = Color.white;
|
|
|
-
|
|
|
- // foreach (var i in spotPoint)
|
|
|
- // {
|
|
|
- // if (!screenIdentification.Screen.UVInScreen(i))
|
|
|
- // continue;
|
|
|
- // pixels2[(int)i.y * texure.width + (int)i.x] = Color.red;
|
|
|
- // }
|
|
|
- // foreach (var i in brightPoint)
|
|
|
- // {
|
|
|
- // if (!screenIdentification.Screen.UVInScreen(i))
|
|
|
- // continue;
|
|
|
- // pixels2[(int)i.y * texure.width + (int)i.x] = Color.black;
|
|
|
- // }
|
|
|
- // texure.filterMode = FilterMode.Point;
|
|
|
- // texure.SetPixels(pixels2);
|
|
|
- // texure.Apply();
|
|
|
- // ScreenLocate.DebugTexture(2, texure);
|
|
|
- //}
|
|
|
+ // 筛掉屏幕外的点
|
|
|
+ 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 spotArea = PixelSpotArea.Cluster(spotPoint, brightPoint, screenIdentification.Screen.UVInScreen);
|
|
|
+ var db = Dbscan.Run(spotPoint, ZIM.Unity.Extension.LengthManhattan, samplingScale * 2, 3);
|
|
|
+ var spotArea = DbscanToSpotAreas(db, brightPoint);
|
|
|
|
|
|
- //times.Add(watch.ElapsedMilliseconds);
|
|
|
- //UnityEngine.Debug.Log("time3: " + (times[times.Count - 1] - times[times.Count - 2]));
|
|
|
+ if (ScreenLocate.Main.DebugOnEditorWin)
|
|
|
+ DebugAreas(spotArea);
|
|
|
|
|
|
- if (spotArea.Count == 0)
|
|
|
- return spotArea;
|
|
|
|
|
|
- // 亮度较低的部分合并到区域中
|
|
|
- for (int i = 0; i < brightPoint.Count; i++)
|
|
|
- {
|
|
|
- var p = brightPoint[i];
|
|
|
- var grid = PixelSpotArea.GetGrid1(p);
|
|
|
- for (int j = 0; j < spotArea.Count; j++)
|
|
|
- {
|
|
|
- var area = spotArea[j];
|
|
|
- if (area.Include1(grid))
|
|
|
- area.Add(p);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- //Debug.Log("Area Count" + spotArea.Count);
|
|
|
+ //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("time4: " + (times[times.Count - 1] - times[times.Count - 2]));
|
|
|
+ //UnityEngine.Debug.Log("time3: " + (times[times.Count - 1] - times[times.Count - 2]));
|
|
|
|
|
|
return spotArea;
|
|
|
|
|
|
@@ -253,7 +236,7 @@ namespace ZIM
|
|
|
foreach (var i in spotArea)
|
|
|
{
|
|
|
var r0 = i.Radius / 2 / mCameraInfo.CurrentWidth;
|
|
|
- var center = i.Center;
|
|
|
+ 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);
|
|
|
@@ -289,168 +272,81 @@ namespace ZIM
|
|
|
return new List<PixelSpotArea>();
|
|
|
}
|
|
|
|
|
|
- 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)
|
|
|
+ private void DebugAreas(List<PixelSpotArea> areas)
|
|
|
{
|
|
|
- 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) =>
|
|
|
+ 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)
|
|
|
{
|
|
|
- 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);
|
|
|
+ foreach (var p in i.Pixels0)
|
|
|
+ texture.SetPixel((int)p.Point.x, (int)p.Point.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;
|
|
|
+ }
|
|
|
|
|
|
- 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) });
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- });
|
|
|
+ texture.Apply();
|
|
|
+ ScreenLocate.DebugTexture(5, texture);
|
|
|
+ }
|
|
|
|
|
|
- //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;
|
|
|
- // }
|
|
|
- //});
|
|
|
+ private List<PixelSpotArea> DbscanToSpotAreas(DbscanResult<Vector2> db, List<Vector2> brightPoint)
|
|
|
+ {
|
|
|
+ if (db.Clusters.Count == 0)
|
|
|
+ return new List<PixelSpotArea>();
|
|
|
|
|
|
- if (brightPixelDic.Count > 0)
|
|
|
+ // 用dbscan的结果创建SpotArea,先用高亮区算一遍半径
|
|
|
+ var clusters = new List<PixelSpotArea>();
|
|
|
+ var areaIncludeRadius = new List<(Vector2, float)>();
|
|
|
+ foreach (var i in db.Clusters.Values)
|
|
|
{
|
|
|
- // 取亮度最高的像素
|
|
|
- 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);
|
|
|
- }
|
|
|
+ var area = new PixelSpotArea(i);
|
|
|
+ areaIncludeRadius.Add((area.Centroid, area.Radius));
|
|
|
+ clusters.Add(area);
|
|
|
+ }
|
|
|
+ foreach (var i in db.Noise)
|
|
|
+ areaIncludeRadius.Add((i.Feature, 0));
|
|
|
|
|
|
- //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));
|
|
|
- }
|
|
|
+ // 将泛光点添加到最近的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();
|
|
|
|
|
|
- //var sum = new Vector2(0, 0);
|
|
|
- //foreach (var i in brightPixels)
|
|
|
- //{
|
|
|
- // sum += i;
|
|
|
- //}
|
|
|
- //var middle = sum/brightPixels.Count;
|
|
|
- //Debug.Log("brightArea.Count: " + brightArea.Count);
|
|
|
+ return clusters;
|
|
|
+ }
|
|
|
|
|
|
- //if (brightArea.Count <= 1)
|
|
|
- // return brightArea.FirstOrDefault().Center;
|
|
|
+ private int FindNearestAreaIndex(Vector2 a, List<(Vector2, float)> areas)
|
|
|
+ {
|
|
|
+ if (areas == null || areas.Count == 0)
|
|
|
+ return -1;
|
|
|
|
|
|
- //PixelArea maxBrightArea = brightArea.First();
|
|
|
- //float maxCircleBrightness = 10000f;
|
|
|
- var CircleAreas = new List<PixelCircleArea>();
|
|
|
+ int nearestIndex = 0;
|
|
|
+ float nearestDistance = Vector2.Distance(a, areas[0].Item1) - areas[0].Item2;
|
|
|
|
|
|
- for (int i = 0; i < brightArea.Count; i++)
|
|
|
+ for (int i = 1; i < areas.Count; i++)
|
|
|
+ {
|
|
|
+ float distance = Vector2.Distance(a, areas[i].Item1) - areas[i].Item2;
|
|
|
+ if (distance < nearestDistance)
|
|
|
{
|
|
|
- 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;
|
|
|
- //}
|
|
|
+ nearestDistance = distance;
|
|
|
+ nearestIndex = i;
|
|
|
}
|
|
|
- //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;
|
|
|
+ return nearestIndex;
|
|
|
}
|
|
|
|
|
|
InfraredSpot[] MatchInfraredRaySingle(List<PixelSpotArea> spotArea)
|
|
|
@@ -632,7 +528,7 @@ namespace ZIM
|
|
|
{
|
|
|
if (i != overlapAera)
|
|
|
{
|
|
|
- var distance = (i.Center - overlapAera.Center).magnitude;
|
|
|
+ var distance = (i.Centroid - overlapAera.Centroid).magnitude;
|
|
|
if (distance < overlapAera.Radius * 2 && distance < split.Item2)
|
|
|
{
|
|
|
split = (i, distance);
|
|
|
@@ -649,7 +545,7 @@ namespace ZIM
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- if ((InfraredSpots[1].Predict.Value - overlapAera.Center).magnitude > overlapAera.Radius / 2)
|
|
|
+ if ((InfraredSpots[1].Predict.Value - overlapAera.Centroid).magnitude > overlapAera.Radius / 2)
|
|
|
InfraredSpots[1].Disappear = true;
|
|
|
|
|
|
//InfraredSpots[0].Update(overlapAera);
|
|
|
@@ -703,6 +599,170 @@ namespace ZIM
|
|
|
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;
|