Halcon SCARA关节机器人手眼标定（眼在手上）例程注解——calibrate_hand_eye_scara_moving_cam.hdev

*****转载请注明出处，版权所有@BetterTL*******

*****calibrate_hand_eye_scara_moving_cam.hdev *******

*****scara关节机器人眼在手上的手眼标定例程*******


* This example explains how to perform the hand-eye calibration for
* a SCARA robot. In this case, the camera is attached to the robot tool
* and the calibration object is stationary with respect to the robot.
 *本例说明如何执行SCARA机器人的手眼校准。 在这种情况下，摄像机连接到机器人工具上，并且校准对象相对于机器人是静止的。
*


* Provide the description file of the calibration plate and the
* camera parameters of the previously calibrated camera
 *提供校准板的描述文件和以前校准的摄像机的摄像机参数


 
 ***窗口设定阶段***
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． 读取相机初始内参，设置描述文件路径
*2． 创建标定数据模型 create_calib_data
*3． 设置相机的参数类型 set_calib_data_cam_parm
*4． 设置标定板的数据类型（此处用到了描述文件）
*5． 设置标定模式 set_calib_data（我使用的是非线性的方法）
***具体步骤如下***




*首先通过相机标定获取相机的初始内参（焦距，畸变系数，单个像素的宽，单个像素高，图像坐标的中心值横坐标，图像坐标中心值的纵坐标，图像宽度，图像高度）
*//1、设置标定相机模型，采用division或者polynomial畸变模型并设置相机模型初始参数
gen_cam_par_area_scan_division (0.004938, -10379.136, 4.65138e-006, 4.65e-006, 617.294, 534.687, 1280, 1024, CameraParam)
* 
* gen_cam_par_area_scan_polynomial (0.008, 0, 0, 0, 0, 0, 5.2e-006, 5.2e-006, 640, 512, 1280, 1024, CameraParam1)


*//2、设置标定板描述文件
CalibObjDescr := 'calibrate_hand_eye_scara_setup_01_calplate.cpd'


* Set the camera parameters in the calibration model
*3、在标定模型中设定前面已经赋初值的相机模型
set_calib_data_cam_param (CalibDataID, 0, [], CameraParam)


* Set the calibration plate in the calibration model
*4、在标定模型中设置标定板描述文件
set_calib_data_calib_object (CalibDataID, 0, CalibObjDescr)


*Create a new calibration model
* 5、生成新的标定模型
create_calib_data ('hand_eye_scara_moving_cam', 1, 1, CalibDataID)


***正式标定阶段：***
*6． 循环读取图像将标定板的位姿信息和机器人末端在基坐标系位姿（TOOL_IN_BASE）保存到标定数据模型中。
*7． 进行手眼标定 calibrate_hand_eye
***具体步骤如下***


*  Acquire calibration images and corresponding robot poses
*采集获取标定板图像和相应的机器人末端相对于基坐标的位姿变换矩阵


*拍摄 14—20 组标定板图像（eye_in_hand），并准确记录每组图像在拍摄时TOOL_IN_BASE 位姿（此位姿从机器人中读取）。
*备注：在获取标定图像时，标定板不动，机器人末端的摆动要尽可能的全面，绕各个轴的旋转角要尽量的大。
*标定板要出现在相机视野的各个角落。理论上图像越多，角度越全面，标定精度越高。
*6、遍历处理各幅标定图像和其采集时的工具端位姿
for Index := 1 to 10 by 1
    *//** Read the calibration image
    **读取标定图像
    read_image (CalibImage, '3d_machine_vision/hand_eye/scara_moving_cam_setup_01_calib_' + Index$'02')
    * Read the corresponding robot pose (pose of the tool in the
    * robot base coordinate system)
    *读取采集上面对应图像时，机器人工具坐标系相对基坐标的位姿
    read_pose ('scara_moving_cam_setup_01_tool_in_base_pose_' + Index$'02' + '.dat', ToolInBasePose)
    * Set the robot pose in the calibration model
    *设定机器人位姿标定模型，set_calib_data函数内涵比较多，建议详细查看帮助文件
    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
