Perform a hand-eye-calibration.

The operator ::hand_eye_calibration can be used to determine the 3D pose of the origin of the camera coordinate system with respect to the origin of the manipulator coordinate system (tool center point). This is typically used for robots equipped with a camera, where the pose of the camera (the ``eye'') relative to the manipulator (the ``hand'') has to be determined, in order to enable a video-based gripping of objects.

The assumption for the following desciption is, that the camera is fixed to the end-effector of the manipulator (hand-in-eye-configuration). In this case the calibration determines the (constant) pose of the camera in respect to the tool coordination system of the end-effector. In the other case (the camera is stationary and looks to the moving manipulator) the (constant) pose of the base coordination system of the manipulator in respect to the camera is computed. This can be performed by using ::hand_eye_calibration, too.

::hand_eye_calibration works in a similar manner to the conventional calibration of the exterior camera parameters (camera pose); see ::camera_calibration. However, ::hand_eye_calibration not only determines the camera pose as a simple transformation, but as a chain of transformations from the world coordinate system to the base coordinate system of the active system (robot or vehicle) to the manipulator coordinate system, and finally to the camera coordinate system. Thus, the transformation of a point P_W in world coordinates to the camera coordinate system can be described by:

               T
  P_C = (x,y,z)  = R*P_W + T

      = R_c ( R_m ( R_v*P_W + T_v ) + T_m ) + T_c

  where

  R_v, T_v: Rotation and translation from the world coordinate system 
            to the base coordinate system of the active system (vehicle)
  R_m, T_m: Rotation and translation from the base coordinate system
            to the manipulator coordinate system 
  R_c, T_c: Rotation and translation from the manipulator coordinate system
            to the camera coordinate system

The goal of the hand-eye-calibration is to determine (R_c, T_c). In order to avoid having to construct an exactly measured set-up, a least-squares error adjustment is used, which estimates (R_v, T_v) as well. Therefore, measurements referring to k different positions of the manipulator are used. Hence, it must be possible to determine the different positions of the manipulator with sufficient accuracy. For a robot arm, they can be determined from the angle positions of the individual joints of the arm. Analogously to the determination of the interior camera parameters (see ::camera_calibration), a large number of different manipulator positions is used to achieve more robust results.

A Newton-type algorithm is used to minimize an error function based on normal equations. Therefore, suitable starting values have to be passed for (R_c, T_c), and (R_v,T_v) by the user.

Starting values for (R_c, T_c) can be obtained by a coarse measurement of the set-up. Starting values for (R_v, T_v) can be obtained from the starting values for (R_c, T_c), the exterior camera parameters (R_a, T_a) (pose of the camera in the entire transformation chain; see ::camera_calibration), and the corresponding pose (R_m, T_m) of the manipulator. We have:

            
  P_C = R_a*P_W + T_a

      = R_c ( R_m ( R_v*P_W + T_v ) + T_m ) + T_c.

  where

  R_a, T_a: Rotation and translation from the world coordinate system 
            to the camera coordinate system (camera pose)

The following sections discuss individual questions arising from the use of ::hand_eye_calibration and are intended to be a guide line for using the operator in an application, as well as to aid the understanding of the operator.

How do I get 3D model points?

3D model points, given in the world coordinate system (NX, NY, NZ), and their associated projections to the camera coordinate system (NRow, NCol) form the basis of the hand-eye-calibration. In order ro be able to perform a successful hand-eye-calibration, 3D model points must be used, which were obtained from sufficiently many different positions of the manipulator.

Arbitrary known points in the world coordinate system along with their associated projections to the camera coordinate system can be used for the calibration. However, it is usually most convenient to use a standard calibration table, e.g., the one that can be generated with ::create_caltab. By using this calibration table, the calibration table and position of the calibration marks can be extracted by using ::find_caltab and ::find_marks_and_pose, respectively.

Please refer to ::camera_calibration for a description of the extraction of the calibration table and its associated 3D model points.

The parameter MPointsOfImage detemines 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 position of the manipulator (MRelPoses).

The 3D model points can be read from a calibration table description file for the standard calibration table with the operator ::caltab_points.

How do I acquire a suitable set of images?

