394 lines
16 KiB
C#
394 lines
16 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora.Api;
|
|
using VisionCommon;
|
|
using System.Collections.Generic;
|
|
|
|
namespace VisionAligner {
|
|
/// <summary>
|
|
/// 얼라인 모드(알고리즘)
|
|
/// </summary>
|
|
public abstract class AlignMode {
|
|
/// <summary>로거</summary>
|
|
protected Logger _Logger;
|
|
/// <summary>얼라인 보정량 제한 체커</summary>
|
|
protected AlignLimitChecker _AlignLimitChecker;
|
|
/// <summary>얼라인 재시도 스펙 체커</summary>
|
|
protected AlignRetryChecker _AlignRetryChecker;
|
|
/// <summary>얼라인 재시도 횟수 체커</summary>
|
|
protected AlignCountChecker _AlignCountChecker;
|
|
|
|
/// <summary>얼라인 레시피</summary>
|
|
protected AlignRecipe _AlignRecipe;
|
|
/// <summary>서브 시스템 정보</summary>
|
|
protected SubSysInfo _SubSysInfo;
|
|
|
|
/// <summary>
|
|
/// 생성자(UnitTest 전용)
|
|
/// </summary>
|
|
protected AlignMode() {
|
|
// For UnitTest
|
|
}
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="alignInfo">얼라인 정보</param>
|
|
public AlignMode(AlignInfo alignInfo) {
|
|
_Logger = new Logger($"AlignMode[{alignInfo.AlignName}]");
|
|
_AlignRecipe = VisionRecipeManager.GetAlignRecipe(alignInfo);
|
|
_SubSysInfo = alignInfo?.ToSubSysInfo();
|
|
|
|
SetAlignCheckers();
|
|
}
|
|
|
|
private void SetAlignCheckers() {
|
|
_AlignLimitChecker = new AlignLimitChecker(_AlignRecipe);
|
|
_AlignRetryChecker = new AlignRetryChecker(_AlignRecipe);
|
|
_AlignCountChecker = new AlignCountChecker(_AlignRecipe);
|
|
}
|
|
|
|
#region N-Point Align
|
|
/// <summary>
|
|
/// N-포인트 얼라인 보정량 XYT를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaPoints">패턴 검출 위치</param>
|
|
/// <param name="refPoints">레퍼런스(기준) 위치</param>
|
|
/// <param name="flipY">좌표계의 Y축 반전 여부</param>
|
|
/// <returns>얼라인 결과</returns>
|
|
public AlignResult Align(
|
|
List<DoublePoint> meaPoints,
|
|
List<DoublePoint> refPoints,
|
|
int flipY) {
|
|
StaticPoint thetaOrigin = GetThetaOrigin();
|
|
eAlignAxis alignAxis = _AlignRecipe.ProcParams.AlignAxis;
|
|
AlignTgtDist alignTgtDist = _AlignRecipe.AlignTgtDist;
|
|
eAlignAlgorithm alignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm;
|
|
|
|
AlignDelta alignDelta = CalculateAlign(
|
|
meaPoints, refPoints, flipY, thetaOrigin, alignAxis, alignTgtDist);
|
|
_Logger.I($"Align delta : {alignDelta}");
|
|
|
|
AlignStatus status = GetAlignStatus(alignDelta);
|
|
int alignCount = _AlignCountChecker.GetCurrentAlignCount(status);
|
|
var alignResult = new AlignResult(alignCount, status, alignDelta, alignAlgorithm);
|
|
|
|
return alignResult;
|
|
}
|
|
|
|
private AlignDelta CalculateAlign(
|
|
List<DoublePoint> meaPoints,
|
|
List<DoublePoint> refPoints,
|
|
int flipY,
|
|
DoublePoint thetaOrigin,
|
|
eAlignAxis alignAxis,
|
|
AlignTgtDist alignTgtDist) {
|
|
if(_AlignRecipe.ProcParams.AlignAlgorithm == eAlignAlgorithm.AVG) {
|
|
return CalculateXYT(meaPoints, refPoints, flipY, thetaOrigin, alignAxis, alignTgtDist);
|
|
}
|
|
else if(_AlignRecipe.ProcParams.AlignAlgorithm == eAlignAlgorithm.ICP) {
|
|
return CalculateXYT_ICP(meaPoints, refPoints, flipY, thetaOrigin, alignAxis, alignTgtDist);
|
|
}
|
|
return new AlignDelta();
|
|
}
|
|
|
|
private AlignDelta CalculateXYT(
|
|
List<DoublePoint> meaPoints,
|
|
List<DoublePoint> refPoints,
|
|
int flipY,
|
|
DoublePoint thetaOrigin,
|
|
eAlignAxis alignAxis,
|
|
AlignTgtDist alignTgtDist) {
|
|
AlignDelta alignDeltaT = CalculateT(meaPoints, refPoints, flipY);
|
|
alignDeltaT.T = ApplyAlignAxisAndTargetDistT(alignDeltaT.T, alignAxis, alignTgtDist);
|
|
|
|
AlignDelta alignDeltaXY = CalculateXY(meaPoints, refPoints, flipY, thetaOrigin, alignDeltaT.T);
|
|
alignDeltaXY.X = ApplyAlignAxisAndTargetDistX(alignDeltaXY.X, alignAxis, alignTgtDist);
|
|
alignDeltaXY.Y = ApplyAlignAxisAndTargetDistY(alignDeltaXY.Y, alignAxis, alignTgtDist);
|
|
|
|
return new AlignDelta(
|
|
alignDeltaXY.Translation,
|
|
alignDeltaT.T);
|
|
}
|
|
|
|
private AlignDelta CalculateXYT_ICP(
|
|
List<DoublePoint> meaPoints,
|
|
List<DoublePoint> refPoints,
|
|
int flipY,
|
|
DoublePoint thetaOrigin,
|
|
eAlignAxis alignAxis,
|
|
AlignTgtDist alignTgtDist) {
|
|
AlignDelta alignDeltaT = CalculateT_ICP(meaPoints, refPoints, flipY);
|
|
alignDeltaT.T = ApplyAlignAxisAndTargetDistT(alignDeltaT.T, alignAxis, alignTgtDist);
|
|
|
|
AlignDelta alignDeltaXY = CalculateXY(meaPoints, refPoints, flipY, thetaOrigin, alignDeltaT.T);
|
|
alignDeltaXY.X = ApplyAlignAxisAndTargetDistX(alignDeltaXY.X, alignAxis, alignTgtDist);
|
|
alignDeltaXY.Y = ApplyAlignAxisAndTargetDistY(alignDeltaXY.Y, alignAxis, alignTgtDist);
|
|
|
|
return new AlignDelta(
|
|
alignDeltaXY.Translation,
|
|
alignDeltaT.T);
|
|
}
|
|
|
|
/// <summary>
|
|
/// N-포인트 얼라인 보정량 T를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaPoints">패턴 검출 위치</param>
|
|
/// <param name="refPoints">레퍼런스(기준) 위치</param>
|
|
/// <param name="flipY">좌표계의 Y축 반전 여부</param>
|
|
/// <returns>얼라인 보정량</returns>
|
|
protected abstract AlignDelta CalculateT(
|
|
List<DoublePoint> meaPoints,
|
|
List<DoublePoint> refPoints,
|
|
int flipY);
|
|
|
|
/// <summary>
|
|
/// ICP 알고리즘을 이용하여 N-포인트 얼라인 보정량 T를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaPoints">패턴 검출 위치</param>
|
|
/// <param name="refPoints">레퍼런스(기준) 위치</param>
|
|
/// <param name="flipY">좌표계의 Y축 반전 여부</param>
|
|
/// <returns>얼라인 보정량</returns>
|
|
protected abstract AlignDelta CalculateT_ICP(
|
|
List<DoublePoint> meaPoints,
|
|
List<DoublePoint> refPoints,
|
|
int flipY);
|
|
|
|
/// <summary>
|
|
/// N-포인트 얼라인 보정량 XY를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaPoints">패턴 검출 위치</param>
|
|
/// <param name="refPoints">레퍼런스(기준) 위치</param>
|
|
/// <param name="flipY">좌표계의 Y축 반전 여부</param>
|
|
/// <param name="thetaOrigin">회전 원점</param>
|
|
/// <param name="alignT">얼라인 보정량 T</param>
|
|
/// <returns>얼라인 보정량</returns>
|
|
protected abstract AlignDelta CalculateXY(
|
|
List<DoublePoint> meaPoints,
|
|
List<DoublePoint> refPoints,
|
|
int flipY,
|
|
DoublePoint thetaOrigin,
|
|
Angle alignT);
|
|
#endregion
|
|
|
|
#region 1-Point Align
|
|
/// <summary>
|
|
/// 1-포인트 얼라인 보정량 XYT를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaPoint">패턴 검출 위치</param>
|
|
/// <param name="refPoint">레퍼런스(기준) 위치</param>
|
|
/// <param name="theta">얼라인 보정량 T</param>
|
|
/// <param name="flipY">좌표계의 Y축 반전 여부</param>
|
|
/// <returns>얼라인 결과</returns>
|
|
public AlignResult Align(
|
|
DoublePoint meaPoint,
|
|
DoublePoint refPoint,
|
|
Angle theta,
|
|
int flipY) {
|
|
StaticPoint thetaOrigin = GetThetaOrigin();
|
|
eAlignAxis alignAxis = _AlignRecipe.ProcParams.AlignAxis;
|
|
AlignTgtDist alignTgtDist = _AlignRecipe.AlignTgtDist;
|
|
eAlignAlgorithm alignAlgorithm = _AlignRecipe.ProcParams.AlignAlgorithm;
|
|
|
|
AlignDelta alignDelta = CalculateXYT(
|
|
meaPoint, refPoint, flipY, thetaOrigin, alignAxis, alignTgtDist, theta);
|
|
_Logger.I($"Align delta : {alignDelta}");
|
|
|
|
AlignStatus status = GetAlignStatus(alignDelta);
|
|
int alignCount = _AlignCountChecker.GetCurrentAlignCount(status);
|
|
|
|
return new AlignResult(alignCount, status, alignDelta, alignAlgorithm);
|
|
}
|
|
|
|
private AlignDelta CalculateXYT(
|
|
DoublePoint meaPoint,
|
|
DoublePoint refPoint,
|
|
int flipY,
|
|
DoublePoint thetaOrigin,
|
|
eAlignAxis alignAxis,
|
|
AlignTgtDist alignTgtDist,
|
|
Angle theta) {
|
|
var alignT = ApplyAlignAxisAndTargetDistT(theta, alignAxis, alignTgtDist);
|
|
|
|
var alignDelta = CalculateXYT(
|
|
meaPoint, refPoint, flipY, thetaOrigin, alignT);
|
|
|
|
alignDelta.X = ApplyAlignAxisAndTargetDistX(alignDelta.X, alignAxis, alignTgtDist);
|
|
alignDelta.Y = ApplyAlignAxisAndTargetDistY(alignDelta.Y, alignAxis, alignTgtDist);
|
|
|
|
return alignDelta;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 1-포인트 얼라인 보정량 XYT를 계산한다.
|
|
/// </summary>
|
|
/// <param name="meaPoint">패턴 검출 위치</param>
|
|
/// <param name="refPoint">레퍼런스(기준) 위치</param>
|
|
/// <param name="flipY">좌표계의 Y축 반전 여부</param>
|
|
/// <param name="thetaOrigin">회전 원점</param>
|
|
/// <param name="theta">얼라인 보정량 T</param>
|
|
/// <returns>얼라인 보정량</returns>
|
|
protected abstract AlignDelta CalculateXYT(
|
|
DoublePoint meaPoint,
|
|
DoublePoint refPoint,
|
|
int flipY,
|
|
DoublePoint thetaOrigin,
|
|
Angle theta);
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// 얼라인 상태를 체크한다.
|
|
/// </summary>
|
|
/// <param name="alignDelta">얼라인 보정량</param>
|
|
/// <returns>얼라인 상태 코드</returns>
|
|
protected AlignStatus GetAlignStatus(AlignDelta alignDelta) {
|
|
AlignStatus status = AlignStatus.OK;
|
|
|
|
if (_AlignLimitChecker.IsEnabled()) {
|
|
status = _AlignLimitChecker.GetAlignLimitStatus(alignDelta);
|
|
|
|
if (status == AlignStatus.ALIGN_LIMIT_OVER) {
|
|
ResetRetrySpecChecker();
|
|
return status;
|
|
}
|
|
}
|
|
|
|
if (_AlignRetryChecker.IsEnabled()) {
|
|
status = _AlignRetryChecker.GetRetryStatus(alignDelta);
|
|
|
|
if (status == AlignStatus.RETRY_COUNT_OVER) {
|
|
ResetRetrySpecChecker();
|
|
}
|
|
return status;
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 재시도 스펙 체커를 초기화 한다.
|
|
/// </summary>
|
|
public void ResetRetrySpecChecker() {
|
|
_AlignRetryChecker.Reset();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 재시도 횟수를 초기화 한다.
|
|
/// </summary>
|
|
public void ResetAlignCountChecker() {
|
|
_AlignCountChecker.Reset();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 재시도 스펙 체커와 재시도 횟수를 초기화 한다.
|
|
/// </summary>
|
|
public void ResetAll() {
|
|
ResetRetrySpecChecker();
|
|
ResetAlignCountChecker();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 회전 원점을 얻어온다.
|
|
/// </summary>
|
|
/// <returns>회전 원점 좌표</returns>
|
|
protected StaticPoint GetThetaOrigin() {
|
|
var calRecipe = VisionRecipeManager.GetCalRecipe(_SubSysInfo);
|
|
return (calRecipe.ThetaOrigin + _AlignRecipe.AlignThetaOrigin.Offset) as StaticPoint;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 보정량 T에 얼라인 타겟 회전량를 적용한다.
|
|
/// </summary>
|
|
/// <param name="alignT">얼라인 보정량 T</param>
|
|
/// <param name="alignAxis">얼라인 사용 축</param>
|
|
/// <param name="alignTgtDist">얼라인 타겟 회전량</param>
|
|
/// <returns>최종 얼라인 보정량 T</returns>
|
|
protected static Angle ApplyAlignAxisAndTargetDistT(
|
|
Angle alignT, eAlignAxis alignAxis, AlignTgtDist alignTgtDist) {
|
|
if (IsRotationEnabled(alignAxis)) {
|
|
if (alignTgtDist.Enabled) {
|
|
return Angle.CreateWithDegree(alignT.Degree - alignTgtDist.DistT);
|
|
}
|
|
return alignT;
|
|
}
|
|
|
|
return new Angle();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 보정량 X에 얼라인 타겟 거리 X를 적용한다.
|
|
/// </summary>
|
|
/// <param name="alignX">얼라인 보정량 X</param>
|
|
/// <param name="alignAxis">얼라인 사용 축</param>
|
|
/// <param name="alignTgtDist">얼라인 타겟 거리 X</param>
|
|
/// <returns>최종 얼라인 보정량 X</returns>
|
|
protected static double ApplyAlignAxisAndTargetDistX(
|
|
double alignX, eAlignAxis alignAxis, AlignTgtDist alignTgtDist) {
|
|
if (IsTranslationXEnabled(alignAxis)) {
|
|
if (alignTgtDist.Enabled) {
|
|
return alignX - alignTgtDist.DistX;
|
|
}
|
|
return alignX;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 보정량 Y에 얼라인 타겟 거리 Y를 적용한다.
|
|
/// </summary>
|
|
/// <param name="alignY">얼라인 보정량 Y</param>
|
|
/// <param name="alignAxis">얼라인 사용 축</param>
|
|
/// <param name="alignTgtDist">얼라인 타겟 거리 Y</param>
|
|
/// <returns>최종 얼라인 보정량 Y</returns>
|
|
protected static double ApplyAlignAxisAndTargetDistY(
|
|
double alignY, eAlignAxis alignAxis, AlignTgtDist alignTgtDist) {
|
|
if (IsTranslationYEnabled(alignAxis)) {
|
|
if (alignTgtDist.Enabled) {
|
|
return alignY - alignTgtDist.DistY;
|
|
}
|
|
return alignY;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 T축 사용여부를 체크한다.
|
|
/// </summary>
|
|
/// <param name="alignAxis">얼라인 축 정보</param>
|
|
/// <returns>체크 결과</returns>
|
|
protected static bool IsRotationEnabled(eAlignAxis alignAxis) {
|
|
return ((int)alignAxis & (int)eAlignAxis.T) != 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 XY축 사용여부를 체크한다.
|
|
/// </summary>
|
|
/// <param name="alignAxis">얼라인 축 정보</param>
|
|
/// <returns>체크 결과</returns>
|
|
protected static bool IsTranslationEnabled(eAlignAxis alignAxis) {
|
|
return ((int)alignAxis & (int)eAlignAxis.XY) != 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 X축 사용여부를 체크한다.
|
|
/// </summary>
|
|
/// <param name="alignAxis">얼라인 축 정보</param>
|
|
/// <returns>체크 결과</returns>
|
|
protected static bool IsTranslationXEnabled(eAlignAxis alignAxis) {
|
|
return ((int)alignAxis & (int)eAlignAxis.X) != 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 얼라인 Y축 사용여부를 체크한다.
|
|
/// </summary>
|
|
/// <param name="alignAxis">얼라인 축 정보</param>
|
|
/// <returns>체크 결과</returns>
|
|
protected static bool IsTranslationYEnabled(eAlignAxis alignAxis) {
|
|
return ((int)alignAxis & (int)eAlignAxis.Y) != 0;
|
|
}
|
|
}
|
|
} |