//相机绑定在3轴垂直机械手上的手眼标定(需要懂匹配), 能实现全自动标定
//(mark点是什么，可以是小圆，小三角形，小矩形及形状分明的任意图形）
//标定思路：产品平面上随便选一个特征形状建立模板（mark），然后机械手走一个田字（确保机械手的9个位置都能在相机上完整成像Mark形状)分别匹配
//(田字共9个交叉点，记录9个位置点的机械手坐标及拍照匹配出9个mark点坐标，田字不能太大也不能太小，太大超过视野范围拍不全Mark，太小精度可能降低）
//解释上面的思路（产品不动，9点运动的第一个点拍出来的图像的mark点中心尽量在图像中心，其它8个拍照位置一定要拍出完整的Mark点）
//col和row是图像模板匹配出来的mark点row,col
//y,x机械手拍照位置y,x坐标
MTuple col,row,x,y, HomMat2D1,HomMat2D2;
col[0] = 869; row[0] = 1372.36; x[0] = 320.881; y[0] = 98.884;
col[1] = 979.07; row[1] = 1758.86; x[1] = 322.27; y[1] = 109.81;
col[2] = 1090.03; row[2] = 2145.75; x[2] = 323.695; y[2] = 120.62;
col[3] = 1461.64; row[3] = 2040.02; x[3] = 313.154; y[3] = 121.88;
col[4] = 1833.96; row[4] = 1935.01; x[4] = 302.671; y[4] = 123.111;
col[5] = 1724.21; row[5] = 1546.79; x[5] = 301.107; y[5] = 112.23;
col[6] = 1613.17; row[6] = 1158.53; x[6] = 299.816; y[6] = 101.626;
col[7] = 1240.49; row[7] = 1265.07; x[7] = 310.447; y[7] = 100.343;
col[8] = 1351.27; row[8] = 1652.6; x[8] = 311.665; y[8] = 111.083;
//根据row,col得到y,x的变换矩阵
VectorToHomMat2d(col, row, x, y, &HomMat2D1);
MTuple Qx, Qy;
//**根据y, x得到row, col的变换矩阵HomMat2D2
VectorToHomMat2d(x, y, col, row, &HomMat2D2);
////**我们要求什么呢？这里是求让mark点移动到相机的中心，机械手的位置是多少。我们已知条件是拍照位置及匹配到的mark点坐标
//*已知拍照位置y: = 1000, x : = 1300, 匹配到的的mark点row : = 240.4, col : = 478.6(拍照的时候记录拍照位置）
//*下面根据已经知道的拍照位置得到虚拟的row, col，得出来值可能很大，下面得y, x对应得就是row, col，注意这里是HomMat2D2而不是HomMat2D1
AffineTransPoint2d(HomMat2D2, 1000, 1300, &Qy, &Qx);
//*因为相机中心坐标为row--480, col--640, 注：这里假设的是图像大小是1280 * 960（取图像中心点的意思), dlt就是Δ的意思
MTuple dltrow, dltcol;
dltrow = 480 - 240.4;
dltcol = 640 - 478.6;
//*下面得到虚拟的图像坐标(在虚拟的基础上 + Δ）
MTuple colcenter, rowcenter;
colcenter = Qx + dltcol;
rowcenter = Qy + dltrow;
//*下面得到机械手应该走到的位置Qy2, Qx2--按理结束了
MTuple Qx2, Qy2;
AffineTransPoint2d(HomMat2D1, rowcenter, colcenter, &Qx2, &Qy2);