479 lines
20 KiB
C#
479 lines
20 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora.Api;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using VisionCalibrator;
|
|
using VisionCommon;
|
|
using VisionCommonUI;
|
|
using VisionCommonUI.Resources;
|
|
|
|
namespace VisionSequence.Setup {
|
|
/// <summary>
|
|
/// 캘리브레이션 시퀀스 요청 파라미터
|
|
/// </summary>
|
|
public class CalSeqParams {
|
|
/// <summary>서브 시스템 정보</summary>
|
|
public SubSysInfo SubSysInfo { get; set; }
|
|
/// <summary>메인 화면 타입</summary>
|
|
public eDisplayType DisplayType { get; set; }
|
|
/// <summary>캘리브레이션 플랜아이템 리스트</summary>
|
|
public List<PlanItem> PlanItems { get; set; }
|
|
/// <summary>캘리브레이션 셋업 레시피</summary>
|
|
public CalSetupRecipe CalSetupRecipe { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캘리브레이션 시퀀스 수행 결과
|
|
/// </summary>
|
|
public class CalSeqResult {
|
|
/// <summary>X축 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
|
|
public List<ImagePoint> XPoints;
|
|
/// <summary>Y축 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
|
|
public List<ImagePoint> YPoints;
|
|
/// <summary>회전 원점 캘리브레이션 패턴 검출 이미지 좌표 리스트</summary>
|
|
public List<ImagePoint> ThetaPoints;
|
|
|
|
/// <summary>X축으로 1회 이동 거리</summary>
|
|
public double LengthX;
|
|
/// <summary>Y축으로 1회 이동 거리</summary>
|
|
public double LengthY;
|
|
/// <summary>T축으로 1회 회전 각도(Degree)</summary>
|
|
public double LengthTheta;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캘리브레이션 시퀀스 수행 결과 파라미터
|
|
/// </summary>
|
|
public class CalSeqResultParams {
|
|
/// <summary>캘리브레이션 시퀀스 수행 결과 리스트</summary>
|
|
public List<CalSeqResult> Results { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캘리브레이션 계산 결과
|
|
/// </summary>
|
|
public class CalCalculateResult {
|
|
/// <summary>카메라 회전 오차</summary>
|
|
public Angle ErrorRot;
|
|
/// <summary>이미지 픽셀 크기 X</summary>
|
|
public double PixelSizeX;
|
|
/// <summary>이미지 픽셀 크기 Y</summary>
|
|
public double PixelSizeY;
|
|
/// <summary>모터 회전 중심</summary>
|
|
public StaticPoint ThetaOrigin;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캘리브레이션 계산 결과 파라미터
|
|
/// </summary>
|
|
public class CalCalculateResultParams {
|
|
/// <summary>캘리브레이션 계산 결과 리스트</summary>
|
|
public List<CalCalculateResult> Results { get; set; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// 캘리브레이션 셋업 시퀀스
|
|
/// </summary>
|
|
public class SetupSeq_Calibration : SubSeqBase {
|
|
private readonly Logger LOG = new Logger(typeof(SetupSeq_Calibration).Name);
|
|
|
|
private CalSeqParams _Params;
|
|
private List<CalSubSeqResultParams> _CalResult;
|
|
private List<FovInfo> _FovInfos;
|
|
private List<FovRecipe> _FovRecipes;
|
|
private SubSysInfo _SubSysInfo;
|
|
private List<eCamId> _CamIds;
|
|
private int _CamCnt;
|
|
private string _SeqName;
|
|
private int _CurrentFov;
|
|
private bool _IsPatternNotFound;
|
|
private CalSetupRecipe _CalSetupRecipe;
|
|
|
|
|
|
#region Sequence Id
|
|
private const string SEQ_ID_INIT = "INIT";
|
|
private const string SEQ_ID_CHECK_PATTERN = "CHECK_PATTERN";
|
|
private const string SEQ_ID_UNDEFINED_PATTERN_NG = "SEQ_ID_UNDEFINED_PATTERN_NG";
|
|
private const string SEQ_ID_PREPARE = "PREPARE";
|
|
private const string SEQ_ID_RUN_CALIBRATION_SEQ = "RUN_CALIBRATION_SEQ";
|
|
private const string SEQ_ID_APPLY_DATA = "ApplyData";
|
|
private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
|
|
private const string SEQ_ID_CALCULATE = "CALCULATE";
|
|
#endregion
|
|
|
|
public override SequenceBuilder Builder(string seqName) {
|
|
_SeqName = seqName;
|
|
|
|
SequenceBuilder sb = new SequenceBuilder(1000);
|
|
SetActions(sb);
|
|
sb.OnStopped(OnStopped);
|
|
sb.OnForceError(OnForceError);
|
|
|
|
sb.Flows(
|
|
sb.Init
|
|
> sb[SEQ_ID_INIT]
|
|
> sb[SEQ_ID_CHECK_PATTERN]
|
|
> sb[SEQ_ID_PREPARE]
|
|
> sb[SEQ_ID_RUN_CALIBRATION_SEQ]
|
|
> sb[SEQ_ID_APPLY_DATA]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_CHECK_PATTERN]
|
|
> sb[SEQ_ID_UNDEFINED_PATTERN_NG]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_PREPARE]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_APPLY_DATA]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
> sb[SEQ_ID_CALCULATE]
|
|
> sb.Final
|
|
);
|
|
|
|
#if SEQ_DRAW
|
|
sb.Draw($"{GetType().Name}.pdf");
|
|
#endif
|
|
|
|
return sb;
|
|
}
|
|
private void SetActions(SequenceBuilder sb) {
|
|
var CalSubSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new SetupSeq_CalSub(), "SetupSeq_CalSub");
|
|
|
|
sb.AddAction(SEQ_ID_INIT, Init);
|
|
sb.AddDecision(SEQ_ID_CHECK_PATTERN, CheckPattern);
|
|
sb.AddAction(SEQ_ID_UNDEFINED_PATTERN_NG, UndefinedPatternNG);
|
|
sb.AddAction(SEQ_ID_PREPARE, Prepare);
|
|
sb.AddSubsequence(SEQ_ID_RUN_CALIBRATION_SEQ, CalSubSeqBuilder);
|
|
sb.AddDecision(SEQ_ID_APPLY_DATA, ApplyData);
|
|
sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
|
|
sb.AddAction(SEQ_ID_CALCULATE, Calculate);
|
|
}
|
|
|
|
private SequenceActionResult Init(SequenceContext context, string id, SequenceActionResult param) {
|
|
if (param.Param != null) {
|
|
_Params = param.Param as CalSeqParams;
|
|
}
|
|
_CalResult = new List<CalSubSeqResultParams>();
|
|
//_FovInfo = _Params.FovInfo;
|
|
_SubSysInfo = _Params.SubSysInfo;
|
|
_FovInfos = VisionRecipeManager.GetFovInfos(_SubSysInfo);
|
|
_FovRecipes = VisionRecipeManager.GetFovRecipes(_FovInfos);
|
|
_CamIds = VisionRecipeManager.GetCamIds(_FovRecipes);
|
|
_CamCnt = _CamIds.Count();
|
|
_CurrentFov = 0;
|
|
_IsPatternNotFound = false;
|
|
_CalSetupRecipe = _Params.CalSetupRecipe;
|
|
|
|
if (_SubSysInfo != null)
|
|
return SequenceAction.Ok();
|
|
else
|
|
return SequenceAction.Error();
|
|
}
|
|
|
|
private string CheckPattern(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
|
|
var subsysRecipe = VisionRecipeManager.GetSubSysRecipe(_SubSysInfo);
|
|
foreach (var fovRecipe in subsysRecipe.FovRecipes) {
|
|
if (fovRecipe.CalFovRecipe.PatternId == -1) {
|
|
_IsPatternNotFound = true;
|
|
return SEQ_ID_UNDEFINED_PATTERN_NG;
|
|
}
|
|
}
|
|
|
|
return SEQ_ID_PREPARE;
|
|
}
|
|
|
|
private SequenceActionResult UndefinedPatternNG(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
string errMsg = "Undefined PatternID Available";
|
|
PrintErrorLogAndShowErrorToast(LOG, errMsg);
|
|
|
|
_CalResult = new List<CalSubSeqResultParams>();
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult Prepare(SequenceContext context, string id, SequenceActionResult param) {
|
|
List<FovInfo> fovInfos = new List<FovInfo>();
|
|
fovInfos.Clear();
|
|
for (int i = 0; i < _CamCnt; i++) {
|
|
fovInfos.Add(_FovInfos[_CurrentFov + i]);
|
|
}
|
|
List<PlanItem> planItems = new List<PlanItem>();
|
|
planItems.Clear();
|
|
planItems = _Params.PlanItems.ConvertAll(item => new PlanItem(item.Step, item.Purpose, item.Target, item.Action, item.X, item.Y, item.T));
|
|
|
|
return SequenceAction.Ok(
|
|
new CalSubSeqParams() {
|
|
FovInfos = fovInfos,
|
|
PlanItems = planItems,
|
|
DisplayType = _Params.DisplayType,
|
|
CamCnt = _CamCnt
|
|
}); ;
|
|
}
|
|
|
|
private string ApplyData(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param) {
|
|
var calResult = param.Param as CalSubSeqResultParams;
|
|
|
|
if (calResult.IsPatternNotFound) {
|
|
LOG.E($"FOV {_CurrentFov} Calibration : Pattern Not Found");
|
|
_IsPatternNotFound = true;
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
}
|
|
|
|
_CalResult.Add(calResult);
|
|
|
|
_CurrentFov = _CamCnt + _CurrentFov;
|
|
|
|
if (_CurrentFov == _FovInfos.Count)
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
|
|
return SEQ_ID_PREPARE;
|
|
}
|
|
|
|
private SequenceActionResult SeqComplete(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (_IsPatternNotFound) {
|
|
return SequenceAction.Error();
|
|
}
|
|
|
|
CalSeqResultParams returnParam = new CalSeqResultParams();
|
|
if (returnParam.Results == null) {
|
|
returnParam.Results = new List<CalSeqResult>();
|
|
}
|
|
|
|
foreach (var calSubSeqResult in _CalResult) {
|
|
for (int i = 0; i < _CamCnt; i++) {
|
|
CalSeqResult calFovResult = new CalSeqResult();
|
|
|
|
if (_CalSetupRecipe.isX()) {
|
|
calFovResult.XPoints = calSubSeqResult.XPoints[i];
|
|
calFovResult.LengthX = calSubSeqResult.LengthX[i];
|
|
}
|
|
if (_CalSetupRecipe.isY()) {
|
|
calFovResult.YPoints = calSubSeqResult.YPoints[i];
|
|
calFovResult.LengthY = calSubSeqResult.LengthY[i];
|
|
}
|
|
|
|
if (_CalSetupRecipe.isT()) {
|
|
calFovResult.ThetaPoints = calSubSeqResult.ThetaPoints[i];
|
|
calFovResult.LengthTheta = calSubSeqResult.LengthTheta[i];
|
|
}
|
|
|
|
returnParam.Results.Add(calFovResult);
|
|
}
|
|
}
|
|
|
|
return SequenceAction.Ok(returnParam);
|
|
}
|
|
|
|
private SequenceActionResult Calculate(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
CalCalculateResultParams returnParam = new CalCalculateResultParams();
|
|
returnParam.Results = new List<CalCalculateResult>();
|
|
List<CalCalculateResult> fovCalResults = new List<CalCalculateResult>();
|
|
var calSeqResult = param.Param as CalSeqResultParams;
|
|
|
|
if (_IsPatternNotFound) {
|
|
return SequenceAction.Error();
|
|
}
|
|
var calResults = calSeqResult.Results;
|
|
|
|
for (int fovId = 0; fovId < calResults.Count(); fovId++) {
|
|
CalCalculateResult fovCalResult = new CalCalculateResult();
|
|
var calibrator = CalibratorManager.GetCalibrator(_FovInfos[fovId]);
|
|
var cameraAgent = CameraAgentManager.GetCameraAgent(_FovInfos[fovId]);
|
|
var imageSize = cameraAgent.GetImageSize();
|
|
Angle errorRotX = Angle.CreateWithDegree(0);
|
|
Angle errorRotY = Angle.CreateWithDegree(0);
|
|
double imgPixelX = 0;
|
|
double imgPixelY = 0;
|
|
|
|
if (_CalSetupRecipe.isX() && _CalSetupRecipe.isY()) {
|
|
if (calResults[fovId].XPoints != null) {
|
|
errorRotX = calibrator.FindStaticErrorRotation(calResults[fovId].XPoints, 'x');
|
|
imgPixelX = Math.Abs(calibrator.GetPixelSize(calResults[fovId].XPoints, calResults[fovId].LengthX, errorRotX.Radian, 'x'));
|
|
}
|
|
if (calResults[fovId].YPoints != null) {
|
|
errorRotY = calibrator.FindStaticErrorRotation(calResults[fovId].YPoints, 'y');
|
|
imgPixelY = Math.Abs(calibrator.GetPixelSize(calResults[fovId].YPoints, calResults[fovId].LengthY, errorRotY.Radian, 'y'));
|
|
}
|
|
fovCalResult.ErrorRot = Angle.CreateWithDegree((errorRotX.Degree + errorRotY.Degree) / 2);
|
|
fovCalResult.PixelSizeX = imgPixelX;
|
|
fovCalResult.PixelSizeY = imgPixelY;
|
|
// 3. theta origin
|
|
if (_CalSetupRecipe.isT()) {
|
|
StaticPoint thetaOrigin;
|
|
if (calResults[fovId].ThetaPoints != null) {
|
|
var thetaStaticPoints = new List<StaticPoint>();
|
|
foreach (var point in calResults[fovId].ThetaPoints) {
|
|
thetaStaticPoints.Add(calibrator.TransformToCalibratedSpace(point, imageSize, fovCalResult.ErrorRot, new StaticPoint(imgPixelX, imgPixelY)));
|
|
}
|
|
thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
|
|
}
|
|
else {
|
|
thetaOrigin = new StaticPoint(0, 0);
|
|
}
|
|
fovCalResult.ThetaOrigin = thetaOrigin;
|
|
}
|
|
}
|
|
|
|
else if (_CalSetupRecipe.isX()) {
|
|
if (calResults[fovId].XPoints != null) {
|
|
errorRotX = calibrator.FindStaticErrorRotation(calResults[fovId].XPoints, 'x');
|
|
imgPixelX = Math.Abs(calibrator.GetPixelSize(calResults[fovId].XPoints, calResults[fovId].LengthX, errorRotX.Radian, 'x'));
|
|
}
|
|
fovCalResult.ErrorRot = errorRotX;
|
|
fovCalResult.PixelSizeX = imgPixelX;
|
|
|
|
if (_CalSetupRecipe.isT()) {
|
|
StaticPoint thetaOrigin;
|
|
if (calResults[fovId].ThetaPoints != null) {
|
|
var thetaStaticPoints = new List<StaticPoint>();
|
|
foreach (var point in calResults[fovId].ThetaPoints) {
|
|
thetaStaticPoints.Add(calibrator.TransformToCalibratedSpaceWithPixelX(point, imageSize, fovCalResult.ErrorRot, fovCalResult.PixelSizeX));
|
|
}
|
|
thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
|
|
}
|
|
else {
|
|
thetaOrigin = new StaticPoint(0, 0);
|
|
}
|
|
fovCalResult.ThetaOrigin = thetaOrigin;
|
|
}
|
|
}
|
|
else if (_CalSetupRecipe.isY()) {
|
|
if (calResults[fovId].YPoints != null) {
|
|
errorRotY = calibrator.FindStaticErrorRotation(calResults[fovId].YPoints, 'y');
|
|
imgPixelY = Math.Abs(calibrator.GetPixelSize(calResults[fovId].YPoints, calResults[fovId].LengthY, errorRotY.Radian, 'y'));
|
|
}
|
|
fovCalResult.ErrorRot = errorRotY;
|
|
fovCalResult.PixelSizeY = imgPixelY;
|
|
|
|
if (_CalSetupRecipe.isT()) {
|
|
StaticPoint thetaOrigin;
|
|
if (calResults[fovId].ThetaPoints != null) {
|
|
var thetaStaticPoints = new List<StaticPoint>();
|
|
foreach (var point in calResults[fovId].ThetaPoints) {
|
|
thetaStaticPoints.Add(calibrator.TransformToCalibratedSpaceWithPixelY(point, imageSize, fovCalResult.ErrorRot, fovCalResult.PixelSizeY));
|
|
}
|
|
thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
|
|
}
|
|
else {
|
|
thetaOrigin = new StaticPoint(0, 0);
|
|
}
|
|
fovCalResult.ThetaOrigin = thetaOrigin;
|
|
}
|
|
}
|
|
|
|
else if (_CalSetupRecipe.isT()) {
|
|
// 3. theta origin
|
|
StaticPoint thetaOrigin;
|
|
if (calResults[fovId].ThetaPoints != null) {
|
|
var thetaStaticPoints = new List<StaticPoint>();
|
|
foreach (var point in calResults[fovId].ThetaPoints) {
|
|
thetaStaticPoints.Add(calibrator.TransformToCalibratedSpace(point, imageSize));
|
|
}
|
|
thetaOrigin = calibrator.GetThetaOriginPoint(thetaStaticPoints, Angle.CreateWithDegree(calResults[fovId].LengthTheta));
|
|
}
|
|
else {
|
|
thetaOrigin = new StaticPoint(0, 0);
|
|
}
|
|
fovCalResult.ThetaOrigin = thetaOrigin;
|
|
}
|
|
fovCalResults.Add(fovCalResult);
|
|
}
|
|
|
|
List<StaticPoint> thetaOrigins = new List<StaticPoint>();
|
|
StaticPoint averageThetaOrigin;
|
|
if (_CalSetupRecipe.isT()) {
|
|
for (int fovId = 0; fovId < calResults.Count(); fovId++) {
|
|
thetaOrigins.Add(fovCalResults[fovId].ThetaOrigin);
|
|
}
|
|
averageThetaOrigin = new StaticPoint(thetaOrigins.Average(item => item.X), thetaOrigins.Average(item => item.Y));
|
|
}
|
|
else {
|
|
averageThetaOrigin = new StaticPoint(0, 0);
|
|
}
|
|
|
|
foreach (var camId in _CamIds) {
|
|
CalCalculateResult camCalResult = new CalCalculateResult();
|
|
List<Angle> errorRots = new List<Angle>();
|
|
List<double> pixelSizesX = new List<double>();
|
|
List<double> pixelSizesY = new List<double>();
|
|
|
|
if (_CalSetupRecipe.isX() && _CalSetupRecipe.isY()) {
|
|
for (int fovId = 0; fovId < calResults.Count(); fovId++) {
|
|
if (_FovRecipes[fovId].CamRecipe.CamId == camId) {
|
|
errorRots.Add(fovCalResults[fovId].ErrorRot);
|
|
pixelSizesX.Add(fovCalResults[fovId].PixelSizeX);
|
|
pixelSizesY.Add(fovCalResults[fovId].PixelSizeY);
|
|
}
|
|
}
|
|
camCalResult.ErrorRot = Angle.CreateWithDegree(errorRots.Average(item => item.Degree));
|
|
camCalResult.PixelSizeX = pixelSizesX.Average();
|
|
camCalResult.PixelSizeY = pixelSizesY.Average();
|
|
}
|
|
else if (_CalSetupRecipe.isX()) {
|
|
for (int fovId = 0; fovId < calResults.Count(); fovId++) {
|
|
if (_FovRecipes[fovId].CamRecipe.CamId == camId) {
|
|
errorRots.Add(fovCalResults[fovId].ErrorRot);
|
|
pixelSizesX.Add(fovCalResults[fovId].PixelSizeX);
|
|
}
|
|
}
|
|
camCalResult.ErrorRot = Angle.CreateWithDegree(errorRots.Average(item => item.Degree));
|
|
camCalResult.PixelSizeX = pixelSizesX.Average();
|
|
}
|
|
else if (_CalSetupRecipe.isY()) {
|
|
for (int fovId = 0; fovId < calResults.Count(); fovId++) {
|
|
if (_FovRecipes[fovId].CamRecipe.CamId == camId) {
|
|
errorRots.Add(fovCalResults[fovId].ErrorRot);
|
|
pixelSizesY.Add(fovCalResults[fovId].PixelSizeY);
|
|
}
|
|
}
|
|
camCalResult.ErrorRot = Angle.CreateWithDegree(errorRots.Average(item => item.Degree));
|
|
camCalResult.PixelSizeY = pixelSizesY.Average();
|
|
}
|
|
if (_CalSetupRecipe.isT()) {
|
|
camCalResult.ThetaOrigin = averageThetaOrigin;
|
|
}
|
|
returnParam.Results.Add(camCalResult);
|
|
}
|
|
|
|
return SequenceAction.Ok(returnParam);
|
|
}
|
|
|
|
void OnStopped(
|
|
SequenceContext context) {
|
|
LOG.I($"{context.SequenceId} - STOP Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void OnForceError(
|
|
SequenceContext context, string id) {
|
|
LOG.I($"{context.SequenceId} - FORCE ERROR Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void HandleAbnormalFinish(
|
|
SequenceContext context) {
|
|
}
|
|
}
|
|
}
|