Eye-in-hand

A camera is mounted on the robot end-effector. The goal is to find the transformation X from the robot end-effector frame E to the camera frame C.

Hand-eye calibration

As seen in the image, both T H R = T E1 R X T H C1 and THR = T E 2 R X T H C2 are transformations from the robot base to the chess board. Equating these gives

T E 1 R X T H C 1 = T E 2 R X T H C 2 .

Now, left-multiplying with T R E 2 and right-multiplying with T C 1 H gives

T R E 2 T E 1 R X = X T H C 2 T C 1 H

which is of the form A X = X B

Chessboard on the robot end-effector

Here a chessboard is mounted on the robot end-effector. The goal is to find the transformation X from the robot end-effector frame E to the chessboard frame H.

Robot camera calibration

Method

The standard procedure is to take pictures of a board with a checkered pattern using the camera. The pose of the checkered pattern in the camera frame C can be found using the OpenCV library. However, the transformation X is unknown. The transformation X can be found using the method described in

@article{park_robot_1994,
  title = {Robot sensor calibration: solving {AX}= {XB} on the {Euclidean} group},
  volume = {10},
  number = {5},
  journal = {IEEE Transactions on Robotics and Automation},
  author = {Park, Frank C and Martin, Bryan J},
  year = {1994},
  pages = {717--721}
}

A Python implementation of the method is provided here

park_martin.py

import numpy
from numpy import dot, eye, zeros, outer
from numpy.linalg import inv

def log(R):
    # Rotation matrix logarithm
    theta = numpy.arccos((R[0,0] + R[1,1] + R[2,2] - 1.0)/2.0)
    return numpy.array([R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1]]) * theta / (2*numpy.sin(theta))

def invsqrt(mat):
    u,s,v = numpy.linalg.svd(mat)
    return u.dot(numpy.diag(1.0/numpy.sqrt(s))).dot(v)

def calibrate(A, B):
    #transform pairs A_i, B_i
    N = len(A)
    M = numpy.zeros((3,3))
    for i in range(N):
        Ra, Rb = A[i][0:3, 0:3], B[i][0:3, 0:3]
        M += outer(log(Rb), log(Ra))

    Rx = dot(invsqrt(dot(M.T, M)), M.T)

    C = zeros((3*N, 3))
    d = zeros((3*N, 1))
    for i in range(N):
        Ra,ta = A[i][0:3, 0:3], A[i][0:3, 3]
        Rb,tb = B[i][0:3, 0:3], B[i][0:3, 3]
        C[3*i:3*i+3, :] = eye(3) - Ra
        d[3*i:3*i+3, 0] = ta - dot(Rx, tb)

    tx = dot(inv(dot(C.T, C)), dot(C.T, d))
    return Rx, tx.flatten()

Example data

The calibration code with example data can be downloaded from: https://github.com/torstem/robcam-calibration