Ask Your Question
0

Calculate robot coordinates from measured chessbaord corners ( hand-eye calibration)

asked 2018-12-07 06:35:46 -0500

Mysterion46 gravatar image

updated 2018-12-07 10:19:11 -0500

Hello guys,

for my project I am trying to calibrate my depth camera with my robot-arm (hand-eye calibration). Therefore, I measured 8 corners of the cheassboard and got 8 pixel vectors and their corresponding robot 3D coordinates (using the gripper to point exactly at the corner). Then, I tried to calculate the transformation matrix. But the result is way off. Where's the mistake? Thank you very much!

Edit: Added the camera matrix

img_pts = np.array(((237., 325.),
                    (242., 245.),
                    (248., 164.),
                    (318., 330.),
                    (322., 250.),
                    (328., 170.),
                    (398., 336.),
                    (403., 255.)), dtype=np.float32)

objectPoints = np.array(((-437., -491., -259.),
                         (-402., -457., -259.),
                         (-360., -421., -259.),
                         (-393., -534., -259.),
                         (-362., -504., -259.),
                         (-332., -476., -259.),
                         (-298., -511., -259.),
                         (-334., -546., -259.)), dtype=np.float32)

camera_matrix = np.array(((542.5860286838929, 0., 310.4749867256299),
                          (0., 544.2396851407029, 246.7577402755397),
                          (0., 0., 1.)), dtype=np.float32)

distortion = np.array([0.03056762860315778,
                       -0.1824835906443329,
                       -0.0007936359893356936,
                       -0.001735795279032343,
                       0])

ret, rvec, tvec = cv2.solvePnP(objectPoints, img_pts, camera_matrix, distortion)
rmtx, _ = cv2.Rodrigues(rvec)
T_gripper_cam = np.array([[rmtx[0][0], rmtx[0][1], rmtx[0][2], tvec[0]],
                         [rmtx[1][0], rmtx[1][1], rmtx[1][2], tvec[1]],
                         [rmtx[2][0], rmtx[2][1], rmtx[2][2], tvec[2]],
                         [0.0, 0.0, 0.0, 1.0]])
T_cam_gripper = np.linalg.inv(T_gripper_cam)
print(T_cam_gripper)

p = np.array((237., 325., 0.), dtype=np.float32)

p_new = np.array([p[0], p[1], p[2], 1])

objectPoint = np.dot(p_new, T_gripper_cam)[:3]

print(objectPoint)
# should be (-437., -491., -259.)
# --> actual = [  33.57 -395.62  -64.46]

------------------------------------- Edit ---------------------------------------------------------- I think I might be on to something. According to this: Stackoverflow

I have to calculate the scaling factor. So i tried to do this in python:

uv = np.array(([237.], [325.], [1.]), dtype=np.float32)
rotInv = np.linalg.inv(rmtx)
camMatInv = np.linalg.inv(camera_matrix)
leftSideMat = np.dot(rotInv, np.dot(camMatInv, uv))
rightSideMat = np.dot(rotInv, tvec)

s = (295 + leftSideMat[2:] / rightSideMat[2:])
temp = (s * np.dot(camMatInv, uv))
tempSub = np.array(temp - tvec)

print(np.dot(rotInv, tempSub))
# should be (-437., -491., -259.)
# --> actual = [  437.2 -501.3  -266.2]

Looks like I am pretty close. But its still way off in the Y direction. Is this the correct approach? Thank you!

edit retag flag offensive close merge delete

Comments

might not change much (and we cannot reproduce w/o your intrinsics Mat) but imho, it shouild be:

p = np.array((237., 325., 1.), dtype=np.float32)

(homogeneous 2d coords, see formulas here)

berak gravatar imageberak ( 2018-12-07 08:06:20 -0500 )edit

Thank you first of all! I think that's what i did in:

p_new = np.array([p[0], p[1], p[2], 1])

p should just be a pixel vector but i added the zero to multiply it with my transformation matrix.

Mysterion46 gravatar imageMysterion46 ( 2018-12-07 08:28:58 -0500 )edit

1 answer

Sort by ยป oldest newest most voted
0

answered 2018-12-07 11:28:14 -0500

Eduardo gravatar image

You should add a figure of your setup to better let other people understand what you want to do. If I have understood correctly, you are doing the following:

  • move the gripper to point to a chessboard corner
  • get the corner location in the image (will be img_pts)
  • get the gripper position (will be in objectPoints)
  • repeat for 8 corners
  • compute solvePnP

This looks not correct for hand-eye calibration:

  • solvePnP should be used to compute the transformation between the camera frame and the object frame
  • in your case, objectPoints are expressed in some reference frame
  • but for each corners in img_pts, the camera has moved
  • and hand-eye calibration is different than perspective-N-points method (pose from 2D/3D)

Hand-eye calibration

This topic is well covered in the literature:

Normally the setup should be:

  • the camera is attached to the end-effector of the robot and you want to estimate the transformation between the camera frame and the end-effector frame T_robot_camera

image description

  • or the camera is static and you want to estimate the transformation between the camera and the robot reference frame T_ext_camera

image description

In the first case, the calibration procedure is:

  • the calibration pattern is static
  • you move the end-effector to capture some images in a sphere around the calibration pattern
  • for each images, you have:
    • the transformation between the camera and the calibration pattern frames T_camera_pattern (with OpenCV, findChessboardCorners() + solvePnP())
    • the transformation between the end-effector and the robot frames T_ext_end-effector

Then, you can estimate the transformation between the end-effector frame and the camera frame using a standard hand-eye calibration method (e.g. Tsai method).

You should be able to find some code that implements the Tsai method or some similar methods, for instance maybe this one: ROS + CamOdoCal Hand Eye Calibration.

My advice is to use ViSP to do the Tsai calibration since I have already used this library:

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2018-12-07 06:35:46 -0500

Seen: 617 times

Last updated: Dec 07 '18