internal void Calibration()
{
for (int i = 0; i < Points.Count; i++)
{
if (Points[i].C_x == 0 && Points[i].C_y == 0 && Points[i].C_z == 0 && Points[i].R_X == 0 && Points[i].R_Y == 0)
{
MessageBox.Show("第" + (i + 1) + "点数据异常", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return;
}
}
HTuple Cam_X_All = new HTuple(), Cam_Y_All = new HTuple(), Cam_Z_ALL = new HTuple();
HTuple Robot_X_All = new HTuple(), Robot_Y_All = new HTuple();
for (int i = 0; i < Points.Count; i++)
{
Cam_X_All[i] = Points[i].C_x;
Cam_Y_All[i] = Points[i].C_y;
Cam_Z_ALL[i] = Points[i].C_z;
Robot_X_All[i] = Points[i].R_X;
Robot_Y_All[i] = Points[i].R_Y;
}
// //九点的矩阵映射
// HOperatorSet.VectorToHomMat2d(Cam_X_All, Cam_Y_All, Robot_X_All, Robot_Y_All, out homMat2D);
//// homMat2D.VectorToHomMat2d(Cam_X_All, Cam_Y_All, Robot_X_All, Robot_Y_All);
// HTuple check_R_XS, check_R_YS;
// //将九点直接验证结果
// HOperatorSet.AffineTransPoint2d(homMat2D, Cam_X_All, Cam_Y_All, out check_R_XS, out check_R_YS);
// //check_R_XS = homMat2D.AffineTransPoint2d(Cam_X_All, Cam_Y_All, out check_R_YS);
// //获取到行及列方向的误差
HTuple Robot_Z_All;
HOperatorSet.TupleGenConst(Robot_X_All.L, new HTuple(0.0), out Robot_Z_All);
HOperatorSet.VectorToHomMat3d("rigid", Cam_X_All, Cam_Y_All, Cam_Z_ALL, Robot_X_All, Robot_Y_All, Robot_Z_All, out MPInCamToolHomMat);
HOperatorSet.HomMat3dToPose(MPInCamToolHomMat, out MPInCamToolPose);
HTuple Robot_X_All_chk, Robot_Y_All_chk, Robot_Z_All_chk;
HOperatorSet.AffineTransPoint3d(MPInCamToolHomMat, Cam_X_All, Cam_Y_All, Cam_Z_ALL, out Robot_X_All_chk, out Robot_Y_All_chk, out Robot_Z_All_chk);
HTuple errR_X = (Robot_X_All - Robot_X_All_chk).TupleAbs();
HTuple errR_Y = (Robot_Y_All - Robot_Y_All_chk).TupleAbs();
errR_X_Max = errR_X.TupleMax();
errR_Y_Max = errR_Y.TupleMax();
StringBuilder stb = new StringBuilder();
stb.Append("X、Y方向误差：" + Environment.NewLine);
for (int i = 0; i < errR_X.Length; i++)
{
string temp = errR_X.D.ToString("F6");
string temp2 = errR_Y.D.ToString("F6");
stb.Append(temp + ";" + temp2 + Environment.NewLine);
}
if (NotifyExcInfo != null)
{
NotifyExcInfo(stb.ToString());
NotifyExcInfo("X方向最大误差：" + errR_X_Max.D.ToString("F6") + ";" + "方向最大误差：" + errR_Y_Max.D.ToString("F6"));
}
}
public void SetGrabPoint(double x, double y)
{
//转换camera 、 projection
MGrabPointSetting.X = x;
MGrabPointSetting.Y = y;
HTuple Cx, Cy, Cz, homMat3d;
HOperatorSet.PoseToHomMat3d(worldPose, out homMat3d);
HOperatorSet.AffineTransPoint3d(homMat3d, x, y, 0, out Cx, out Cy, out Cz);
HTuple row, column;
HOperatorSet.Project3dPoint(Cx, Cy, Cz, cameraParam, out row, out column);
MGrabPointSetting.SetGrabPoint(row.D, column.D);
}
public override string GetSendResult()
{
row_Send = new HTuple();
col_Send = new HTuple();
Angle_Send = new HTuple();
id_Send = new HTuple();
NG_Reason = new HTuple();
if (!bFindShapeMode || !bGrabPointSetting)
{
return("视觉程序异常" + Environment.NewLine);
}
if (MFindShapeMode.row == null || MFindShapeMode.row.Length < 1)
{
return("未找到缝纫机" + Environment.NewLine);
}
int count = MFindShapeMode.angle.Length;
#region 子功能运行是否异常
//防呆
if (bFangDai_Enable == false)
{
HOperatorSet.TupleGenConst(count, 1, out MFangDai.FangDai_OkNg);
}
else if (bFangDai_Enable && bFangDai_Result == false)
{
HOperatorSet.TupleGenConst(count, 0, out MFangDai.FangDai_OkNg);
}
if (count != MFangDai.FangDai_OkNg.Length)
{
return("视觉程序异常" + Environment.NewLine);
}
#endregion
for (int i = 0; i < MFindShapeMode.angle.Length; i++)
{
//NG
if (MFangDai.FangDai_OkNg[i] != 1)
{
//保存图像
DateTime dt = DateTime.Now;
string timeNow = dt.ToString("yyyy_MM_dd_HH_mm_ss_fff");
string NGImagePath = "D:\\data\\" + "\\NgImage\\" + "\\工具组" + settingIndex + "\\";
SaveImage(NGImagePath + timeNow + ".png", ImageTestIn);
NG_Reason = NG_Reason.TupleConcat(new HTuple("防呆"));
id_Send = id_Send.TupleConcat(new HTuple(0));
}
//OK
else
{
id_Send = id_Send.TupleConcat(new HTuple(1));
}
row_Send = row_Send.TupleConcat(MGrabPointSetting.GrabRowTarget[i]);
col_Send = col_Send.TupleConcat(MGrabPointSetting.GrabColTarget[i]);
Angle_Send = Angle_Send.TupleConcat(MFindShapeMode.angle[i]);
}
StringBuilder outCoord = new StringBuilder();
//坐标转换
if (bIsCalibration)
{
outCoord.Clear();
if (cameraParam == null || cameraParam.Length != 9)
{
Util.Notify("相机参数异常");
if (NotifyExcInfo != null)
{
NotifyExcInfo("相机参数异常");
}
}
if (MPInCamPose == null || MPInCamPose.Length != 7)
{
Util.Notify("参考坐标系异常");
if (NotifyExcInfo != null)
{
NotifyExcInfo("参考坐标系异常");
}
}
if (MPInToolPose == null || MPInToolPose.Length != 7)
{
Util.Notify("手臂坐标系异常");
if (NotifyExcInfo != null)
{
NotifyExcInfo("手臂坐标系异常");
}
}
if (cameraParam != null && cameraParam.Length == 9 &&
MPInCamPose != null && MPInCamPose.Length == 7 &&
MPInToolPose != null && MPInToolPose.Length == 7)
{
outCoord.Append("Image" + Environment.NewLine);
for (int i = 0; i < row_Send.Length; i++)
{
HTuple mpX, mpY;
HOperatorSet.ImagePointsToWorldPlane(cameraParam, MPInCamPose, row_Send[i], col_Send[i], 1, out mpX, out mpY);
HTuple homMat3d;
HOperatorSet.PoseToHomMat3d(MPInToolPose, out homMat3d);
HTuple rx, ry, rz;
HOperatorSet.AffineTransPoint3d(homMat3d, mpX, mpY, new HTuple(0.0), out rx, out ry, out rz);
HTuple angle;
HOperatorSet.TupleDeg(Angle_Send[i].D, out angle);
outCoord.Append("[");
string temp =
"X:" + (1000 * (rx.D + Robot_Tool_Comp[0].D) + x_Compensation).ToString("F2") + ";"
+ "Y:" + (1000 * (ry.D + Robot_Tool_Comp[1].D) + y_Compensation).ToString("F2") + ";"
+ "A:" + (angle.D + angle_Compensation).ToString("F2") + ";"
+ "ID:" + id_Send.I.ToString();
outCoord.Append(temp);
outCoord.Append("]" + Environment.NewLine);
}
outCoord.Append("Done" + Environment.NewLine);
}
else
{
return("视觉程序异常" + Environment.NewLine);
}
}
else
{
outCoord.Clear();
outCoord.Append("Image" + Environment.NewLine);
for (int i = 0; i < row_Send.Length; i++)
{
HTuple angle;
HOperatorSet.TupleDeg(Angle_Send[i].D, out angle);
outCoord.Append("[");
string temp =
"X:" + (row_Send[i].D + x_Compensation).ToString("F2") + ";" +
"Y:" + (col_Send[i].D + y_Compensation).ToString("F2") + ";" +
"A:" + (angle.D + angle_Compensation).ToString("F2") + ";" +
"ID:" + id_Send.I.ToString() + ";";
outCoord.Append(temp);
outCoord.Append("];" + Environment.NewLine);
}
outCoord.Append("Done" + Environment.NewLine);
}
return(outCoord.ToString());
}
/// <summary>
/// Display the axes of a 3d coordinate system.
/// </summary>
private void disp_3d_coord_system(HTuple hv_WindowHandle,
HTuple hv_CamParam,
HTuple hv_Pose,
HTuple hv_CoordAxesLength)
{
// Local iconic variables
HObject ho_ContX, ho_ContY, ho_ContZ;
// Local control variables
HTuple hv_TransWorld2Cam, hv_OrigCamX, hv_OrigCamY;
HTuple hv_OrigCamZ, hv_Row0, hv_Column0, hv_X, hv_Y, hv_Z;
HTuple hv_RowAxX, hv_ColumnAxX, hv_RowAxY, hv_ColumnAxY;
HTuple hv_RowAxZ, hv_ColumnAxZ;
// Initialize local and output iconic variables
HOperatorSet.GenEmptyObj(out ho_ContX);
HOperatorSet.GenEmptyObj(out ho_ContY);
HOperatorSet.GenEmptyObj(out ho_ContZ);
if ((int)(new HTuple((new HTuple(hv_Pose.TupleLength())).TupleNotEqual(7))) != 0)
{
ho_ContX.Dispose();
ho_ContY.Dispose();
ho_ContZ.Dispose();
return;
}
if ((int)(new HTuple(((hv_Pose.TupleSelect(5))).TupleEqual(0.0))) != 0)
{
ho_ContX.Dispose();
ho_ContY.Dispose();
ho_ContZ.Dispose();
return;
}
HOperatorSet.PoseToHomMat3d(hv_Pose, out hv_TransWorld2Cam);
//Project the world origin into the image
HOperatorSet.AffineTransPoint3d(hv_TransWorld2Cam, 0, 0, 0, out hv_OrigCamX,
out hv_OrigCamY, out hv_OrigCamZ);
HOperatorSet.Project3dPoint(hv_OrigCamX, hv_OrigCamY, hv_OrigCamZ, hv_CamParam,
out hv_Row0, out hv_Column0);
//Project the coordinate axes into the image
HOperatorSet.AffineTransPoint3d(hv_TransWorld2Cam, hv_CoordAxesLength, 0, 0,
out hv_X, out hv_Y, out hv_Z);
HOperatorSet.Project3dPoint(hv_X, hv_Y, hv_Z, hv_CamParam, out hv_RowAxX, out hv_ColumnAxX);
HOperatorSet.AffineTransPoint3d(hv_TransWorld2Cam, 0, hv_CoordAxesLength, 0,
out hv_X, out hv_Y, out hv_Z);
HOperatorSet.Project3dPoint(hv_X, hv_Y, hv_Z, hv_CamParam, out hv_RowAxY, out hv_ColumnAxY);
HOperatorSet.AffineTransPoint3d(hv_TransWorld2Cam, 0, 0, hv_CoordAxesLength,
out hv_X, out hv_Y, out hv_Z);
HOperatorSet.Project3dPoint(hv_X, hv_Y, hv_Z, hv_CamParam, out hv_RowAxZ, out hv_ColumnAxZ);
ho_ContX.Dispose();
gen_arrow_cont(out ho_ContX, hv_Row0, hv_Column0, hv_RowAxX, hv_ColumnAxX);
ho_ContY.Dispose();
gen_arrow_cont(out ho_ContY, hv_Row0, hv_Column0, hv_RowAxY, hv_ColumnAxY);
ho_ContZ.Dispose();
gen_arrow_cont(out ho_ContZ, hv_Row0, hv_Column0, hv_RowAxZ, hv_ColumnAxZ);
if (HDevWindowStack.IsOpen())
{
//dev_display (ContX)
}
if (HDevWindowStack.IsOpen())
{
//dev_display (ContY)
}
if (HDevWindowStack.IsOpen())
{
//dev_display (ContZ)
}
HOperatorSet.DispObj(ho_ContX, hv_WindowHandle);
HOperatorSet.DispObj(ho_ContY, hv_WindowHandle);
HOperatorSet.DispObj(ho_ContZ, hv_WindowHandle);
HOperatorSet.SetTposition(hv_WindowHandle, hv_RowAxX + 3, hv_ColumnAxX + 3);
HOperatorSet.WriteString(hv_WindowHandle, "X");
HOperatorSet.SetTposition(hv_WindowHandle, hv_RowAxY + 3, hv_ColumnAxY + 3);
HOperatorSet.WriteString(hv_WindowHandle, "Y");
HOperatorSet.SetTposition(hv_WindowHandle, hv_RowAxZ + 3, hv_ColumnAxZ + 3);
HOperatorSet.WriteString(hv_WindowHandle, "Z");
ho_ContX.Dispose();
ho_ContY.Dispose();
ho_ContZ.Dispose();
return;
}
public static int ShowOrigin(HWindow _windown, HTuple CamParam, HTuple Pose, HTuple Length)
{
try
{
// Convert to pose to a transformation matrix
HOperatorSet.PoseToHomMat3d(Pose, out HTuple TransWorld2Cam);
// Project the world origin into the image
HOperatorSet.AffineTransPoint3d(TransWorld2Cam, 0, 0, 0, out HTuple OrigCamX, out HTuple OrigCamY, out HTuple OrigCamZ);
HOperatorSet.Project3dPoint(OrigCamX, OrigCamY, OrigCamZ, CamParam, out HTuple Row0, out HTuple Column0);
//Project the coordinate axes into the image
HOperatorSet.AffineTransPoint3d(TransWorld2Cam, Length, 0, 0, out HTuple X, out HTuple Y, out HTuple Z);
HOperatorSet.Project3dPoint(X, Y, Z, CamParam, out HTuple RowAxX, out HTuple ColumnAxX);
HOperatorSet.AffineTransPoint3d(TransWorld2Cam, 0, Length, 0, out X, out Y, out Z);
HOperatorSet.Project3dPoint(X, Y, Z, CamParam, out HTuple RowAxY, out HTuple ColumnAxY);
HOperatorSet.AffineTransPoint3d(TransWorld2Cam, 0, 0, Length, out X, out Y, out Z);
HOperatorSet.Project3dPoint(X, Y, Z, CamParam, out HTuple RowAxZ, out HTuple ColumnAxZ);
//Generate an XLD contour for each axis
HOperatorSet.DistancePp(new HTuple(Row0, Row0, Row0), new HTuple(Column0, Column0, Column0), new HTuple(RowAxX, RowAxY, RowAxZ), new HTuple(ColumnAxX, ColumnAxY, ColumnAxZ), out HTuple Distance);
double HeadLength = new HTuple(new HTuple(5.0), Distance.TupleMax() / 12.0).TupleMax();
//Init
HOperatorSet.GenEmptyObj(out HObject Arrow);
// Calculate auxiliary variables.
HTuple DR = 1.0 * (new HTuple(RowAxX, RowAxY, RowAxZ) - new HTuple(Row0, Row0, Row0)) / Distance;
HTuple DC = 1.0 * (new HTuple(ColumnAxX, ColumnAxY, ColumnAxZ) - new HTuple(Column0, Column0, Column0)) / Distance;
HTuple Row2 = new HTuple(RowAxX, RowAxY, RowAxZ);
HTuple Column2 = new HTuple(ColumnAxX, ColumnAxY, ColumnAxZ);
HTuple HalfHeadWidth = HeadLength / 2.0;
//Calculate end points of the arrow head.
HTuple RowP1 = new HTuple(Row0, Row0, Row0) + (Distance - HeadLength) * DR + HalfHeadWidth * DC;
HTuple ColP1 = new HTuple(Column0, Column0, Column0) + (Distance - HeadLength) * DC - HalfHeadWidth * DR;
HTuple RowP2 = new HTuple(Row0, Row0, Row0) + (Distance - HeadLength) * DR - HalfHeadWidth * DC;
HTuple ColP2 = new HTuple(Column0, Column0, Column0) + (Distance - HeadLength) * DC + HalfHeadWidth * DR;
//Finally create output XLD contour for each input point pair
HObject TempArrow = new HObject();
for (int i = 0; i < Distance.Length; i++)
{
if (Distance[i] == (-1.0))
//Create_ single points for arrows with identical start and end point
{
HOperatorSet.GenContourPolygonXld(out TempArrow, Row0, Column0);
}
else
{
HOperatorSet.GenContourPolygonXld(out TempArrow, new HTuple(Row0, Row2[i], RowP1[i], Row2[i], RowP2[i], Row2[i]), new HTuple(Column0, Column2[i], ColP1[i], Column2[i], ColP2[i], Column2[i]));
}
//Create arrow contour
HOperatorSet.ConcatObj(Arrow, TempArrow, out Arrow);
}
//Display coordinate system
_windown.SetColored(12);
HOperatorSet.DispXld(Arrow, _windown);
HOperatorSet.GetRgb(_windown, out HTuple Red, out HTuple Green, out HTuple Blue);
HOperatorSet.SetRgb(_windown, Red[0], Green[0], Blue[0]);
HOperatorSet.SetTposition(_windown, RowAxX + 3, ColumnAxX + 3);
HOperatorSet.WriteString(_windown, "X");
HOperatorSet.SetRgb(_windown, Red[1 % Red.Length], Green[1 % Green.Length], Blue[1 % Blue.Length]);
HOperatorSet.SetTposition(_windown, RowAxY + 3, ColumnAxY + 3);
HOperatorSet.WriteString(_windown, "Y");
HOperatorSet.SetRgb(_windown, Red[2 % Red.Length], Green[2 % Green.Length], Blue[2 % Blue.Length]);
HOperatorSet.SetTposition(_windown, RowAxZ + 3, ColumnAxZ + 3);
HOperatorSet.WriteString(_windown, "Z");
HOperatorSet.SetRgb(_windown, Red, Green, Blue);
return(4);
}
catch (Exception)
{
return(0);
}
}