|
|
@@ -28,7 +28,7 @@ namespace ZIM
|
|
|
Match1 = 2
|
|
|
}
|
|
|
|
|
|
- // 支持2个红外点的识别
|
|
|
+ // 鏀�寔2涓�孩澶栫偣鐨勮瘑鍒�
|
|
|
public class InfraredLocate
|
|
|
{
|
|
|
public static object locker = new object();
|
|
|
@@ -48,8 +48,8 @@ namespace ZIM
|
|
|
return positions;
|
|
|
}
|
|
|
|
|
|
- readonly int samplingScale = 2;
|
|
|
- readonly float[] spotBrightness = new float[] { 0.93f, 0.5f }; // 亮点阈值
|
|
|
+ readonly float[] spotBrightness = new float[] { 0.93f, 0.5f }; // 浜�偣闃堝€�
|
|
|
+ readonly int samplingScale = 1;
|
|
|
//const float circleVariance = 30f;
|
|
|
//const int brightAreaRadius = 30;
|
|
|
//const int LeastBrightPoint = 10000;
|
|
|
@@ -72,11 +72,19 @@ namespace ZIM
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ // 鍙傛暟鏄� 绾㈠�鐏�殑浜�害闃堝€硷紝闃堝€艰秺灏忚兘澶熸�娴嬪埌鐨勪寒搴﹀氨瓒婁綆
|
|
|
+ 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(Serenegiant.UVC.UVCManager.CameraInfo cameraInfo, ScreenIdentification infraredIdentification, InfraredSpotSettings infraredSpotSettings)
|
|
|
{
|
|
|
this.mCameraInfo = cameraInfo;
|
|
|
this.screenIdentification = infraredIdentification;
|
|
|
this.infraredSpotSettings = infraredSpotSettings;
|
|
|
+
|
|
|
+ //samplingScale = 2;
|
|
|
}
|
|
|
|
|
|
readonly int CheakFrame = 10;
|
|
|
@@ -87,21 +95,17 @@ namespace ZIM
|
|
|
public InfraredSpot[] UpdateSingle(Color[] cameraPixels)
|
|
|
{
|
|
|
var spotArea = LocateToScreen(cameraPixels, screenIdentification.Screen.QuadRect);
|
|
|
- if (spotArea.Count > 0)
|
|
|
- {
|
|
|
- InfraredSpots[0].Update(spotArea.Max((a, b) => a.Radius.CompareTo(b.Radius)));
|
|
|
- }
|
|
|
- return InfraredSpots;
|
|
|
+ return MatchInfraredRaySingle(spotArea);
|
|
|
}
|
|
|
|
|
|
- // New, 通过透视映射计算红外点的相对位置, 返回的红外点根据半径 从大到小排序
|
|
|
+ // New, 閫氳繃閫忚�鏄犲皠璁$畻绾㈠�鐐圭殑鐩稿�浣嶇疆, 杩斿洖鐨勭孩澶栫偣鏍规嵁鍗婂緞 浠庡ぇ鍒板皬鎺掑簭
|
|
|
public InfraredSpot[] Update(Color[] cameraPixels)
|
|
|
{
|
|
|
var spotArea = LocateToScreen(cameraPixels, screenIdentification.Screen.QuadRect);
|
|
|
return MatchInfraredRay(spotArea);
|
|
|
}
|
|
|
|
|
|
- // New, 返回由大到小排序的点
|
|
|
+ // New, 杩斿洖鐢卞ぇ鍒板皬鎺掑簭鐨勭偣
|
|
|
public List<PixelSpotArea> LocateToScreen(Color[] pixels, Rect rect)
|
|
|
{
|
|
|
if (InfraredSpots == null)
|
|
|
@@ -115,7 +119,7 @@ namespace ZIM
|
|
|
//watch.Start();
|
|
|
//var times = new List<double>() { 0.0 };
|
|
|
|
|
|
- /* 根据亮点情况调整samplingScale
|
|
|
+ /* 鏍规嵁浜�偣鎯呭喌璋冩暣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);
|
|
|
@@ -140,14 +144,14 @@ namespace ZIM
|
|
|
});
|
|
|
if (brightCount > 1000)
|
|
|
{
|
|
|
- samplingScale = (int)Math.Ceiling(samplingScale * Math.Sqrt(brightCount / 1000.0)); // 如果亮点太多,控制亮点数量在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 spotPoint = new List<Vector2>((int)rect.width * (int)rect.height / 256); // 棰勪及鐨勫垵濮嬪�閲�
|
|
|
var brightPoint = new List<Vector2>((int)rect.width * (int)rect.height / 64);
|
|
|
|
|
|
Parallel.For(rectMin.x, rectMax.x, (i) =>
|
|
|
@@ -183,7 +187,7 @@ namespace ZIM
|
|
|
//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]));
|
|
|
|
|
|
@@ -224,7 +228,7 @@ namespace ZIM
|
|
|
if (spotArea.Count == 0)
|
|
|
return spotArea;
|
|
|
|
|
|
- // 亮度较低的部分合并到区域中
|
|
|
+ // 浜�害杈冧綆鐨勯儴鍒嗗悎骞跺埌鍖哄煙涓�
|
|
|
for (int i = 0; i < brightPoint.Count; i++)
|
|
|
{
|
|
|
var p = brightPoint[i];
|
|
|
@@ -244,7 +248,7 @@ namespace ZIM
|
|
|
|
|
|
return spotArea;
|
|
|
|
|
|
- //半径再按透视修正一遍,降低一点常规角度下反射的影响
|
|
|
+ //鍗婂緞鍐嶆寜閫忚�淇��涓€閬嶏紝闄嶄綆涓€鐐瑰父瑙勮�搴︿笅鍙嶅皠鐨勫奖鍝�
|
|
|
foreach (var i in spotArea)
|
|
|
{
|
|
|
var r0 = i.Radius / 2 / mCameraInfo.CurrentWidth;
|
|
|
@@ -257,7 +261,7 @@ namespace ZIM
|
|
|
(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); // 摄像机位置不同参数也可能不同
|
|
|
+ i.Radius *= (float)Math.Pow(r1 / r0, 2); // 鎽勫儚鏈轰綅缃�笉鍚屽弬鏁颁篃鍙�兘涓嶅悓
|
|
|
}
|
|
|
|
|
|
//if (spotArea.Count == 1)
|
|
|
@@ -265,7 +269,7 @@ namespace ZIM
|
|
|
|
|
|
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)
|
|
|
@@ -284,7 +288,7 @@ namespace ZIM
|
|
|
return new List<PixelSpotArea>();
|
|
|
}
|
|
|
|
|
|
- public List<Vector2> GetOld(int[] brightness) // 取整后的亮度图
|
|
|
+ public List<Vector2> GetOld(int[] brightness) // 鍙栨暣鍚庣殑浜�害鍥�
|
|
|
{
|
|
|
return LocateOld(brightness, new Rect(0, 0, mCameraInfo.CurrentWidth, mCameraInfo.CurrentHeight));
|
|
|
}
|
|
|
@@ -334,7 +338,7 @@ namespace ZIM
|
|
|
|
|
|
if (brightPixelDic.Count > 0)
|
|
|
{
|
|
|
- // 取亮度最高的像素
|
|
|
+ // 鍙栦寒搴︽渶楂樼殑鍍忕礌
|
|
|
var keys = brightPixelDic.Keys.ToList();
|
|
|
var maxIndex = o0.o0.MaxIndex(keys);
|
|
|
//keys.Sort((a, b) => -a.CompareTo(b));
|
|
|
@@ -381,7 +385,7 @@ namespace ZIM
|
|
|
//foreach (var j in brightArea)
|
|
|
//{
|
|
|
// var offset = (p - j.Center);
|
|
|
- // if (offset.magnitude < brightAreaRadius) // 距离近的并入该区域
|
|
|
+ // if (offset.magnitude < brightAreaRadius) // 璺濈�杩戠殑骞跺叆璇ュ尯鍩�
|
|
|
// {
|
|
|
// j.Pixels.Add(p);
|
|
|
// j.Center += offset / j.Pixels.Count;
|
|
|
@@ -448,6 +452,38 @@ namespace ZIM
|
|
|
return null;
|
|
|
}
|
|
|
|
|
|
+ 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 } };
|
|
|
@@ -459,7 +495,7 @@ namespace ZIM
|
|
|
if (!v0 && !v1)
|
|
|
{
|
|
|
//Application.targetFrameRate = 1;
|
|
|
- //Debug.Log($"{Time.time}全失败 spotArea {spotArea.Count}");
|
|
|
+ //Debug.Log($"{Time.time}鍏ㄥけ璐� spotArea {spotArea.Count}");
|
|
|
if (spotArea.Count == 0)
|
|
|
{
|
|
|
//InfraredSpots[0].UpdateByPredict();
|
|
|
@@ -491,7 +527,7 @@ namespace ZIM
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius)); // 排序亮区
|
|
|
+ spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius)); // 鎺掑簭浜�尯
|
|
|
matchedArea[InfraredMatch.Match0] = spotArea[0];
|
|
|
matchedArea[InfraredMatch.Match1] = spotArea[1];
|
|
|
}
|
|
|
@@ -516,8 +552,8 @@ namespace ZIM
|
|
|
|
|
|
if (oneFailed != InfraredMatch.Nomatch)
|
|
|
{
|
|
|
- //Debug.LogWarning($"{Time.time}成功1个 {oneFailed}");
|
|
|
- spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius)); // 排序亮区
|
|
|
+ //Debug.LogWarning($"{Time.time}鎴愬姛1涓� {oneFailed}");
|
|
|
+ spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius)); // 鎺掑簭浜�尯
|
|
|
foreach (var i in spotArea)
|
|
|
{
|
|
|
if (i != select)
|
|
|
@@ -538,10 +574,10 @@ namespace ZIM
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- //Debug.LogWarning($"{Time.time}成功2个");
|
|
|
+ //Debug.LogWarning($"{Time.time}鎴愬姛2涓�");
|
|
|
if (matchedArea[InfraredMatch.Match0] == matchedArea[InfraredMatch.Match1])
|
|
|
{
|
|
|
- // 重叠
|
|
|
+ // 閲嶅彔
|
|
|
Infrared12Overlap = true;
|
|
|
//if (spotArea.Count == 1)
|
|
|
//{
|
|
|
@@ -554,7 +590,7 @@ namespace ZIM
|
|
|
//else
|
|
|
//{
|
|
|
// Infrared12Overlap = false;
|
|
|
- // spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius)); // 排序亮区
|
|
|
+ // spotArea.Sort((a, b) => b.Radius.CompareTo(a.Radius)); // 鎺掑簭浜�尯
|
|
|
// InfraredSpots[0].Update(spotArea[0]);
|
|
|
// InfraredSpots[1].Update(spotArea[1]);
|
|
|
//}
|
|
|
@@ -565,17 +601,17 @@ namespace ZIM
|
|
|
//InfraredSpots[0].Update(matchedArea[InfraredMatch.Match0]);
|
|
|
//InfraredSpots[1].Update(matchedArea[InfraredMatch.Match1]);
|
|
|
|
|
|
- // 每隔20帧更新阈值
|
|
|
+ // 姣忛殧20甯ф洿鏂伴槇鍊�
|
|
|
if (++cheakCounter >= CheakFrame)
|
|
|
{
|
|
|
- //Debug.Log($"{Time.time}更新阈值2个");
|
|
|
+ //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(); // 防止异常
|
|
|
+ //Debug.Log($"{Time.time}澶у皬寮傚父");
|
|
|
+ InfraredSpots[0].Reset(); // 闃叉�寮傚父
|
|
|
InfraredSpots[1].Reset();
|
|
|
}
|
|
|
}
|
|
|
@@ -650,7 +686,7 @@ namespace ZIM
|
|
|
return InfraredSpots;
|
|
|
}
|
|
|
|
|
|
- // 亮度图是64位整形,返回卷积后的均值是float
|
|
|
+ // 浜�害鍥炬槸64浣嶆暣褰�紝杩斿洖鍗风Н鍚庣殑鍧囧€兼槸float
|
|
|
float ConvBrightness(int[] pixels, (int x, int y) point, int kernel_size = 3)
|
|
|
{
|
|
|
var sum = 0f;
|
|
|
@@ -659,7 +695,7 @@ namespace ZIM
|
|
|
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) // 超出边缘不计算
|
|
|
+ if (index > 0 && index < pixels.Length) // 瓒呭嚭杈圭紭涓嶈�绠�
|
|
|
sum += pixels[index];
|
|
|
}
|
|
|
}
|