在基於位置的視覺控制中，手眼標定起着關鍵性作用，手眼標定是解決相機與機器人之間位置關系的理論，手眼關系一般有兩種形式，一種是相機固定在機器人執行器末端隨機器人運動，稱為Eye-in-Hand,另一種為相機固定在機器人附近的地面或設備上，不隨機器人運動，稱為Eye-to-Hand.無論是怎樣的形式，最終解決的問題都是同樣的，即看到目標物在什么位置，告訴機器人去什么位置執行加工動作。Eye-in-Hand的形式中，相機隨機器人運動視野比較寬不會受到機械臂的阻擋，但是存在運動過量可能看不到目標物體，導致目標丟失。Eye-to-Hand的形式中，相機不隨機器人運動，固定在一處，能夠看到在視野中目標，但是在機械臂移動的過程中容易受到阻擋。兩種形式各有利弊，下面是每種形式求取手眼關系的過程。
在上圖Eye-in-Hand關系圖中，機械臂末端帶着相機從{C1}移動到{C2}的過程中相機和機械臂末端執行器的固定關系保持不變即：
\[{}^{C1}{T_{H1}} = {}^{C2}{T_{H2}}\]
同時帶有Marker的目標物在機器人世界坐標系中的關系也不變。
\[{}^B{T_O} = {}^{\rm{B}}{T_{H1}} \bullet {}^{C1}T_{H1}^{ - 1} \bullet {}^{C1}{T_O} = {}^{\rm{B}}{T_{H2}} \bullet {}^{C2}T_{H2}^{ - 1} \bullet {}^{C2}{T_O}\]
\[({}^{\rm{B}}T_{H2}^{ - 1} \bullet {}^{\rm{B}}{T_{H1}}) \bullet {}^{C1}T_{H1}^{ - 1} = {}^{C2}T_{H2}^{ - 1} \bullet ({}^{C2}{T_O} \bullet {}^{C1}T_O^{ - 1})\]
寫作：$AX = XB$，求解X即可得出手眼之間的關系。
在上圖Eye-to-Hand關系圖中對於機器人末端執行器夾着Marker從{O1}移動到和{O2}執行任務時，執行器末端和Marker之間的位置關系保持不變，因此在兩個位置存在如下的轉換關系：
\[{}^{O1}{T_{H1}} = {}^CT_{O1}^{ - 1} \bullet {}^C{T_B} \bullet {}^{\rm{B}}{T_{H1}}\]
\[{}^{O2}{T_{H2}} = {}^CT_{O2}^{ - 1} \bullet {}^C{T_B} \bullet {}^{\rm{B}}{T_{H2}}\]
\[{}^CT_{O1}^{ - 1} \bullet {}^C{T_B} \bullet {}^{\rm{B}}{T_{H1}} = {}^CT_{O2}^{ - 1} \bullet {}^C{T_B} \bullet {}^{\rm{B}}{T_{H2}}\]
\[({}^C{T_{O2}} \bullet {}^CT_{O1}^{ - 1}) \bullet {}^C{T_B} = {}^C{T_B} \bullet ({}^{\rm{B}}{T_{H2}} \bullet {}^{\rm{B}}T_{H1}^{ - 1})\]
寫作：$AX = XB$，求解X即可得出手眼之間的關系。
注意在求取的過程中多采集幾組A，B的數據結果才會准確，不然無法求解。
1 //向量轉反對稱矩陣 2 3 cv::Mat Calculate::skew(cv::Mat vec) 4 { 5 6 cv::Mat skewM(3, 3, CV_64FC1); 7 8 double vx = vec.at<double>(0, 0); 9 10 double vy = vec.at<double>(1, 0); 11 12 double vz = vec.at<double>(2, 0); 13 14 skewM.at<double>(0, 0) = 0.0; skewM.at<double>(0, 1) = -vz; skewM.at<double>(0, 2) = vy; 15 16 skewM.at<double>(1, 0) = vz; skewM.at<double>(1, 1) = 0.0; skewM.at<double>(1, 2) = -vx; 17 18 skewM.at<double>(2, 0) = -vy; skewM.at<double>(2, 1) = vx; skewM.at<double>(2, 2) = 0.0; 19 20 return skewM; 21 22 } 23 Solve AX=XB, A-RT_Tcpij, B-RT_Camij, B2A 24 25 Tsai_HandEye(cv::Mat Hcg, vector<cv::Mat> Hgij, vector<cv::Mat> Hcij) 26 27 { 28 29 CV_Assert(Hgij.size() == Hcij.size()); 30 31 int nStatus = Hgij.size(); 32 33 cv::Mat Rgij(3, 3, CV_64FC1); 34 35 cv::Mat Rcij(3, 3, CV_64FC1); 36 37 cv::Mat rgij(3, 1, CV_64FC1); 38 39 cv::Mat rcij(3, 1, CV_64FC1); 40 41 double theta_gij; 42 43 double theta_cij; 44 45 cv::Mat rngij(3, 1, CV_64FC1); 46 47 cv::Mat rncij(3, 1, CV_64FC1); 48 49 cv::Mat Pgij(3, 1, CV_64FC1); 50 51 cv::Mat Pcij(3, 1, CV_64FC1); 52 53 cv::Mat tempA(3, 3, CV_64FC1); 54 55 cv::Mat tempb(3, 1, CV_64FC1); 56 57 cv::Mat A; 58 59 cv::Mat b; 60 61 cv::Mat pinA; 62 63 cv::Mat Pcg_prime(3, 1, CV_64FC1); 64 65 cv::Mat Pcg(3, 1, CV_64FC1); 66 67 cv::Mat PcgTrs(1, 3, CV_64FC1); 68 69 cv::Mat Rcg(3, 3, CV_64FC1); 70 71 cv::Mat eyeM = cv::Mat::eye(3, 3, CV_64FC1); 72 73 cv::Mat Tgij(3, 1, CV_64FC1); 74 75 cv::Mat Tcij(3, 1, CV_64FC1); 76 77 cv::Mat tempAA(3, 3, CV_64FC1); 78 79 cv::Mat tempbb(3, 1, CV_64FC1); 80 81 cv::Mat AA; 82 83 cv::Mat bb; 84 85 cv::Mat pinAA; 86 87 cv::Mat Tcg(3, 1, CV_64FC1); 88 89 for (int i = 0; i < nStatus; i++) 90 91 { 92 93 Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij); 94 95 Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij); 96 97 Rodrigues(Rgij, rgij); 98 99 Rodrigues(Rcij, rcij); 100 101 theta_gij = norm(rgij); 102 103 theta_cij = norm(rcij); 104 105 rngij = rgij / theta_gij; 106 107 rncij = rcij / theta_cij; 108 109 Pgij = 2 * sin(theta_gij / 2)*rngij; 110 111 Pcij = 2 * sin(theta_cij / 2)*rncij; 112 113 tempA = skew(Pgij + Pcij); 114 115 tempb = Pcij - Pgij; 116 117 A.push_back(tempA); 118 119 b.push_back(tempb); 120 121 } 122 123 //Compute rotation 124 125 invert(A, pinA, cv::DECOMP_SVD); 126 127 Pcg_prime = pinA * b; 128 129 Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime)); 130 131 PcgTrs = Pcg.t(); 132 133 Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg)); 134 135 //Compute Translation 136 137 for (int i = 0; i < nStatus; i++) 138 139 { 140 141 Hgij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rgij); 142 143 Hcij[i](cv::Rect(0, 0, 3, 3)).copyTo(Rcij); 144 145 Hgij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tgij); 146 147 Hcij[i](cv::Rect(3, 0, 1, 3)).copyTo(Tcij); 148 149 tempAA = Rgij - eyeM; 150 151 tempbb = Rcg * Tcij - Tgij; 152 153 AA.push_back(tempAA); 154 155 bb.push_back(tempbb); 156 157 } 158 159 invert(AA, pinAA, cv::DECOMP_SVD); 160 161 Tcg = pinAA * bb; 162 163 Rcg.copyTo(Hcg(cv::Rect(0, 0, 3, 3))); 164 165 Tcg.copyTo(Hcg(cv::Rect(3, 0, 1, 3))); 166 167 Hcg.at<double>(3, 0) = 0.0; 168 169 Hcg.at<double>(3, 1) = 0.0; 170 171 Hcg.at<double>(3, 2) = 0.0; 172 173 Hcg.at<double>(3, 3) = 1.0; 174 175 176 }