* 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. *本例說明如何執(zhí)行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． 創(chuàng)建標定數據模型 create_calib_data*3． 設置相機的參數類型 set_calib_data_cam_parm*4． 設置標定板的數據類型（此處用到了描述文件）*5． 設置標定模式 set_calib_data（我使用的是非線性的方法）***具體步驟如下***
*首先通過相機標定獲取相機的初始內參（焦距，畸變系數，單個像素的寬，單個像素高，圖像坐標的中心值橫坐標，圖像坐標中心值的縱坐標，圖像寬度，圖像高度）*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)
*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． 循環(huán)讀取圖像將標定板的位姿信息和機器人末端在基坐標系位姿（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機器人時，不可能精確地確定所有姿態(tài)參數。在移動照相機的情況下，*ObjInBasePose的Z軸位姿變換無法確定。因此，有必要通過在機器人相機坐標系中將機器人移動到*已知高度的姿態(tài)來校準Z中的未知平移。因為如果工具移動到物體上，照相機通常不會看到物體，*機器人會移動到兩個姿勢。為此，校準板放置在任意位置。然后手動移動機器人，使照相機可以觀察校準板。*現在，獲取校準板的圖像并查詢機器人姿勢（ - > ToolInBasePoseRef1）。*從圖像中可以確定攝像機坐標系中校準板的姿態(tài)（ - > 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)* * Visualizedisp_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):*執(zhí)行手眼校準后，得到的姿勢ToolInCamPose可用于機器人抓握應用：讓我們假設攝像機獲取應該抓住的物體的圖像。*該圖像是在某個機器人姿勢（ - > ToolInBasePose）下拍攝的。*從圖像中，必須使用圖像處理確定（ - > ObjInCamPose）攝像機坐標系中對象的姿態(tài)。*基于這兩個姿勢以及校準結果（ToolInCamPose），可以計算機器人姿態(tài)，*該姿勢是抓取對象所必需的（ - > 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)