标定学习笔记（六）-- Halcon手眼标定例程：Hand-eye-Calibration with a stationary cam

1  问题概述

        本例程演示了如何对一个 Eye-to-Hand 问题进行手眼标定，即相机与机器人基座是固定不动的，用于进行标定的标定板则固定于机械手末端的夹具上。

        在本例程中，手眼标定的目的是为了解出两个未知量：

        1）机器人基座在相机坐标系下的位姿：BaseInCamPose。

        2）标定物在夹具坐标系下的位姿：ToolInBasePose。

        理论上，需要至少三个机器人基座在相机坐标系下的位姿以及至少三个标定物在夹具坐标系下的位姿。但实际上则需要至少十个。

        标定物的位姿可以通过固定相机所拍摄的图像获得。标定物可以通过机械手进行相应的移动以校准对象便于相机进行拍摄。为了获得更好的标定结果，拍摄所得的标定板的图像最好存在一定的倾斜。

2  例程解读

2.1  初始化

        在这一部分例程中，主要进行以下内容的初始化：

        1）文件打开路径初始化，只写文件的通用部分，方便读取相应的文件；

        2）显示窗口初始化，通过输入图像的像素信息，打开一个合适的视口；

        3）标定模型初始化，通过标定模型的ID初始化一些参数，包括：相机内参，标定板信息，以及标定的方法。

