Perform a hand-eye calibration.

The operator hand_eye_calibration determines the 3D pose of a robot (``hand'') relative to a camera (``eye''). With this information, the results of image processing can be transformed into the coordinate system of the robot, which can then, e.g., grasp an inspected part. There are two possible configurations of robot-camera (hand-eye) systems: The camera can be mounted on the robot or be stationary and observe the robot. Note that the term robot is used in place for a mechanism that moves objects. Thus, you can use hand_eye_calibration to calibrate many different systems, from pan-tilt heads to multi-axis manipulators.

A hand-eye calibration is performed similarly to the calibration of the external camera parameters (see camera_calibration): You acquire a set of images of a calibration object, determine correspondences between known model points and their projection in the images and pass them to hand_eye_calibration via the parameters NX, NY, NZ, NRow, NCol, and MPointsOfImage. If you use the standard calibration plate, the correspondences can be determined very easily with the operators find_caltab and find_marks_and_pose. Furthermore, you must specify the internal camera parameters in CamParam.

In contrast to the camera calibration, the calibration object is not moved manually. This task is delegated to the robot, which either moves the camera (mounted camera) or the calibration object (stationary camera). The robot's movements are assumed to be known and therefore are also used as an input for the calibration (parameter MRelPoses).

The two hand-eye configurations are discussed in more detail below, followed by general information about the process of hand-eye calibration.

Moving camera (mounted on a robot)

In this configuration, the calibration object remains stationary and the camera is moved to different positions by the robot. The main idea behind the hand-eye calibration is that the information extracted from a calibration image, i.e., the pose of the calibration object relative to the camera (i.e., the external camera parameters), can be seen as a chain of poses or homogeneous transformation matrices, from the calibration object via the base of the robot to its tool (end-effector) and finally to the camera:

  Moving camera:      H(cam<-cal) = H(cam<-tool) * H(tool<-base) * H(base<-cal)
                                         |               |               | 
                                         |               |               |
                                    CamStartPose     MRelPoses    BaseStartPose
                                    CamFinalPose                  BaseFinalPose

From the set of calibration images, the operator hand_eye_calibration determines the two transformations at the ends of the chain, i.e., the pose of the robot tool in camera coordinates (H(cam<-tool)) and the pose of the calibration object in the robot base coordinate system (H(base<-cal)). In the input parameters CamStartPose and BaseStartPose, you must specify suitable starting values for these transformations, which are constant over all calibration images. hand_eye_calibration then returns the calibrated values in CamFinalPose and BaseFinalPose.

In contrast, the transformation in the middle of the chain, H(tool<-base), is known but changes for each calibration image, because it describes the pose of the robot moving the camera, or to be more exact its inverse pose (pose of the base coordinate system in robot tool coordinates). You must specify the (inverse) robot poses in the calibration images in the parameter MRelPoses.

Internally, hand_eye_calibration uses a Newton-type algorithm to minimize an error function based on normal equations. Analogously to the calibration of the camera itself (see camera_calibration), the hand-eye calibration becomes more robust if you use many calibration images that were acquired with different robot poses.

Stationary camera

In this configuration, the robot grasps the calibration object and moves it in front of the camera. Again, the information extracted from a calibration image, i.e., the pose of the calibration object in camera coordinates (i.e., the external camera parameters), are equal to a chain of poses or homogeneous transformation matrices, this time from the calibration object via the robot's tool to its base and finally to the camera:

  Stationary camera:  H(cam<-cal) = H(cam<-base) * H(base<-tool) * H(tool<-cal)
                                         |               |               | 
                                         |               |               |
                                    CamStartPose     MRelPoses    BaseStartPose
                                    CamFinalPose                  BaseFinalPose

Analogously to the configuration with a moving camera, the operator hand_eye_calibration determines the two transformations at the ends of the chain, here the pose of the robot base coordinate system in camera coordinates (H(cam<-base)) and the pose of the calibration object relative to the robot tool (H(tool<-cal)). In the input parameters CamStartPose and BaseStartPose, you must specify suitable starting values for these transformations. hand_eye_calibration then returns the calibrated values in CamFinalPose and BaseFinalPose. Please note that the names of the parameters BaseStartPose and BaseFinalPose are misleading for this configuration!

The transformation in the middle of the chain, H(base<-tool), describes the pose of the robot moving the calibration object, i.e., the pose of the tool relative to the base coordinate system. You must specify the robot poses in the calibration images in the parameter MRelPoses.

