public override DrawROIable AffineTransROI(HHomMat2D homMat2D)
{
double row1 = homMat2D.AffineTransPoint2d(Row1, Column1, out double column1);
double row2 = homMat2D.AffineTransPoint2d(Row2, Column2, out double column2);
return(new DrawRectangle1(row1, column1, row2, column2));
}
public override DrawROIable AffineTransROI(HHomMat2D homMat2D)
{
double startRow = homMat2D.AffineTransPoint2d(StartRow, StartColumn, out double startColumn);
double endRow = homMat2D.AffineTransPoint2d(EndColumn, EndRow, out double endColumn);
return(new DrawLine(startRow, startColumn, endRow, endColumn));
}
private double DistanceToClosestHandle2(double row, double col)
{
HTuple data = base.GetModeData();
double[] val = new double[_numHandles];
val[0] = HalconDotNet.HMisc.DistancePp(row, col, data[0], data[1]);
for (int i = 1; i < _numHandles; i++)
{
val[i] = 99999;
}
HHomMat2D mat2D = new HHomMat2D();
HTuple init_Arrow_Row = data[0];
HTuple init_Arrow_Col = data[1] + data[3] * 0.8;
mat2D = mat2D.HomMat2dRotate(data[2].D, data[0], data[1]);
double arrow_Row;
double arrow_Col;
arrow_Row = mat2D.AffineTransPoint2d(init_Arrow_Row, init_Arrow_Col, out arrow_Col);
val[_numHandles - 1] = HalconDotNet.HMisc.DistancePp(row, col, arrow_Row, arrow_Col);
double minvalue = 0.0;
int idx = 0;
if (this.MinValueAndIndex(val, out minvalue, out idx))
{
this._activeHandleIdx = idx;
}
return(minvalue);
}
public override DrawROIable AffineTransROI(HHomMat2D homMat2D)
{
double row = homMat2D.AffineTransPoint2d(Row, Column, out double column);
homMat2D.HomMat2dToAffinePar(out double sy, out double phi, out double theta, out double tx, out double ty);
return(new DrawEllipse(row, column, Phi + phi, Radius1, Radius2));
}
public override DrawROIable AffineTransROI(HHomMat2D homMat2D)
{
double row = homMat2D.AffineTransPoint2d(Row, Colulmn, out double column);
homMat2D.HomMat2dToAffinePar(out double sy, out double phi, out double theta, out double tx, out double ty);
return(new DrawRectangle2(row, column, Phi + phi, Length1, Length2));
}
/// <summary>
///更新操作点
/// </summary>
private void updateHandlePos()
{
_hom2D.HomMat2dIdentity();
_hom2D = _hom2D.HomMat2dTranslate(this._mid_col_x, this._mid_row_y);
_hom2D = _hom2D.HomMat2dRotateLocal(this._phi);
_tmp = _hom2D.HomMat2dScaleLocal(this._len1, this._len2);
_cols_x = _tmp.AffineTransPoint2d(_colsInit_x, _rowsInit_y, out _rows_y);
updateInterface();
}
private bool PixelToRobot(double img_x, double img_y, out double robot_x, out double robot_y)
{
robot_x = robot_y = -1.0;
if (RobotHommat != null)
{
try
{
robot_x = RobotHommat.AffineTransPoint2d(img_x, img_y, out robot_y);
return(true);
}
catch
{
return(false);
}
}
return(false);
}
public override DrawROIable AffineTransROI(HHomMat2D homMat2D)
{
double row = homMat2D.AffineTransPoint2d(Row, Column, out double column);
return(new DrawCircle(row, column, Radius));
}
public void MatrixTransToPixel(double x, double y, out double row, out double col)
{
row = WorldToPixel.AffineTransPoint2d(x, y, out col);
}
public void MatrixTransToWorld(double row, double col, out double x, out double y)
{
x = PixelToWorld.AffineTransPoint2d(row, col, out y);
}
public void TestMethod1()
{
//220.2 1531.51 - 226.423 340.238
//951.28 2967.35 - 303.982 335.088
//2386.24 2233.19 - 308.787 413.195
//1653.1 800.28 - 231.388 417.857
List <PosXY> campos = new List <PosXY>()
{
new PosXY()
{
X = 220.2, Y = 1531.51
},
new PosXY()
{
X = 951.28, Y = 2967.35
},
new PosXY()
{
X = 2386.24, Y = 2233.19
},
new PosXY()
{
X = 1653.1, Y = 800.28
},
};
List <PosXY> robpos = new List <PosXY>()
{
new PosXY()
{
X = -226.423, Y = 340.238
},
new PosXY()
{
X = -303.982, Y = 335.088
},
new PosXY()
{
X = -308.787, Y = 413.195
},
new PosXY()
{
X = -231.388, Y = 417.857
},
};
Console.WriteLine($"CAMPOS:\r\n{string.Join("\r\n", campos.Select(p => p.ToString()))}\r\n");
Console.WriteLine($"ROBOTPOS:\r\n{string.Join("\r\n", robpos.Select(p => p.ToString()))}\r\n");
//X缩放: 0.048
//Y缩放: 0.048
//坐标系旋转: 2.104
//Y轴错切: 0.004
//X轴平移: -157.528
//Y轴平移: 368.798
Console.WriteLine("HALCON CALIBRATION: \r\n");
HHomMat2D coorCalMat = new HHomMat2D();
HTuple cx = new HTuple(campos.Select(d => d.X).ToArray());
HTuple cy = new HTuple(campos.Select(d => d.Y).ToArray());
HTuple rx = new HTuple(robpos.Select(d => d.X).ToArray());
HTuple ry = new HTuple(robpos.Select(d => d.Y).ToArray());
coorCalMat.VectorToHomMat2d(cx, cy, rx, ry);
//calculate halcon errors
List <double> errors = new List <double>();
for (int i = 0; i < campos.Count; i++)
{
double rx1, ry1;
rx1 = coorCalMat.AffineTransPoint2d(campos[i].X, campos[i].Y, out ry1);
errors.Add((robpos[i].X - rx1) * (robpos[i].X - rx1) + (robpos[i].Y - ry1) * (robpos[i].Y - ry1));
}
Console.WriteLine($"HALCON FIT MAX ERROR: {errors.Max():F3}\r\n");
double sx, sy, phi, theta, tx, ty;
sx = coorCalMat.HomMat2dToAffinePar(out sy, out phi, out theta, out tx, out ty);
Console.WriteLine($"sx:{sx:F6}\r\nsy:{sy:F6}\r\nphi:{phi:F6}\r\ntheta:{theta:F6}\r\ntx:{tx:F6}\r\nty:{ty:F6}\r\n");
{
double rx1, ry1;
rx1 = coorCalMat.AffineTransPoint2d(220.2, 1531.51, out ry1);
Console.WriteLine("\r\nHALCON transform 220.2, 1531.51 into " + rx1.ToString("F6") + "," + ry1.ToString("F6"));
}
{
Console.WriteLine("\r\nRIGID ALIGN CALIBRATION: \r\n");
var res = RigidAlign.RigidAlign.Align(campos.Select(d => d.X).ToArray(), campos.Select(d => d.Y).ToArray(), robpos.Select(d => d.X).ToArray(), robpos.Select(d => d.Y).ToArray());
Console.WriteLine("RIGID ALIGN MAX ERROR:" + res.Item2.ToString("F6") + "\r\n");
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if (j == 2)
{
Console.Write($"{res.Item1[i, j]:F6}\r\n");
}
else
{
Console.Write($"{res.Item1[i, j]:F6},");
}
}
}
Console.WriteLine("\r\nRIGID ALIGN transform 220.2, 1531.51 into " + string.Join(",", RigidAlign.RigidAlign.Transform(new[] { 220.2, 1531.51 }, res.Item1).Select(d => d.ToString("F3"))));
}
}
/// <summary>
/// 设置点位
/// </summary>
/// <param name="col_x"></param>
/// <param name="row_y"></param>
public override void setOpoint(double col_x, double row_y)
{
double vX, vY, x = 0, y = 0;
switch (this._operationing)
{
case 0:
#region 无用代码
//_tmp = _hom2D.HomMat2dInvert();
//x = _tmp.AffineTransPoint2d(col_x, row_y, out y);
//this._len2 = Math.Abs(y);
//this._len1 = Math.Abs(x);
#region 无用代码
//col_x = col_x - this._mid_col_x;
//row_y = row_y - this._mid_row_y;
//checkForRange(col_x, row_y);
#endregion
//break;
#endregion
case 1:
#region 无用代码
//_tmp = _hom2D.HomMat2dInvert();
//x = _tmp.AffineTransPoint2d(col_x, row_y, out y);
//this._len2 = Math.Abs(y);
//this._len1 = Math.Abs(x);
#region 无用代码
//col_x = col_x - this._mid_col_x;
//row_y = row_y - this._mid_row_y;
//checkForRange(col_x, row_y);
#endregion
//break;
#endregion
case 2:
#region 无用代码
//_tmp = _hom2D.HomMat2dInvert();
//x = _tmp.AffineTransPoint2d(col_x, row_y, out y);
//this._len2 = Math.Abs(y);
//this._len1 = Math.Abs(x);
#region 无用代码
//col_x = col_x - this._mid_col_x;
//row_y = row_y - this._mid_row_y;
//checkForRange(col_x, row_y);
#endregion
//break;
#endregion
case 3:
_tmp = _hom2D.HomMat2dInvert();
x = _tmp.AffineTransPoint2d(col_x, row_y, out y);
this._len2 = Math.Abs(y);
this._len1 = Math.Abs(x);
//checkForRange(x,y);
#region 无用代码
//col_x = col_x - this._mid_col_x;
//row_y = row_y - this._mid_row_y;
//checkForRange(col_x, row_y);
#endregion
break;
case 4:
this._mid_col_x = col_x;
this._mid_row_y = row_y;
break;
case 5:
vY = row_y - _rows_y[4].D;
vX = col_x - _cols_x[4].D;
this._phi = Math.Atan2(vY, vX);
break;
default:
break;
}
updateHandlePos();
}
