291 lines
19 KiB
C#
291 lines
19 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora.Api;
|
|
using Aurora.Api.DataType;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Windows;
|
|
|
|
namespace VisionCommon {
|
|
/// <summary>
|
|
/// FOV 간에 상관관계를 이용하여 라인을 교정하기 위한 매니저
|
|
/// </summary>
|
|
public class LineFittingManager {
|
|
private static readonly Logger _Logger = new Logger(typeof(LineFittingManager).Name);
|
|
private static readonly object LockThis = new object();
|
|
|
|
// 라인 중점 보관
|
|
private static Dictionary<SubSysInfo, Dictionary<FovInfo, Dictionary<eLineDirection, StaticPoint>>> _LineCenterPoints =
|
|
new Dictionary<SubSysInfo, Dictionary<FovInfo, Dictionary<eLineDirection, StaticPoint>>>();
|
|
|
|
/// <summary>
|
|
/// 라인 중점 보관 (자재 좌표)
|
|
/// </summary>
|
|
/// <param name="fovInfo">FOV 정보</param>
|
|
/// <param name="lineType">수평/수직 라인 타입</param>
|
|
/// <param name="calibratedPoint">라인 중점 (자재 좌표)</param>
|
|
public static void SetLineCenterPoint(FovInfo fovInfo, eLineDirection lineType, StaticPoint calibratedPoint) {
|
|
lock (LockThis) {
|
|
var subSysInfo = fovInfo.ToSubSysInfo();
|
|
if (!_LineCenterPoints.ContainsKey(subSysInfo)) {
|
|
_LineCenterPoints.Add(subSysInfo, new Dictionary<FovInfo, Dictionary<eLineDirection, StaticPoint>>());
|
|
}
|
|
if (!_LineCenterPoints[subSysInfo].ContainsKey(fovInfo)) {
|
|
_LineCenterPoints[subSysInfo].Add(fovInfo, new Dictionary<eLineDirection, StaticPoint>());
|
|
}
|
|
if (!_LineCenterPoints[subSysInfo][fovInfo].ContainsKey(lineType)) {
|
|
_LineCenterPoints[subSysInfo][fovInfo].Add(lineType, null);
|
|
}
|
|
_LineCenterPoints[subSysInfo][fovInfo][lineType] = calibratedPoint.Clone() as StaticPoint;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 라인 중점 가져오기 (자재 좌표)
|
|
/// </summary>
|
|
/// <param name="fovInfo">FOV 정보</param>
|
|
/// <param name="lineType">수평/수직 라인 타입</param>
|
|
/// <returns>라인 중점 (자재 좌표)</returns>
|
|
public static StaticPoint GetLineCenterPoint(FovInfo fovInfo, eLineDirection lineType) {
|
|
var subSysInfo = fovInfo.ToSubSysInfo();
|
|
if (_LineCenterPoints.ContainsKey(subSysInfo)) {
|
|
if (_LineCenterPoints[subSysInfo].ContainsKey(fovInfo)) {
|
|
if (_LineCenterPoints[subSysInfo][fovInfo].ContainsKey(lineType)) {
|
|
return _LineCenterPoints[subSysInfo][fovInfo][lineType];
|
|
}
|
|
else {
|
|
_Logger.FuncE($"_LineCenterPoints of [{fovInfo}] of [{subSysInfo}] contains not key [lineType : {lineType}]");
|
|
}
|
|
}
|
|
else {
|
|
_Logger.FuncE($"_LineCenterPoints of [{subSysInfo}] contains not key [fovInfo : {fovInfo}]");
|
|
}
|
|
}
|
|
else {
|
|
_Logger.FuncE($"_LineCenterPoints contains not key [subSysInfo : {subSysInfo}]");
|
|
}
|
|
return null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 라인 중점 초기화
|
|
/// </summary>
|
|
public static void ClearLineCenterPoints(SubSysInfo subSysInfo) {
|
|
_LineCenterPoints[subSysInfo].Clear();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 각 FOV에서 찾은 라인의 중점 기반으로 새로 그려진 라인의 교점으로 meaPoints를 새롭게 정의
|
|
/// </summary>
|
|
/// <returns>보상된 교점 위치</returns>
|
|
public static List<DoublePoint> GetFixedMeasurePoints(SubSysInfo subSysInfo) {
|
|
try {
|
|
// 라인 중점 (자재 좌표)
|
|
StaticPoint cpFov1_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov1), eLineDirection.H);
|
|
StaticPoint cpFov1_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov1), eLineDirection.V);
|
|
StaticPoint cpFov2_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov2), eLineDirection.H);
|
|
StaticPoint cpFov2_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov2), eLineDirection.V);
|
|
StaticPoint cpFov3_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov3), eLineDirection.H);
|
|
StaticPoint cpFov3_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov3), eLineDirection.V);
|
|
StaticPoint cpFov4_H = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov4), eLineDirection.H);
|
|
StaticPoint cpFov4_V = GetLineCenterPoint(VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov4), eLineDirection.V);
|
|
|
|
// 라인 방정식 (자재 좌표)
|
|
var lineFov1_2 = new LineEquation(cpFov2_H.X, cpFov2_H.Y, cpFov1_H.X, cpFov1_H.Y);
|
|
var lineFov1_3 = new LineEquation(cpFov1_V.X, cpFov1_V.Y, cpFov3_V.X, cpFov3_V.Y);
|
|
var lineFov2_4 = new LineEquation(cpFov4_V.X, cpFov4_V.Y, cpFov2_V.X, cpFov2_V.Y);
|
|
var lineFov3_4 = new LineEquation(cpFov3_H.X, cpFov3_H.Y, cpFov4_H.X, cpFov4_H.Y);
|
|
|
|
// 라인 교점 (자재 좌표)
|
|
StaticPoint intersectFov1 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_2, lineFov1_3);
|
|
StaticPoint intersectFov2 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov2_4, lineFov1_2);
|
|
StaticPoint intersectFov3 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_3, lineFov3_4);
|
|
StaticPoint intersectFov4 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov3_4, lineFov2_4);
|
|
|
|
// 얼라인 포인트 재정의
|
|
var fixedMeaPoints = new List<DoublePoint>();
|
|
fixedMeaPoints.Add(intersectFov1); // 반드시 refPoints의 Fov쌍과 맞아야 함. (Fov1,2,3,4 순서대로 넣어야 함)
|
|
fixedMeaPoints.Add(intersectFov2);
|
|
fixedMeaPoints.Add(intersectFov3);
|
|
fixedMeaPoints.Add(intersectFov4);
|
|
|
|
return fixedMeaPoints;
|
|
}
|
|
catch (Exception e) {
|
|
_Logger.FuncE(e.Message);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 라인 교점들로 라인 그리기
|
|
/// </summary>
|
|
/// <param name="fixedMeaPoints">라인 교점들</param>
|
|
public static void DrawFixedLinesAndIntersects(SubSysInfo subSysInfo, eDisplayType displayType) {
|
|
try {
|
|
Application.Current.Dispatcher.BeginInvoke(
|
|
new System.Action(() => {
|
|
var fovInfoFov1 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov1);
|
|
var fovInfoFov2 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov2);
|
|
var fovInfoFov3 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov3);
|
|
var fovInfoFov4 = VisionRecipeManager.GetFovInfo(subSysInfo, eFovId.Fov4);
|
|
var fovRecipeFov1 = VisionRecipeManager.GetFovRecipe(fovInfoFov1);
|
|
var fovRecipeFov2 = VisionRecipeManager.GetFovRecipe(fovInfoFov2);
|
|
var fovRecipeFov3 = VisionRecipeManager.GetFovRecipe(fovInfoFov3);
|
|
var fovRecipeFov4 = VisionRecipeManager.GetFovRecipe(fovInfoFov4);
|
|
var camConfigFov1 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov1.CamRecipe.CamId.ToString()) as CamConfig;
|
|
var camConfigFov2 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov2.CamRecipe.CamId.ToString()) as CamConfig;
|
|
var camConfigFov3 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov3.CamRecipe.CamId.ToString()) as CamConfig;
|
|
var camConfigFov4 = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(fovRecipeFov4.CamRecipe.CamId.ToString()) as CamConfig;
|
|
var cameraDisplayFov1 = CameraDisplayManager.GetDisplay(fovInfoFov1, displayType);
|
|
var cameraDisplayFov2 = CameraDisplayManager.GetDisplay(fovInfoFov2, displayType);
|
|
var cameraDisplayFov3 = CameraDisplayManager.GetDisplay(fovInfoFov3, displayType);
|
|
var cameraDisplayFov4 = CameraDisplayManager.GetDisplay(fovInfoFov4, displayType);
|
|
|
|
// 라인 중점 (자재 좌표)
|
|
StaticPoint cpFov1_H = GetLineCenterPoint(fovInfoFov1, eLineDirection.H);
|
|
StaticPoint cpFov1_V = GetLineCenterPoint(fovInfoFov1, eLineDirection.V);
|
|
StaticPoint cpFov2_H = GetLineCenterPoint(fovInfoFov2, eLineDirection.H);
|
|
StaticPoint cpFov2_V = GetLineCenterPoint(fovInfoFov2, eLineDirection.V);
|
|
StaticPoint cpFov3_H = GetLineCenterPoint(fovInfoFov3, eLineDirection.H);
|
|
StaticPoint cpFov3_V = GetLineCenterPoint(fovInfoFov3, eLineDirection.V);
|
|
StaticPoint cpFov4_H = GetLineCenterPoint(fovInfoFov4, eLineDirection.H);
|
|
StaticPoint cpFov4_V = GetLineCenterPoint(fovInfoFov4, eLineDirection.V);
|
|
|
|
// 라인 중점 (픽셀 좌표)
|
|
ImagePoint cpFov1_I_H = GetPixelPos(fovInfoFov1, cpFov1_H, camConfigFov1.ImageSize);
|
|
ImagePoint cpFov1_I_V = GetPixelPos(fovInfoFov1, cpFov1_V, camConfigFov1.ImageSize);
|
|
ImagePoint cpFov2_I_H = GetPixelPos(fovInfoFov2, cpFov2_H, camConfigFov1.ImageSize);
|
|
ImagePoint cpFov2_I_V = GetPixelPos(fovInfoFov2, cpFov2_V, camConfigFov1.ImageSize);
|
|
ImagePoint cpFov3_I_H = GetPixelPos(fovInfoFov3, cpFov3_H, camConfigFov1.ImageSize);
|
|
ImagePoint cpFov3_I_V = GetPixelPos(fovInfoFov3, cpFov3_V, camConfigFov1.ImageSize);
|
|
ImagePoint cpFov4_I_H = GetPixelPos(fovInfoFov4, cpFov4_H, camConfigFov1.ImageSize);
|
|
ImagePoint cpFov4_I_V = GetPixelPos(fovInfoFov4, cpFov4_V, camConfigFov1.ImageSize);
|
|
|
|
// 라인 방정식 (자재 좌표)
|
|
var lineFov1_2 = new LineEquation(cpFov2_H.X, cpFov2_H.Y, cpFov1_H.X, cpFov1_H.Y);
|
|
var lineFov1_3 = new LineEquation(cpFov1_V.X, cpFov1_V.Y, cpFov3_V.X, cpFov3_V.Y);
|
|
var lineFov2_4 = new LineEquation(cpFov4_V.X, cpFov4_V.Y, cpFov2_V.X, cpFov2_V.Y);
|
|
var lineFov3_4 = new LineEquation(cpFov3_H.X, cpFov3_H.Y, cpFov4_H.X, cpFov4_H.Y);
|
|
|
|
// 라인 교점 (자재 좌표)
|
|
StaticPoint intersectFov1 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_2, lineFov1_3);
|
|
StaticPoint intersectFov2 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov2_4, lineFov1_2);
|
|
StaticPoint intersectFov3 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov1_3, lineFov3_4);
|
|
StaticPoint intersectFov4 = NativePattern.GetIntersectionPoint<StaticPoint>(lineFov3_4, lineFov2_4);
|
|
|
|
// 라인 교점 (픽셀 좌표)
|
|
ImagePoint intersectFov1_I = GetPixelPos(fovInfoFov1, intersectFov1, camConfigFov1.ImageSize);
|
|
ImagePoint intersectFov2_I = GetPixelPos(fovInfoFov2, intersectFov2, camConfigFov2.ImageSize);
|
|
ImagePoint intersectFov3_I = GetPixelPos(fovInfoFov3, intersectFov3, camConfigFov3.ImageSize);
|
|
ImagePoint intersectFov4_I = GetPixelPos(fovInfoFov4, intersectFov4, camConfigFov4.ImageSize);
|
|
|
|
// 교정된 교점 십자가 그리기
|
|
cameraDisplayFov1.AddCross(new Cross(intersectFov1_I), eColor.Magenta);
|
|
cameraDisplayFov2.AddCross(new Cross(intersectFov2_I), eColor.Magenta);
|
|
cameraDisplayFov3.AddCross(new Cross(intersectFov3_I), eColor.Magenta);
|
|
cameraDisplayFov4.AddCross(new Cross(intersectFov4_I), eColor.Magenta);
|
|
|
|
// 교정된 교점 위치 테스트 뿌리기
|
|
var txtPosOffsetY = -150;
|
|
|
|
var msgsFov1 = new List<string>();
|
|
msgsFov1.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov1_I.X, intersectFov1_I.Y));
|
|
msgsFov1.Add(ConvertMaterialPosition(intersectFov1));
|
|
var txtPos_Fov1 = new ImagePoint(intersectFov1_I.X, intersectFov1_I.Y + txtPosOffsetY);
|
|
cameraDisplayFov1.AddTextList(eColor.Magenta, txtPos_Fov1, msgsFov1);
|
|
|
|
var msgsFov2 = new List<string>();
|
|
msgsFov2.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov2_I.X, intersectFov2_I.Y));
|
|
msgsFov2.Add(ConvertMaterialPosition(intersectFov2));
|
|
var txtPos_Fov2 = new ImagePoint(intersectFov2_I.X, intersectFov2_I.Y + txtPosOffsetY);
|
|
cameraDisplayFov2.AddTextList(eColor.Magenta, txtPos_Fov2, msgsFov2);
|
|
|
|
var msgsFov3 = new List<string>();
|
|
msgsFov3.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov3_I.X, intersectFov3_I.Y));
|
|
msgsFov3.Add(ConvertMaterialPosition(intersectFov3));
|
|
var txtPos_Fov3 = new ImagePoint(intersectFov3_I.X, intersectFov3_I.Y + txtPosOffsetY);
|
|
cameraDisplayFov3.AddTextList(eColor.Magenta, txtPos_Fov3, msgsFov3);
|
|
|
|
var msgsFov4 = new List<string>();
|
|
msgsFov4.Add(string.Format("I({0:0.0}, {1:0.0})", intersectFov4_I.X, intersectFov4_I.Y));
|
|
msgsFov4.Add(ConvertMaterialPosition(intersectFov4));
|
|
var txtPos_Fov4 = new ImagePoint(intersectFov4_I.X, intersectFov4_I.Y + txtPosOffsetY);
|
|
cameraDisplayFov4.AddTextList(eColor.Magenta, txtPos_Fov4, msgsFov4);
|
|
|
|
#region 라인 그리기 불필요하여 주석처리
|
|
// 교정된 라인 방정식 (픽셀 좌표)
|
|
//var lineFov1_I_H = new LineEquation(intersectFov1_I.X, intersectFov1_I.Y, cpFov1_I_H.X, cpFov1_I_H.Y);
|
|
//var lineFov1_I_V = new LineEquation(intersectFov1_I.X, intersectFov1_I.Y, cpFov1_I_V.X, cpFov1_I_V.Y);
|
|
//var lineFov2_I_H = new LineEquation(intersectFov2_I.X, intersectFov2_I.Y, cpFov2_I_H.X, cpFov2_I_H.Y);
|
|
//var lineFov2_I_V = new LineEquation(intersectFov2_I.X, intersectFov2_I.Y, cpFov2_I_V.X, cpFov2_I_V.Y);
|
|
//var lineFov3_I_H = new LineEquation(intersectFov3_I.X, intersectFov3_I.Y, cpFov3_I_H.X, cpFov3_I_H.Y);
|
|
//var lineFov3_I_V = new LineEquation(intersectFov3_I.X, intersectFov3_I.Y, cpFov3_I_V.X, cpFov3_I_V.Y);
|
|
//var lineFov4_I_H = new LineEquation(intersectFov4_I.X, intersectFov4_I.Y, cpFov4_I_H.X, cpFov4_I_H.Y);
|
|
//var lineFov4_I_V = new LineEquation(intersectFov4_I.X, intersectFov4_I.Y, cpFov4_I_V.X, cpFov4_I_V.Y);
|
|
|
|
// 교정된 연장 라인 얻어오기 (이미지의 끝단까지 그림)
|
|
//var extLineFov1_I_H = GetMinLengthLine(lineFov1_I_H, camConfigFov1.ImageSize);
|
|
//var extLineFov1_I_V = GetMinLengthLine(lineFov1_I_V, camConfigFov1.ImageSize);
|
|
//var extLineFov2_I_H = GetMinLengthLine(lineFov2_I_H, camConfigFov2.ImageSize);
|
|
//var extLineFov2_I_V = GetMinLengthLine(lineFov2_I_V, camConfigFov2.ImageSize);
|
|
//var extLineFov3_I_H = GetMinLengthLine(lineFov3_I_H, camConfigFov3.ImageSize);
|
|
//var extLineFov3_I_V = GetMinLengthLine(lineFov3_I_V, camConfigFov3.ImageSize);
|
|
//var extLineFov4_I_H = GetMinLengthLine(lineFov4_I_H, camConfigFov4.ImageSize);
|
|
//var extLineFov4_I_V = GetMinLengthLine(lineFov4_I_V, camConfigFov4.ImageSize);
|
|
|
|
// 교정된 라인 그리기
|
|
//cameraDisplayFov1.AddLine(eColor.Orange, extLineFov1_I_H.StartPoint, extLineFov1_I_H.EndPoint);
|
|
//cameraDisplayFov1.AddLine(eColor.Orange, extLineFov1_I_V.StartPoint, extLineFov1_I_V.EndPoint);
|
|
//cameraDisplayFov2.AddLine(eColor.Orange, extLineFov2_I_H.StartPoint, extLineFov2_I_H.EndPoint);
|
|
//cameraDisplayFov2.AddLine(eColor.Orange, extLineFov2_I_V.StartPoint, extLineFov2_I_V.EndPoint);
|
|
//cameraDisplayFov3.AddLine(eColor.Orange, extLineFov3_I_H.StartPoint, extLineFov3_I_H.EndPoint);
|
|
//cameraDisplayFov3.AddLine(eColor.Orange, extLineFov3_I_V.StartPoint, extLineFov3_I_V.EndPoint);
|
|
//cameraDisplayFov4.AddLine(eColor.Orange, extLineFov4_I_H.StartPoint, extLineFov4_I_H.EndPoint);
|
|
//cameraDisplayFov4.AddLine(eColor.Orange, extLineFov4_I_V.StartPoint, extLineFov4_I_V.EndPoint);
|
|
#endregion
|
|
|
|
ClearLineCenterPoints(subSysInfo);
|
|
}));
|
|
}
|
|
catch (Exception e) {
|
|
_Logger.FuncE(e.Message);
|
|
}
|
|
}
|
|
|
|
private static ImagePoint GetPixelPos(FovInfo fovInfo, StaticPoint staticPoint, ImageSize imageSize) {
|
|
var imagePoint = CalAdapter.Instance.CalManager.TransformToImagePoint(fovInfo, staticPoint, imageSize);
|
|
|
|
return new ImagePoint(imagePoint.X, imagePoint.Y);
|
|
}
|
|
|
|
public static string ConvertMaterialPosition(StaticPoint intersect) {
|
|
if (VisionSettingManager.VisionSetting.CoordUnit == eCoordUnit.UM) {
|
|
return String.Format(
|
|
"S({0:0.0}, {1:0.0})",
|
|
MathUtil.ConvertUnitMilliToMicro(intersect.X),
|
|
MathUtil.ConvertUnitMilliToMicro(intersect.Y));
|
|
}
|
|
|
|
return String.Format(
|
|
"S({0:0.00}, {1:0.00})",
|
|
intersect.X,
|
|
intersect.Y);
|
|
}
|
|
|
|
private static Line<ImagePoint> GetMinLengthLine(
|
|
LineEquation lineEquation, ImageSize imageSize) {
|
|
var line1 = new Line<ImagePoint>(
|
|
new ImagePoint(0, lineEquation.GetY(0)),
|
|
new ImagePoint(imageSize.Width, lineEquation.GetY(imageSize.Width)));
|
|
|
|
var line2 = new Line<ImagePoint>(
|
|
new ImagePoint(lineEquation.GetX(0), 0),
|
|
new ImagePoint(lineEquation.GetX(imageSize.Height), imageSize.Height));
|
|
|
|
return line1.Length < line2.Length ? line1 : line2;
|
|
}
|
|
}
|
|
} |