Estimate Pose of Moving Camera Mounted on a Robot

Since R2024b

This example uses:

This example shows how to perform and verify hand-eye calibration for a robot arm or manipulator equipped with a camera in the eye-in-hand configuration.

Screenshot 2024-06-03 114947.png

Overview

Performing hand-eye calibration is useful for operating a robot arm equipped with an end-effector, referred to as 'the hand' that relies on visual data from a camera. Once hand-eye calibration is complete, the robot arm can accurately move to specific pixel locations identified by the camera. This capability is fundamental for executing precise pick-and-place tasks, such as sorting, stacking, and bin picking, even in scenarios where the camera's exact position and orientation are unknown.

Robot-camera systems are available in two configurations: eye-in-hand and eye-to-hand. In the eye-in-hand configuration, the camera is mounted directly on the robot arm's end-effector. Conversely, in the eye-to-hand configuration, the camera is fixed on a stationary object with the robot arm within its field of view. This example focuses on hand-eye calibration specifically for an eye-in-hand configuration, yet the principles and techniques described can be adapted to suit eye-to-hand configuration.

In this example, you first calibrate the camera to determine its intrinsic parameters. Following this, hand-eye calibration is performed. The outcome of this calibration is then utilized to transform image points to coordinates in the robot's world frame. This example requires the Computer Vision Toolbox and Robotics System Toolbox.

Estimate Camera Intrinsic Parameters

Camera calibration is the first step in hand-eye calibration. This step focuses on estimating the intrinsic parameters (focal length, distortion, etc.) of the camera, which are required for removing image distortion and estimating the camera's pose relative to a calibration board.

To accurately calibrate your camera, collect 10-20 calibration images using the guidelines listed in Prepare Camera and Capture Images For Camera Calibration. In this example, the camera was calibrated with a checkerboard calibration pattern using the Using the Single Camera Calibrator App and the camera parameters were exported to cameraParams.mat.

Screenshot 2024-04-11 163012.png

Load these estimated camera intrinsic parameters into MATLAB.

ld = load("cameraParams.mat");
intrinsics = ld.cameraParams.Intrinsics;

Collect Images for Hand-Eye Calibration

For hand-eye calibration, you must take 15-30 photos from different perspectives. You should ensure the set of robot poses covers a wide range of the robot manipulator workspace. Using these images and the intrinsic parameters of the camera, you can determine the transformation between the calibration board and the camera.

Screenshot 2024-04-11 151315.png

It's important to ensure that the calibration board remains fully visible within the camera's field of view throughout this process. To optimize accuracy, capture a distinct set of images for the camera calibration process separate from those used for hand-eye calibration. For each pose, save a camera image along with the robot joint angles. This example uses a KINOVA Gen3 robot arm, which has a camera mounted near the end-effector. The Robotics System Toolbox Support Package for KINOVA Gen3 Manipulators includes tools to collect camera images and their corresponding pose angles in addition to offering capabilities to control the movement of the arm. Load the camera images into an imageDatastore in MATLAB.

numPoses = 30;
imds = imageDatastore("calibrationData");
montage(imds)

Estimate Camera Extrinsics

In this step, you estimate the camera's extrinsic parameters, which involve determining the transformation from the calibration board's origin to the camera for each pose. The extrinsics estimation process determines the location of the calibration checkerboard points within the undistorted image. These points are then used to determine the camera's position and orientation relative to the calibration board. For details on the camera extrinsics estimation process, see estimateExtrinsics.

squareSize = 0.022; % Measured in meters
camExtrinsics(numPoses,1) = rigidtform3d;

% Estimate transform from the board to the camera for each pose.
for i = 1:numPoses

    % Undistort the image using the estimated camera intrinsics.
    calibrationImage = readimage(imds,i);
    undistortedImage = undistortImage(calibrationImage,intrinsics);

    % Estimate the extrinsics while disabling the partial checkerboard
    % detections to ensure consistent checkerboard origin across poses. 
    [imagePoints, boardSize] = detectCheckerboardPoints(undistortedImage,PartialDetections=false);
    worldPoints = patternWorldPoints("checkerboard",boardSize,squareSize);
    camExtrinsics(i) = estimateExtrinsics(imagePoints,worldPoints,intrinsics);
end

Compute Robot End-Effector to Base Transformations

