673 lines
30 KiB
C#
673 lines
30 KiB
C#
using Aurora.Api;
|
|
using VisionCommon;
|
|
using System.Windows.Input;
|
|
using Aurora.View;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Windows.Data;
|
|
using System.Linq;
|
|
using System.Windows.Media;
|
|
using VisionAligner;
|
|
using VisionCalibrator;
|
|
using DevExpress.Mvvm.Native;
|
|
using System.Text;
|
|
using DevExpress.Office.Utils;
|
|
using DevExpress.Xpf.Diagram.Native;
|
|
|
|
namespace VisionUI.ProcTest {
|
|
/// <summary>
|
|
/// 자재 공차 테스트 뷰 모델
|
|
/// </summary>
|
|
class MaterialToleranceTestViewModel : Aurora.View.NotifyPropertyChangedBase {
|
|
private Logger _Logger = new Logger(typeof(MaterialToleranceTestViewModel).Name);
|
|
|
|
private SubSysInfo _SubSysInfo;
|
|
|
|
private ObservableCollection<MaterialToleranceTestModel> canvasMainModelShapes;
|
|
/// <summary>
|
|
/// 캔번스 메인모델 모양 컬렉션(자재공차테스트모델)
|
|
/// </summary>
|
|
public ObservableCollection<MaterialToleranceTestModel> CanvasMainModelShapes {
|
|
get => canvasMainModelShapes;
|
|
set {
|
|
SetProperty(ref canvasMainModelShapes, value);
|
|
}
|
|
}
|
|
/// <summary>
|
|
/// 원의 포인트
|
|
/// </summary>
|
|
/// <param name="name">이름</param>
|
|
/// <returns>원 모델</returns>
|
|
public CircleModel CirclePoint(string name) {
|
|
MaterialToleranceTestModel m;
|
|
GetShape(name, out m);
|
|
return m as CircleModel;
|
|
}
|
|
private MaterialToleranceTestModel _refPos;
|
|
/// <summary>
|
|
/// 기준 위치
|
|
/// </summary>
|
|
public PathModel RefPos {
|
|
get {
|
|
GetShape("ref", out _refPos);
|
|
return _refPos as PathModel;
|
|
}
|
|
}
|
|
private MaterialToleranceTestModel _meaPos;
|
|
/// <summary>
|
|
/// 측정 위치
|
|
/// </summary>
|
|
public PathModel MeaPos {
|
|
get {
|
|
GetShape("mea", out _meaPos);
|
|
return _meaPos as PathModel;
|
|
}
|
|
}
|
|
private MaterialToleranceTestModel _norPos;
|
|
/// <summary>
|
|
/// 레퍼런스 위치
|
|
/// </summary>
|
|
public PathModel NorPos {
|
|
get {
|
|
GetShape("nor", out _norPos);
|
|
return _norPos as PathModel;
|
|
}
|
|
}
|
|
private MaterialToleranceTestModel _icpPos;
|
|
/// <summary>
|
|
/// ICP 위치
|
|
/// </summary>
|
|
public PathModel IcpPos {
|
|
get {
|
|
GetShape("icp", out _icpPos);
|
|
return _icpPos as PathModel;
|
|
}
|
|
}
|
|
// Recipe Info
|
|
private string _alignType;
|
|
/// <summary>
|
|
/// 얼라인 타입
|
|
/// </summary>
|
|
public string AlignType {
|
|
get => _alignType;
|
|
set { SetProperty(ref _alignType, value); }
|
|
}
|
|
private double _centerX;
|
|
/// <summary>
|
|
/// 센터 X 위치 ( Center Point Properties )
|
|
/// </summary>
|
|
public double CenterX {
|
|
get => _centerX;
|
|
set { SetProperty(ref _centerX, value); }
|
|
}
|
|
|
|
private double _centerY;
|
|
|
|
/// <summary>
|
|
/// 센터 Y 위치 ( Center Point Properties )
|
|
/// </summary>
|
|
public double CenterY {
|
|
get => _centerY;
|
|
set { SetProperty(ref _centerY, value); }
|
|
}
|
|
private string _infoText;
|
|
/// <summary>
|
|
/// 정보 텍스트
|
|
/// </summary>
|
|
public string InfoText {
|
|
get => _infoText;
|
|
set { SetProperty(ref _infoText, value); }
|
|
}
|
|
private bool _isChecked_n;
|
|
/// <summary>
|
|
/// 레퍼런스 확인 여부
|
|
/// </summary>
|
|
public bool IsChecked_N {
|
|
get => _isChecked_n;
|
|
set {
|
|
SetProperty(ref _isChecked_n, value);
|
|
if (value && NorPos != null) {
|
|
NorPos.ShapeVisible = System.Windows.Visibility.Visible;
|
|
}
|
|
else {
|
|
NorPos.ShapeVisible = System.Windows.Visibility.Hidden;
|
|
}
|
|
CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
|
|
}
|
|
}
|
|
private bool _isChecked;
|
|
/// <summary>
|
|
/// ICP 타입인지 확인여부
|
|
/// </summary>
|
|
public bool IsChecked_I {
|
|
get => _isChecked;
|
|
set {
|
|
SetProperty(ref _isChecked, value);
|
|
if (value && IcpPos != null) {
|
|
IcpPos.ShapeVisible=System.Windows.Visibility.Visible;
|
|
}
|
|
else {
|
|
IcpPos.ShapeVisible = System.Windows.Visibility.Hidden;
|
|
}
|
|
CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
|
|
}
|
|
}
|
|
private bool _isChecked_m;
|
|
/// <summary>
|
|
/// 측정포인트 타입인지 확인여부
|
|
/// </summary>
|
|
public bool IsChecked_M {
|
|
get => _isChecked_m;
|
|
set {
|
|
SetProperty(ref _isChecked_m, value);
|
|
if (value && MeaPos != null) {
|
|
MeaPos.ShapeVisible = System.Windows.Visibility.Visible;
|
|
}
|
|
else {
|
|
MeaPos.ShapeVisible = System.Windows.Visibility.Hidden;
|
|
}
|
|
CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
|
|
}
|
|
}
|
|
// Measure Point Properties
|
|
private double _x1, _y1, _x2, _y2, _x3, _y3, _x4, _y4, _ax, _ay, _at, _ox, _oy, rmse, rmse_icp;
|
|
private double _axa, _aya, _ata, _axi, _ayi,_ati;
|
|
/// <summary>측정 포인트 1의 X</summary>
|
|
public double MeasureX1 { get => _x1; set { SetProperty(ref _x1, value); } }
|
|
/// <summary>측정 포인트 1의 Y</summary>
|
|
public double MeasureY1 { get => _y1; set { SetProperty(ref _y1, value); } }
|
|
/// <summary>측정 포인트 2의 X</summary>
|
|
public double MeasureX2 { get => _x2; set { SetProperty(ref _x2, value); } }
|
|
/// <summary>측정 포인트 2의 Y</summary>
|
|
public double MeasureY2 { get => _y2; set { SetProperty(ref _y2, value); } }
|
|
/// <summary>측정 포인트 3의 X</summary>
|
|
public double MeasureX3 { get => _x3; set { SetProperty(ref _x3, value); } }
|
|
/// <summary>측정 포인트 3의 Y</summary>
|
|
public double MeasureY3 { get => _y3; set { SetProperty(ref _y3, value); } }
|
|
/// <summary>측정 포인트 4의 X</summary>
|
|
public double MeasureX4 { get => _x4; set { SetProperty(ref _x4, value); } }
|
|
/// <summary>측정 포인트 4의 Y</summary>
|
|
public double MeasureY4 { get => _y4; set { SetProperty(ref _y4, value); } }
|
|
/// <summary>얼라인 이동을 위한 dX</summary>
|
|
public double AlignX { get => _ax; set { SetProperty(ref _ax, value); } }
|
|
/// <summary>얼라인 이동을 위한 dY</summary>
|
|
public double AlignY { get => _ay; set { SetProperty(ref _ay, value); } }
|
|
/// <summary>얼라인 이동을 위한 dT</summary>
|
|
public double AlignT { get => _at; set { SetProperty(ref _at, value); } }
|
|
/// <summary>얼라인 결과값 dX (기존)</summary>
|
|
public double AlignX_A { get => _axa; set { SetProperty(ref _axa, value); } }
|
|
/// <summary>얼라인 결과값 dY (기존)</summary>
|
|
public double AlignY_A { get => _aya; set { SetProperty(ref _aya, value); } }
|
|
/// <summary>얼라인 결과값 dT (기존)</summary>
|
|
public double AlignT_A { get => _ata; set { SetProperty(ref _ata, value); } }
|
|
/// <summary>얼라인 결과값 dX (ICP)</summary>
|
|
public double AlignX_I { get => _axi; set { SetProperty(ref _axi, value); } }
|
|
/// <summary>얼라인 결과값 dY (ICP)</summary>
|
|
public double AlignY_I { get => _ayi; set { SetProperty(ref _ayi, value); } }
|
|
/// <summary>얼라인 결과값 dT (ICP)</summary>
|
|
public double AlignT_I { get => _ati; set { SetProperty(ref _ati, value); } }
|
|
/// <summary>Theta 기준점 X</summary>
|
|
public double ThetaOrigin_X { get => _ox; set { SetProperty(ref _ox, value); } }
|
|
/// <summary>Theta 기준점 Y</summary>
|
|
public double ThetaOrigin_Y { get => _oy; set { SetProperty(ref _oy, value); } }
|
|
/// <summary>기존 알고리즘 평균 제곱근 편차 </summary>
|
|
public double RMSE { get => rmse; set { SetProperty(ref rmse, value); } }
|
|
/// <summary>ICP 알고리즘 평균 제곱근 편차 </summary>
|
|
public double RMSE_ICP { get => rmse_icp; set { SetProperty(ref rmse_icp, value); } }
|
|
|
|
// Count Of FOV
|
|
private string _f1, _f2, _f3, _f4;
|
|
/// <summary>첫번째 Fov</summary>
|
|
public string Fov1 { get => _f1; set { SetProperty(ref _f1, value); } }
|
|
/// <summary>두번째 Fov</summary>
|
|
public string Fov2 { get => _f2; set { SetProperty(ref _f2, value); } }
|
|
/// <summary>세번째 Fov</summary>
|
|
public string Fov3 { get => _f3; set { SetProperty(ref _f3, value); } }
|
|
/// <summary>네번째 Fov</summary>
|
|
public string Fov4 { get => _f4; set { SetProperty(ref _f4, value); } }
|
|
|
|
|
|
// Command
|
|
/// <summary>적용 커맨드</summary>
|
|
public ICommand ApplyCommand { get; }
|
|
/// <summary>얼라인 커맨드</summary>
|
|
public ICommand AlignCommand { get; }
|
|
/// <summary>리셋 커맨드</summary>
|
|
public ICommand ResetCommand { get; }
|
|
/// <summary>이동 커맨드</summary>
|
|
public ICommand MoveCommand { get; }
|
|
|
|
private List<StaticPoint> refPoints = new List<StaticPoint>();
|
|
private StaticPoint thetaOrigin = null;
|
|
private AlignDelta alignDelta = new AlignDelta();
|
|
private VisionAligner.Aligner _Aligner;
|
|
private AlignRecipe _AlignRecipe = null;
|
|
private Calibrator _Calibrator;
|
|
private int FOV_count = 0;
|
|
|
|
private double cx = 400;
|
|
private double cy = 300;
|
|
|
|
private (double x, double y, double t) _align_res;
|
|
private (double x, double y, double t) _align_res_icp;
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="subSysInfo">서브시스템 정보</param>
|
|
public MaterialToleranceTestViewModel(SubSysInfo subSysInfo) {
|
|
_SubSysInfo = subSysInfo;
|
|
CanvasMainModelShapes = new ObservableCollection<MaterialToleranceTestModel>();
|
|
thetaOrigin = VisionRecipeManager.GetThetaOrigin(_SubSysInfo);
|
|
_Aligner = AlignAgentManager.GetAlignAgent(new AlignInfo(subSysInfo, eAlignId.Align1, "Align1"));
|
|
_AlignRecipe = VisionRecipeManager.GetAlignRecipe(new AlignInfo(subSysInfo, eAlignId.Align1, "Align1"));
|
|
var fovInfos = VisionRecipeManager.GetFovInfos(_SubSysInfo);
|
|
_Calibrator = CalibratorManager.GetCalibrator(fovInfos[(int)eFovId.Fov1]);
|
|
ApplyCommand = new DelegateCommand(Apply);
|
|
AlignCommand = new DelegateCommand(Align);
|
|
ResetCommand = new DelegateCommand(Reset);
|
|
MoveCommand = new DelegateCommand(Move);
|
|
|
|
CanvasMainModelShapes.Add(new PathModel("ref", ""));
|
|
CanvasMainModelShapes.Add(new PathModel("nor", ""));
|
|
CanvasMainModelShapes.Add(new PathModel("icp", ""));
|
|
CanvasMainModelShapes.Add(new PathModel("mea", ""));
|
|
|
|
CanvasMainModelShapes.Add(new CircleModel("origin", thetaOrigin.X + cx, thetaOrigin.Y + cy, 10));
|
|
//CanvasMainModelShapes.Add(new CircleModel("ref1", cx, cy, 10));
|
|
//CanvasMainModelShapes.Add(new CircleModel("ref2", cx, cy, 10));
|
|
//CanvasMainModelShapes.Add(new CircleModel("ref3", cx, cy, 10));
|
|
//CanvasMainModelShapes.Add(new CircleModel("ref4", cx, cy, 10));
|
|
CanvasMainModelShapes.Add(new CircleModel("mea1", cx, cy, 10));
|
|
CanvasMainModelShapes.Add(new CircleModel("mea2", cx, cy, 10));
|
|
CanvasMainModelShapes.Add(new CircleModel("mea3", cx, cy, 10));
|
|
CanvasMainModelShapes.Add(new CircleModel("mea4", cx, cy, 10));
|
|
CanvasMainModelShapes.Add(new CircleModel("center", cx, cy, 10));
|
|
|
|
FOV_count = VisionRecipeManager.GetFovCnt(_SubSysInfo);
|
|
ThetaOrigin_X = thetaOrigin.X;
|
|
ThetaOrigin_Y = thetaOrigin.Y;
|
|
Reset();
|
|
}
|
|
|
|
private void Move(object obj) {
|
|
List<DoublePoint> meas = new List<DoublePoint>();
|
|
|
|
for (int i = 1; i <= FOV_count; i++) {
|
|
if (i == 1) meas.Add(new StaticPoint(MeasureX1, MeasureY1));
|
|
else if (i == 2) meas.Add(new StaticPoint(MeasureX2, MeasureY2));
|
|
else if (i == 3) meas.Add(new StaticPoint(MeasureX3, MeasureY3));
|
|
else if (i == 4) meas.Add(new StaticPoint(MeasureX4, MeasureY4));
|
|
}
|
|
var n_measure = new List<DoublePoint>();
|
|
n_measure = ApplyAlignment(meas, AlignT, AlignX, AlignY, thetaOrigin);
|
|
|
|
for (int i = 1; i <= FOV_count; i++) {
|
|
if (i == 1) {
|
|
MeasureX1 = n_measure[(int)eFovId.Fov1].X;
|
|
MeasureY1 = n_measure[(int)eFovId.Fov1].Y;
|
|
}
|
|
else if (i == 2) {
|
|
MeasureX2 = n_measure[(int)eFovId.Fov2].X;
|
|
MeasureY2 = n_measure[(int)eFovId.Fov2].Y;
|
|
}
|
|
else if (i == 3) {
|
|
MeasureX3 = n_measure[(int)eFovId.Fov3].X;
|
|
MeasureY3 = n_measure[(int)eFovId.Fov3].Y;
|
|
}
|
|
else if (i == 4) {
|
|
MeasureX4 = n_measure[(int)eFovId.Fov4].X;
|
|
MeasureY4 = n_measure[(int)eFovId.Fov4].Y;
|
|
}
|
|
}
|
|
SetMeasureUI();
|
|
StringBuilder pathData = new StringBuilder();
|
|
pathData.Append($"M {MeasureX1 + cx},{MeasureY1 + cy} ");
|
|
for (int i = 2; i <= FOV_count; i++) {
|
|
if (i == 2) pathData.Append($"L {MeasureX2 + cx},{MeasureY2 + cy} ");
|
|
else if (i == 3) pathData.Append($"L {MeasureX3 + cx},{MeasureY3 + cy} ");
|
|
else if (i == 4) pathData.Append($"L {MeasureX4 + cx},{MeasureY4 + cy} ");
|
|
}
|
|
pathData.Append("Z");
|
|
MeaPos.PathData = pathData.ToString();
|
|
CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
|
|
}
|
|
|
|
private void Reset(object obj = null) {
|
|
NorPos.StrokeColor = new SolidColorBrush(Colors.Cyan);
|
|
IcpPos.StrokeColor = new SolidColorBrush(Colors.MediumPurple);
|
|
RefPos.StrokeColor = new SolidColorBrush(Colors.White);
|
|
MeaPos.StrokeColor = new SolidColorBrush(Colors.Yellow);
|
|
MeaPos.Opacity = 0.8;
|
|
CirclePoint("center").Tooltip = "x : 0\ny : 0";
|
|
if(thetaOrigin.X!=0 || thetaOrigin.Y != 0) {
|
|
CirclePoint("origin").Tooltip = $"x : {thetaOrigin.X}\nY : {thetaOrigin.Y}";
|
|
}
|
|
Fov1 = Fov2 = Fov3 = Fov4 = "0";
|
|
AlignType = _AlignRecipe.ProcParams.AlignMode.ToString();
|
|
List<StaticPoint> refpos = new List<StaticPoint>();
|
|
switch (FOV_count) {
|
|
case 4:
|
|
refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov4));
|
|
Fov4 = "1*";
|
|
goto case 3;
|
|
case 3:
|
|
refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov3));
|
|
Fov3 = "1*";
|
|
goto case 2;
|
|
case 2:
|
|
refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov2));
|
|
Fov2 = "1*";
|
|
goto case 1;
|
|
case 1:
|
|
refpos.Add(VisionRecipeManager.GetRefPoint(_SubSysInfo, eFovId.Fov1));
|
|
Fov1 = "1*";
|
|
CirclePoint("center").FillColor = new SolidColorBrush(Colors.Green);
|
|
break;
|
|
default:
|
|
CirclePoint("center").FillColor = new SolidColorBrush(Colors.Red);
|
|
break;
|
|
|
|
}
|
|
|
|
refPoints = SortRefAndSetMeaPoint(refpos);
|
|
SetMeasureUI();
|
|
var toShow = new List<StaticPoint>();
|
|
foreach (var point in refPoints) {
|
|
toShow.Add(new StaticPoint(point.X, point.Y));
|
|
}
|
|
AddOffsetsToPoints(toShow, cx, cy);
|
|
RefPos.PathData = $"M {toShow[(int)eFovId.Fov1].X},{toShow[(int)eFovId.Fov1].Y} " +
|
|
string.Join(" ", Enumerable.Range(1, toShow.Count - 1).Select(i =>
|
|
$"L {toShow[(int)eFovId.Fov1 + i].X},{toShow[(int)eFovId.Fov1 + i].Y}")) +
|
|
" Z";
|
|
MeaPos.PathData = "";
|
|
IcpPos.PathData = "";
|
|
NorPos.PathData = "";
|
|
AlignX = 0; AlignY = 0; AlignT = 0;
|
|
_align_res = (0, 0, 0);
|
|
_align_res_icp = (0, 0, 0);
|
|
RMSE = RMSE_ICP = 0;
|
|
AlignX_A = 0;
|
|
AlignY_A = 0;
|
|
AlignT_A = 0;
|
|
AlignX_I = 0;
|
|
AlignY_I = 0;
|
|
AlignT_I = 0;
|
|
IsChecked_I = IsChecked_N = IsChecked_M = true;
|
|
CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
|
|
|
|
if (FOV_count<2) {
|
|
InfoText = "One-Point Align does not involve calulating T";
|
|
}
|
|
else {
|
|
InfoText = "This represents the measure points ( reset to referece points )";
|
|
}
|
|
}
|
|
|
|
private void Align(object obj) {
|
|
|
|
List<DoublePoint> meas = new List<DoublePoint>();
|
|
List<DoublePoint> refs = new List<DoublePoint>();
|
|
|
|
for (int i = 1; i <= FOV_count; i++) {
|
|
if (i == 1) meas.Add(new StaticPoint(MeasureX1, MeasureY1));
|
|
else if (i == 2) meas.Add(new StaticPoint(MeasureX2, MeasureY2));
|
|
else if (i == 3) meas.Add(new StaticPoint(MeasureX3, MeasureY3));
|
|
else if (i == 4) meas.Add(new StaticPoint(MeasureX4, MeasureY4));
|
|
}
|
|
for (int i = 1; i <= FOV_count; i++) {
|
|
if (i == 1) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov1].X, refPoints[(int)eFovId.Fov1].Y));
|
|
else if (i == 2) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov2].X, refPoints[(int)eFovId.Fov2].Y));
|
|
else if (i == 3) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov3].X, refPoints[(int)eFovId.Fov3].Y));
|
|
else if (i == 4) refs.Add(new StaticPoint(refPoints[(int)eFovId.Fov4].X, refPoints[(int)eFovId.Fov4].Y));
|
|
}
|
|
|
|
AlignResult res;
|
|
if (FOV_count<2) {
|
|
res = _Aligner.Align(meas[0], refs[0], new Angle(), (int)_Calibrator.GetFlipY());
|
|
_align_res_icp= _align_res = (res.Delta.X, res.Delta.Y, res.Delta.T.Degree);
|
|
}
|
|
else if(FOV_count==2){
|
|
res = _Aligner.Align(meas, refs, (int)_Calibrator.GetFlipY());
|
|
_align_res_icp =_align_res = (res.Delta.X, res.Delta.Y, res.Delta.T.Degree);
|
|
}
|
|
else {
|
|
res = _Aligner.Align(meas, refs, (int)_Calibrator.GetFlipY());
|
|
var res_new = CalculateICP(meas, refs);
|
|
_align_res = (res.Delta.X, res.Delta.Y, res.Delta.T.Degree);
|
|
_align_res_icp = (res_new.translation[0], res_new.translation[1], res_new.rotate * (180 / Math.PI));
|
|
}
|
|
|
|
var meatest = ApplyAlignment(meas, _align_res.t, _align_res.x, _align_res.y, thetaOrigin);
|
|
var meatest_icp = ApplyAlignment(meas, _align_res_icp.t, _align_res_icp.x, _align_res_icp.y, thetaOrigin);
|
|
RMSE = CalculateRMSE(refPoints, meatest);
|
|
RMSE_ICP = CalculateRMSE(refPoints, meatest_icp);
|
|
AlignX_A = _align_res.x;
|
|
AlignY_A = _align_res.y;
|
|
AlignT_A = _align_res.t;
|
|
AlignX_I = _align_res_icp.x;
|
|
AlignY_I = _align_res_icp.y;
|
|
AlignT_I = _align_res_icp.t;
|
|
StringBuilder pathData = new StringBuilder();
|
|
pathData.Append($"M {meatest[0].X + cx},{meatest[0].Y + cy} ");
|
|
for (int i = 2; i <= FOV_count; i++) {
|
|
if (i == 2) pathData.Append($"L {meatest[1].X + cx},{meatest[1].Y + cy} ");
|
|
else if (i == 3) pathData.Append($"L {meatest[2].X + cx},{meatest[2].Y + cy} ");
|
|
else if (i == 4) pathData.Append($"L {meatest[3].X + cx},{meatest[3].Y + cy} ");
|
|
}
|
|
pathData.Append("Z");
|
|
NorPos.PathData = pathData.ToString();
|
|
pathData.Clear();
|
|
pathData.Append($"M {meatest_icp[0].X + cx},{meatest_icp[0].Y + cy} ");
|
|
for (int i = 2; i <= FOV_count; i++) {
|
|
if (i == 2) pathData.Append($"L {meatest_icp[1].X + cx},{meatest_icp[1].Y + cy} ");
|
|
else if (i == 3) pathData.Append($"L {meatest_icp[2].X + cx},{meatest_icp[2].Y + cy} ");
|
|
else if (i == 4) pathData.Append($"L {meatest_icp[3].X + cx},{meatest_icp[3].Y + cy} ");
|
|
}
|
|
pathData.Append("Z");
|
|
IcpPos.PathData = pathData.ToString();
|
|
CollectionViewSource.GetDefaultView(CanvasMainModelShapes).Refresh();
|
|
}
|
|
|
|
private void Apply(object obj) {
|
|
|
|
|
|
}
|
|
|
|
private void SetMeasureUI() {
|
|
CirclePoint("mea1").FillColor = new SolidColorBrush(Colors.Pink);
|
|
CirclePoint("mea2").FillColor = new SolidColorBrush(Colors.Pink);
|
|
CirclePoint("mea3").FillColor = new SolidColorBrush(Colors.Pink);
|
|
CirclePoint("mea4").FillColor = new SolidColorBrush(Colors.Pink);
|
|
CirclePoint("mea1").Tooltip = $" x1 : +{MeasureX1:F2}\ny1 : +{MeasureY1:F2}";
|
|
CirclePoint("mea2").Tooltip = $" x2 : {MeasureX2:F2}\ny2 : +{MeasureY2:F2}";
|
|
CirclePoint("mea3").Tooltip = $" x3 : {MeasureX3:F2}\ny3 : {MeasureY3:F2}";
|
|
CirclePoint("mea4").Tooltip = $" x4 : +{MeasureX4:F2}\ny4 : {MeasureY4:F2}";
|
|
CirclePoint("mea1").SetTopLeft(MeasureY1 + cy, MeasureX1 + cx);
|
|
CirclePoint("mea2").SetTopLeft(MeasureY2 + cy, MeasureX2 + cx);
|
|
CirclePoint("mea3").SetTopLeft(MeasureY3 + cy, MeasureX3 + cx);
|
|
CirclePoint("mea4").SetTopLeft(MeasureY4 + cy, MeasureX4 + cx);
|
|
}
|
|
|
|
private List<StaticPoint> SortRefAndSetMeaPoint(List<StaticPoint> points) {
|
|
CirclePoint("mea1").ShapeVisible = System.Windows.Visibility.Hidden;
|
|
CirclePoint("mea2").ShapeVisible = System.Windows.Visibility.Hidden;
|
|
CirclePoint("mea3").ShapeVisible = System.Windows.Visibility.Hidden;
|
|
CirclePoint("mea4").ShapeVisible = System.Windows.Visibility.Hidden;
|
|
// 1 -> 2 -> 3 -> 4 분면 순서
|
|
var res = points.OrderBy(p => GetQuadrantOrder(p)).ToList();
|
|
switch (res.Count) {
|
|
case 4:
|
|
MeasureX4 = res[(int)eFovId.Fov4].X;
|
|
MeasureY4 = res[(int)eFovId.Fov4].Y;
|
|
CirclePoint("mea4").ShapeVisible = System.Windows.Visibility.Visible;
|
|
goto case 3;
|
|
case 3:
|
|
MeasureX3 = res[(int)eFovId.Fov3].X;
|
|
MeasureY3 = res[(int)eFovId.Fov3].Y;
|
|
CirclePoint("mea3").ShapeVisible = System.Windows.Visibility.Visible;
|
|
goto case 2;
|
|
case 2:
|
|
MeasureX2 = res[(int)eFovId.Fov2].X;
|
|
MeasureY2 = res[(int)eFovId.Fov2].Y;
|
|
CirclePoint("mea2").ShapeVisible = System.Windows.Visibility.Visible;
|
|
goto case 1;
|
|
case 1:
|
|
MeasureX1 = res[(int)eFovId.Fov1].X;
|
|
MeasureY1 = res[(int)eFovId.Fov1].Y;
|
|
CirclePoint("mea1").ShapeVisible = System.Windows.Visibility.Visible;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return res;
|
|
|
|
}
|
|
|
|
private void ResetCenter(double centerX, double centerY) {
|
|
CirclePoint("center").Left = centerX - CirclePoint("center").EllipseWidth;
|
|
CirclePoint("center").Top = centerY - CirclePoint("center").EllipseWidth;
|
|
}
|
|
/// <summary>
|
|
/// 모양 가져오기
|
|
/// </summary>
|
|
/// <param name="name">이름</param>
|
|
/// <param name="shapemodel">모양모델</param>
|
|
/// <returns></returns>
|
|
public eShapeType GetShape(string name, out MaterialToleranceTestModel shapemodel) {
|
|
MaterialToleranceTestModel matchedModel = CanvasMainModelShapes.FirstOrDefault(shape => shape.Name == name);
|
|
shapemodel = matchedModel;
|
|
switch (matchedModel) {
|
|
case CircleModel m:
|
|
return eShapeType.Circle;
|
|
//case RectangleModel m:
|
|
// return eShapeType.Rectangle;
|
|
//case TextBoxModel m:
|
|
// return eShapeType.TextBox;
|
|
case PathModel m:
|
|
return eShapeType.Path;
|
|
default:
|
|
return eShapeType.Path;
|
|
}
|
|
}
|
|
|
|
static int GetQuadrantOrder(StaticPoint p) {
|
|
if (p.X >= 0 && p.Y >= 0) return 1; //이미지 좌표계상 ++
|
|
if (p.X < 0 && p.Y >= 0) return 2; // -+
|
|
if (p.X < 0 && p.Y < 0) return 3; // --
|
|
if (p.X >= 0 && p.Y < 0) return 4; // +-
|
|
return 0;
|
|
}
|
|
|
|
static void AddOffsetsToPoints(List<StaticPoint> points, double cx, double cy) {
|
|
foreach (var point in points) {
|
|
point.X += cx;
|
|
point.Y += cy;
|
|
}
|
|
}
|
|
|
|
private List<DoublePoint> ApplyAlignment(List<DoublePoint> points, AlignDelta delta, DoublePoint rotationCenter) {
|
|
var transformedPoints = new List<DoublePoint>();
|
|
|
|
foreach (var point in points) {
|
|
// 회전 중심으로 이동
|
|
double translatedX = point.X - rotationCenter.X;
|
|
double translatedY = point.Y - rotationCenter.Y;
|
|
|
|
// 회전 변환 적용 (라디안 단위)
|
|
double rad = delta.T.Radian;
|
|
double rotatedX = translatedX * Math.Cos(rad) - translatedY * Math.Sin(rad);
|
|
double rotatedY = translatedX * Math.Sin(rad) + translatedY * Math.Cos(rad);
|
|
|
|
// 회전 중심 복원 및 이동 변환 적용
|
|
double newX = rotatedX + rotationCenter.X + delta.X;
|
|
double newY = rotatedY + rotationCenter.Y + delta.Y;
|
|
|
|
transformedPoints.Add(new StaticPoint(newX, newY));
|
|
}
|
|
|
|
return transformedPoints;
|
|
}
|
|
|
|
private List<DoublePoint> ApplyAlignment(List<DoublePoint> points, double t, double x, double y, DoublePoint rotationCenter) {
|
|
var transformedPoints = new List<DoublePoint>();
|
|
|
|
foreach (var point in points) {
|
|
// 회전 중심으로 이동
|
|
double translatedX = point.X - rotationCenter.X;
|
|
double translatedY = point.Y - rotationCenter.Y;
|
|
|
|
// 회전 변환 적용 (라디안 단위)
|
|
double rad = t * (Math.PI / 180);
|
|
double rotatedX = translatedX * Math.Cos(rad) - translatedY * Math.Sin(rad);
|
|
double rotatedY = translatedX * Math.Sin(rad) + translatedY * Math.Cos(rad);
|
|
|
|
// 회전 중심 복원 및 이동 변환 적용
|
|
double newX = rotatedX + rotationCenter.X + x;
|
|
double newY = rotatedY + rotationCenter.Y + y;
|
|
|
|
transformedPoints.Add(new StaticPoint(newX, newY));
|
|
}
|
|
|
|
return transformedPoints;
|
|
}
|
|
|
|
private static double CalculateRMSE(List<StaticPoint> refPoints, List<DoublePoint> meaPoints) {
|
|
if (refPoints == null || meaPoints == null) return -1;
|
|
int count = refPoints.Count;
|
|
double sumOfSquaredDistances = 0.0;
|
|
for (int i = 0; i < count; i++) {
|
|
double diffX = refPoints[i].X - meaPoints[i].X;
|
|
double diffY = refPoints[i].Y - meaPoints[i].Y;
|
|
double squareDistance = diffX * diffX + diffY * diffY;
|
|
sumOfSquaredDistances += squareDistance;
|
|
}
|
|
double meanSquaredDistance = Math.Sqrt(sumOfSquaredDistances/count);
|
|
return meanSquaredDistance;
|
|
}
|
|
/// <summary>
|
|
/// ICP 알고리즘 계산
|
|
/// </summary>
|
|
/// <param name="meas">평균값들</param>
|
|
/// <param name="refs">기준점들</param>
|
|
/// <returns></returns>
|
|
public static (double rotate, double[] translation) CalculateICP(List<DoublePoint> meas, List<DoublePoint> refs) {
|
|
int n = meas.Count;
|
|
DoublePoint centroidMeas = CalculateCentroid(meas);
|
|
DoublePoint centroidRefs = CalculateCentroid(refs);
|
|
List<DoublePoint> centeredMeas = CenterPoints(meas, centroidMeas);
|
|
List<DoublePoint> centeredRefs = CenterPoints(refs, centroidRefs);
|
|
double Sxx = 0, Sxy = 0, Syx = 0, Syy = 0;
|
|
for (int i = 0; i < n; i++) {
|
|
Sxx += centeredMeas[i].X * centeredRefs[i].X;
|
|
Sxy += centeredMeas[i].X * centeredRefs[i].Y;
|
|
Syx += centeredMeas[i].Y * centeredRefs[i].X;
|
|
Syy += centeredMeas[i].Y * centeredRefs[i].Y;
|
|
}
|
|
double theta = Math.Atan2(Sxy - Syx, Sxx + Syy);
|
|
double[] translation = new double[2];
|
|
translation[0] = centroidRefs.X - (centroidMeas.X * Math.Cos(theta) - centroidMeas.Y * Math.Sin(theta));
|
|
translation[1] = centroidRefs.Y - (centroidMeas.X * Math.Sin(theta) + centroidMeas.Y * Math.Cos(theta));
|
|
return (theta, translation);
|
|
}
|
|
private static DoublePoint CalculateCentroid(List<DoublePoint> points) {
|
|
double sumX = 0, sumY = 0;
|
|
foreach (var point in points) {
|
|
sumX += point.X;
|
|
sumY += point.Y;
|
|
}
|
|
return new StaticPoint { X = sumX / points.Count, Y = sumY / points.Count };
|
|
}
|
|
private static List<DoublePoint> CenterPoints(List<DoublePoint> points, DoublePoint centroid) {
|
|
List<DoublePoint> centeredPoints = new List<DoublePoint>();
|
|
foreach (var point in points) {
|
|
centeredPoints.Add(new StaticPoint {
|
|
X = point.X - centroid.X,
|
|
Y = point.Y - centroid.Y
|
|
});
|
|
}
|
|
return centeredPoints;
|
|
}
|
|
}
|
|
} |