288 lines
12 KiB
C#
288 lines
12 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora.Api;
|
|
using Aurora.Api.DataType;
|
|
using Cognex.VisionPro;
|
|
using Cognex.VisionPro.Display;
|
|
using CogCommon;
|
|
using System;
|
|
using VisionCalibrator;
|
|
using VisionCommon;
|
|
using System.Collections.Generic;
|
|
|
|
namespace CogUI {
|
|
internal enum eGridMode {
|
|
None, Narrow, Wide
|
|
}
|
|
|
|
public class GridGuideLine {
|
|
private const string KEY_INTERACTIVE_GRID_WIDE = "INTERACTIVE_GRID_WIDE";
|
|
private const string KEY_INTERACTIVE_GRID_NARROW = "INTERACTIVE_GRID_NARROW";
|
|
private const string KEY_INTERACTIVE_REF_LINE = "INTERACTIVE_REF_LINE";
|
|
private const string KEY_INTERACTIVE_COORD_AXES = "KEY_INTERACTIVE_COORD_AXES";
|
|
private const string KEY_INTERACTIVE_LABEL_X = "KEY_INTERACTIVE_LABEL_X";
|
|
private const string KEY_INTERACTIVE_LABEL_Y = "KEY_INTERACTIVE_LABEL_Y";
|
|
|
|
private Logger _Logger = new Logger("GridGuideLine");
|
|
private CogDisplay _cogDisplay;
|
|
private FovInfo _fovInfo;
|
|
private eGridMode _gridMode = eGridMode.None;
|
|
|
|
public GridGuideLine(CogDisplay cogDisplay, FovInfo fovInfo) {
|
|
_cogDisplay = cogDisplay;
|
|
_fovInfo = fovInfo;
|
|
}
|
|
|
|
private CoordSystem GetCameraSetupDirection(FovInfo fovInfo) {
|
|
var camRecipe = VisionRecipeManager.GetCamRecipe(fovInfo);
|
|
var camConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(camRecipe.CamId.ToString()) as CamConfig;
|
|
if (camConfig != null) {
|
|
return camConfig.SetupDirection.ToCoordSystem();
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private CoordSystem GetStaticCoordSystem(SubSysInfo subSysInfo) {
|
|
var calRecipe = VisionRecipeManager.GetCalRecipe(subSysInfo);
|
|
return calRecipe?.CoordSystem;
|
|
}
|
|
|
|
public void Update(ImageSize imageSize) {
|
|
switch (_gridMode) {
|
|
case eGridMode.Narrow:
|
|
NarrowOn(imageSize);
|
|
return;
|
|
case eGridMode.Wide:
|
|
WideOn(imageSize);
|
|
return;
|
|
case eGridMode.None:
|
|
return;
|
|
}
|
|
}
|
|
|
|
public void NarrowOn(ImageSize imageSize, bool errRotation = false) {
|
|
Clear();
|
|
var calFovRecipe = VisionRecipeManager.GetCalFovRecipe(_fovInfo);
|
|
|
|
if (errRotation) {
|
|
DrawGridLines(eGridMode.Narrow, imageSize, calFovRecipe.ErrRotationX);
|
|
}
|
|
else {
|
|
DrawGridLines(eGridMode.Narrow, imageSize);
|
|
}
|
|
|
|
DrawRefLine(imageSize, calFovRecipe.ErrRotationX);
|
|
DrawCoordDirection(imageSize);
|
|
_gridMode = eGridMode.Narrow;
|
|
}
|
|
|
|
public void WideOn(ImageSize imageSize, bool errRotation = false) {
|
|
Clear();
|
|
var calFovRecipe = VisionRecipeManager.GetCalFovRecipe(_fovInfo);
|
|
|
|
if (errRotation) {
|
|
DrawGridLines(eGridMode.Wide, imageSize, calFovRecipe.ErrRotationX);
|
|
}
|
|
else {
|
|
DrawGridLines(eGridMode.Wide, imageSize);
|
|
}
|
|
|
|
DrawRefLine(imageSize, calFovRecipe.ErrRotationX);
|
|
DrawCoordDirection(imageSize);
|
|
_gridMode = eGridMode.Wide;
|
|
}
|
|
|
|
private void DrawGridLines(eGridMode gridMode, ImageSize imageSize, Angle errRotation = null) {
|
|
var centerPos = new ImagePoint(imageSize.Width / 2, imageSize.Height / 2);
|
|
var lineStyle = CogGraphicLineStyleConstants.Solid;
|
|
|
|
if (gridMode == eGridMode.Narrow) {
|
|
DrawGuideLines(imageSize, centerPos, 11, lineStyle, CogColorConstants.LightGrey, KEY_INTERACTIVE_GRID_NARROW, errRotation);
|
|
DrawGuideLines(imageSize, centerPos, 1, lineStyle, CogColorConstants.Orange, KEY_INTERACTIVE_GRID_NARROW, errRotation);
|
|
}
|
|
else {
|
|
DrawGuideLines(imageSize, centerPos, 5, lineStyle, CogColorConstants.LightGrey, KEY_INTERACTIVE_GRID_WIDE, errRotation);
|
|
DrawGuideLines(imageSize, centerPos, 1, lineStyle, CogColorConstants.Orange, KEY_INTERACTIVE_GRID_WIDE, errRotation);
|
|
}
|
|
}
|
|
|
|
public void DrawRefLine(ImageSize imageSize, Angle errRotation) {
|
|
var refPointRecipe = VisionRecipeManager.GetRefPointRecipe(_fovInfo);
|
|
if (refPointRecipe.Offset.X == 0 && refPointRecipe.Offset.Y == 0) {
|
|
// 레퍼런스 위치가 센터이므로 또 그릴 필요가 없음.
|
|
return;
|
|
}
|
|
|
|
var refPos = GetRefPos(imageSize);
|
|
var lineStyle = CogGraphicLineStyleConstants.Dot;
|
|
DrawGuideLines(imageSize, refPos, 1, lineStyle, CogColorConstants.Yellow, KEY_INTERACTIVE_REF_LINE, errRotation);
|
|
}
|
|
|
|
private ImagePoint GetRefPos(ImageSize imageSize) {
|
|
var calibrator = CalibratorManager.GetCalibrator(_fovInfo);
|
|
var staticRefPoint = VisionRecipeManager.GetRefPoint(_fovInfo);
|
|
var imageRefPoint = calibrator.TransformToImagePoint(staticRefPoint, imageSize);
|
|
|
|
return new ImagePoint(imageRefPoint.X, imageRefPoint.Y);
|
|
}
|
|
|
|
public void DrawGuideLines(
|
|
ImageSize imageSize,
|
|
ImagePoint crossPos,
|
|
int lineCount,
|
|
CogGraphicLineStyleConstants lineStyle,
|
|
CogColorConstants color,
|
|
string groupName,
|
|
Angle errRotation = null) {
|
|
|
|
try {
|
|
int lineCnt = ConvertToOddNumber(lineCount);
|
|
|
|
double[] lineStartX = new double[lineCnt * 2];
|
|
double[] lineStartY = new double[lineCnt * 2];
|
|
double[] lineEndX = new double[lineCnt * 2];
|
|
double[] lineEndY = new double[lineCnt * 2];
|
|
|
|
var width = imageSize.Width;
|
|
var horizontalGap = width / (lineCnt + 1);
|
|
var height = imageSize.Height;
|
|
var verticalGap = height / (lineCnt + 1);
|
|
int index = (lineCnt / 2) * -1;
|
|
|
|
for (int i = 0; i < lineCnt; i++) {
|
|
// Vertical
|
|
var vLineStart = new ImagePoint(crossPos.X + (horizontalGap * index), 0);
|
|
var vLineEnd = new ImagePoint(vLineStart.X, height);
|
|
var verticalLine = new CogLineSegment();
|
|
verticalLine.Color = color;
|
|
verticalLine.LineStyle = lineStyle;
|
|
|
|
// Horizontal
|
|
var hLineStart = new ImagePoint(0, crossPos.Y + (verticalGap * index));
|
|
var hLineEnd = new ImagePoint(width, hLineStart.Y);
|
|
var horizontalLine = new CogLineSegment();
|
|
horizontalLine.Color = color;
|
|
horizontalLine.LineStyle = lineStyle;
|
|
|
|
if (errRotation != null) { // 카메라 틀어진 각도를 고려한 그리드 라인
|
|
// Vertical
|
|
var vLineStartRotated = MathUtil.Rotate(vLineStart, errRotation, crossPos);
|
|
var vLineEndRotated = MathUtil.Rotate(vLineEnd, errRotation, crossPos);
|
|
lineStartX[i] = vLineStartRotated.X;
|
|
lineStartY[i] = vLineStartRotated.Y;
|
|
lineEndX[i] = vLineEndRotated.X;
|
|
lineEndY[i] = vLineEndRotated.Y;
|
|
|
|
// Horizontal
|
|
var hLineStartRotated = MathUtil.Rotate(hLineStart, errRotation, crossPos);
|
|
var hLineEndRotated = MathUtil.Rotate(hLineEnd, errRotation, crossPos);
|
|
lineStartX[lineCnt + i] = hLineStartRotated.X;
|
|
lineStartY[lineCnt + i] = hLineStartRotated.Y;
|
|
lineEndX[lineCnt + i] = hLineEndRotated.X;
|
|
lineEndY[lineCnt + i] = hLineEndRotated.Y;
|
|
}
|
|
else {
|
|
// Vertical
|
|
lineStartX[i] = vLineStart.X;
|
|
lineStartY[i] = vLineStart.Y;
|
|
lineEndX[i] = vLineEnd.X;
|
|
lineEndY[i] = vLineEnd.Y;
|
|
|
|
// Horizontal
|
|
lineStartX[lineCnt + i] = hLineStart.X;
|
|
lineStartY[lineCnt + i] = hLineStart.Y;
|
|
lineEndX[lineCnt + i] = hLineEnd.X;
|
|
lineEndY[lineCnt + i] = hLineEnd.Y;
|
|
}
|
|
|
|
verticalLine.SetStartEnd(lineStartX[i], lineStartY[i], lineEndX[i], lineEndY[i]);
|
|
horizontalLine.SetStartEnd(lineStartX[lineCnt + i], lineStartY[lineCnt + i], lineEndX[lineCnt + i], lineEndY[lineCnt + i]);
|
|
|
|
index++;
|
|
|
|
_cogDisplay.InteractiveGraphics.Add(verticalLine, groupName, false);
|
|
_cogDisplay.InteractiveGraphics.Add(horizontalLine, groupName, false);
|
|
}
|
|
}
|
|
catch (Exception e) {
|
|
_Logger.FuncE(e.Message);
|
|
}
|
|
}
|
|
|
|
private int ConvertToOddNumber(int number) {
|
|
if (number % 2 == 0) {
|
|
return number + 1;
|
|
}
|
|
|
|
return number;
|
|
}
|
|
|
|
public void Clear() {
|
|
RemoveInteractiveGraphics(KEY_INTERACTIVE_GRID_WIDE);
|
|
RemoveInteractiveGraphics(KEY_INTERACTIVE_GRID_NARROW);
|
|
RemoveInteractiveGraphics(KEY_INTERACTIVE_REF_LINE);
|
|
RemoveInteractiveGraphics(KEY_INTERACTIVE_COORD_AXES);
|
|
RemoveInteractiveGraphics(KEY_INTERACTIVE_LABEL_X);
|
|
RemoveInteractiveGraphics(KEY_INTERACTIVE_LABEL_Y);
|
|
_gridMode = eGridMode.None;
|
|
}
|
|
|
|
private void RemoveInteractiveGraphics(string key) {
|
|
try {
|
|
int index = _cogDisplay.InteractiveGraphics.FindItem(key, CogDisplayZOrderConstants.Front);
|
|
if (index != -1) {
|
|
_cogDisplay.InteractiveGraphics.Remove(key);
|
|
}
|
|
}
|
|
catch (Exception e) {
|
|
_Logger.FuncE(e.Message);
|
|
}
|
|
}
|
|
|
|
public void DrawCoordDirection(ImageSize imageSize) {
|
|
DrawCoordDirection(
|
|
GetCameraSetupDirection(_fovInfo),
|
|
GetStaticCoordSystem(_fovInfo.ToSubSysInfo()),
|
|
new ImagePoint(imageSize.Width - 150, 150), "S");
|
|
}
|
|
|
|
private void DrawCoordDirection(CoordSystem imageCoordSystem, CoordSystem staticCoordSystem, ImagePoint drawPosition, string prefix) {
|
|
if (imageCoordSystem != null && staticCoordSystem != null) {
|
|
var corrCS = staticCoordSystem - imageCoordSystem;
|
|
var corrRotX = GetCorrectionRotationX(imageCoordSystem, staticCoordSystem);
|
|
var corrFlipY = (int)GetCorrectionFlipY(imageCoordSystem, staticCoordSystem);
|
|
|
|
var axisX = new ImagePoint(100, 0);
|
|
var axisY = new ImagePoint(0, 100);
|
|
axisX = MathUtil.Rotate(axisX, corrRotX) as ImagePoint;
|
|
axisY = MathUtil.Rotate(axisY, corrRotX) * corrFlipY as ImagePoint;
|
|
|
|
DisplayDrawer.AddVectors(_cogDisplay, CogColorConstants.Orange,
|
|
drawPosition,
|
|
new List<ImagePoint>() { axisX, axisY },
|
|
KEY_INTERACTIVE_COORD_AXES);
|
|
|
|
var textOffset = -20;
|
|
var textX = new ImagePoint(100 + textOffset, 50 - textOffset);
|
|
var textY = new ImagePoint(0 - textOffset, 150 + textOffset);
|
|
textX = (MathUtil.Rotate(textX, corrRotX) + drawPosition) as ImagePoint;
|
|
textY = (MathUtil.Rotate(textY, corrRotX) * corrFlipY + drawPosition) as ImagePoint;
|
|
|
|
DisplayDrawer.AddText(_cogDisplay, CogColorConstants.Orange, textX, prefix + "X", KEY_INTERACTIVE_LABEL_X);
|
|
DisplayDrawer.AddText(_cogDisplay, CogColorConstants.Orange, textY, prefix + "Y", KEY_INTERACTIVE_LABEL_Y);
|
|
}
|
|
}
|
|
|
|
public Angle GetCorrectionRotationX(CoordSystem sourceCS, CoordSystem targetCS) {
|
|
return Angle.CreateWithDegree(
|
|
(targetCS.RotationX.Degree - sourceCS.RotationX.Degree) * (int)sourceCS.FlipY);
|
|
}
|
|
|
|
public eFlipY GetCorrectionFlipY(CoordSystem sourceCS, CoordSystem targetCS) {
|
|
return (eFlipY)((int)targetCS.FlipY * (int)sourceCS.FlipY);
|
|
}
|
|
}
|
|
}
|