In this step, you use the robot poses collected earlier to compute the transformations from the robot's end-effector joint to its base for each pose. Load the pose data into MATLAB.

jointConfiguration = load("calibrationData/jntconfig.mat");
jointPositionsDeg = jointConfiguration.jointPositions;

Use loadrobot to load a rigidBodyTree model of the Kinova Gen3 robot. A list of supported robots can be found here.

robotModel = loadrobot("kinovaGen3");
robotModel.DataFormat = "column";

Compute the 4-by-4 transformation from the robot's end-effector joint to its base for each calibration pose. The distances in these transformations are measured in meters.

endEffectorToBaseTform(numPoses,1) = rigidtform3d; 
for i = 1:numPoses   
    jointPositionsRad = deg2rad(jointPositionsDeg(i,:))'; % Convert the pose angles from degrees to radians.
    endEffectorToBaseTform(i) = getTransform(robotModel,jointPositionsRad,"EndEffector_Link");
end

Estimate the Camera to End-Effector Transformation

Use the helperEstimateHandEyeTransform function to estimate the camera to end-effector transform for the "eye-in-hand" configuration. To use the "eye-to-hand" configuration, see the Estimate Pose of Fixed Camera Relative to Robot Base (Robotics System Toolbox) example.

config = "eye-in-hand";
cameraToEndEffectorTform = helperEstimateHandEyeTransform(camExtrinsics,endEffectorToBaseTform,config)

cameraToEndEffectorTform = 
  rigidtform3d with properties:

    Dimensionality: 3
       Translation: [-0.0079 0.0475 0.0075]
                 R: [3×3 double]

                 A: [-0.9953    0.0949   -0.0168   -0.0079
                     -0.0951   -0.9954    0.0141    0.0475
                     -0.0154    0.0156    0.9998    0.0075
                           0         0         0    1.0000]

The helper function returns a rigidtform3d object that contains the transformation from the camera to the end-effector joint, which can be used to compute the camera to robot base transformation.

Verify Calibration By Performing Object Pick-up

Verify the calibrated system by instructing the robot to retrieve an object detected within the camera's field of view. This example demonstrates the verification process using an AprilTag attached to a cube. AprilTags are fiducial markers that facilitate accurate camera extrinsic estimation, thus enabling the robot to approach and pick up the cube.

Screenshot 2024-03-05 160532.png

Begin by positioning the robot arm so that the AprilTag is in the camera's field of view. Then, capture an image and record the robot's current pose, including its joint angles.

% Load the test pose angles.
load("testData/poses.mat");
testPose = pose_angles';

% Load and undistort the test image.
testImage = imread("testData/im1.png");
undistortedTestImage = undistortImage(testImage,ld.cameraParams);

% Compute the end-effector joint to base transformation for the test pose.
testPoseRad = deg2rad(testPose);
endEffectorToBaseTformTest = getTransform(robotModel,testPoseRad,"EndEffector_Link");

Use the readAprilTag function to compute the AprilTag to camera transformation.

% Specify the tag family and tag size of the AprilTag.
tagFamily = 'tag36h11';
tagSize = .049; % AprilTag size in meters

% Detect AprilTag in test image.
[~,~,aprilTagToCameraTform] = readAprilTag(undistortedTestImage,tagFamily,intrinsics,tagSize);

Now, multiply the transforms to determine the transformation of the AprilTag in relation to the robot's base.

% Find the transformation from the robot base to the camera.
cameraToBaseTestTform = rigidtform3d(endEffectorToBaseTformTest * cameraToEndEffectorTform.A);

% Find the transformation from the robot base to the April Tag.
tagToBaseTestTform = cameraToBaseTestTform.A * aprilTagToCameraTform.A;
cubePosition = tagToBaseTestTform(1:3,4)

cubePosition = 3×1

    0.6936
   -0.0042
    0.1760

The AprilTag's position is 0.69 meters in front of, 0.004 meters to the left of, and 0.176 meters above the base of the robot.

To verify the accuracy of your calculation, create a plot that the displays the positions of the robot, camera, and cube. The visual representation can then be compared with the initial test setup.

% Show the 3D Robot model, with the base at the origin.
show(robotModel,testPoseRad);
hold on

