Halcon 标定例程：calibrate_hand_eye_scara_stationary_cam

* This example explains how to perform the hand-eye calibration for
* a SCARA robot. In this case, the camera is stationary with respect
* to the robot and observes the workspace of the robot. The
* calibration plate is attached to the robot tool.
* Finally, the calibration result as well as data that is required
* to determine the position of the objects to be grasped is written
* to file. This information is then used by the example program
* pick_and_place_scara_stationary_cam.hdev.
* 此例程讲解SCARA机器人的手眼标定方法。在此例程中，相机相对于机器人静止，用于观察机器人的工作范围。标定板位于机器人工具上。
* 最后，标定结果用于计算被抓取对象的位置，并写入文件，供下个例程使用。
*
* Provide the description file of the calibration plate and the
* camera parameters of the previously calibrated camera
* 提供标定板的描述文件和标定相机的参数（焦距，畸变，单个像素的大小，图像的中心位置，图像的分辨率）
CalibObjDescr := 'calibrate_hand_eye_scara_setup_01_calplate.cpd'
CalibrationPlateThickness := 0.003
gen_cam_par_area_scan_division (0.0165251, -642.277, 4.65521e-006, 4.65e-006, 595.817, 521.75, 1280, 1024, CameraParam)
*
* Set the thickness of the objects to be grasped to be able to
* define the pose of the measurement plane correctly
* 设置被抓取对象的厚度，准确计算平面的位置
ObjectThickness := 0.001
*
* 窗口定义
dev_close_window ()
dev_open_window_fit_size (0, 0, 1280, 1024, 640, -1, WindowHandle)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
dev_update_off ()
*
* 1. Acquire calibration images and corresponding robot poses
* 1. 获取标定图像和对应的机器人位置
*
* Create a new calibration model 新建标定模型
create_calib_data ('hand_eye_scara_stationary_cam', 1, 1, CalibDataID)
* Set the camera parameters in the calibration model 设置标定模型中的相机参数
set_calib_data_cam_param (CalibDataID, 0, [], CameraParam)
* Set the calibration plate in the calibration model 设置标定模型中的标定板信息
set_calib_data_calib_object (CalibDataID, 0, CalibObjDescr)
for Index := 1 to 10 by 1
    * Read the calibration image 读取标定图像
    read_image (CalibImage, '3d_machine_vision/hand_eye/scara_stationary_cam_setup_01_calib_' + Index$'02')
    * Read the corresponding robot pose (pose of the tool in the
    * robot base coordinate system) 读取对应的位置信息
    read_pose ('scara_stationary_cam_setup_01_tool_in_base_pose_' + Index$'02' + '.dat', ToolInBasePose)
    * Set the robot pose in the calibration model 设置标定模型中的机器人位置信息
    set_calib_data (CalibDataID, 'tool', Index, 'tool_in_base_pose', ToolInBasePose)
    * Determine the pose of the calibration plate in the camera
    * coordinate system and set the pose in the calibration model
    * 计算相机坐标系统下标定板的位置，并在标定模型中设置
    find_calib_object (CalibImage, CalibDataID, 0, 0, Index, [], [])
    * Visualize 显示
    dev_display (CalibImage)
    * 获取标定模型中标定板的位置信息，并显示在窗口上
    get_calib_data_observ_pose (CalibDataID, 0, 0, Index, ObjInCameraPose)
    disp_caltab (WindowHandle, CalibObjDescr, CameraParam, ObjInCameraPose, 1)
    disp_message (WindowHandle, 'Calibration image ' + Index + ' of 10', 'window', 12, 12, 'black', 'true')
    wait_seconds (0.2)