If a planar standard calibration table is used, the following procedure should be used: Images of the calibration table are taken with the active system to be calibrated, i.e., the combination of the base of the manipulator (the vehicle, in case of mobile systems, which is used as a fixed point for the manipulator), the manipulator (robot arm, pan-tilt-head, etc.), and the camera system; see ::open_framegrabber and ::grab_image. For the image acquisition, the following points should be taken into account:

   - At least 10 to 20 images from different positions should be
     taken in which the position of the camera with respect to the
     calibration table is sufficiently different.  The position of
     the calibration table must not be changed between images.

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

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

   - The set of images must show the calibration table from very
     different positions of the manipulator.  The calibration table
     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 camera with respect to
     the calibration table) should take on many different values.
     Consider that the manipulator only has six degrees of freedom
     (three for rotation and three for translation), while twelve
     degrees of freedom have to be estimated for BaseFinalPose
     and CamFinalPose.  This means that the space of the
     degrees of freedom for the manipulator has to be used as
     exhaustively as possible.

   - In each image, the calibration table 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., neither focal length, nor aperture,
     nor focus must 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 table.  Therefore, bright
     lighting conditions for the calibration table are important
     because smaller apertures result in larger depth of focus.

How do I obtain suitable starting values?

The starting values for (R_c, T_c) and (R_v, T_v) are passed as 3D poses in the parameters CamStartPose and BaseStartPose, just like the exterior camera parameters.

CamStartPose can be obtained by measuring the pose of the camera in the manipulator coordinate system. This is the same procedure as the determination of starting values for the calibration of the exterior camera parameters without using the standard calibration table. Use ::create_pose to create a pose from your measurements. For the measurement, it is usually easier to determine the translation of the origin of the camera coordinate system within the manipulator coordinate system first, followed by the determination of the orientation.

The camera coordinate system is oriented such that the line of sight of the camera corresponds to the positive z-axis. The orientation of the camera coordinate system with respect to the world coordinate system can be described by three rotation angles. In order to determine the orientation, one can, for example, first ``rotate'' around the z-axis of the manipulator coordinate system, then around the new y-axis, and finally around the new x-axis created by the previous two rotations. In this case, the rotation angles around the x-, y-, and z-axis correspond directly to the rotation parameters of ::create_pose. Hence, according to this description of the pose, the following settings must be passed to ::create_pose: 'coordinate_system' for the sight of the transformation (a transformation of a coordinate system is described), 'R(p-T)' for the order of the transformations (first a translation, then a rotation), and 'gba' for the order of rotations (gamma, beta, alpha). This corresponds to representation type 10. For further information, please refer to the description of ::create_pose.

In order to determine BaseStartPose, a calibration of the exterior camera parameters (::camera_calibration) should be performed for one of the acquired images. From the pose of the camera coordinate system in the world coordinate system thus determined, and from the other poses in the transformation chain (starting values for (R_c, T_c) and exact values for the corresponding position of the manipulator (R_m, T_m)), the starting value can be determined very elegantly and easily by the use of homogeneous 3D transformation matrices. We have:

   P_C = R * R_W + T

       = H * P_W
and
             -1            -1 
   H_v = (H_m) * (H_c_start) * H_a
      
where

   H_v: Transformation from world coordinate system to
        base coordinate system (starting value)
   H_m: Transformation from base coordinate system to
        manipulator coordinate system
   H_c_start: Transformation from manipulator coordinate system
        to camera coordinate system (starting value, measured)
   H_a: Transformation from world coordinate system to camera
        coordinate system (determined from exterior calibration).

How do I obtain the poses of the manipulator?

The parameter MRelPoses determines the positions of the manipulator. If m positions were obtained by positioning the manipulator, these m positions must be passed linearized. The representation of these 3D poses is the same as for the exterior camera parameters and the starting parameters CamStartPose and BaseStartPose. 3D poses are tuples of length 7, see ::create_pose.

It is extremely important for the algorithm of the hand-eye-calibration that the poses MRelPoses can be obtained exactly from the manipulator. The transformation of the angles of a robot arm to the representation of a pose (rotation and translation) can be performed with ::create_pose. It is advisable to use the same procedure as for the determination of the starting value CamStartPose of the camera pose in the manipulator coordinate system. The manipulator coordinate system must be oriented such that it is identical or at least parallel to the base coordinate system in the initial position of the manipulator. In order to determine the poses of the individual manipulator positions, first the three translation parameters between the origin of the base coordinate system and the origin of the manipulator coordinate system should be determined. The translations should be determined along the axes of the base coordinate system when passing them to ::create_pose. For the orientation of the coordinate systems with respect to each other, one first ``rotates'' around the z-axis of the translated coordinate system, then around the new y-axis, and finally around the new x-axis created by the previous two rotations. The three rotation angles are used as parameters to ::create_pose. Hence, according to this description of the pose, the following settings must be passed to ::create_pose: 'coordinate_system' for the sight of the transformation, 'R(p-T)' for the order of the transformations, and 'gba' for the order of rotations. This corresponds to representation type 10. See also the description of ::create_pose.