% Show the estimated positions and orientations of the camera and cube.
plotCamera(AbsolutePose = cameraToBaseTestTform, Opacity=0, size=0.02)
scatter3(cubePosition(1), cubePosition(2), cubePosition(3), 300, 'square', 'filled')
cubeRotationQuaternion = rotm2quat(tagToBaseTestTform(1:3,1:3));
plotTransforms(cubePosition', cubeRotationQuaternion)
title("Robot Arm and Estimated Position of AprilTag Cube")
xlim([-.3,.9])
ylim([-.5, .5])
zlim([-.2,.9])

Given that the plot markers for both the camera and the cube are approximately in the right position and orientation, pass the coordinates of the cube to the robot and command it to move the end-effector joint to the correct position. The Robotics System Toolbox and ROS Toolbox provide functions to calculate the motor inputs needed to move the end-effector joint into position and send those controls to the robot. The inverseKinematics function from the Robotics System Toolbox provides a solver to find a set of joint angles to accomplish a end-effector joint position and rotation relative to the robot base.

In the following video, the robot calculates the position of the cube using hand-eye calibration and moves its gripper to the cube's coordinates to pick it up.

Conclusion

In this example, robot hand-eye calibration facilitated an accurate pick-and-place operation, allowing the robot arm to locate an object in the camera's field of view and translate that location to the robot's coordinate system. Hand-eye calibration is useful for integrating a camera into a robot arm system whenever the precise location of the camera is unknown or difficult to measure.

Supporting Functions

The helperEstimateHandEyeTransform function follows the algorithm by Tsai and Lenz [1] to estimate the transformation from the end-effector joint to the camera.

function cameraToEndEffectorTform = helperEstimateHandEyeTransform(boardToCameraTform, endEffectorToBaseTform, configuration)
    arguments
        boardToCameraTform (:,1) rigidtform3d
        endEffectorToBaseTform (:,1) rigidtform3d
        configuration {mustBeMember(configuration, ["eye-in-hand","eye-to-hand"])}
    end
    numPoses = size(boardToCameraTform,1);

    % In the eye-to-hand case, the camera is mounted in the environment and
    % the calibration board is mounted to the robot end-effector joint.
    if configuration == "eye-to-hand"
        for i = 1:numPoses
            currAInv = boardToCameraTform(i).invert().A;
            endEffectorToBaseTform(i) = rigidtform3d(currAInv);
        end
    end
    
    % Reorder the poses to have greater angles between each pair.
    orderOfPoses = helperOptimalPoseOrder(boardToCameraTform);
    boardToCameraTform(:,:,:) = boardToCameraTform(:,:,orderOfPoses);
    endEffectorToBaseTform(:,:,:) = endEffectorToBaseTform(:,:,orderOfPoses);

    PEndEffectorIToJ=repmat(zeros(3,1), 1, 1, numPoses-1);
    PCameraIToJ=PEndEffectorIToJ;

    % Iterate through pairs of poses to determine transformation angle.
    for i = 1:numPoses-1
        j=i+1;

        % Collect the 4 transforms of interest.
        TCameraI = boardToCameraTform(i).A;
        TCameraJ = boardToCameraTform(j).A; 
        TEndEffectorI = endEffectorToBaseTform(i).A;
        TEndEffectorJ = endEffectorToBaseTform(j).A;

        % Get transforms grom end-effector joint I to end-effector joint J, and for camera I and
        % camera J.
        TEndEffectorIJ = TEndEffectorJ\TEndEffectorI;
        TCameraIJ = (TCameraI'\TCameraJ')';

        % Get the axes and angles of the for both transforms.
        axangCIJ = tform2axang(TCameraIJ);
        axangGIJ = tform2axang(TEndEffectorIJ);
        thetaCIJ = axangCIJ(4);
        thetaGIJ = axangGIJ(4);
        PCameraIToJax = axangCIJ(1:3);
        PEndEffectorIToJax = axangGIJ(1:3);

        % P vectors contain the axis of rotation and are scaled to represent
        % the amount of rotation using Rodrigues' rotation formula.
        PCameraIToJ(:,:,i) = 2*sin(thetaCIJ/2)*PCameraIToJax';
        PEndEffectorIToJ(:,:,i) = 2*sin(thetaGIJ/2)*PEndEffectorIToJax';
    end

    % Set up least squares problem for rotation estimation.
    A=zeros((numPoses-1)*3,3);
    b=zeros((numPoses-1)*3,1);
    for i = 1:numPoses-1
        A((i-1)*3+1:i*3,:) = helperSkewMatrix(PEndEffectorIToJ(:,:,i) + PCameraIToJ(:,:,i));
        b((i-1)*3+1:i*3,1) = PCameraIToJ(:,:,i) - PEndEffectorIToJ(:,:,i);
    end
    [PEndEffectorToCameraUnscaled,~] = lsqr(A,b);
    PEndEffectorToCameraScaled = (2 * PEndEffectorToCameraUnscaled) / sqrt(1 + norm(PEndEffectorToCameraUnscaled)^2);

    % Find rotation given in Tsai, Lenz Equation 10.
    PSkew = helperSkewMatrix(PEndEffectorToCameraScaled);
    REndEffectorToCamera = (1 - (0.5 * norm(PEndEffectorToCameraScaled)^2)) * eye(3) + 0.5 * (PEndEffectorToCameraScaled * ...
        PEndEffectorToCameraScaled' + sqrt(4 - norm(PEndEffectorToCameraScaled)^2) * PSkew);
    
    % Clear A and b.
    A(:,:) = 0;
    b(:,:) = 0;

    % Iterate through numPoses-1 pairs of poses to find the translation part
    % of the transformation.
    for i = 1:numPoses-1
        j = i+1;

        % Use known transforms to compute transforms between poses.
        TCameraI = boardToCameraTform(i).A;
        TCameraJ = boardToCameraTform(j).A;
        TEndEffectorI = endEffectorToBaseTform(i).A;
        TEndEffectorJ = endEffectorToBaseTform(j).A;
        TEndEffectorIJ = TEndEffectorJ \ TEndEffectorI;
        TCameraIJ = (TCameraI' \ TCameraJ')';

        % Set up least squares to estimate translation.
        A((i-1)*3+1:i*3,:) = TEndEffectorIJ(1:3,1:3)-eye(3);
        b((i-1)*3+1:i*3,1) = REndEffectorToCamera*TCameraIJ(1:3,4)-TEndEffectorIJ(1:3,4);
    end

    % Compute translation using least squares.
    [TranslationEndEffectorToCamera,~] = lsqr(A,b);
    TEndEffectorToCamera = trvec2tform(TranslationEndEffectorToCamera')*rotm2tform(REndEffectorToCamera);
    cameraToEndEffectorTform = rigidtform3d(TEndEffectorToCamera);
end

The helperSkewMatrix function creates a 3x3 skew symmetric matrix from a 3d vector, which is used in helperEstimateHandEyeTransform.

function skew = helperSkewMatrix(v)
    skew = [0,-v(3), v(2) ;
            v(3), 0 , -v(1);
           -v(2), v(1), 0];   
end

The helperOptimalPoseOrder function gives a greedy-optimal ordering of the robot arm poses such that every consecutive pair of poses has a maximum angle difference between the camera positions, without reusing poses. Increasing the angles between each pair of poses has been shown to increase the accuracy of Tsai and Lenz's algorithm [1].

function orderOfPoses = helperOptimalPoseOrder(TCameraToBoard)
    % Create necessary vectors.
    numPoses = size(TCameraToBoard,3);
    anglesBetween = ones(numPoses-1,1);
    orderOfPoses = 1:numPoses;
    
    % Iterate over the indices to choose each subsequent pose.
    for i = 1:numPoses-2
        TCameraI = TCameraToBoard(:,:,i);

        % Collect the angles between pose i and the remaining poses.
        for j = i+1:numPoses
            TCameraJ = TCameraToBoard(:,:,j);
            TCameraIJ = TCameraI \ TCameraJ;
            axangcij = tform2axang(TCameraIJ);
            anglesBetween(j) = axangcij(4);
        end

        % Select the pose with the maximum angle to appear next in the
        % ordering.
        [~, idMax] = max(anglesBetween(i:end));
        tmp = orderOfPoses(i+1);
        orderOfPoses(i+1) = orderOfPoses(idMax+i-1);
        orderOfPoses(idMax+i-1) = tmp;
    end
end

References

[1] Tsai, R.Y. and Lenz, R.K., 1989. A new technique for fully autonomous and efficient 3d robotics hand/eye calibration. IEEE Transactions on robotics and automation, 5(3), pp.345-358.