Eye In Hand calibration

Dear community,

I am currently working on calibrating my Intel RealSense D435 camera, which is mounted on the tool flange of a 6 DOF robot arm, for a vision-based Pick and Place application. I have written a code that captures images of a chessboard at various user-defined positions and generates the rotation (R_cam2gripper) and translation (T_cam2gripper) matrices to calibrate the camera to the robot’s gripper. The relevant code snippet is included below.

I have two specific questions:

  1. Am I performing the calculations correctly in my code?

  2. After obtaining the R_cam2gripper and T_cam2gripper matrices, what are the next steps to detect objects in the robot’s base frame and perform the Pick and Place operation?

Here’s the relevant portion of my code:

import time
import pyrealsense2 as rs
import cv2
import numpy as np
import robot
import os
from math import sin, cos
os.environ["QT_QPA_PLATFORM"] = "xcb"

def initialize_camera():
    Initialize the Intel RealSense camera.

        pipeline (rs.pipeline): The configured RealSense pipeline.
    pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
    return pipeline

def initialize_robot(ip_address=""):
    Initialize the UR robot.

        ip_address (str): The IP address of the UR robot.

        robot (urx.Robot): The initialized robot object.
    my_robot = robot.Robot(ip_address)
    return my_robot

def capture_calibration_images(pipeline, robot, chessboard_size, square_size, num_images=10):
    Capture images from the camera and detect chessboard corners.

        pipeline (rs.pipeline): The RealSense pipeline.
        robot (urx.Robot): The UR robot object.
        chessboard_size (tuple): The number of inner corners per a chessboard row and column.
        square_size (float): The size of one square on the chessboard in meters.
        num_images (int): The number of images to capture for calibration.

        objpoints (list): The 3D points in real world space.
        imgpoints (list): The 2D points in image plane.
        robot_poses (list): The robot poses corresponding to each successful image capture.
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    objp = np.zeros((np.prod(chessboard_size), 3), np.float32)
    objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
    objp *= square_size

    objpoints = []  # 3d points in real world space
    imgpoints = []  # 2d points in image plane
    robot_poses = []

    for i in range(num_images):
        input("Move the robot to a new position and press Enter...")
        robot_pose = robot.get_pose()
        # receive the latest robot pose
        robot_pose = robot.get_pose()

        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        color_image = np.asanyarray(color_frame.get_data())

        gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

        if ret:
            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
            cv2.drawChessboardCorners(color_image, chessboard_size, corners2, ret)
            cv2.imshow('Calibration Image', color_image)
            print(f"Failed to detect chessboard at position {i+1}")

    return objpoints, imgpoints, robot_poses

def calibrate_camera(objpoints, imgpoints, image_shape):
    Calibrate the camera given object points and image points.

        objpoints (list): The 3D points in real world space.
        imgpoints (list): The 2D points in image plane.
        image_shape (tuple): The shape of the image used for calibration.

        camera_matrix (numpy.ndarray): The camera intrinsic matrix.
        dist_coeffs (numpy.ndarray): The distortion coefficients.
    ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
        objpoints, imgpoints, image_shape, None, None
    return camera_matrix, dist_coeffs, rvecs, tvecs

def ur_pose_to_opencv(pose):
    Convert a UR pose to the OpenCV format.

        pose (list): The pose from the UR robot in the form [x, y, z, rx, ry, rz].

        rotation (numpy.ndarray): The rotation matrix.
        translation (numpy.ndarray): The translation vector.
    translation = np.array([pose[0], pose[1], pose[2]])

    # Extract the rotation vector (rx, ry, rz)
    rx, ry, rz = pose[3:6]

    # Convert the rotation vector to a rotation matrix using Rodrigues' formula
    def rodrigues_rot_matrix(rx, ry, rz):
        theta = np.linalg.norm([rx, ry, rz])
        if theta < 1e-6:
            return np.eye(3)
        r = np.array([rx, ry, rz]) / theta
        c = cos(theta)
        s = sin(theta)
        C = 1 - c
        x, y, z = r
        R = np.array([
            [c + x * x * C, x * y * C - z * s, x * z * C + y * s],
            [y * x * C + z * s, c + y * y * C, y * z * C - x * s],
            [z * x * C - y * s, z * y * C + x * s, c + z * z * C]
        return R

    rotation = rodrigues_rot_matrix(rx, ry, rz)

    return rotation, translation

def perform_hand_eye_calibration(robot_poses, rvecs, tvecs):
    Perform hand-eye calibration.

        robot_poses (list): The robot poses.
        rvecs (list): The rotation vectors (from camera calibration).
        tvecs (list): The translation vectors (from camera calibration).

        R_cam2gripper (numpy.ndarray): Rotation from camera to gripper.
        T_cam2gripper (numpy.ndarray): Translation from camera to gripper.
    robot_rotations = []
    robot_translations = []

    for pose in robot_poses:
        rot, trans = ur_pose_to_opencv(pose)

    R_cam2gripper, T_cam2gripper = cv2.calibrateHandEye(
        robot_rotations, robot_translations, rvecs, tvecs, method=cv2.CALIB_HAND_EYE_TSAI

    return R_cam2gripper, T_cam2gripper

def main():
    # Initialize the camera and robot
    pipeline = initialize_camera()
    robot = initialize_robot()

    # Chessboard parameters
    chessboard_size = (8, 6)
    square_size = 0.025  # Size of a square in meters

    # Capture calibration images
    objpoints, imgpoints, robot_poses = capture_calibration_images(
        pipeline, robot, chessboard_size, square_size, num_images=10
    # Stop the camera pipeline

    # Calibrate the camera
    camera_matrix, dist_coeffs, rvecs, tvecs = calibrate_camera(
        objpoints, imgpoints, (640,480)

    # Perform hand-eye calibration
    R_cam2gripper, T_cam2gripper = perform_hand_eye_calibration(
        robot_poses, rvecs, tvecs

    # 3x3 rotation matrix from camera to gripper.
    # 3x1 translation vector from camera to gripper.

    print(f"Camera Matrix:\n{camera_matrix}")
    # Camera intrinsic matrix

    print(f"Distortion Coefficients:\n{dist_coeffs}")
    # Distortion coefficients

if __name__ == "__main__":