1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html.
4
5 #include "precomp.hpp"
6 #include "opencv2/calib3d.hpp"
7
8 namespace cv {
9
homogeneousInverse(const Mat & T)10 static Mat homogeneousInverse(const Mat& T)
11 {
12 CV_Assert(T.rows == 4 && T.cols == 4);
13
14 Mat R = T(Rect(0, 0, 3, 3));
15 Mat t = T(Rect(3, 0, 1, 3));
16 Mat Rt = R.t();
17 Mat tinv = -Rt * t;
18 Mat Tinv = Mat::eye(4, 4, T.type());
19 Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
20 tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
21
22 return Tinv;
23 }
24
25 // q = rot2quatMinimal(R)
26 //
27 // R - 3x3 rotation matrix, or 4x4 homogeneous matrix
28 // q - 3x1 unit quaternion <qx, qy, qz>
29 // q = sin(theta/2) * v
30 // theta - rotation angle
31 // v - unit rotation axis, |v| = 1
rot2quatMinimal(const Mat & R)32 static Mat rot2quatMinimal(const Mat& R)
33 {
34 CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
35
36 double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
37 double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
38 double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
39 double trace = m00 + m11 + m22;
40
41 double qx, qy, qz;
42 if (trace > 0) {
43 double S = sqrt(trace + 1.0) * 2; // S=4*qw
44 qx = (m21 - m12) / S;
45 qy = (m02 - m20) / S;
46 qz = (m10 - m01) / S;
47 } else if ((m00 > m11)&(m00 > m22)) {
48 double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
49 qx = 0.25 * S;
50 qy = (m01 + m10) / S;
51 qz = (m02 + m20) / S;
52 } else if (m11 > m22) {
53 double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
54 qx = (m01 + m10) / S;
55 qy = 0.25 * S;
56 qz = (m12 + m21) / S;
57 } else {
58 double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
59 qx = (m02 + m20) / S;
60 qy = (m12 + m21) / S;
61 qz = 0.25 * S;
62 }
63
64 return (Mat_<double>(3,1) << qx, qy, qz);
65 }
66
skew(const Mat & v)67 static Mat skew(const Mat& v)
68 {
69 CV_Assert(v.type() == CV_64FC1 && v.rows == 3 && v.cols == 1);
70
71 double vx = v.at<double>(0,0);
72 double vy = v.at<double>(1,0);
73 double vz = v.at<double>(2,0);
74 return (Mat_<double>(3,3) << 0, -vz, vy,
75 vz, 0, -vx,
76 -vy, vx, 0);
77 }
78
79 // R = quatMinimal2rot(q)
80 //
81 // q - 3x1 unit quaternion <qx, qy, qz>
82 // R - 3x3 rotation matrix
83 // q = sin(theta/2) * v
84 // theta - rotation angle
85 // v - unit rotation axis, |v| = 1
quatMinimal2rot(const Mat & q)86 static Mat quatMinimal2rot(const Mat& q)
87 {
88 CV_Assert(q.type() == CV_64FC1 && q.rows == 3 && q.cols == 1);
89
90 Mat p = q.t()*q;
91 double w = sqrt(1 - p.at<double>(0,0));
92
93 Mat diag_p = Mat::eye(3,3,CV_64FC1)*p.at<double>(0,0);
94 return 2*q*q.t() + 2*w*skew(q) + Mat::eye(3,3,CV_64FC1) - 2*diag_p;
95 }
96
97 // q = rot2quat(R)
98 //
99 // q - 4x1 unit quaternion <qw, qx, qy, qz>
100 // R - 3x3 rotation matrix
rot2quat(const Mat & R)101 static Mat rot2quat(const Mat& R)
102 {
103 CV_Assert(R.type() == CV_64FC1 && R.rows >= 3 && R.cols >= 3);
104
105 double m00 = R.at<double>(0,0), m01 = R.at<double>(0,1), m02 = R.at<double>(0,2);
106 double m10 = R.at<double>(1,0), m11 = R.at<double>(1,1), m12 = R.at<double>(1,2);
107 double m20 = R.at<double>(2,0), m21 = R.at<double>(2,1), m22 = R.at<double>(2,2);
108 double trace = m00 + m11 + m22;
109
110 double qw, qx, qy, qz;
111 if (trace > 0) {
112 double S = sqrt(trace + 1.0) * 2; // S=4*qw
113 qw = 0.25 * S;
114 qx = (m21 - m12) / S;
115 qy = (m02 - m20) / S;
116 qz = (m10 - m01) / S;
117 } else if ((m00 > m11)&(m00 > m22)) {
118 double S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
119 qw = (m21 - m12) / S;
120 qx = 0.25 * S;
121 qy = (m01 + m10) / S;
122 qz = (m02 + m20) / S;
123 } else if (m11 > m22) {
124 double S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
125 qw = (m02 - m20) / S;
126 qx = (m01 + m10) / S;
127 qy = 0.25 * S;
128 qz = (m12 + m21) / S;
129 } else {
130 double S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
131 qw = (m10 - m01) / S;
132 qx = (m02 + m20) / S;
133 qy = (m12 + m21) / S;
134 qz = 0.25 * S;
135 }
136
137 return (Mat_<double>(4,1) << qw, qx, qy, qz);
138 }
139
140 // R = quat2rot(q)
141 //
142 // q - 4x1 unit quaternion <qw, qx, qy, qz>
143 // R - 3x3 rotation matrix
quat2rot(const Mat & q)144 static Mat quat2rot(const Mat& q)
145 {
146 CV_Assert(q.type() == CV_64FC1 && q.rows == 4 && q.cols == 1);
147
148 double qw = q.at<double>(0,0);
149 double qx = q.at<double>(1,0);
150 double qy = q.at<double>(2,0);
151 double qz = q.at<double>(3,0);
152
153 Mat R(3, 3, CV_64FC1);
154 R.at<double>(0, 0) = 1 - 2*qy*qy - 2*qz*qz;
155 R.at<double>(0, 1) = 2*qx*qy - 2*qz*qw;
156 R.at<double>(0, 2) = 2*qx*qz + 2*qy*qw;
157
158 R.at<double>(1, 0) = 2*qx*qy + 2*qz*qw;
159 R.at<double>(1, 1) = 1 - 2*qx*qx - 2*qz*qz;
160 R.at<double>(1, 2) = 2*qy*qz - 2*qx*qw;
161
162 R.at<double>(2, 0) = 2*qx*qz - 2*qy*qw;
163 R.at<double>(2, 1) = 2*qy*qz + 2*qx*qw;
164 R.at<double>(2, 2) = 1 - 2*qx*qx - 2*qy*qy;
165
166 return R;
167 }
168
169 // Kronecker product or tensor product
170 // https://stackoverflow.com/a/36552682
kron(const Mat & A,const Mat & B)171 static Mat kron(const Mat& A, const Mat& B)
172 {
173 CV_Assert(A.channels() == 1 && B.channels() == 1);
174
175 Mat1d Ad, Bd;
176 A.convertTo(Ad, CV_64F);
177 B.convertTo(Bd, CV_64F);
178
179 Mat1d Kd(Ad.rows * Bd.rows, Ad.cols * Bd.cols, 0.0);
180 for (int ra = 0; ra < Ad.rows; ra++)
181 {
182 for (int ca = 0; ca < Ad.cols; ca++)
183 {
184 Kd(Range(ra*Bd.rows, (ra + 1)*Bd.rows), Range(ca*Bd.cols, (ca + 1)*Bd.cols)) = Bd.mul(Ad(ra, ca));
185 }
186 }
187
188 Mat K;
189 Kd.convertTo(K, A.type());
190 return K;
191 }
192
193 // quaternion multiplication
qmult(const Mat & s,const Mat & t)194 static Mat qmult(const Mat& s, const Mat& t)
195 {
196 CV_Assert(s.type() == CV_64FC1 && t.type() == CV_64FC1);
197 CV_Assert(s.rows == 4 && s.cols == 1);
198 CV_Assert(t.rows == 4 && t.cols == 1);
199
200 double s0 = s.at<double>(0,0);
201 double s1 = s.at<double>(1,0);
202 double s2 = s.at<double>(2,0);
203 double s3 = s.at<double>(3,0);
204
205 double t0 = t.at<double>(0,0);
206 double t1 = t.at<double>(1,0);
207 double t2 = t.at<double>(2,0);
208 double t3 = t.at<double>(3,0);
209
210 Mat q(4, 1, CV_64FC1);
211 q.at<double>(0,0) = s0*t0 - s1*t1 - s2*t2 - s3*t3;
212 q.at<double>(1,0) = s0*t1 + s1*t0 + s2*t3 - s3*t2;
213 q.at<double>(2,0) = s0*t2 - s1*t3 + s2*t0 + s3*t1;
214 q.at<double>(3,0) = s0*t3 + s1*t2 - s2*t1 + s3*t0;
215
216 return q;
217 }
218
219 // dq = homogeneous2dualQuaternion(H)
220 //
221 // H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
222 // dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
homogeneous2dualQuaternion(const Mat & H)223 static Mat homogeneous2dualQuaternion(const Mat& H)
224 {
225 CV_Assert(H.type() == CV_64FC1 && H.rows == 4 && H.cols == 4);
226
227 Mat dualq(8, 1, CV_64FC1);
228 Mat R = H(Rect(0, 0, 3, 3));
229 Mat t = H(Rect(3, 0, 1, 3));
230
231 Mat q = rot2quat(R);
232 Mat qt = Mat::zeros(4, 1, CV_64FC1);
233 t.copyTo(qt(Rect(0, 1, 1, 3)));
234 Mat qprime = 0.5 * qmult(qt, q);
235
236 q.copyTo(dualq(Rect(0, 0, 1, 4)));
237 qprime.copyTo(dualq(Rect(0, 4, 1, 4)));
238
239 return dualq;
240 }
241
242 // H = dualQuaternion2homogeneous(dq)
243 //
244 // H - 4x4 homogeneous transformation: [R | t; 0 0 0 | 1]
245 // dq - 8x1 dual quaternion: <q (rotation part), qprime (translation part)>
dualQuaternion2homogeneous(const Mat & dualq)246 static Mat dualQuaternion2homogeneous(const Mat& dualq)
247 {
248 CV_Assert(dualq.type() == CV_64FC1 && dualq.rows == 8 && dualq.cols == 1);
249
250 Mat q = dualq(Rect(0, 0, 1, 4));
251 Mat qprime = dualq(Rect(0, 4, 1, 4));
252
253 Mat R = quat2rot(q);
254 q.at<double>(1,0) = -q.at<double>(1,0);
255 q.at<double>(2,0) = -q.at<double>(2,0);
256 q.at<double>(3,0) = -q.at<double>(3,0);
257
258 Mat qt = 2*qmult(qprime, q);
259 Mat t = qt(Rect(0, 1, 1, 3));
260
261 Mat H = Mat::eye(4, 4, CV_64FC1);
262 R.copyTo(H(Rect(0, 0, 3, 3)));
263 t.copyTo(H(Rect(3, 0, 1, 3)));
264
265 return H;
266 }
267
268 //Reference:
269 //R. Y. Tsai and R. K. Lenz, "A new technique for fully autonomous and efficient 3D robotics hand/eye calibration."
270 //In IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345-358, June 1989.
271 //C++ code converted from Zoran Lazarevic's Matlab code:
272 //http://lazax.com/www.cs.columbia.edu/~laza/html/Stewart/matlab/handEye.m
calibrateHandEyeTsai(const std::vector<Mat> & Hg,const std::vector<Mat> & Hc,Mat & R_cam2gripper,Mat & t_cam2gripper)273 static void calibrateHandEyeTsai(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
274 Mat& R_cam2gripper, Mat& t_cam2gripper)
275 {
276 //Number of unique camera position pairs
277 int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
278 //Will store: skew(Pgij+Pcij)
279 Mat A(3*K, 3, CV_64FC1);
280 //Will store: Pcij - Pgij
281 Mat B(3*K, 1, CV_64FC1);
282
283 std::vector<Mat> vec_Hgij, vec_Hcij;
284 vec_Hgij.reserve(static_cast<size_t>(K));
285 vec_Hcij.reserve(static_cast<size_t>(K));
286
287 int idx = 0;
288 for (size_t i = 0; i < Hg.size(); i++)
289 {
290 for (size_t j = i+1; j < Hg.size(); j++, idx++)
291 {
292 //Defines coordinate transformation from Gi to Gj
293 //Hgi is from Gi (gripper) to RW (robot base)
294 //Hgj is from Gj (gripper) to RW (robot base)
295 Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i]; //eq 6
296 vec_Hgij.push_back(Hgij);
297 //Rotation axis for Rgij which is the 3D rotation from gripper coordinate frame Gi to Gj
298 Mat Pgij = 2*rot2quatMinimal(Hgij);
299
300 //Defines coordinate transformation from Ci to Cj
301 //Hci is from CW (calibration target) to Ci (camera)
302 //Hcj is from CW (calibration target) to Cj (camera)
303 Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]); //eq 7
304 vec_Hcij.push_back(Hcij);
305 //Rotation axis for Rcij
306 Mat Pcij = 2*rot2quatMinimal(Hcij);
307
308 //Left-hand side: skew(Pgij+Pcij)
309 skew(Pgij+Pcij).copyTo(A(Rect(0, idx*3, 3, 3)));
310 //Right-hand side: Pcij - Pgij
311 Mat diff = Pcij - Pgij;
312 diff.copyTo(B(Rect(0, idx*3, 1, 3)));
313 }
314 }
315
316 Mat Pcg_;
317 //Rotation from camera to gripper is obtained from the set of equations:
318 // skew(Pgij+Pcij) * Pcg_ = Pcij - Pgij (eq 12)
319 solve(A, B, Pcg_, DECOMP_SVD);
320
321 Mat Pcg_norm = Pcg_.t() * Pcg_;
322 //Obtained non-unit quaternion is scaled back to unit value that
323 //designates camera-gripper rotation
324 Mat Pcg = 2 * Pcg_ / sqrt(1 + Pcg_norm.at<double>(0,0)); //eq 14
325
326 Mat Rcg = quatMinimal2rot(Pcg/2.0);
327
328 idx = 0;
329 for (size_t i = 0; i < Hg.size(); i++)
330 {
331 for (size_t j = i+1; j < Hg.size(); j++, idx++)
332 {
333 //Defines coordinate transformation from Gi to Gj
334 //Hgi is from Gi (gripper) to RW (robot base)
335 //Hgj is from Gj (gripper) to RW (robot base)
336 Mat Hgij = vec_Hgij[static_cast<size_t>(idx)];
337 //Defines coordinate transformation from Ci to Cj
338 //Hci is from CW (calibration target) to Ci (camera)
339 //Hcj is from CW (calibration target) to Cj (camera)
340 Mat Hcij = vec_Hcij[static_cast<size_t>(idx)];
341
342 //Left-hand side: (Rgij - I)
343 Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1);
344 diff.copyTo(A(Rect(0, idx*3, 3, 3)));
345
346 //Right-hand side: Rcg*Tcij - Tgij
347 diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3));
348 diff.copyTo(B(Rect(0, idx*3, 1, 3)));
349 }
350 }
351
352 Mat Tcg;
353 //Translation from camera to gripper is obtained from the set of equations:
354 // (Rgij - I) * Tcg = Rcg*Tcij - Tgij (eq 15)
355 solve(A, B, Tcg, DECOMP_SVD);
356
357 R_cam2gripper = Rcg;
358 t_cam2gripper = Tcg;
359 }
360
361 //Reference:
362 //F. Park, B. Martin, "Robot Sensor Calibration: Solving AX = XB on the Euclidean Group."
363 //In IEEE Transactions on Robotics and Automation, 10(5): 717-721, 1994.
364 //Matlab code: http://math.loyola.edu/~mili/Calibration/
calibrateHandEyePark(const std::vector<Mat> & Hg,const std::vector<Mat> & Hc,Mat & R_cam2gripper,Mat & t_cam2gripper)365 static void calibrateHandEyePark(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
366 Mat& R_cam2gripper, Mat& t_cam2gripper)
367 {
368 Mat M = Mat::zeros(3, 3, CV_64FC1);
369
370 for (size_t i = 0; i < Hg.size(); i++)
371 {
372 for (size_t j = i+1; j < Hg.size(); j++)
373 {
374 Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
375 Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
376
377 Mat Rgij = Hgij(Rect(0, 0, 3, 3));
378 Mat Rcij = Hcij(Rect(0, 0, 3, 3));
379
380 Mat a, b;
381 Rodrigues(Rgij, a);
382 Rodrigues(Rcij, b);
383
384 M += b * a.t();
385 }
386 }
387
388 Mat eigenvalues, eigenvectors;
389 eigen(M.t()*M, eigenvalues, eigenvectors);
390
391 Mat v = Mat::zeros(3, 3, CV_64FC1);
392 for (int i = 0; i < 3; i++) {
393 v.at<double>(i,i) = 1.0 / sqrt(eigenvalues.at<double>(i,0));
394 }
395
396 Mat R = eigenvectors.t() * v * eigenvectors * M.t();
397 R_cam2gripper = R;
398
399 int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
400 Mat C(3*K, 3, CV_64FC1);
401 Mat d(3*K, 1, CV_64FC1);
402 Mat I3 = Mat::eye(3, 3, CV_64FC1);
403
404 int idx = 0;
405 for (size_t i = 0; i < Hg.size(); i++)
406 {
407 for (size_t j = i+1; j < Hg.size(); j++, idx++)
408 {
409 Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
410 Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
411
412 Mat Rgij = Hgij(Rect(0, 0, 3, 3));
413
414 Mat tgij = Hgij(Rect(3, 0, 1, 3));
415 Mat tcij = Hcij(Rect(3, 0, 1, 3));
416
417 Mat I_tgij = I3 - Rgij;
418 I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
419
420 Mat A_RB = tgij - R*tcij;
421 A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
422 }
423 }
424
425 Mat t;
426 solve(C, d, t, DECOMP_SVD);
427 t_cam2gripper = t;
428 }
429
430 //Reference:
431 //R. Horaud, F. Dornaika, "Hand-Eye Calibration"
432 //In International Journal of Robotics Research, 14(3): 195-210, 1995.
433 //Matlab code: http://math.loyola.edu/~mili/Calibration/
calibrateHandEyeHoraud(const std::vector<Mat> & Hg,const std::vector<Mat> & Hc,Mat & R_cam2gripper,Mat & t_cam2gripper)434 static void calibrateHandEyeHoraud(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
435 Mat& R_cam2gripper, Mat& t_cam2gripper)
436 {
437 Mat A = Mat::zeros(4, 4, CV_64FC1);
438
439 for (size_t i = 0; i < Hg.size(); i++)
440 {
441 for (size_t j = i+1; j < Hg.size(); j++)
442 {
443 Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
444 Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
445
446 Mat Rgij = Hgij(Rect(0, 0, 3, 3));
447 Mat Rcij = Hcij(Rect(0, 0, 3, 3));
448
449 Mat qgij = rot2quat(Rgij);
450 double r0 = qgij.at<double>(0,0);
451 double rx = qgij.at<double>(1,0);
452 double ry = qgij.at<double>(2,0);
453 double rz = qgij.at<double>(3,0);
454
455 // Q(r) Appendix A
456 Matx44d Qvi(r0, -rx, -ry, -rz,
457 rx, r0, -rz, ry,
458 ry, rz, r0, -rx,
459 rz, -ry, rx, r0);
460
461 Mat qcij = rot2quat(Rcij);
462 r0 = qcij.at<double>(0,0);
463 rx = qcij.at<double>(1,0);
464 ry = qcij.at<double>(2,0);
465 rz = qcij.at<double>(3,0);
466
467 // W(r) Appendix A
468 Matx44d Wvi(r0, -rx, -ry, -rz,
469 rx, r0, rz, -ry,
470 ry, -rz, r0, rx,
471 rz, ry, -rx, r0);
472
473 // Ai = (Q(vi') - W(vi))^T (Q(vi') - W(vi))
474 A += (Qvi - Wvi).t() * (Qvi - Wvi);
475 }
476 }
477
478 Mat eigenvalues, eigenvectors;
479 eigen(A, eigenvalues, eigenvectors);
480
481 Mat R = quat2rot(eigenvectors.row(3).t());
482 R_cam2gripper = R;
483
484 int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
485 Mat C(3*K, 3, CV_64FC1);
486 Mat d(3*K, 1, CV_64FC1);
487 Mat I3 = Mat::eye(3, 3, CV_64FC1);
488
489 int idx = 0;
490 for (size_t i = 0; i < Hg.size(); i++)
491 {
492 for (size_t j = i+1; j < Hg.size(); j++, idx++)
493 {
494 Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
495 Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
496
497 Mat Rgij = Hgij(Rect(0, 0, 3, 3));
498
499 Mat tgij = Hgij(Rect(3, 0, 1, 3));
500 Mat tcij = Hcij(Rect(3, 0, 1, 3));
501
502 Mat I_tgij = I3 - Rgij;
503 I_tgij.copyTo(C(Rect(0, 3*idx, 3, 3)));
504
505 Mat A_RB = tgij - R*tcij;
506 A_RB.copyTo(d(Rect(0, 3*idx, 1, 3)));
507 }
508 }
509
510 Mat t;
511 solve(C, d, t, DECOMP_SVD);
512 t_cam2gripper = t;
513 }
514
515 // sign function, return -1 if negative values, +1 otherwise
sign_double(double val)516 static int sign_double(double val)
517 {
518 return (0 < val) - (val < 0);
519 }
520
521 //Reference:
522 //N. Andreff, R. Horaud, B. Espiau, "On-line Hand-Eye Calibration."
523 //In Second International Conference on 3-D Digital Imaging and Modeling (3DIM'99), pages 430-436, 1999.
524 //Matlab code: http://math.loyola.edu/~mili/Calibration/
calibrateHandEyeAndreff(const std::vector<Mat> & Hg,const std::vector<Mat> & Hc,Mat & R_cam2gripper,Mat & t_cam2gripper)525 static void calibrateHandEyeAndreff(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
526 Mat& R_cam2gripper, Mat& t_cam2gripper)
527 {
528 int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
529 Mat A(12*K, 12, CV_64FC1);
530 Mat B(12*K, 1, CV_64FC1);
531
532 Mat I9 = Mat::eye(9, 9, CV_64FC1);
533 Mat I3 = Mat::eye(3, 3, CV_64FC1);
534 Mat O9x3 = Mat::zeros(9, 3, CV_64FC1);
535 Mat O9x1 = Mat::zeros(9, 1, CV_64FC1);
536
537 int idx = 0;
538 for (size_t i = 0; i < Hg.size(); i++)
539 {
540 for (size_t j = i+1; j < Hg.size(); j++, idx++)
541 {
542 Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
543 Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
544
545 Mat Rgij = Hgij(Rect(0, 0, 3, 3));
546 Mat Rcij = Hcij(Rect(0, 0, 3, 3));
547
548 Mat tgij = Hgij(Rect(3, 0, 1, 3));
549 Mat tcij = Hcij(Rect(3, 0, 1, 3));
550
551 //Eq 10
552 Mat a00 = I9 - kron(Rgij, Rcij);
553 Mat a01 = O9x3;
554 Mat a10 = kron(I3, tcij.t());
555 Mat a11 = I3 - Rgij;
556
557 a00.copyTo(A(Rect(0, idx*12, 9, 9)));
558 a01.copyTo(A(Rect(9, idx*12, 3, 9)));
559 a10.copyTo(A(Rect(0, idx*12 + 9, 9, 3)));
560 a11.copyTo(A(Rect(9, idx*12 + 9, 3, 3)));
561
562 O9x1.copyTo(B(Rect(0, idx*12, 1, 9)));
563 tgij.copyTo(B(Rect(0, idx*12 + 9, 1, 3)));
564 }
565 }
566
567 Mat X;
568 solve(A, B, X, DECOMP_SVD);
569
570 Mat R = X(Rect(0, 0, 1, 9));
571 int newSize[] = {3, 3};
572 R = R.reshape(1, 2, newSize);
573 //Eq 15
574 double det = determinant(R);
575 R = pow(sign_double(det) / abs(det), 1.0/3.0) * R;
576
577 Mat w, u, vt;
578 SVDecomp(R, w, u, vt);
579 R = u*vt;
580
581 if (determinant(R) < 0)
582 {
583 Mat diag = (Mat_<double>(3,3) << 1.0, 0.0, 0.0,
584 0.0, 1.0, 0.0,
585 0.0, 0.0, -1.0);
586 R = u*diag*vt;
587 }
588
589 R_cam2gripper = R;
590
591 Mat t = X(Rect(0, 9, 1, 3));
592 t_cam2gripper = t;
593 }
594
595 //Reference:
596 //K. Daniilidis, "Hand-Eye Calibration Using Dual Quaternions."
597 //In The International Journal of Robotics Research,18(3): 286-298, 1998.
598 //Matlab code: http://math.loyola.edu/~mili/Calibration/
calibrateHandEyeDaniilidis(const std::vector<Mat> & Hg,const std::vector<Mat> & Hc,Mat & R_cam2gripper,Mat & t_cam2gripper)599 static void calibrateHandEyeDaniilidis(const std::vector<Mat>& Hg, const std::vector<Mat>& Hc,
600 Mat& R_cam2gripper, Mat& t_cam2gripper)
601 {
602 int K = static_cast<int>((Hg.size()*Hg.size() - Hg.size()) / 2.0);
603 Mat T = Mat::zeros(6*K, 8, CV_64FC1);
604
605 int idx = 0;
606 for (size_t i = 0; i < Hg.size(); i++)
607 {
608 for (size_t j = i+1; j < Hg.size(); j++, idx++)
609 {
610 Mat Hgij = homogeneousInverse(Hg[j]) * Hg[i];
611 Mat Hcij = Hc[j] * homogeneousInverse(Hc[i]);
612
613 Mat dualqa = homogeneous2dualQuaternion(Hgij);
614 Mat dualqb = homogeneous2dualQuaternion(Hcij);
615
616 Mat a = dualqa(Rect(0, 1, 1, 3));
617 Mat b = dualqb(Rect(0, 1, 1, 3));
618
619 Mat aprime = dualqa(Rect(0, 5, 1, 3));
620 Mat bprime = dualqb(Rect(0, 5, 1, 3));
621
622 //Eq 31
623 Mat s00 = a - b;
624 Mat s01 = skew(a + b);
625 Mat s10 = aprime - bprime;
626 Mat s11 = skew(aprime + bprime);
627 Mat s12 = a - b;
628 Mat s13 = skew(a + b);
629
630 s00.copyTo(T(Rect(0, idx*6, 1, 3)));
631 s01.copyTo(T(Rect(1, idx*6, 3, 3)));
632 s10.copyTo(T(Rect(0, idx*6 + 3, 1, 3)));
633 s11.copyTo(T(Rect(1, idx*6 + 3, 3, 3)));
634 s12.copyTo(T(Rect(4, idx*6 + 3, 1, 3)));
635 s13.copyTo(T(Rect(5, idx*6 + 3, 3, 3)));
636 }
637 }
638
639 Mat w, u, vt;
640 SVDecomp(T, w, u, vt);
641 Mat v = vt.t();
642
643 Mat u1 = v(Rect(6, 0, 1, 4));
644 Mat v1 = v(Rect(6, 4, 1, 4));
645 Mat u2 = v(Rect(7, 0, 1, 4));
646 Mat v2 = v(Rect(7, 4, 1, 4));
647
648 //Solves Eq 34, Eq 35
649 Mat ma = u1.t()*v1;
650 Mat mb = u1.t()*v2 + u2.t()*v1;
651 Mat mc = u2.t()*v2;
652
653 double a = ma.at<double>(0,0);
654 double b = mb.at<double>(0,0);
655 double c = mc.at<double>(0,0);
656
657 double s1 = (-b + sqrt(b*b - 4*a*c)) / (2*a);
658 double s2 = (-b - sqrt(b*b - 4*a*c)) / (2*a);
659
660 Mat sol1 = s1*s1*u1.t()*u1 + 2*s1*u1.t()*u2 + u2.t()*u2;
661 Mat sol2 = s2*s2*u1.t()*u1 + 2*s2*u1.t()*u2 + u2.t()*u2;
662 double s, val;
663 if (sol1.at<double>(0,0) > sol2.at<double>(0,0))
664 {
665 s = s1;
666 val = sol1.at<double>(0,0);
667 }
668 else
669 {
670 s = s2;
671 val = sol2.at<double>(0,0);
672 }
673
674 double lambda2 = sqrt(1.0 / val);
675 double lambda1 = s * lambda2;
676
677 Mat dualq = lambda1 * v(Rect(6, 0, 1, 8)) + lambda2*v(Rect(7, 0, 1, 8));
678 Mat X = dualQuaternion2homogeneous(dualq);
679
680 Mat R = X(Rect(0, 0, 3, 3));
681 Mat t = X(Rect(3, 0, 1, 3));
682 R_cam2gripper = R;
683 t_cam2gripper = t;
684 }
685
calibrateHandEye(InputArrayOfArrays R_gripper2base,InputArrayOfArrays t_gripper2base,InputArrayOfArrays R_target2cam,InputArrayOfArrays t_target2cam,OutputArray R_cam2gripper,OutputArray t_cam2gripper,HandEyeCalibrationMethod method)686 void calibrateHandEye(InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base,
687 InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam,
688 OutputArray R_cam2gripper, OutputArray t_cam2gripper,
689 HandEyeCalibrationMethod method)
690 {
691 CV_Assert(R_gripper2base.isMatVector() && t_gripper2base.isMatVector() &&
692 R_target2cam.isMatVector() && t_target2cam.isMatVector());
693
694 std::vector<Mat> R_gripper2base_, t_gripper2base_;
695 R_gripper2base.getMatVector(R_gripper2base_);
696 t_gripper2base.getMatVector(t_gripper2base_);
697
698 std::vector<Mat> R_target2cam_, t_target2cam_;
699 R_target2cam.getMatVector(R_target2cam_);
700 t_target2cam.getMatVector(t_target2cam_);
701
702 CV_Assert(R_gripper2base_.size() == t_gripper2base_.size() &&
703 R_target2cam_.size() == t_target2cam_.size() &&
704 R_gripper2base_.size() == R_target2cam_.size());
705 CV_Assert(R_gripper2base_.size() >= 3);
706
707 //Notation used in Tsai paper
708 //Defines coordinate transformation from G (gripper) to RW (robot base)
709 std::vector<Mat> Hg;
710 Hg.reserve(R_gripper2base_.size());
711 for (size_t i = 0; i < R_gripper2base_.size(); i++)
712 {
713 Mat m = Mat::eye(4, 4, CV_64FC1);
714 Mat R = m(Rect(0, 0, 3, 3));
715 R_gripper2base_[i].convertTo(R, CV_64F);
716
717 Mat t = m(Rect(3, 0, 1, 3));
718 t_gripper2base_[i].convertTo(t, CV_64F);
719
720 Hg.push_back(m);
721 }
722
723 //Defines coordinate transformation from CW (calibration target) to C (camera)
724 std::vector<Mat> Hc;
725 Hc.reserve(R_target2cam_.size());
726 for (size_t i = 0; i < R_target2cam_.size(); i++)
727 {
728 Mat m = Mat::eye(4, 4, CV_64FC1);
729 Mat R = m(Rect(0, 0, 3, 3));
730 R_target2cam_[i].convertTo(R, CV_64F);
731
732 Mat t = m(Rect(3, 0, 1, 3));
733 t_target2cam_[i].convertTo(t, CV_64F);
734
735 Hc.push_back(m);
736 }
737
738 Mat Rcg = Mat::eye(3, 3, CV_64FC1);
739 Mat Tcg = Mat::zeros(3, 1, CV_64FC1);
740
741 switch (method)
742 {
743 case CALIB_HAND_EYE_TSAI:
744 calibrateHandEyeTsai(Hg, Hc, Rcg, Tcg);
745 break;
746
747 case CALIB_HAND_EYE_PARK:
748 calibrateHandEyePark(Hg, Hc, Rcg, Tcg);
749 break;
750
751 case CALIB_HAND_EYE_HORAUD:
752 calibrateHandEyeHoraud(Hg, Hc, Rcg, Tcg);
753 break;
754
755 case CALIB_HAND_EYE_ANDREFF:
756 calibrateHandEyeAndreff(Hg, Hc, Rcg, Tcg);
757 break;
758
759 case CALIB_HAND_EYE_DANIILIDIS:
760 calibrateHandEyeDaniilidis(Hg, Hc, Rcg, Tcg);
761 break;
762
763 default:
764 break;
765 }
766
767 Rcg.copyTo(R_cam2gripper);
768 Tcg.copyTo(t_cam2gripper);
769 }
770 }
771