private void ReadCalibData()
{
if (System.IO.File.Exists("./Model/Calib.dat"))
{
BinaryReader br;
br = new BinaryReader(new FileStream("./Model/Calib.dat", FileMode.Open));
HTuple Image_X = new HTuple(), Image_Y = new HTuple(), Robot_X = new HTuple(), Robot_Y = new HTuple();
int Num = (int)br.ReadDouble();
for (int i = 0; i < Num; i++)
{
DataGridViewRow row = new DataGridViewRow();
int Count = Calib_dataView.Rows.Add(row);
double i_x = br.ReadDouble();
double i_y = br.ReadDouble();
double r_x = br.ReadDouble();
double r_y = br.ReadDouble();
Calib_dataView.Rows[Count].Cells[0].Value = i_x;
Calib_dataView.Rows[Count].Cells[1].Value = i_y;
Calib_dataView.Rows[Count].Cells[2].Value = r_x;
Calib_dataView.Rows[Count].Cells[3].Value = r_y;
Image_X = Image_X.TupleConcat(i_x);
Image_Y = Image_Y.TupleConcat(i_y);
Robot_X = Robot_X.TupleConcat(r_x);
Robot_Y = Robot_Y.TupleConcat(r_y);
}
try
{
RobotHommat = new HHomMat2D();
RobotHommat.VectorToHomMat2d(Image_X, Image_Y, Robot_X, Robot_Y);
}
catch {
RobotHommat = null;
}
}
}
private void Btn_SaveCalib_Click(object sender, EventArgs e)
{
RobotHommat = null;
int Num = Calib_dataView.RowCount - 1;
HTuple Image_X = new HTuple(), Image_Y = new HTuple(), Robot_X = new HTuple(), Robot_Y = new HTuple();
for (int i = 0; i < Num; i++)
{
double i_x, i_y, r_x, r_y;
try
{
i_x = Convert.ToDouble(Calib_dataView.Rows[i].Cells[0].Value.ToString());
i_y = Convert.ToDouble(Calib_dataView.Rows[i].Cells[1].Value.ToString());
r_x = Convert.ToDouble(Calib_dataView.Rows[i].Cells[2].Value.ToString());
r_y = Convert.ToDouble(Calib_dataView.Rows[i].Cells[3].Value.ToString());
}
catch { continue; }
Image_X = Image_X.TupleConcat(i_x);
Image_Y = Image_Y.TupleConcat(i_y);
Robot_X = Robot_X.TupleConcat(r_x);
Robot_Y = Robot_Y.TupleConcat(r_y);
}
try
{
HTuple Length;
HOperatorSet.TupleLength(Image_X, out Length);
if (Length[0].I < 3)
{
ShowStatus("mes:有效标定点不足");
}
else
{
RobotHommat = new HHomMat2D();
RobotHommat.VectorToHomMat2d(Image_X, Image_Y, Robot_X, Robot_Y);
ShowStatus("mes:标定成功");
WriteCalibData(Image_X, Image_Y, Robot_X, Robot_Y);
}
}
catch
{
RobotHommat = null;
ShowStatus("mes:标定失败");
}
}
/// <summary>
/// 更具对应点，构建标定关系
/// </summary>
public void BuildTransferMatrix()
{
HTuple hv_worldX = new HTuple(), hv_worldY = new HTuple();
HTuple hv_imageR = new HTuple(), hv_imageC = new HTuple();
int count = CalibrateData.Count;
if (count < 3)
{
return;
}
for (int i = 0; i < count; i++)
{
hv_worldX[i] = CalibrateData[i].X;
hv_worldY[i] = CalibrateData[i].Y;
hv_imageR[i] = CalibrateData[i].PixelRow;
hv_imageC[i] = CalibrateData[i].PixelCol;
}
try
{
PixelToWorld.VectorToHomMat2d(hv_imageR, hv_imageC, hv_worldX, hv_worldY);
WorldToPixel.VectorToHomMat2d(hv_worldX, hv_worldY, hv_imageR, hv_imageC);
IsBuiltted = true;
OnChangeCalibrateData();
}
catch (Exception)
{
PixelToWorld = new HHomMat2D();
WorldToPixel = new HHomMat2D();
IsBuiltted = false;
return;
}
}
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"))));
}
}