dev_update_off ()
* Directories with calibration images and data files
ImageNameStart := '3d_machine_vision/hand_eye/stationarycam_calib3cm_'
DataNameStart := 'hand_eye/stationarycam_'
NumImages := 17
* Read image
read_image (Image, ImageNameStart + '00')
get_image_size (Image, Width, Height)
* Open window
dev_close_window ()
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)
dev_set_line_width (2)
dev_set_draw ('margin')
dev_display (Image)
* Set font
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
* Load the calibration plate description file.
* Make sure that the file is in the current directory,
* the HALCONROOT/calib directory, or use an absolut path
CalTabFile := 'caltab_30mm.descr'
* Read the initial values for the internal camera parameters
read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam)
* Create the calibration model for the hand eye calibration
create_calib_data ('hand_eye_stationary_cam', 1, 1, CalibDataID)
set_calib_data_cam_param (CalibDataID, 0, [], StartCamParam)
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)
set_calib_data (CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')
disp_message (WindowHandle, 'The calibration data model was created', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

2.2  获取ToolInBasePose

        这部分的例程主要完成以下工作：

        1）遍历所有输入的图片，根据标定模型寻找图片中的标定板，建立标定板坐标系；

        2）从已有文件中读取每张图片所对应的夹具在基座坐标系下的位姿ToolInBasePose，根据相应的索引初始化标定模型。

        到这一步为止，解决  问题所需的转换矩阵  都已经得出。

* Start the loop over the calibration images
for I := 0 to NumImages - 1 by 1
    read_image (Image, ImageNameStart + I$'02d')
    * Search for the calibration plate, extract the marks and the
    * pose of it, and store the results in the calibration data model of the
    * hand-eye calibration
    find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
    get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
    get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, CalObjInCamPose)
    * Visualize the extracted calibration marks and the estimated pose (coordinate system)
    dev_set_color ('green')
    dev_display (Image)
    dev_display (Caltab)
    dev_set_color ('yellow')
    disp_cross (WindowHandle, RCoord, CCoord, 6, 0)
    dev_set_colored (3)
    disp_3d_coord_system (WindowHandle, StartCamParam, CalObjInCamPose, 0.01)
    * Read pose of tool in robot base coordinates (ToolInBasePose)
    read_pose (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
    * Set the pose tool in robot base coordinates in the calibration data model
    set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
    * Uncomment to inspect visualization
*     disp_message (WindowHandle, 'Extracting data from calibration image ' + (I + 1) + ' of ' + NumImages, 'window', 12, 12, 'black', 'true')
*     disp_continue_message (WindowHandle, 'black', 'true')
*     stop ()
endfor
disp_message (WindowHandle, 'All relevant data has been set in the calibration data model', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

2.3  标定并保存参数

        这部分例程主要完成以下任务：

        1）利用标定模型进行手眼标定，计算相应的误差；

        2）获取并保存以下参数的数据：相机标定误差CamCalibError、相机参数CamParam、机器人基座在相机坐标系下的位姿BaseInCamPose、标定板在夹具坐标系下的位姿ObjInToolPose；

        3）完成标定，在视口中显示标定信息。

* Check the input poses for consistency
check_hand_eye_calibration_input_poses (CalibDataID, 0.05, 0.005, Warnings)
if (|Warnings| != 0)
    * There were problem detected in the input poses. Inspect Warnings and
    * remove erroneous poses with remove_calib_data and remove_calib_data_observ.
    dev_inspect_ctrl (Warnings)
    stop ()
endif
* Perform hand-eye calibration
* Internally before performing the hand-eye calibration the cameras are calibrated
* and the calibrated poses of the calibration object in the camera are used.
dev_display (Image)
disp_message (WindowHandle, 'Performing the hand-eye calibration', 'window', 12, 12, 'black', 'true')
calibrate_hand_eye (CalibDataID, Errors)
* Query the error of the camera calibration
get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError)
* Query the camera parameters and the poses
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
* Get poses computed by the hand eye calibration
get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPose)
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', ObjInToolPose)
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
try
    * Store the camera parameters to file
    write_cam_par (CamParam, DataNameStart + 'final_campar.dat')
    * Save the hand eye calibration results to file
    write_pose (BaseInCamPose, DataNameStart + 'final_pose_cam_base.dat')
    write_pose (ObjInToolPose, DataNameStart + 'final_pose_tool_calplate.dat')
catch (Exception)
    * Do nothing
endtry
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
* Display calibration errors of the hand-eye calibration
disp_results (WindowHandle, CamCalibError, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

2.4  计算相应参数指标

        本部分例程完成的任务：

        1）根据标定结果，遍历每张图片，计算标定物在相机中的位姿：CalObjInCamPose；

        2）保存相应的转换关系：CamInBasePose、ObjInBasePose。

* For the given camera, get the corresponding pose indices and calibration object indices
query_calib_data_observ_indices (CalibDataID, 'camera', 0, CalibObjIdx, PoseIds)
* Compute the pose of the calibration object in the camera coordinate
* system via calibrated poses and the ToolInBasePose and visualize it.
for I := 0 to NumImages - 1 by 1
    read_image (Image, ImageNameStart + I$'02d')
    * Obtain the pose of the tool in robot base coordinates used in the calibration.
    * The index corresponds to the index of the pose of the observation object.
    get_calib_data (CalibDataID, 'tool', PoseIds[I], 'tool_in_base_pose', ToolInBasePose)
    dev_display (Image)
    * Compute the pose of the calibration plate with respect to the camera
    * and visualize it
    calc_calplate_pose_stationarycam (ObjInToolPose, BaseInCamPose, ToolInBasePose, CalObjInCamPose)
    dev_set_colored (3)
    disp_3d_coord_system (WindowHandle, CamParam, CalObjInCamPose, 0.01)
    Message := 'Using the calibration results to display the'
    Message[1] := 'coordinate system in image ' + (I + 1) + ' of ' + NumImages
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    if (I < NumImages - 1)
        disp_continue_message (WindowHandle, 'black', 'true')
        stop ()
    endif
endfor
* Clear the data model
clear_calib_data (CalibDataID)
* 
* After the hand-eye calibration, the computed pose
* BaseInCamPose can be used in robotic grasping applications.
* To grasp an object with the robot, typically, its pose
* with respect to the camera is determined (which
* is simulated here by setting the object's pose to the
* pose of the calibration object)
ObjInCamPose := CalObjInCamPose
* If the tool coordinate system is placed at the gripper
* and an object detected at ObjInCamPose shall be grasped,
* the pose of the detected object relative
* to the robot base coordinate system has to be computed.
pose_invert (BaseInCamPose, CamInBasePose)
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)

3  总结思路

        针对 Eye-to-Hand 问题，我们不如进行以下分析：

        1）在 Eye-to-Hand 问题中，我们的已知量是：

        （1）夹具在机器人基座坐标系下的位姿：ToolInBasePose；

        （2）相机的内参（可以通过张正友标定法获取）：StartCamParam。

        已知量保存在相应的文件中，方便读取。需要注意的是：ToolInBasePose是一个跟随机械手变化而变化的量，每张输入的标定图像都应当有一个与之对应的文件。

        2）在 Eye-to-Hand 问题中，我们的未知量是：

        （1）标定板在夹具坐标系下的位姿：ObjInToolPose；

        （2）相机在机器人基座坐标系下的位姿：CamInBasePose；

        以上的两个待求参数虽然是未知的，但这两个未知量在 Eye-to-Hand 问题中是固定的，可以通过手眼标定得出。

        （3）标定板在基座坐标系下的位姿：ObjInBasePose；

        （4）标定板在相机坐标系下的位姿：ObjInCamPose；

        这两个待求参数可以通过转换关系推导计算得出。需要注意的是由于机械手的位姿是不断变化的，需要通过遍历图像计算出ObjInBasePose。