writen by wqj1212@yahoo.com.cn
位置0到位置1 各个参数关系
位置0 末端到机器人基坐标位置
---- PulseNO.S 82501 ,L 69953 ,U - 114458 ,R - 2 ,B - 1 ,T 1
---- CartPos.S 190.184 ,L 355.823 ,U - 141.89 ,R 180 ,B - 4.86 ,T 61.88
a = 180 / 180 * pi;
b =- 4.86 / 180 * pi;
r = 61.88 / 180 * pi;
Rx = [ 1 0 0 ; 0 cos(a) - sin(a); 0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b); 0 1 0 ; - sin(b) 0 cos(b)];
Rz = [cos(r) - sin(r) 0 ;sin(r) cos(r) 0 ; 0 0 1 ];
R = Rz * Ry * Rx
R =
0.4696 0.8820 0.0399
0.8788 - 0.4713 0.0747
0.0847 0.0000 - 0.9964
G = [ 0.4696 0.8820 0.0399 190.184 ;
0.8788 - 0.4713 0.0747 355.823 ;
0.0847 0.0000 - 0.9964 - 141.89 ;
0 0 0 1 ];
0 位置图像外参数
Extrinsic parameters:
Translation vector: Tc_ext = [ - 15.564192 - 31.592337 447.897520 ]
Rotation vector: omc_ext = [ - 2.359346 - 2.071994 0.076281 ]
Rotation matrix: Rc_ext = [ 0.128482 0.991024 - 0.036917
0.991056 - 0.129661 - 0.031550
- 0.036054 - 0.032533 - 0.998820 ]
Pixel error: err = [ 0.15477 0.26929 ]
G = [ 0.128482 0.991024 - 0.036917 - 15.564192 ;
0.991056 - 0.129661 - 0.031550 - 31.592337 ;
- 0.036054 - 0.032533 - 0.998820 447.897520 ;
0 0 0 1 ];
位置1 末端到机器人基坐标位置
---- PulseNO.S 75359 ,L 48202 ,U - 107236 ,R - 3 ,B - 1 ,T 1
---- CartPos.S 214.629 ,L 324.508 ,U - 101.991 ,R 180 ,B - 10.24 ,T 56.52
a = 180 / 180 * pi;
b =- 10.24 / 180 * pi;
r = 56.52 / 180 * pi;
Rx = [ 1 0 0 ; 0 cos(a) - sin(a); 0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b); 0 1 0 ; - sin(b) 0 cos(b)];
Rz = [cos(r) - sin(r) 0 ;sin(r) cos(r) 0 ; 0 0 1 ];
R = Rz * Ry * Rx
R =
0.5429 0.8341 0.0981
0.8208 - 0.5516 0.1483
0.1778 0.0000 - 0.9841
G = [ 0.5429 0.8341 0.0981 214.629 ;
0.8208 - 0.5516 0.1483 324.508 ;
0.1778 0.0000 - 0.9841 - 101.991 ;
0 0 0 1 ];
1位置图像外参数
Extrinsic parameters:
Translation vector: Tc_ext = [ - 63.060175 - 21.839437 500.823439 ]
Rotation vector: omc_ext = [ 2.391549 1.911963 - 0.154389 ]
Rotation matrix: Rc_ext = [ 0.218178 0.975410 - 0.031214
0.967780 - 0.220369 - 0.121813
- 0.125696 - 0.003632 - 0.992062 ]
Pixel error: err = [ 0.11963 0.22145 ]
G = [ 0.218178 0.975410 - 0.031214 - 63.060175 ;
0.967780 - 0.220369 - 0.121813 - 21.839437 ;
- 0.125696 - 0.003632 - 0.992062 500.823439 ;
0 0 0 1 ];
由位置0 到位置1
摄像机外参数
Bc1 1右图像外参数 Ac1 0右图像外参数
Bc1 = [ 0.218178 0.975410 - 0.031214 - 63.060175 ;
0.967780 - 0.220369 - 0.121813 - 21.839437 ;
- 0.125696 - 0.003632 - 0.992062 500.823439 ;
0 0 0 1 ];
Ac1 = [ 0.128482 0.991024 - 0.036917 - 15.564192 ;
0.991056 - 0.129661 - 0.031550 - 31.592337 ;
- 0.036054 - 0.032533 - 0.998820 447.897520 ;
0 0 0 1 ];
C1 = Ac1 * inv(Bc1)
工具在基坐标系下
Ad1 0图像时,工具的姿态;Bd1 1图像时工具的姿态
Ad1 = [ 0.4696 0.8820 0.0399 190.184 ;
0.8788 - 0.4713 0.0747 355.823 ;
0.0847 0.0000 - 0.9964 - 141.89 ;
0 0 0 1 ];
Bd1 = [ 0.5429 0.8341 0.0981 214.629 ;
0.8208 - 0.5516 0.1483 324.508 ;
0.1778 0.0000 - 0.9841 - 101.991 ;
0 0 0 1 ];
d1 = inv(Ad1) * Bd1
结果:
右摄像机在A(0个右图像 ) B(1个右图像)两位置的关系
C1 = [ 0.9958 - 0.0896 0.0169 36.8261 ;
0.0907 0.9915 - 0.0928 42.2616 ;
- 0.0084 0.0939 0.9955 - 49.1724 ;
0 0 0 1.0000 ];
工具在两位置的关系
d1 = [ 0.9913 - 0.0931 0.0931 - 12.6618 ;
0.0920 0.9956 0.0166 36.3170 ;
- 0.0942 - 0.0079 0.9956 - 41.1195 ;
0 0 0 1.0000 ];
位置2 末端到机器人基坐标位置
---- PulseNO.S 86502 ,L 25258 ,U - 100704 ,R - 3 ,B - 1 ,T 1
---- CartPos.S 153.909 ,L 328.212 ,U - 70.151 ,R 180 ,B - 15.1 ,T 64.87
--- Status: 1 - cycle
a = 180 / 180 * pi;
b =- 15.1 / 180 * pi;
r = 64.87 / 180 * pi;
Rx = [ 1 0 0 ; 0 cos(a) - sin(a); 0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b); 0 1 0 ; - sin(b) 0 cos(b)];
Rz = [cos(r) - sin(r) 0 ;sin(r) cos(r) 0 ; 0 0 1 ];
R = Rz * Ry * Rx
R =
0.4100 0.9053 0.1106
0.8741 - 0.4247 0.2358
0.2605 0.0000 - 0.9655
G = [ 0.4100 0.9053 0.1106 153.909 ;
0.8741 - 0.4247 0.2358 328.212 ;
0.2605 0.0000 - 0.9655 - 70.151 ;
0 0 0 1 ];
位置2图像外参数
Extrinsic parameters:
Translation vector: Tc_ext = [ 14.913514 - 27.935731 544.266948 ]
Rotation vector: omc_ext = [ 2.201095 2.042448 - 0.254298 ]
Rotation matrix: Rc_ext = [ 0.070836 0.996832 - 0.036178
0.975269 - 0.076825 - 0.207238
- 0.209361 - 0.020603 - 0.977621 ]
Pixel error: err = [ 0.08458 0.19457 ]
G = [ 0.070836 0.996832 - 0.036178 14.913514 ;
0.975269 - 0.076825 - 0.207238 - 27.935731 ;
- 0.209361 - 0.020603 - 0.977621 544.266948 ;
0 0 0 1 ];
由位置1 - 位置2
摄像机外参数
Bc2 2右图像外参数 Ac2 1右图像外参数
Bc2 = [ 0.070836 0.996832 - 0.036178 14.913514 ;
0.975269 - 0.076825 - 0.207238 - 27.935731 ;
- 0.209361 - 0.020603 - 0.977621 544.266948 ;
0 0 0 1 ];
Ac2 = [ 0.218178 0.975410 - 0.031214 - 63.060175 ;
0.967780 - 0.220369 - 0.121813 - 21.839437 ;
- 0.125696 - 0.003632 - 0.992062 500.823439 ;
0 0 0 1 ];
C2 = Ac2 * inv(Bc2)
工具在基坐标系下
Ad2 1图像时,工具的姿态;Bd2 2图像时工具的姿态
Ad2 = [ 0.5429 0.8341 0.0981 214.629 ;
0.8208 - 0.5516 0.1483 324.508 ;
0.1778 0.0000 - 0.9841 - 101.991 ;
0 0 0 1 ];
Bd2 = [ 0.4100 0.9053 0.1106 153.909 ;
0.8741 - 0.4247 0.2358 328.212 ;
0.2605 0.0000 - 0.9655 - 70.151 ;
0 0 0 1 ];
d2 = inv(Ad2) * Bd2
结果:
右摄像机在A(0个右图像 ) B(1个右图像)两位置的关系
C2 = [ 0.9889 0.1443 - 0.0353 - 54.5862 ;
- 0.1467 0.9860 - 0.0790 50.8843 ;
0.0234 0.0833 0.9963 - 39.4254 ;
0 0 0 1.0000 ];
工具在两位置的关系
d2 = [ 0.9863 0.1428 0.0819 - 24.2573 ;
- 0.1403 0.9894 - 0.0378 - 52.6877 ;
- 0.0865 0.0258 0.9959 - 36.7371 ;
0 0 0 1.0000 ];
标定结果
Rc1 = [ 0.9958 - 0.0896 0.0169 ;
0.0907 0.9915 - 0.0928 ;
- 0.0084 0.0939 0.9955 ];
Rd1 = [ 0.9913 - 0.0931 0.0931
0.0920 0.9956 0.0166
- 0.0942 - 0.0079 0.9956 ];
Rc2 = [ 0.9889 0.1443 - 0.0353
- 0.1467 0.9860 - 0.0790
0.0234 0.0833 0.9963 ]
Rd2 = [ 0.9863 0.1428 0.0819
- 0.1403 0.9894 - 0.0378
- 0.0865 0.0258 0.9959 ];
q = acos((trace(R) - 1 ) / 2 );
w( 1 , 1 ) = q / ( 2 * sin(q)) * (R( 3 , 2 ) - R( 2 , 3 ));
w( 2 , 1 ) = q / ( 2 * sin(q)) * (R( 1 , 3 ) - R( 3 , 1 ));
w( 3 , 1 ) = q / ( 2 * sin(q)) * (R( 2 , 1 ) - R( 1 , 2 ))
结果
kc1 = [ 0.0936 ;
0.0127 ;
0.0904 ];
kc2 = [ 0.0815 ;
- 0.0295 ;
- 0.1462 ];
kd1 = [ - 0.0123 ;
0.0939 ;
0.0928 ];
kd2 = [ 0.0320 ;
0.0846 ;
- 0.1422 ];
kc3 = cross(kc1,kc2);
kd3 = cross(kd1,kd2);
a = [kc1 kc2 kc3]
b = [kd1 kd2 kd3]
R = a * inv(b)
R = [kc1 kc2 cross(kc1,kc2)] * inv([kd1 kd2 cross(kd1,kd2)])
最终
R = [ 0.0155 0.9835 0.0155 ;
- 0.9906 0.0131 - 0.0077 ;
- 0.0147 - 0.0328 1.0053 ];
Rc1 = [ 0.9958 - 0.0896 0.0169 ;
0.0907 0.9915 - 0.0928 ;
- 0.0084 0.0939 0.9955 ];
Rd1 = [ 0.9913 - 0.0931 0.0931
0.0920 0.9956 0.0166
- 0.0942 - 0.0079 0.9956 ];
Rc2 = [ 0.9889 0.1443 - 0.0353
- 0.1467 0.9860 - 0.0790
0.0234 0.0833 0.9963 ]
Rd2 = [ 0.9863 0.1428 0.0819
- 0.1403 0.9894 - 0.0378
- 0.0865 0.0258 0.9959 ];
tc1 = [ 36.8261 ; 42.2616 ; - 49.1724 ];
td1 = [ - 12.6618 ; 36.3170 ; - 41.1195 ];
tc2 = [ - 54.5862 ; 50.8843 ; - 39.4254 ];
td2 = [ - 24.2573 ; - 52.6877 ; - 36.7371 ];
c = R * td1 - tc1;
d = R * td2 - tc2;
a = Rc1 - [ 1 0 0 ; 0 1 0 ; 0 0 1 ];
b = Rc2 - [ 1 0 0 ; 0 1 0 ; 0 0 1 ];
h = [a;b];
y = [c;d];
t = inv(h ' *h)*h ' * y
t =
t =
7.7295
84.1694
312.1403
位置0 末端到机器人基坐标位置
---- PulseNO.S 82501 ,L 69953 ,U - 114458 ,R - 2 ,B - 1 ,T 1
---- CartPos.S 190.184 ,L 355.823 ,U - 141.89 ,R 180 ,B - 4.86 ,T 61.88
a = 180 / 180 * pi;
b =- 4.86 / 180 * pi;
r = 61.88 / 180 * pi;
Rx = [ 1 0 0 ; 0 cos(a) - sin(a); 0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b); 0 1 0 ; - sin(b) 0 cos(b)];
Rz = [cos(r) - sin(r) 0 ;sin(r) cos(r) 0 ; 0 0 1 ];
R = Rz * Ry * Rx
R =
0.4696 0.8820 0.0399
0.8788 - 0.4713 0.0747
0.0847 0.0000 - 0.9964
G = [ 0.4696 0.8820 0.0399 190.184 ;
0.8788 - 0.4713 0.0747 355.823 ;
0.0847 0.0000 - 0.9964 - 141.89 ;
0 0 0 1 ];
0 位置图像外参数
Extrinsic parameters:
Translation vector: Tc_ext = [ - 15.564192 - 31.592337 447.897520 ]
Rotation vector: omc_ext = [ - 2.359346 - 2.071994 0.076281 ]
Rotation matrix: Rc_ext = [ 0.128482 0.991024 - 0.036917
0.991056 - 0.129661 - 0.031550
- 0.036054 - 0.032533 - 0.998820 ]
Pixel error: err = [ 0.15477 0.26929 ]
G = [ 0.128482 0.991024 - 0.036917 - 15.564192 ;
0.991056 - 0.129661 - 0.031550 - 31.592337 ;
- 0.036054 - 0.032533 - 0.998820 447.897520 ;
0 0 0 1 ];
位置1 末端到机器人基坐标位置
---- PulseNO.S 75359 ,L 48202 ,U - 107236 ,R - 3 ,B - 1 ,T 1
---- CartPos.S 214.629 ,L 324.508 ,U - 101.991 ,R 180 ,B - 10.24 ,T 56.52
a = 180 / 180 * pi;
b =- 10.24 / 180 * pi;
r = 56.52 / 180 * pi;
Rx = [ 1 0 0 ; 0 cos(a) - sin(a); 0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b); 0 1 0 ; - sin(b) 0 cos(b)];
Rz = [cos(r) - sin(r) 0 ;sin(r) cos(r) 0 ; 0 0 1 ];
R = Rz * Ry * Rx
R =
0.5429 0.8341 0.0981
0.8208 - 0.5516 0.1483
0.1778 0.0000 - 0.9841
G = [ 0.5429 0.8341 0.0981 214.629 ;
0.8208 - 0.5516 0.1483 324.508 ;
0.1778 0.0000 - 0.9841 - 101.991 ;
0 0 0 1 ];
1位置图像外参数
Extrinsic parameters:
Translation vector: Tc_ext = [ - 63.060175 - 21.839437 500.823439 ]
Rotation vector: omc_ext = [ 2.391549 1.911963 - 0.154389 ]
Rotation matrix: Rc_ext = [ 0.218178 0.975410 - 0.031214
0.967780 - 0.220369 - 0.121813
- 0.125696 - 0.003632 - 0.992062 ]
Pixel error: err = [ 0.11963 0.22145 ]
G = [ 0.218178 0.975410 - 0.031214 - 63.060175 ;
0.967780 - 0.220369 - 0.121813 - 21.839437 ;
- 0.125696 - 0.003632 - 0.992062 500.823439 ;
0 0 0 1 ];
由位置0 到位置1
摄像机外参数
Bc1 1右图像外参数 Ac1 0右图像外参数
Bc1 = [ 0.218178 0.975410 - 0.031214 - 63.060175 ;
0.967780 - 0.220369 - 0.121813 - 21.839437 ;
- 0.125696 - 0.003632 - 0.992062 500.823439 ;
0 0 0 1 ];
Ac1 = [ 0.128482 0.991024 - 0.036917 - 15.564192 ;
0.991056 - 0.129661 - 0.031550 - 31.592337 ;
- 0.036054 - 0.032533 - 0.998820 447.897520 ;
0 0 0 1 ];
C1 = Ac1 * inv(Bc1)
工具在基坐标系下
Ad1 0图像时,工具的姿态;Bd1 1图像时工具的姿态
Ad1 = [ 0.4696 0.8820 0.0399 190.184 ;
0.8788 - 0.4713 0.0747 355.823 ;
0.0847 0.0000 - 0.9964 - 141.89 ;
0 0 0 1 ];
Bd1 = [ 0.5429 0.8341 0.0981 214.629 ;
0.8208 - 0.5516 0.1483 324.508 ;
0.1778 0.0000 - 0.9841 - 101.991 ;
0 0 0 1 ];
d1 = inv(Ad1) * Bd1
结果:
右摄像机在A(0个右图像 ) B(1个右图像)两位置的关系
C1 = [ 0.9958 - 0.0896 0.0169 36.8261 ;
0.0907 0.9915 - 0.0928 42.2616 ;
- 0.0084 0.0939 0.9955 - 49.1724 ;
0 0 0 1.0000 ];
工具在两位置的关系
d1 = [ 0.9913 - 0.0931 0.0931 - 12.6618 ;
0.0920 0.9956 0.0166 36.3170 ;
- 0.0942 - 0.0079 0.9956 - 41.1195 ;
0 0 0 1.0000 ];
位置2 末端到机器人基坐标位置
---- PulseNO.S 86502 ,L 25258 ,U - 100704 ,R - 3 ,B - 1 ,T 1
---- CartPos.S 153.909 ,L 328.212 ,U - 70.151 ,R 180 ,B - 15.1 ,T 64.87
--- Status: 1 - cycle
a = 180 / 180 * pi;
b =- 15.1 / 180 * pi;
r = 64.87 / 180 * pi;
Rx = [ 1 0 0 ; 0 cos(a) - sin(a); 0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b); 0 1 0 ; - sin(b) 0 cos(b)];
Rz = [cos(r) - sin(r) 0 ;sin(r) cos(r) 0 ; 0 0 1 ];
R = Rz * Ry * Rx
R =
0.4100 0.9053 0.1106
0.8741 - 0.4247 0.2358
0.2605 0.0000 - 0.9655
G = [ 0.4100 0.9053 0.1106 153.909 ;
0.8741 - 0.4247 0.2358 328.212 ;
0.2605 0.0000 - 0.9655 - 70.151 ;
0 0 0 1 ];
位置2图像外参数
Extrinsic parameters:
Translation vector: Tc_ext = [ 14.913514 - 27.935731 544.266948 ]
Rotation vector: omc_ext = [ 2.201095 2.042448 - 0.254298 ]
Rotation matrix: Rc_ext = [ 0.070836 0.996832 - 0.036178
0.975269 - 0.076825 - 0.207238
- 0.209361 - 0.020603 - 0.977621 ]
Pixel error: err = [ 0.08458 0.19457 ]
G = [ 0.070836 0.996832 - 0.036178 14.913514 ;
0.975269 - 0.076825 - 0.207238 - 27.935731 ;
- 0.209361 - 0.020603 - 0.977621 544.266948 ;
0 0 0 1 ];
由位置1 - 位置2
摄像机外参数
Bc2 2右图像外参数 Ac2 1右图像外参数
Bc2 = [ 0.070836 0.996832 - 0.036178 14.913514 ;
0.975269 - 0.076825 - 0.207238 - 27.935731 ;
- 0.209361 - 0.020603 - 0.977621 544.266948 ;
0 0 0 1 ];
Ac2 = [ 0.218178 0.975410 - 0.031214 - 63.060175 ;
0.967780 - 0.220369 - 0.121813 - 21.839437 ;
- 0.125696 - 0.003632 - 0.992062 500.823439 ;
0 0 0 1 ];
C2 = Ac2 * inv(Bc2)
工具在基坐标系下
Ad2 1图像时,工具的姿态;Bd2 2图像时工具的姿态
Ad2 = [ 0.5429 0.8341 0.0981 214.629 ;
0.8208 - 0.5516 0.1483 324.508 ;
0.1778 0.0000 - 0.9841 - 101.991 ;
0 0 0 1 ];
Bd2 = [ 0.4100 0.9053 0.1106 153.909 ;
0.8741 - 0.4247 0.2358 328.212 ;
0.2605 0.0000 - 0.9655 - 70.151 ;
0 0 0 1 ];
d2 = inv(Ad2) * Bd2
结果:
右摄像机在A(0个右图像 ) B(1个右图像)两位置的关系
C2 = [ 0.9889 0.1443 - 0.0353 - 54.5862 ;
- 0.1467 0.9860 - 0.0790 50.8843 ;
0.0234 0.0833 0.9963 - 39.4254 ;
0 0 0 1.0000 ];
工具在两位置的关系
d2 = [ 0.9863 0.1428 0.0819 - 24.2573 ;
- 0.1403 0.9894 - 0.0378 - 52.6877 ;
- 0.0865 0.0258 0.9959 - 36.7371 ;
0 0 0 1.0000 ];
标定结果
Rc1 = [ 0.9958 - 0.0896 0.0169 ;
0.0907 0.9915 - 0.0928 ;
- 0.0084 0.0939 0.9955 ];
Rd1 = [ 0.9913 - 0.0931 0.0931
0.0920 0.9956 0.0166
- 0.0942 - 0.0079 0.9956 ];
Rc2 = [ 0.9889 0.1443 - 0.0353
- 0.1467 0.9860 - 0.0790
0.0234 0.0833 0.9963 ]
Rd2 = [ 0.9863 0.1428 0.0819
- 0.1403 0.9894 - 0.0378
- 0.0865 0.0258 0.9959 ];
q = acos((trace(R) - 1 ) / 2 );
w( 1 , 1 ) = q / ( 2 * sin(q)) * (R( 3 , 2 ) - R( 2 , 3 ));
w( 2 , 1 ) = q / ( 2 * sin(q)) * (R( 1 , 3 ) - R( 3 , 1 ));
w( 3 , 1 ) = q / ( 2 * sin(q)) * (R( 2 , 1 ) - R( 1 , 2 ))
结果
kc1 = [ 0.0936 ;
0.0127 ;
0.0904 ];
kc2 = [ 0.0815 ;
- 0.0295 ;
- 0.1462 ];
kd1 = [ - 0.0123 ;
0.0939 ;
0.0928 ];
kd2 = [ 0.0320 ;
0.0846 ;
- 0.1422 ];
kc3 = cross(kc1,kc2);
kd3 = cross(kd1,kd2);
a = [kc1 kc2 kc3]
b = [kd1 kd2 kd3]
R = a * inv(b)
R = [kc1 kc2 cross(kc1,kc2)] * inv([kd1 kd2 cross(kd1,kd2)])
最终
R = [ 0.0155 0.9835 0.0155 ;
- 0.9906 0.0131 - 0.0077 ;
- 0.0147 - 0.0328 1.0053 ];
Rc1 = [ 0.9958 - 0.0896 0.0169 ;
0.0907 0.9915 - 0.0928 ;
- 0.0084 0.0939 0.9955 ];
Rd1 = [ 0.9913 - 0.0931 0.0931
0.0920 0.9956 0.0166
- 0.0942 - 0.0079 0.9956 ];
Rc2 = [ 0.9889 0.1443 - 0.0353
- 0.1467 0.9860 - 0.0790
0.0234 0.0833 0.9963 ]
Rd2 = [ 0.9863 0.1428 0.0819
- 0.1403 0.9894 - 0.0378
- 0.0865 0.0258 0.9959 ];
tc1 = [ 36.8261 ; 42.2616 ; - 49.1724 ];
td1 = [ - 12.6618 ; 36.3170 ; - 41.1195 ];
tc2 = [ - 54.5862 ; 50.8843 ; - 39.4254 ];
td2 = [ - 24.2573 ; - 52.6877 ; - 36.7371 ];
c = R * td1 - tc1;
d = R * td2 - tc2;
a = Rc1 - [ 1 0 0 ; 0 1 0 ; 0 0 1 ];
b = Rc2 - [ 1 0 0 ; 0 1 0 ; 0 0 1 ];
h = [a;b];
y = [c;d];
t = inv(h ' *h)*h ' * y
t =
t =
7.7295
84.1694
312.1403