* 7、进行手眼标定
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, 'tool_in_cam_pose', ToolInCamPosePre)
* Free the calibration model
*释放标定模型内存
clear_calib_data (CalibDataID)
* Visualize
*可视化显示初步标定结果
disp_preliminary_result (WindowHandle, ToolInCamPosePre, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
* 


***标定结果校准阶段：***


* 3. Fix the pose ambiguity
*3、对初步标定的位姿进行修正
* 
* When calibrating a SCARA robot, it is impossible to determine
* all pose parameters unambiguously（精准的）. In case of a moving
* camera, the Z translation of ObjInBasePose cannot be determined.
* 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. Because normally the camera does not see
* the object if the tool is moved to the object, the robot is
* moved to two poses. For this, the calibration plate is placed
* at an arbitrary（任意） position. The robot is then manually moved such
* that the camera can observe the calibration plate. Now, an image
* of the calibration plate is acquired and the robot pose is
* queried (-> ToolInBasePoseRef1). From the image, the pose of the
* calibration plate in the camera coordinate system can be
* determined (-> ObjInCamPoseRef1). Afterwards, the tool of the
* robot is manually moved to the origin of the calibration plate
* (-> ToolInBasePoseRef2). These three poses and the result of the
* calibration (ToolInCamPosePre) can be used to fix the
* Z ambiguity by using the procedure fix_scara_ambiguity_moving_cam:
*校准SCARA机器人时，不可能精确地确定所有姿态参数。在移动照相机的情况下，
*ObjInBasePose的Z轴位姿变换无法确定。因此，有必要通过在机器人相机坐标系中将机器人移动到
*已知高度的姿态来校准Z中的未知平移。因为如果工具移动到物体上，照相机通常不会看到物体，
*机器人会移动到两个姿势。为此，校准板放置在任意位置。然后手动移动机器人，使照相机可以观察校准板。
*现在，获取校准板的图像并查询机器人姿势（ - > ToolInBasePoseRef1）。
*从图像中可以确定摄像机坐标系中校准板的姿态（ - > ObjInCamPoseRef1）。
*之后，机器人的工具被手动移动到校准板的原点（ - > ToolInBasePoseRef2）。
*这三个姿势和校准结果（ToolInCamPosePre）可用于通过使用过程fix_scara_ambiguity_moving_cam来校准Z不精确度：
read_image (ImageRef1, '3d_machine_vision/hand_eye/scara_moving_cam_setup_01_calib_ref_1')
get_calib_plate_pose (ImageRef1, CameraParam, CalibObjDescr, ObjInCamPoseRef1)
read_pose ('scara_moving_cam_setup_01_tool_in_base_pose_ref_1.dat', ToolInBasePoseRef1)
read_pose ('scara_moving_cam_setup_01_tool_in_base_pose_ref_2.dat', ToolInBasePoseRef2)
*通过ObjInCamPoseRef1, ToolInBasePoseRef1, ToolInBasePoseRef2三个位姿校准Z不精确度
fix_scara_ambiguity_moving_cam (ToolInCamPosePre, ObjInCamPoseRef1, ToolInBasePoseRef1, ToolInBasePoseRef2, ZCorrection)
set_origin_pose (ToolInCamPosePre, 0, 0, ZCorrection, ToolInCamPose)
* 
* Visualize
disp_final_results (WindowHandle, ToolInCamPosePre, ToolInCamPose)
disp_end_of_program_message (WindowHandle, 'black', 'true')
* 


***标定位姿转换为最终位姿阶段：***




* After the hand-eye calibration is performed, the resulting pose
* ToolInCamPose can be used in robotic grasping applications:
* Let us assume that the camera acquires an image of the object that
* should be grasped. This image was taken at a certain robot pose
* (-> ToolInBasePose). From the image, the pose of the object in the
* camera coordinate system must be determined (-> ObjInCamPose) by
* using image processing.
* Based on these two poses and the result of the calibration
* (ToolInCamPose), the robot pose can be computed that is necessary
* to grasp the object (-> ObjInBasePose):
*执行手眼校准后，得到的姿势ToolInCamPose可用于机器人抓握应用：让我们假设摄像机获取应该抓住的物体的图像。
*该图像是在某个机器人姿势（ - > ToolInBasePose）下拍摄的。 
*从图像中，必须使用图像处理确定（ - > ObjInCamPose）摄像机坐标系中对象的姿态。 
*基于这两个姿势以及校准结果（ToolInCamPose），可以计算机器人姿态，
*该姿势是抓取对象所必需的（ - > ObjInBasePose）：
pose_invert (ToolInCamPose, CamInToolPose)
create_pose (-0.0043, 0.0085, 0.087, 0.445, 0.068, 355.9, 'Rp+T', 'gba', 'point', ObjInCamPose)
create_pose (0.2612, 0.084, 0.1731, 0, 0, 178.128, 'Rp+T', 'gba', 'point', ToolInBasePose)
pose_compose (CamInToolPose, ObjInCamPose, ObjInToolPose)
pose_compose (ToolInBasePose, ObjInToolPose, ObjInBasePose)