Additional information about the calibration process

The following sections discuss individual questions arising from the use of hand_eye_calibration. They are intended to be a guideline for using the operator in an application, as well as to help understanding the operator.

How do I get 3D model points and their projections?

3D model points given in the world coordinate system (NX, NY, NZ) and their associated projections in the image (NRow, NCol) form the basis of the hand-eye calibration. In order to be able to perform a successful hand-eye calibration, you need images of the 3D model points that were obtained for sufficiently many different poses of the manipulator.

In principle, you can use arbitrary known points for the calibration. However, it is usually most convenient to use the standard calibration plate, e.g., the one that can be generated with create_caltab. In this case, you can use the operators find_caltab and find_marks_and_pose to extract the position of the calibration plate and of the calibration marks and the operator caltab_points to access the 3D coordinates of the calibration marks (see also the description of camera_calibration).

The parameter MPointsOfImage specifies the number of 3D model points used for each pose of the manipulator, i.e., for each image. With this, the 3D model points, which are stored in a linearized fashion in NX, NY, NZ, and their corresponding projections (NRow, NCol) can be associated with the corresponding pose of the manipulator (MRelPoses). Note that in contrast to the operator camera_calibration the 3D coordinates of the model points must be specified for each calibration image, not only once.

How do I acquire a suitable set of images?

