* 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')