//相機綁定在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);