If a standard calibration plate is used, the following procedure should be used:

   - At least 10 to 20 images from different positions should be
     taken, in which the position of the camera with respect to the
     calibration plate is sufficiently different.  The position of
     the calibration plate (moving camera: relative to the robot's base;
     stationary camera: relative to the robot's tool) must not be changed 
     between images.

   - In each image, the calibration plate must be completely visible
     (including its border).

   - No reflections or other disturbances should be visible on the
     calibration plate.

   - The set of images must show the calibration plate from very
     different positions of the robot.  The calibration plate
     can and should be visible in different parts of the images.
     Furthermore, it should be slightly to moderately rotated around
     its x- or y-axis, in order to clearly exhibit distortions of
     the calibration marks.  In other words, the corresponding
     exterior camera parameters (pose of the calibration plate in camera
     coordinates) should take on many different values. 

   - In each image, the calibration plate should fill at least one
     quarter of the entire image, in order to ensure the robust
     detection of the calibration marks.

   - The interior camera parameters of the camera to be used must
     have been determined earlier and must be passed in
     CamParam (see camera_calibration).  Note that
     changes of the image size, the focal length, the aperture, or
     the focus effect a change of the interior camera parameters.

   - The camera must not be modified between the acquisition of the
     individual images, i.e., focal length, aperture,
     and focus must not be changed, because all calibration images use
     the same interior camera parameters.  Please make sure that the
     focus is sufficient for the expected changes of the distance
     the camera from the calibration plate.  Therefore, bright
     lighting conditions for the calibration plate are important,
     because then you can use smaller apertures, which result in a larger 
     depth of focus. 

How do I obtain suitable starting values?

Depending on the used hand-eye configuration, you need starting values for the following poses:

  Moving camera: 
    BaseStartPose = pose of the calibration object in robot base coordinates
    CamStartPose  = pose of the robot tool in camera coordinates
                 

  Stationary camera: 
    BaseStartPose = pose of the calibration object in robot tool coordinates
    CamStartPose  = pose of the robot base in camera coordinates

The camera's coordinate system is oriented such that its optical axis corresponds to the z-axis, the x-axis points to the right, and the y-axis downwards. The coordinate system of the standard calibration plate is located in the middle of the surface of the calibration plate, its z-axis points into the calibration plate, its x-axis to the right, and it y-axis downwards.

For more information about creating a 3D pose please refer to the description of create_pose, which also contains a short example.

In fact, you need a starting value only for one of the two poses (BaseStartPose or CamStartPose). The other can be computed from one of the calibration images. This means that you can pick the pose that is easier to determine and let HALCON compute the other one for you.

The main idea is to exploit the fact that the two poses for which we need starting values are connected via the already described chain of transformations, here shown for a configuration with a moving camera:

  Moving camera:      H(cam<-cal) = H(cam<-tool) * H(tool<-base) * H(base<-cal)
                                         |               |               | 
                                         |               |               |
                                    CamStartPose     MRelPoses    BaseStartPose

In this configuration, it is typically easy to determine a starting value for H(cam<-tool) (CamStartPose). Thus, we solve the equation for H(base<-cal) (BaseStartPose):

                                                                    -1
  Moving camera:      H(base<-cal) = (H(cam<-tool)  * H(tool<-base))   *  H(cam<-cal)
                                   =  H(base<-tool) * H(tool<-cam)     *  H(cam<-cal)

Thus, to compute BaseStartPose you need one of the robot poses (e.g., the one in the first image), your estimate for CamStartPose, and the pose of the calibration object in camera coordinates in the selected image. If you use the standard calibration plate, you typically already obtained its pose when applying the operator find_marks_and_pose to determine the projections of the marks. An example program can be found below.

For a configuration with a stationary camera, the chain of transformations is:

  Stationary camera:  H(cam<-cal) = H(cam<-base) * H(base<-tool) * H(tool<-cal)
                                         |               |               | 
                                         |               |               |
                                    CamStartPose     MRelPoses    BaseStartPose

In this configuration, it is typically easier to determine a starting value for H(tool<-cal) (BaseStartPose). Thus, we solve the equation for H(cam<-base) (CamStartPose):

                                                                                  -1
  Stationary camera:  H(cam<-base) =  H(cam<-cal) * (H(base<-tool) * H(tool<-cal))
                                   =  H(cam<-cal) *  H(cal<-tool)  * H(tool<-base)

Thus, to compute CamStartPose you need one of the robot poses (e.g., the one in the first image), your estimate for BaseStartPose, and the pose of the calibration object in camera coordinates in the selected image. If you use the standard calibration plate, you typically already obtained its pose when applying the operator find_marks_and_pose to determine the projections of the marks. An example program can be found below.

How do I obtain the poses of the robot?

In the parameter MRelPoses you must pass the poses of the robot in the calibration images (moving camera: pose of the robot base in robot tool coordinates; stationary camera: pose of the robot tool in robot base coordinates) in a linearized fashion. We recommend to create the robot poses in a separate program and save in files using write_pose. In the calibration program you can then read and accumulate them in a tuple as shown in the example program below. Besides, we recommend to save the pose of the robot tool in robot base coordinates independent of the hand-eye configuration. When using a moving camera, you then invert the read poses before accumulating them. This is also shown in the example program.

Via the cartesian interface of the robot, you can typically obtain the pose of the tool in base coordinates in a notation that corresponds to the pose representations with the codes 0 or 2 (OrderOfRotation = 'gba' or 'abg', see create_pose). In this case, you can directly use the pose values obtained from the robot as input for create_pose.

If the cartesian interface of your robot describes the orientation in a different way, e.g., with the representation ZYZ (Rz(phi1) * Ry(phi2) * Rz(phi3)), you can create the corresponding homogeneous transformation matrix step by step using the operators hom_mat3d_rotate and hom_mat3d_translate and then convert the matrix into a pose using hom_mat3d_to_pose. The following example code creates a pose from the ZYZ representation described above:

  hom_mat3d_identity (HomMat3DIdent) 
  hom_mat3d_rotate (HomMat3DIdent, phi3, 'z', 0, 0, 0, HomMat3DRotZ) 
  hom_mat3d_rotate (HomMat3DRotZ, phi2, 'y', 0, 0, 0, HomMat3DRotZY) 
  hom_mat3d_rotate (HomMat3DRotZY, phi1, 'z', 0, 0, 0, HomMat3DRotZYZ) 
  hom_mat3d_translate (HomMat3DRotZYZ, Tx, Ty, Tz, base_H_tool) 
  hom_mat3d_to_pose (base_H_tool, RobPose) 

Please note that the hand-eye calibration only works if the robot poses MRelPoses are specified with high accuracy!

How can I exclude individual pose parameters from the estimation?

hand_eye_calibration estimates a maximum of 12 pose parameters, i.e., 6 parameters each for the two computed poses CamFinalPose and BaseFinalPose. However, it is possible to exclude some of these pose parameters from the estimation. This means that the starting values of the poses remain unchanged and are assumed constant for the estimation of all other pose parameters. The parameter ToEstimate is used to determine which pose parameters should be estimated. In ToEstimate, a list of keywords for the parameters to be estimated is passed. The possible values are:

BaseFinalPose:
  'baseTx' = translation along the x-axis
  'baseTy' = translation along the y-axis
  'baseTz' = translation along the z-axis
  'baseRa' = rotation around the x-axis
  'baseRb' = rotation around the y-axis
  'baseRg' = rotation around the z-axis

CamFinalPose:
  'camTx' = translation along the x-axis
  'camTy' = translation along the y-axis
  'camTz' = translation along the z-axis
  'camRa' = rotation around the x-axis
  'camRb' = rotation around the y-axis
  'camRg' = rotation around the z-axis

In order to estimate all 12 pose parameters, you can pass the keyword 'all' (or of course a tuple containing all 12 keywords listed above).

It is useful to exclude individual parameters from the estimation if some of the pose parameters have already been measured exactly.

Which terminating criteria can be used for the error minimization?

The error minimization terminates either after a fixed number of iterations or if the error falls below a given minimum error. The parameter StopCriterion is used to choose between these two alternatives. If 'CountIterations' is passed, the algorithm terminates after MaxIterations iterations.

If StopCriterion is passed as 'MinError', the algorithm runs until the error falls below the error threshold given in MinError. If, however, the number of iterations reaches the number given in MaxIterations, the algorithm terminates with an error message.

What is the order of the individual parameters?

The length of the tuple MPointsOfImage corresponds to the number of different positions of the manipulator and thus to the number of calibration images. The parameter MPointsOfImage determines the number of model points used in the individual positions. If the standard calibration plate is used, this means 49 points per position (image). If for example 15 images were acquired, MPointsOfImage is a tuple of length 15, where all elements of the tuple have the value 49.

The number of calibration images, which is determined by the length of MPointsOfImage, must also be taken into account for the tuples for the 3D model points and for the extracted 2D marks, respectively. Hence, for 15 calibration images with 49 model points each, the tuples NX, NY, NZ, NRow, and NCol must contain 15*49=735 values each. These tuples are ordered according to the image the respective points lie in, i.e., the first 49 values correspond to the 49 model points in the first image. The order of the 3D model points and the extracted 2D model points must be the same in each image.

The length of the tuple MRelPoses also depends on the number of calibration images. If, for example, 15 images and therefore 15 poses are used, the length of the tuple MRelPoses is 15*7=105 (15 times 7 pose parameters). The first seven parameters thus determine the pose of the manipulator in the first image, and so on.

What do the output parameters mean?

If StopCriterion was set to 'CountIterations', the output parameters BaseFinalPose and CamFinalPose are returned even if the algorithm didn't converge. If, however, StopCriterion was set to 'MinError', the error must fall below 'MinError' in order for output parameters to be returned.

The representation type of BaseFinalPose and CamFinalPose is the same as in the corresponding starting values. It can be changed with the operator convert_pose_type. The description of the different representation types and of their conversion can be found with the documentation of the operator create_pose.

The parameter NumErrors contains a list of (numerical) errors from the individual iterations of the algorithm. Based on the evolution of the errors, it can be decided whether the algorithm has converged for the given starting values. The error values are returned as 3D deviations in meters. Thus, the last entry of the error list corresponds to an estimate of the accuracy of the returned pose parameters.

The quality of the calibration depends on the accuracy of the input parameters (position of the calibration marks, robot poses MRelPoses, and the starting positions BaseStartPose, CamStartPose). Based on the returned error measures NumErrors, it can be decided, whether the algorithm has converged. Furthermore, the accuracy of the returned pose can be estimated. The error measures are 3D differences in meters.

read_cam_par('campar.dat', CamParam)
CalDescr := 'caltab.descr'
caltab_points(CalDescr, X, Y, Z)
* process all calibration images
for i := 0 to NumImages-1 by 1
  read_image(Image, 'calib_'+i$'02d')
  * find marks on the calibration plate in every image
  find_caltab(Image, CalPlate, CalDescr, 3, 150, 5)
  find_marks_and_pose(Image, CalPlate, CalDescr, CamParam, 128, 10, 
                      RCoordTmp, CCoordTmp, StartPose)
  * accumulate 2D and 3D coordinates of the marks
  RCoord := [RCoord, RCoordTmp]
  CCoord := [CCoord, CCoordTmp]
  XCoord := [XCoord, X]
  YCoord := [YCoord, Y]
  ZCoord := [ZCoord, Z]
  NumMarker := [NumMarker, |RCoordTmp|]
  * read pose of the robot tool in robot base coordinates
  read_pose('robpose_'+i$'02d'+'.dat', RobPose)
  * moving camera? invert pose
  if (IsMovingCameraConfig='true')
    pose_to_hom_mat3d(RobPose, base_H_tool)
    hom_mat3d_invert(base_H_tool, tool_H_base)
    hom_mat3d_to_pose(tool_H_base, RobPose)
  endif
  * accumulate robot poses
  MRelPoses := [MRelPoses, RobPose]
  * store the pose of the calibration plate in the first image and the
  * corresponding pose of the robot for later use
  if (i=0)
    cam_P_cal := StartPose
    RelPose0 := RobPose
  endif
endfor
* obtain starting values: read one, compute the other
if (IsMovingCameraConfig='true')
  * mov. camera:  read pose of robot tool in camera coordinates
  *               compute pose of calibration plate in robot base coordinates
  read_pose('cam_P_tool.dat', CamStartPose)
  * BaseStartPose = inverse(CamStartPose * RelPose0) * cam_P_cal
  pose_to_hom_mat3d(CamStartPose, cam_H_tool)
  pose_to_hom_mat3d(RelPose0, tool_H_base)
  pose_to_hom_mat3d(StartPose, cam_H_cal)
  hom_mat3d_compose(cam_H_tool, tool_H_base, cam_H_base)
  hom_mat3d_invert(cam_H_base, base_H_cam)
  hom_mat3d_compose(base_H_cam, cam_H_cal, base_H_cal)
  hom_mat3d_to_pose(base_H_cal, BaseStartPose)
else
  * stat. camera: read pose of calibration plate in robot tool coordinates
  *               compute pose of robot base in camera coordinates
  read_pose('tool_P_cal.dat', BaseStartPose)
  * CamStartPose = cam_P_cal * inverse(RelPose0 * BaseStartPose)
  pose_to_hom_mat3d(BaseStartPose, tool_H_cal)
  pose_to_hom_mat3d(RelPose0, base_H_tool)
  pose_to_hom_mat3d(StartPose, cam_H_cal)
  hom_mat3d_compose(base_H_tool, tool_H_cal, base_H_cal)
  hom_mat3d_invert(base_H_call, cal_H_base)
  hom_mat3d_compose(cam_H_cal, cal_H_base, cam_H_base)
  hom_mat3d_to_pose(cam_H_base, CamStartPose)
endif
*
* perform hand-eye calibration
*
hand_eye_calibration(XCoord, YCoord, ZCoord, RCoord, CCoord, NumMarker,
                     MRelPoses, BaseStartPose, CamStartPose, CamParam,
                     "all", "CountIterations", 20, 0.000670,
                     BaseFinalPose, CamFinalPose, NumErrors)
*
* measure some point P in camera coordinates (cam_px, cam_py, cam_pz)
*
* transform point into robot base coordinates: base_p = base_H_cam * cam_p
if (IsMovingCameraConfig='true')
  * mov. camera: base_H_cam = base_H_tool * tool_H_cam 
  *              base_P_cam =    RobPose  * inverse(CamFinalPose)
  pose_to_hom_mat3d(CamFinalPose, cam_H_tool)
  hom_mat3d_invert(cam_H_tool, tool_H_cam)
  * obtain current robot pose RobPose from robot
  pose_to_hom_mat3d(RobPose, base_H_tool)
  hom_mat3d_compose(base_H_tool, tool_H_cam, base_H_cam)
else
  * stat. camera: base_P_cam = inverse(CamFinalPose)
  pose_to_hom_mat3d(CamFinalPose, cam_H_base)
  hom_mat3d_invert(cam_H_base, base_H_cam)
endif
affine_trans_point_3d(base_H_cam, cam_px, cam_py, cam_pz, 
                      base_px, base_py, base_pz)

hand_eye_calibration returns 2 (H_MSG_TRUE) if all parameter values are correct and the method converges with an error less than the specified minimum error (if StopCriterion = 'MinError'). If necessary, an exception handling is raised.

hand_eye_calibration is reentrant and processed without parallelization.

find_marks_and_pose

write_pose, convert_pose_type, pose_to_hom_mat3d, disp_caltab, sim_caltab

find_caltab, find_marks_and_pose, disp_caltab, sim_caltab, write_cam_par, read_cam_par, create_pose, convert_pose_type, write_pose, read_pose, pose_to_hom_mat3d, hom_mat3d_to_pose, caltab_points, create_caltab

Camera calibration