127 lines
4.6 KiB
C#
127 lines
4.6 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 {
|
|
public class AlignerImpl {
|
|
private static Logger _Logger = new Logger("AlignerImpl");
|
|
|
|
#region N-Point Align
|
|
/// <summary>
|
|
/// Source(이동)를 Target(고정)에 맞추는 얼라인 XYT
|
|
/// <para>
|
|
/// Head 얼라인 : Source(RefPoint), Target(MeaPoint)
|
|
/// </para>
|
|
/// <para>
|
|
/// Stage 얼라인 : Source(MeaPoint), Target(RefPoint)
|
|
/// </para>
|
|
/// </summary>
|
|
/// <param name="srcPoints">이동 포인트</param>
|
|
/// <param name="tgtPoints">고정 포인트</param>
|
|
/// <param name="flipY">Y축 반전</param>
|
|
/// <returns>보정량 XYT</returns>
|
|
public static AlignDelta CalculateT(
|
|
List<DoublePoint> srcPoints,
|
|
List<DoublePoint> tgtPoints,
|
|
int flipY) {
|
|
var alignDeltaT = NativeAligner.AlignT(srcPoints, tgtPoints);
|
|
alignDeltaT.T = CoordSystem.DenormalizeAngleDirection(alignDeltaT.T, flipY);
|
|
|
|
return alignDeltaT;
|
|
}
|
|
|
|
/// <summary>
|
|
/// ICP 알고리즘을 이용하여 Source(이동)를 Target(고정)에 맞추는 얼라인 XYT
|
|
/// <para>
|
|
/// Head 얼라인 : Source(RefPoint), Target(MeaPoint)
|
|
/// </para>
|
|
/// <para>
|
|
/// Stage 얼라인 : Source(MeaPoint), Target(RefPoint)
|
|
/// </para>
|
|
/// </summary>
|
|
/// <param name="srcPoints">이동 포인트</param>
|
|
/// <param name="tgtPoints">고정 포인트</param>
|
|
/// <param name="flipY">Y축 반전</param>
|
|
/// <returns>보정량 XYT</returns>
|
|
public static AlignDelta CalculateT_ICP(
|
|
List<DoublePoint> srcPoints,
|
|
List<DoublePoint> tgtPoints,
|
|
int flipY) {
|
|
var alignDeltaT = NativeAligner.AlignT_ICP(srcPoints, tgtPoints);
|
|
alignDeltaT.T = CoordSystem.DenormalizeAngleDirection(alignDeltaT.T, flipY);
|
|
|
|
return alignDeltaT;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Source(이동)를 Target(고정)에 맞추는 얼라인 XY
|
|
/// </summary>
|
|
/// <param name="srcPoints">이동 포인트 리스트</param>
|
|
/// <param name="tgtPoints">고정 포인트 리스트</param>
|
|
/// <returns>보정량 XY</returns>
|
|
public static AlignDelta CalculateXY(
|
|
List<DoublePoint> srcPoints,
|
|
List<DoublePoint> tgtPoints) {
|
|
var alignDeltaXY = NativeAligner.AlignXY(srcPoints, tgtPoints);
|
|
|
|
return new AlignDelta(
|
|
alignDeltaXY.Translation,
|
|
Angle.CreateWithDegree(0));
|
|
}
|
|
#endregion
|
|
|
|
#region 1-Point Align
|
|
/// <summary>
|
|
/// Source(이동)를 Target(고정)에 맞추는 얼라인 XYT
|
|
/// <para>
|
|
/// Head 얼라인 : Source(RefPoint), Target(MeaPoint)
|
|
/// </para>
|
|
/// <para>
|
|
/// Stage 얼라인 : Source(MeaPoint), Target(RefPoint)
|
|
/// </para>
|
|
/// </summary>
|
|
/// <param name="srcPoint">이동 포인트</param>
|
|
/// <param name="tgtPoint">고정 포인트</param>
|
|
/// <param name="theta">패턴 틀어진 각도 보정량</param>
|
|
/// <param name="thetaOrigin">회전 원점</param>
|
|
/// <param name="flipY">Y축 반전</param>
|
|
/// <returns>보정량 XYT</returns>
|
|
public static AlignDelta CalculateXYT(
|
|
DoublePoint srcPoint,
|
|
DoublePoint tgtPoint,
|
|
Angle theta,
|
|
DoublePoint thetaOrigin,
|
|
int flipY) {
|
|
|
|
var rotatedSrcPoints = MathUtil.Rotate(
|
|
srcPoint,
|
|
CoordSystem.NormalizeAngleDirection(theta, flipY),
|
|
thetaOrigin);
|
|
AlignDelta alignDeltaXY = CalculateXY(rotatedSrcPoints, tgtPoint);
|
|
|
|
return new AlignDelta(alignDeltaXY.Translation, theta);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Source(이동)를 Target(고정)에 맞추는 얼라인 XY
|
|
/// </summary>
|
|
/// <param name="srcPoint">이동 포인트</param>
|
|
/// <param name="tgtPoint">고정 포인트</param>
|
|
/// <returns>보정량 XY</returns>
|
|
public static AlignDelta CalculateXY(
|
|
DoublePoint srcPoint,
|
|
DoublePoint tgtPoint) {
|
|
var alignDeltaXY = NativeAligner.AlignXY(srcPoint, tgtPoint);
|
|
|
|
return new AlignDelta(
|
|
alignDeltaXY.Translation,
|
|
Angle.CreateWithDegree(0));
|
|
}
|
|
#endregion
|
|
}
|
|
}
|