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

Question

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

Answer 1

The most likely issues are too much noise, inaccurate camera calibration, not ensuring everything has settled when you take each data point, your camera and robot not being fixed rigidly enough, or your transforms being transposed or otherwise inconsistent.

A detailed overview of pitfalls and how to improve your results can be found at https://github.com/jhu-lcsr/handeye_calib_camodocal. Many of the instructions there apply to any method of solving for hand eye calibration and I've been updating the document and code every time someone encounters a new problem over the last two years, so at this point it should be pretty good.