# Poor accuracy of hand eye calibration when using vanilla Tsai's method

I’m working on the hand eye calibration for the robotic arm. I attached the camera near the tip of the robot (or end effector) and took around 40 pictures of an asymmetric circles pattern. I implemented the basic hand-eye calibration code (at the bottom) based on the papers such as this (http://people.csail.mit.edu/tieu/stuff/Tsai.pdf). I referred the similar questions such as Hand Eye Calibration or Hand Eye Calibration Solver. The implementation is almost same as this (http://lazax.com/www.cs.columbia.edu/~laza/html/Stewart/matlab/handEye.m).

When I ran my code, I noticed that the quality of the result was very bad, especially in the rotation matrix. I calculated the quality based on R squared or the coefficient of determination. R2 for the rotation matrix after the least square regression was always around 0.2~0.3 (regarding the translation vector, R2 was around 0.6).

When I compared the returned homogeneous matrix from the ground truth (which I measured and calculated carefully by hand), they are very different as shown below. The rotation around z axis should be around -90 degrees, but the output was around a half.:

• The output from the code

• Homogeneous matrix:
• [[ 0.6628663 0.74871869 0.02212943 44.34775423] [ -0.74841069 0.66234872 0.02672996 -21.83390692] [ 0.00534759 -0.0342871 0.99939772 39.74953567] [ 0. 0. 0. 1. ]]
• Rotation radian (rz, ry, rx):
• (-0.8461431892931816, -0.005347615473812833, -0.03429431203996466)
• Ground truth

• Homogeneous matrix:
• [[ -0.01881762 0.9997821 -0.00903642 -70.90496041] [ -0.99782701 -0.01820849 0.06332229 -19.55120885] [ 0.06314395 0.01020836 0.99795222 60.04617152] [ 0. 0. 0. 1. ]]
• Rotation radian (rz, ry, rx):
• (-1.5896526911533568, -0.06318598618916291, 0.01022895059953901)

My questions are

• Is it common to get the poor result from the vanilla Tsai’s method?
• If yes, how can I improve the result?
• If no, where did I make a mistake?

Here is the code I used:

import numpy as np
from transforms3d.axangles import mat2axangle, axangle2mat
def find_hand_to_camera_transform(list_of_cHo, list_of_bHe):
"""
:param list_of_cHo: List of homogeneous matrices from Camera frame to Object frame(calibration pattern such as chessboard or asymmetric circles)
:param list_of_bHe: List of homogeneous matrices from robot's Base frame to End effector (hand) frame
:return: eHc: Homogeneous matrix from End effector frame to Camera frame
Notation:
- H: 4x4 homogeneous matrix
- R: 3x3 rotation matrix
- T: 3x1 translation matrix (vector)
- P: Axis vector for Axis-Angle representation
- TH: Angle (theta) for Axis-Angle representation
"""

# Calculate rotational component
lhs = []
rhs = []
for i in range(num_of_poses):
bRei = extract_rotation(list_of_bHe[i])
ciRo = extract_rotation(list_of_cHo[i])
for j in range(i + 1, num_of_poses):  # We don't to use two times same couples
bRej = extract_rotation(list_of_bHe[j])
cjRo = extract_rotation(list_of_cHo[j])
eiRej = np.dot(bRei.T, bRej)  # Rotation from i to j
ciRcj = np.dot(ciRo, cjRo.T)  # Rotation from i to j
eiPej, eiTHej = mat2axangle(eiRej)  # Note: mat2axangle returns with normalization (norm = 1.0)
ciPcj, ciTHcj = mat2axangle(ciRcj)
lhs.append(find_skew_matrix(eiPej + ciPcj))
rhs.append(ciPcj - eiPej)

lhs = np.array(lhs)
lhs = lhs.reshape(lhs.shape[0] * 3, 3)
rhs = np.array(rhs)
rhs = rhs.reshape(rhs.shape[0] * 3)
cPe_, res, _, _ = np.linalg.lstsq(lhs, rhs)
r2_rot = 1 - res / (rhs.size * rhs.var())

cTHe = 2 * np.arctan(np.linalg.norm(cPe_))
cPe = 2 * cPe_ / np.sqrt(1 + np.dot(cPe_.reshape(3), cPe_.reshape(3)))
cPe = cPe / np.linalg.norm(cPe)
cRe = axangle2mat(cPe, cTHe, is_normalized=True)
eRc = cRe.T

# Calculate translational component
lhs = []
rhs = []
for i in range(num_of_poses):
bRei = extract_rotation(list_of_bHe[i])
bTei = extract_translation(list_of_bHe[i])
ciTo = extract_translation(list_of_cHo[i])
for j in range(i + 1, num_of_poses):  # We don't to use two times same couples
bRej = extract_rotation(list_of_bHe[j])
bTej = extract_translation(list_of_bHe[j])
cjTo = extract_translation(list_of_cHo[j])
eiRej = np.dot(bRei.T, bRej)
eiTej = np.dot(bRei.T, bTei - bTej)
lhs.append(eiRej - np.eye(3))
rhs.append(np.dot(eRc, ciTo) - np.dot(np.dot(eiRej, eRc), cjTo) + eiTej)

lhs = np.array(lhs)
lhs = lhs.reshape(lhs.shape[0] * 3, 3)
rhs = np.array(rhs)
rhs = rhs.reshape(rhs.shape[0] * 3)
eTc, res, _, _ = np.linalg.lstsq(lhs, rhs)
r2_trans = 1 - res / (rhs.size * rhs.var())

eHc = np.eye(4)
eHc[:3, :3] = eRc
eHc[0:3, 3] = eTc
return eHc