endfor
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*
* 2. Check the input poses for consistency
* 2. 检查输入的标定板信息是否有误
*
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
*
* 3. Perform the hand-eye calibration
* 3. 进行手眼标定
*
calibrate_hand_eye (CalibDataID, Errors)
* Get the result of the calibration, i.e., the pose of
* the robot base in the camera coordinate system
* 获取标定结果：在相机坐标系下机器人的位置
get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPosePre)
* Visualize 将机器人的位置信息显示在窗口上
disp_preliminary_result (WindowHandle, BaseInCamPosePre, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*
* 3. Fix the pose ambiguity 修正标定的位置信息
*
* When calibrating a SCARA robot, it is impossible to determine
* all pose parameters unambiguously. In case of a stationary
* camera, the Z translation of ObjInToolPose cannot be determined.
* 标定SCARA机器人，不可能精确标定所有的位置。在相机静止时，Z轴的移动信息无法确定。
* Therefore, it is necessary to fix the unknown translation in
* Z by moving the robot to a pose of known height in the camera
* coordinate system.
* 因此，必须修正Z轴的移动信息。可以通过将机器人移动到相机坐标系下的一个已知高度来获取。
* For this, the calibration plate is (detached from
* the robot and) placed at an arbitrary position such that it can be
* observed by the camera. The pose of the calibration plate must
* then be determined in the camera coordinate system
* (-> ObjInCamPoseRef). Afterwards, the tool of the robot is manually
* moved to the origin of the calibration plate (-> ToolInBasePoseRef).
* 将标定板从机器人工具上取下，放在相机能拍照的范围之内的一个固定位置。用之前的标定结果计算得到相机坐标系下标定板的位置
* 将机器人的手动移动到标定板的原点，计算相机坐标系下的位置信息
* The two poses can be used to fix the Z ambiguity by using the
* procedure fix_scara_ambiguity_stationary_cam.
* 通过这两个位置休息修正Z轴的位移信息
read_image (ImageRef, '3d_machine_vision/hand_eye/scara_stationary_cam_setup_01_calib_ref')
get_calib_plate_pose (ImageRef, CameraParam, CalibObjDescr, ObjInCamPoseRef)
read_pose ('scara_stationary_cam_setup_01_tool_in_base_pose_ref.dat', ToolInBasePoseRef)
fix_scara_ambiguity_stationary_cam (BaseInCamPosePre, ToolInBasePoseRef, ObjInCamPoseRef, ZCorrection)
set_origin_pose (BaseInCamPosePre, 0, 0, ZCorrection, BaseInCamPose)
*
* Visualize
* 将最终的结果显示在窗口中
disp_final_results (WindowHandle, BaseInCamPosePre, BaseInCamPose)
disp_end_of_program_message (WindowHandle, 'black', 'true')
*
* After the hand-eye calibration is performed, the resulting pose
* BaseInCamPose can be used in robotic grasping applications:
* 标定结果用于机器人的抓取
* Let us assume that the camera acquires an image of the object that
* should be grasped. From the image, the pose of the object in the
* camera coordinate system must be determined (-> ObjInCamPose) by
* using image processing.
* 假设相机获取了将要被抓取对象的图像。从图像中，通过图像处理可以获取到对象的位置。
* Based on this pose and the result of the calibration
* (BaseInCamPose), the robot pose can be computed that is necessary
* to grasp the object (-> ObjInBasePose):
* 基于图像中对象的位置和标定的结果，抓取对象的位置信息可以计算得到
pose_invert (BaseInCamPose, CamInBasePose)
create_pose (0.0035, -0.0128, 0.7981, 1.345, 356.158, 180.194, 'Rp+T', 'gba', 'point', ObjInCamPose)
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)
*
* 4. Prepare additional data required for the pose estimation of the
*    objects to be grasped 预估抓取对象的位置需要准备的数据
*
* Determine the pose of the measurement plane 设置计算平面的位置/初始位置
set_origin_pose (ObjInCamPoseRef, 0, 0, CalibrationPlateThickness - ObjectThickness, MPInCamPose)
*
* 5. Write the results for using them in a pick-and-place application 将结果写入文档
* - Results of hand-eye calibration
write_pose (CamInBasePose, 'cam_in_base_pose.dat')
* - Data required for pose estimation of the objects to be grasped
*   from the original image
write_cam_par (CameraParam, 'camera_parameters.dat')
write_pose (MPInCamPose, 'measurement_plane_in_cam_pose.dat')