Camera and Hand-Eye Calibration using OpenCV

Overview

This section is for the new camera and hand-eye calibration functions; for the C3D previous version, please refer to C3D camera calibration.

1. Camera and hand-eye calibration

To invoke OpenCV calibration, one must edit rh_calibration/src/calibration_input.xml according to the partner's configuration (inside the xml, comments have been included to help the configuration). The most important ones are:

  • BoardSize_Width -> Number of squares on the chessboard pattern (long side) (see pattern_opencv.png)
  • BoardSize_Height -> Number of squares on the chessboard pattern (short side) (see pattern_opencv.png)
  • Square_Size -> Size of one square in milimeters or meters (see pattern_opencv.png)
  • Max_Error -> Maximum error allowed on each individual target (in pixels)
  • Save_Mode -> if "1" saves images to the path specified in "Write_dirPathImages"; otherwise, nothing is saved.

The rest might not be modified as some are for Debug purposes and file paths. File paths, in this file, are defined relative to the node location, therefore they should not be changed. Make sure that Debug_Mode, NoImages_Camera and NoImages_HandEye are set to zero, otherwise the calibration node will keep capturing images continuously; these are used in debug mode only! There are two different ways to calibrate the robot head:

  1. Automatic mode - The dual-arm robot is used to pick up, change the pose of and return the calibration target.
  2. Manual mode - The user has to change the pose of the calibration target manually.

For both modes, there exists simulation packages that allows the user to re-use captured images if "Save_Mode" was used. See Camera and hand-eye calibration (Simulation) for more information.

1.1 Automatic camera and hand-eye calibration mode

1.1.1 Defining arm poses and calibrating cameras

Before running the automatic mode, please ensure that you have defined the arm positions according to your settings and the XACRO/URDF model of the robot head (located in rh_ptu/description) is updated according to your settings. For picking up, lifting and dropping calibration target states, one needs to defined them manually. This can be done with the following commands:

roslaunch clopema_launch start_robot.launch
rosrun RHsavetfpoints /base_link /r1_ee grab_target.py

The picking up state consists of moving the arm to a position where the calibration target can be easily grasped. In UG, the calibration target is place close to the border of the front-forward table as illustrated below

<image>

The lifting state consists of moving the arm such that the calibration target is lifted over the "z-axis" (robot coordinate frame) in order to avoid collisions with the table. The dropping state consists of reverting the arm poses of picking up state. These arm poses should be stored as in this file: pose_list.py.

Now, arm poses for camera and hand-eye calibration can now be defined. This can be done by issuing the following commands;

roslaunch clopema_launch start_robot.launch
roslaunch rh_calibration RHcalibration.launch
rosrun RHsavetfpoints /base_link /r1_ee arm_poses.py

At this stage, one needs to move the arm manually using the teach pendant. Make sure that the calibration target is on the field of view of the cameras and corners have been localised. For each arm pose selected, just follow the on-screen instructions in terminal where it was issued RHcalibration.launch and RHsavetfpoints. For RHcalibration.launch, it is as performing manual calibration. For RHsavetfpoints, one can save the pose by pressing 's' in the terminal.

After calibrating the cameras, press 'q' in RHsavetfpoints terminal. The generated file is saved at the rh_calibration package. One has to move that file to ../rh_robot_interface/src and rename it to <any_name>.py. This file will contain the arm poses as follows (see rh_calibration (hydro)):

<replace_for_variable>.append([x, y, z, qx, qy, qz, qw])

One needs to change the name of <replace_for_variable> to calib_target_. You also have to create a new entry in this file for the hand-eye calibration arm pose as illustrated in cal.py. The hand-eye calibration arm pose can be defined as the first arm pose for camera calibration.

1.1.2 Camera and hand-eye calibration using the dual-arm robot

For this, the cal.py file must be present in ../rh_robot_interface/src and defined in ./rh_robot_interface/src/pose_list.py at the top of this file as follow:

from cal.py import calib_target_ he_target_

Afterwards, run the following:

roslaunch clopema_launch start_robot.launch
roslaunch rh_calibration RHcalibration_service.launch (wait until this launch file has loaded completed, images will be displayed)
rosrun rh_robotInterface calibration_rh_complete.py

RHcalibration_service is an interactive node, so just follow the on-screen instructions. In all cases, keyword input must be on the left image window. RHcalibration_service will run camera calibration and hand-eye calibration.

After camera calibration, camera intrinsic and extrinsic parameters are saved on the default location, e.g.

and these will be loaded automatically each time the camera capturing control nodes are initialised. If PTU are updated with a new position, rh_cameras (hydro) nodes will update extrinsic camera parameters in order to maintain the system calibrated (It has not been fully validated this function and it has not been released)

1.2 Manual camera and hand-eye calibration mode

For camera calibration, the user must change the orientation, pose, and depth of the calibration target; the recommended number of images for each cameras that must be captured is 12. For hand-eye calibration, the calibration target must remain static and the user needs to select a point as indicated on the on-screen instructions. After modifying calibration_input.xml accordingly, the following can be typed (on different terminals):

roslaun rh_calibration RHcalibration.launch
rostopic pub -1 /RH/cmd/acquire rh_cameras/CamerasSync '{timeStamp: now, data: full}'

The last command starts image capturing and it must be run once. It is assumed that the target is in front of both cameras, it is static and both cameras are connected to different USB buses (see Robot Head Software Operation above); otherwise camera calibration will fail.

RHcalibration is an interactive node, so just follow the on-screen instructions. In all cases, keyword input must be on the left image window. RHcalibration will run camera calibration and hand-eye calibration.

After camera calibration, camera intrinsic and extrinsic parameters are saved on the default location, e.g.

  • rh_cameras/calibrations/calL.xml
  • rh_cameras/calibrations/calR.xml

and these will be loaded automatically each time the camera capturing control nodes are initialised. If PTU are updated with a new position, rh_cameras (hydro) nodes will update extrinsic camera parameters in order to maintain the system calibrated (It has not been fully validated this function and it has not been released)

image.jpeg (521 KB) Gerardo Aragon, 10/16/2013 17:17