How can I exclude individual pose parameters from the estimation?

::hand_eye_calibration estimates a maximum of 12 pose parameters. They are three rotation and three translation parameters each for the pose of the base coordinate system in the world coordinate system and for the pose of the camera coordinate system in the manipulator coordinate system. 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. They are:

For the pose of the base coordinate system in the world
coordinate system:
'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
For the pose of the camera coordinate system in the manipulator
coordinate system:
'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

It is useful to exclude individual parameters from the estimation if some of the pose parameters have been measured exactly. On the other hand, fixing some parameters reduces the degrees of freedom in the estimation, and thus increases the robustness to the estimated parameters, if the manipulator only has limited movement abilities, e.g., a pan-tilt head.

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. The parameter MPointsOfImage determines the number of model points used in the individual positions. If the standard calibration table is used, this means 49 points per position (image). If 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 3D model point tuples and the extracted 2D marks tuples, 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 corresponds to the number of calibration images. If, for example, 15 images from different positions were acquired, 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 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 output parameters are the pose of the base coordinate system in the world coordinate system (BaseFinalPose) and the pose of the camera coordinate system in the manipulator coordinate system (CamFinalPose).

The representation type of BaseFinalPose and CamFinalPose is the same as 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 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.

HTuple    CamParam, RCoord, CCoord;
HTuple    ManuPose, NumMarker, X, Y, Z, XPositions, YPositions;
HTuple    ZPositions, RCoordTmp, CCoordTmp, StartPose;
HTuple    ManuPoseTmp, BaseStartPose, CamStartPose, BaseFinalPose;
HTuple    CamFinalPose, NumErrors;
Hobject   Image, Caltab;
char      Datname[256];
char      CaltabName[256];

sprintf(CaltabName, "Caltab.descr");
::read_cam_par("CamParam.cal",&CamParam);
RCoord = HTuple();
CCoord = HTuple();
ManuPose = HTuple();
NumMarker = HTuple();
::caltab_points(CaltabName,&X,&Y,&Z);
XPositions = HTuple();
YPositions = HTuple();
ZPositions = HTuple();
// find landmarks in every image
for (long i=0; i<=22; i++)
{
  sprintf(Datname,"Image.%03ld.tiff",i);
  HImage Image(Datname);
  HRegion Caltab = Image.FindCaltab(CaltabName,3,150,5);
  RCoordTmp = Image.FindMarksAndPose(Caltab,CaltabName,CamParam,128,10,
                                     &CCoordTmp,&StartPose);
  RCoord = ((HTuple)RCoord).Concat(RCoordTmp);
  CCoord = ((HTuple)CCoord).Concat(CCoordTmp);
  sprintf(Datname,"Pose.%03ld.cal",i);
  ::read_pose(Datname,&ManuPoseTmp);
  ManuPose = ((HTuple)ManuPose).Concat(ManuPoseTmp);
  XPositions = ((HTuple)XPositions).Concat(X);
  YPositions = ((HTuple)YPositions).Concat(Y);
  ZPositions = ((HTuple)ZPositions).Concat(Z);
  NumMarker = ((HTuple)NumMarker).Concat(RCoordTmp.Num());
}
// read start poses
::read_pose("BaseStartPose.cal",&BaseStartPose);
::read_pose("CameraStartPose.cal",&CamStartPose);
// H A N D - E Y E - C A L I B R A T I O N 
::hand_eye_calibration(XPositions,YPositions,ZPositions,RCoord,CCoord,
                       NumMarker,ManuPose,BaseStartPose,CamStartPose,
                       CamParam,"all","CountIterations",20,0.000670,
                       &BaseFinalPose,&CamFinalPose,&NumErrors);
// save pose of difference between 'hand' and 'eye'
::write_pose(CamFinalPose,"CameraFinalPose.cal");

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

::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