282 lines
14 KiB
C#
282 lines
14 KiB
C#
using Aurora;
|
|
using Aurora.Api;
|
|
using VisionCommon;
|
|
using System;
|
|
using System.Reactive.Linq;
|
|
using System.Collections.Generic;
|
|
|
|
namespace VisionCalibrator {
|
|
/// <summary>
|
|
/// 자재 좌표를 모션 좌표로 변환하기 위한 클래스이다.
|
|
/// </summary>
|
|
public class CoordConverter {
|
|
private static Logger _StaticLogger = new Logger("StaticCoordConverter");
|
|
private Logger _Logger;
|
|
private IEventObservable _EventObservable;
|
|
private SubSysInfo _SubSysInfo;
|
|
private CalRecipe _CalRecipe;
|
|
private CoordConvRecipe _CoordConvRecipe;
|
|
|
|
/// <summary>
|
|
/// CoordConverter 생성자
|
|
/// </summary>
|
|
/// <param name="subSysInfo">서브 시스템 정보</param>
|
|
public CoordConverter(SubSysInfo subSysInfo) {
|
|
_SubSysInfo = subSysInfo;
|
|
_Logger = new Logger(_SubSysInfo.SubSysName);
|
|
_EventObservable = VisionEnv.AuroraEnv.EventObservable;
|
|
UpdateRecipe();
|
|
|
|
var observable = VisionEventId.GetRecipeUpdateObservable();
|
|
observable.Subscribe(_ => UpdateRecipe());
|
|
}
|
|
|
|
/// <summary>
|
|
/// 리스트로 전달된 자재 좌표를 모션 좌표로 변환한다.
|
|
/// </summary>
|
|
/// <param name="staticPoints">자재 좌표 리스트</param>
|
|
/// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
|
|
/// <param name="verticalDistance">패턴 간의 수직 거리</param>
|
|
/// <param name="llMotion">수평 Low, 수직 Low 모션 좌표</param>
|
|
/// <param name="hlMotion">수평 High, 수직 Low 모션 좌표</param>
|
|
/// <param name="lhMotion">수평 Low, 수직 High 모션 좌표</param>
|
|
/// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
|
|
/// <returns>모션 좌표 리스트</returns>
|
|
public List<MotionPoint> CoordConvert(
|
|
List<StaticPoint> staticPoints,
|
|
double horiozntalDistance, double verticalDistance,
|
|
MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {
|
|
List<MotionPoint> motionPoints = new List<MotionPoint>();
|
|
|
|
foreach (var staticPoint in staticPoints) {
|
|
motionPoints.Add(
|
|
CoordConvert(
|
|
staticPoint.X, staticPoint.Y,
|
|
horiozntalDistance, verticalDistance,
|
|
llMotion, hlMotion, lhMotion, hhMotion));
|
|
}
|
|
|
|
return motionPoints;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 좌표를 모션 좌표로 변환한다.
|
|
/// </summary>
|
|
/// <param name="staticPoint">자재 좌표</param>
|
|
/// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
|
|
/// <param name="verticalDistance">패턴 간의 수직 거리</param>
|
|
/// <param name="llMotion">수평 Low, 수직 Low 모션 좌표</param>
|
|
/// <param name="hlMotion">수평 High, 수직 Low 모션 좌표</param>
|
|
/// <param name="lhMotion">수평 Low, 수직 High 모션 좌표</param>
|
|
/// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
|
|
/// <returns>모션 좌표</returns>
|
|
public MotionPoint CoordConvert(
|
|
StaticPoint staticPoint,
|
|
double horiozntalDistance, double verticalDistance,
|
|
MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {
|
|
|
|
return CoordConvert(
|
|
staticPoint.X, staticPoint.Y,
|
|
horiozntalDistance, verticalDistance,
|
|
llMotion, hlMotion, lhMotion, hhMotion);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 좌표를 모션 좌표로 변환한다.
|
|
/// </summary>
|
|
/// <param name="staticPointX">자재 X좌표</param>
|
|
/// <param name="staticPointY">자재 Y좌표</param>
|
|
/// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
|
|
/// <param name="verticalDistance">패턴 간의 수직 거리</param>
|
|
/// <param name="llMotion">수평 Low, 수직 Low 모션 좌표</param>
|
|
/// <param name="hlMotion">수평 High, 수직 Low 모션 좌표</param>
|
|
/// <param name="lhMotion">수평 Low, 수직 High 모션 좌표</param>
|
|
/// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
|
|
/// <returns>모션 좌표</returns>
|
|
public MotionPoint CoordConvert(
|
|
double staticPointX, double staticPointY,
|
|
double horiozntalDistance, double verticalDistance,
|
|
MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {
|
|
|
|
var translatedPoint = new StaticPoint(
|
|
staticPointX - _CalRecipe.ThetaOrigin.X,
|
|
staticPointY - _CalRecipe.ThetaOrigin.Y);
|
|
|
|
var motionPoint = CoordConvertInternal(
|
|
translatedPoint.X, translatedPoint.Y,
|
|
horiozntalDistance, verticalDistance,
|
|
llMotion, hlMotion, lhMotion, hhMotion,
|
|
_CalRecipe.CoordSystem.Clone() as CoordSystem,
|
|
_CoordConvRecipe.CoordSystem.Clone() as CoordSystem);
|
|
_Logger.I($"{new StaticPoint(staticPointX, staticPointY)} -> translated : {translatedPoint} -> {motionPoint}");
|
|
|
|
return motionPoint;
|
|
}
|
|
|
|
private static MotionPoint CoordConvertInternal(
|
|
double staticPointX, double staticPointY,
|
|
double horizonntalDist, double verticalDist,
|
|
MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion,
|
|
CoordSystem csStatic, CoordSystem csMotion) {
|
|
|
|
return NativeCalibrator.CoordConveter(
|
|
staticPointX, staticPointY,
|
|
horizonntalDist, verticalDist,
|
|
new PointData() { X = llMotion.X, Y = llMotion.Y },
|
|
new PointData() { X = hlMotion.X, Y = hlMotion.Y },
|
|
new PointData() { X = lhMotion.X, Y = lhMotion.Y },
|
|
new PointData() { X = hhMotion.X, Y = hhMotion.Y },
|
|
csStatic.RotationX.Radian, (int)csStatic.FlipY,
|
|
csMotion.RotationX.Radian, (int)csMotion.FlipY);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 모션 좌표를 자재 좌표로 변환한다.
|
|
/// </summary>
|
|
/// <param name="motionPoint">변환하고 하는 모션 좌표</param>
|
|
/// <param name="s1">자재 좌표 1</param>
|
|
/// <param name="s2">자재 좌표 2/param>
|
|
/// <param name="s3">자재 좌표 3/param>
|
|
/// <param name="s4">자재 좌표 4/param>
|
|
/// <param name="m1">모션 좌표 1</param>
|
|
/// <param name="m2">모션 좌표 2</param>
|
|
/// <param name="m3">모션 좌표 3</param>
|
|
/// <param name="m4">모션 좌표 4</param>
|
|
/// <returns>변환된 자재 좌표</returns>
|
|
public static StaticPoint CoordConvertInverse(
|
|
MotionPoint motionPoint,
|
|
StaticPoint s1, StaticPoint s2, StaticPoint s3, StaticPoint s4,
|
|
MotionPoint m1, MotionPoint m2, MotionPoint m3, MotionPoint m4) {
|
|
|
|
var staticPoint = CoordConvertInverseInternal(
|
|
motionPoint.X, motionPoint.Y,
|
|
s1, s2, s3, s4,
|
|
m1, m2, m3, m4);
|
|
_StaticLogger.I($"{motionPoint} -> {staticPoint}");
|
|
|
|
return staticPoint;
|
|
}
|
|
|
|
private static StaticPoint CoordConvertInverseInternal(
|
|
double motionPointX, double motionPointY,
|
|
StaticPoint s1, StaticPoint s2, StaticPoint s3, StaticPoint s4,
|
|
MotionPoint m1, MotionPoint m2, MotionPoint m3, MotionPoint m4) {
|
|
var staticPoint = NativeCalibrator.CoordConvertInverse(
|
|
motionPointX, motionPointY,
|
|
ToPointData(s1), ToPointData(s2), ToPointData(s3), ToPointData(s4),
|
|
ToPointData(m1), ToPointData(m2), ToPointData(m3), ToPointData(m4));
|
|
|
|
return staticPoint;
|
|
}
|
|
|
|
private static PointData ToPointData(DoublePoint p) {
|
|
return new PointData() {
|
|
X = p.X,
|
|
Y = p.Y
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 좌표에 얼라인 보정량을 적용한다.
|
|
/// </summary>
|
|
/// <param name="staticPoint">자재 좌표</param>
|
|
/// <param name="offsetX">얼라인 X보정량</param>
|
|
/// <param name="offsetY">얼라인 Y보정량</param>
|
|
/// <param name="offsetT">얼라인 T보정량</param>
|
|
/// <returns>얼라인 보정량이 적용된 자재 좌표</returns>
|
|
public StaticPoint GetAlignedPoint(StaticPoint staticPoint, double offsetX, double offsetY, Angle offsetT) {
|
|
// 이미지 좌표계가 아닌 경우, 시계 방향으로 회전하기 위해 회전 값을 반전(* -1)
|
|
var rotAngle = offsetT * (int)_CalRecipe.CoordSystem.FlipY;
|
|
|
|
// 이미지 좌표 기준 시계 방향으로 회전
|
|
var rotatedPoint = MathUtil.Rotate(staticPoint, rotAngle, _CalRecipe.ThetaOrigin);
|
|
var alignedPoint = new StaticPoint(rotatedPoint.X + offsetX, rotatedPoint.Y + offsetY);
|
|
_Logger.I($"{staticPoint} -> aligned : {alignedPoint}");
|
|
|
|
return alignedPoint;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 리스트로 전달된 자재 좌표에 얼라인 보정량을 적용한다.
|
|
/// </summary>
|
|
/// <param name="staticPoints">자재 좌표 리스트</param>
|
|
/// <param name="offsetX">얼라인 X보정량</param>
|
|
/// <param name="offsetY">얼라인 Y보정량</param>
|
|
/// <param name="offsetT">얼라인 T보정량</param>
|
|
/// <returns>얼라인 보정량이 적용된 자재 좌표 리스트</returns>
|
|
public List<StaticPoint> GetAlignedPoints(List<StaticPoint> staticPoints, double offsetX, double offsetY, Angle offsetT) {
|
|
List<StaticPoint> alignedStaticPoints = new List<StaticPoint>();
|
|
foreach (var staticPoint in staticPoints) {
|
|
alignedStaticPoints.Add(
|
|
GetAlignedPoint(staticPoint, offsetX, offsetY, offsetT));
|
|
}
|
|
return alignedStaticPoints;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 리스트로 전달된 자재 좌표에 얼라인 보정량을 적용하고 모션 좌표로 변환한다.
|
|
/// </summary>
|
|
/// <param name="staticPoints">자재 좌표 리스트</param>
|
|
/// <param name="offsetX">얼라인 X보정량</param>
|
|
/// <param name="offsetY">얼라인 Y보정량</param>
|
|
/// <param name="offsetT">얼라인 T보정량</param>
|
|
/// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
|
|
/// <param name="verticalDistance">패턴 간의 수직 거리</param>
|
|
/// <param name="llMotion">수평 Low, 수직 Low 모션 좌표</param>
|
|
/// <param name="hlMotion">수평 High, 수직 Low 모션 좌표</param>
|
|
/// <param name="lhMotion">수평 Low, 수직 High 모션 좌표</param>
|
|
/// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
|
|
/// <returns>얼라인 보정량이 적용된 모션 좌표 리스트</returns>
|
|
public List<MotionPoint> CoordConvertWithAlignOffset(
|
|
List<StaticPoint> staticPoints,
|
|
double offsetX, double offsetY, Angle offsetT,
|
|
double horiozntalDistance, double verticalDistance,
|
|
MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {
|
|
List<MotionPoint> motionPoints = new List<MotionPoint>();
|
|
|
|
foreach (var staticPoint in staticPoints) {
|
|
motionPoints.Add(
|
|
CoordConvertWithAlignOffset(
|
|
staticPoint,
|
|
offsetX, offsetY, offsetT,
|
|
horiozntalDistance, verticalDistance,
|
|
llMotion, hlMotion, lhMotion, hhMotion));
|
|
}
|
|
|
|
return motionPoints;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자재 좌표에 얼라인 보정량을 적용하고 모션 좌표로 변환한다.
|
|
/// </summary>
|
|
/// <param name="staticPoint">자재 좌표 리스트</param>
|
|
/// <param name="offsetX">얼라인 X보정량</param>
|
|
/// <param name="offsetY">얼라인 Y보정량</param>
|
|
/// <param name="offsetT">얼라인 T보정량</param>
|
|
/// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
|
|
/// <param name="verticalDistance">패턴 간의 수직 거리</param>
|
|
/// <param name="llMotion">수평 Low, 수직 Low 모션 좌표</param>
|
|
/// <param name="hlMotion">수평 High, 수직 Low 모션 좌표</param>
|
|
/// <param name="lhMotion">수평 Low, 수직 High 모션 좌표</param>
|
|
/// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
|
|
/// <returns>얼라인 보정량이 적용된 모션 좌표</returns>
|
|
public MotionPoint CoordConvertWithAlignOffset(
|
|
StaticPoint staticPoint,
|
|
double offsetX, double offsetY, Angle offsetT,
|
|
double horiozntalDistance, double verticalDistance,
|
|
MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {
|
|
|
|
var alignedPoint = GetAlignedPoint(staticPoint, offsetX, offsetY, offsetT);
|
|
return CoordConvert(
|
|
alignedPoint.X, alignedPoint.Y,
|
|
horiozntalDistance, verticalDistance,
|
|
llMotion, hlMotion, lhMotion, hhMotion);
|
|
}
|
|
|
|
private void UpdateRecipe() {
|
|
_CalRecipe = VisionRecipeManager.GetCalRecipe(_SubSysInfo);
|
|
_CoordConvRecipe = VisionRecipeManager.GetCoordConvRecipe(_SubSysInfo);
|
|
}
|
|
}
|
|
}
|