在基于位置的视觉控制中，手眼标定起着关键性作用，手眼标定是解决相机与机器人之间位置关系的理论，手眼关系一般有两种形式，一种是相机固定在机器人执行器末端随机器人运动，称为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 }