Halcon学习：calibrate_hand_eye_scara_moving_cam

* 本示例说明了如何对SCARA机器人执行手眼校准。 在这种情况下，将摄像机安装到机器人工具上，并且校准对象相对于机器人是固定的。
*提供标定板的描述文件和先前校准过的摄像机的摄像机参数
CalibObjDescr := 'calibrate_hand_eye_scara_setup_01_calplate.cpd' //标定板的描述文件
gen_cam_par_area_scan_division (0.004938, -10379.136, 4.65138e-006, 4.65e-006, 617.294, 534.687, 1280, 1024, CameraParam) //为面扫描摄像机生成摄像机参数元组，其畸变由除 division model 建模。先前校准过的摄像机参数
* 

---

---

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.获取标定图像和相应的机器人位姿

*（1）． 读取相机初始内参，设置描述文件路径 

*（2）． 创建标定数据模型 create_calib_data

*（3）． 设置相机的参数类型 set_calib_data_cam_parm

*（4）． 设置标定板的数据类型（此处用到了描述文件）

*（5）． 设置标定模式 set_calib_data
//创建一个新的标定模型
create_calib_data ('hand_eye_scara_moving_cam', 1, 1, CalibDataID)
 //在标定模型中设置相机参数
set_calib_data_cam_param (CalibDataID, 0, [], CameraParam)
//在标定模型中设置标定板
set_calib_data_calib_object (CalibDataID, 0, CalibObjDescr)

拍摄 14—20 组标定板图像（eye_in_hand），并准确记录每组图像在拍摄时TOOL_IN_BASE 位姿（此位姿从机器人中读取）。
*备注：在获取标定图像时，标定板不动，机器人末端的摆动要尽可能的全面，绕各个轴的旋转角要尽量的大。
*标定板要出现在相机视野的各个角落。理论上图像越多，角度越全面，标定精度越高。

for Index := 1 to 10 by 1
    //读取标定图像
    read_image (CalibImage, '3d_machine_vision/hand_eye/scara_moving_cam_setup_01_calib_' + Index$'02')
    //读取相应的机器人姿态（机器人基础坐标系中工具的姿态）
    read_pose ('scara_moving_cam_setup_01_tool_in_base_pose_' + Index$'02' + '.dat', ToolInBasePose)
   //在标定模型中设置机器人位姿
    set_calib_data (CalibDataID, 'tool', Index, 'tool_in_base_pose', ToolInBasePose)
   //确定相机坐标系中标定板的位姿，并在标定模型中设置位姿
    find_calib_object (CalibImage, CalibDataID, 0, 0, Index, [], [])//找到HALCON标定板，并在标定数据模型中设置提取的点和轮廓。
   //可视化
    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_hand_eye_calibration_input_poses (CalibDataID, 0.05, 0.005, Warnings)
if (|Warnings| != 0)
    * 在输入位姿中检测到问题。
    * 检查警告并使用remove_calib_data和remove_calib_data_observ消除错误的位姿。
    dev_inspect_ctrl (Warnings)
    stop ()
endif
* 
* 3. 执行手眼校准
* 
calibrate_hand_eye (CalibDataID, Errors) //进行手眼校准
* 获取校准结果，即相机坐标系中机器人基座的位姿
get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPosePre) //查询在校准数据模型中存储或计算的数据。
* 显示
disp_preliminary_result (WindowHandle, ToolInCamPosePre, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
* 
* 3. 修正姿态歧义
* 
* 校准SCARA机器人时，不可能明确确定所有姿态参数。如果是移动摄像机，则无法确定ObjInBasePose的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)
fix_scara_ambiguity_moving_cam (ToolInCamPosePre, ObjInCamPoseRef1, ToolInBasePoseRef1, ToolInBasePoseRef2, ZCorrection) //通过ObjInCamPoseRef1, ToolInBasePoseRef1, ToolInBasePoseRef2三个位姿校准Z不精确度

set_origin_pose (ToolInCamPosePre, 0, 0, ZCorrection, ToolInCamPose) //平移ToolInCamPosePre的原点，输出为新的原点ToolInCamPose。注意，平移沿着OBJ的坐标新进行，而非沿着摄像机的坐标系进行。

* 
* 显示
disp_final_results (WindowHandle, ToolInCamPosePre, ToolInCamPose)
disp_end_of_program_message (WindowHandle, 'black', 'true')
* 
*执行手眼校准后，得到的位姿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)

可参考博客

1.Calibration校准halcon算子，持续更新

https://www.iambigboss.top/post/26567_1_1.html#set_calib_data_observ_pose_45

2.http://www.skcircle.com/?id=1235

 

find_calib_object (CalibImage, CalibDataID, 0, 0, Index, [], []) 获取标定点信息，并获取标定板在相机坐标系中的位姿

get_calib_data_observ_pose (CalibDataID, 0, 0, Index, ObjInCameraPose)  从标定模型中读取标定板位姿信息

 

calibrate_hand_eye (CalibDataID, Errors) //进行手眼校准

get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPosePre) //查询在校准数据模型中存储或计算的数据。

fix_scara_ambiguity_moving_cam (ToolInCamPosePre, ObjInCamPoseRef1, ToolInBasePoseRef1, ToolInBasePoseRef2, ZCorrection) //通过ObjInCamPoseRef1, ToolInBasePoseRef1, ToolInBasePoseRef2三个位姿校准Z不精确度