Skip to content

Latest commit

 

History

History
145 lines (86 loc) · 4.72 KB

hand_eye_calibration.md

File metadata and controls

145 lines (86 loc) · 4.72 KB

1. Hand Eye Calibration

1.1 Eye-in-Hand



From robot kinematic we have the position of griper in robot base at time i,j:

https://latex.codecogs.com/svg.latex?A_i={}^{b}T_{g}^{(i)}



https://latex.codecogs.com/svg.latex?A_j={}^{b}T_{g}^{(j)}

From the camera we have the position of target in camera at time i,j:

https://latex.codecogs.com/svg.latex?B_i={}^{c}T_{t}^{(i)}



https://latex.codecogs.com/svg.latex?B_j={}^{c}T_{t}^{(j)}



Now

https://latex.codecogs.com/svg.latex?B=B_iB_j^{-1}

https://latex.codecogs.com/svg.latex?T_{c_j}^{c_i}={}^{c}T_{t}^{(i)}  \left ( {}^{c}T_{t}^{(j)} \right )^{-1}=
{}^{c}T_{t}^{(i)}   {}^{t}T_{c}^{(j)}

and:

https://latex.codecogs.com/svg.latex?A=A_j^{-1} A_i

https://latex.codecogs.com/svg.latex?T_{g_j}^{g_i}=\left ({}^{b}T_{g}^{(i)}  \right )^{-1}  {}^{b}T_{g}^{(j)}=
{}^{g}T_{b}^{(i)}   {}^{b}T_{g}^{(j)}

Now if we solve a set of equation in the from of:

https://latex.codecogs.com/svg.latex?\\
AX=XB
\\
T_{g_i}^{g_j}X=XT_{c_i}^{c_j}
\\
X=T_{c}^{g}





https://latex.codecogs.com/svg.latex?\\
AT_c^{g}=T_c^{g}B
\\
\\
T_{g_j}^{g_i}T_c^{g}=T_c^{g}T_{c_j}^{c_i}
\\
\\
T^{g_i}_c=T^g_c_j

since the relative position of griper and camera is always fixed at time i and j we can drop them.

Refs: 1, 2, 3

1.2 Eye-to-Hand

1.3 OpenCV Eye-in-Hand API

The following API computes Hand-Eye calibration: _{}^{g}\textrm{T}_c

calibrateHandEye	(	InputArrayOfArrays 	R_gripper2base,
InputArrayOfArrays 	t_gripper2base,
InputArrayOfArrays 	R_target2cam,
InputArrayOfArrays 	t_target2cam,
OutputArray 	R_cam2gripper,
OutputArray 	t_cam2gripper,
HandEyeCalibrationMethod 	method = CALIB_HAND_EYE_TSAI 
)	

2. Robot-World/Hand-Eye Calibration

robot-world_hand-eye_figure





This API computes Robot-World/Hand-Eye calibration:

https://latex.codecogs.com/svg.latex?_{}^{w}\textrm{T}_b \text{ and } _{}^{c}\textrm{T}_g

1.3 OpenCV Robot-World/Hand-Eye API

cv::calibrateRobotWorldHandEye	(	InputArrayOfArrays 	R_world2cam,
InputArrayOfArrays 	t_world2cam,
InputArrayOfArrays 	R_base2gripper,
InputArrayOfArrays 	t_base2gripper,
OutputArray 	R_base2world,
OutputArray 	t_base2world,
OutputArray 	R_gripper2cam,
OutputArray 	t_gripper2cam,
RobotWorldHandEyeCalibrationMethod 	method = CALIB_ROBOT_WORLD_HAND_EYE_SHAH 
)	

Refs: 1, 2, 3