Halcon手眼標定例程註釋 ——hand_eye_movingcam_calibration.hdev
阿新 • • 發佈：2019-01-29
*
* This example explains how to use the hand eye calibration for the case where
* the camera is attached to the robot tool and the calibration object
* is stationary with respect to the robot. The robot positions the
* camera with respect to the calibration plate.
* In this case, the goal of the hand eye calibration is to determine two unknown poses:
* - the pose of the robot base in the coordinate system
* of the calibration object (CalObjInBasePose).
* - the pose of the camera in the coordinate system of the
* tool center point (ToolInCamPose).
* Theoretically, as input the method needs at least 3 poses of the
* calibration object in the camera coordinate system.
* However, it is recommended to use at least 10 Poses.
* The corresponding poses of the robot tool in the robot base coordinate system
* (ToolInBasePose) changes for each calibration image,
* because it describes the pose of the robot moving the camera.
* The poses of the calibration object are obtained from images of the
* calibration object recorded with the camera attached to the robot.
* To obtain good calibration results, it its essential to position
* the camera with respect to the calibration object so that the object appears
* tilted in the image.
* After the hand eye calibration, the computed transformations are
* extracted and used to compute the pose of the calibration object in the
* camera coordinate system.
dev_update_off ()
* Directories with calibration images and data files
*//設定標定影象初始影象名和初始引數檔名，便於遍歷讀取
ImageNameStart := '3d_machine_vision/hand_eye/movingcam_calib3cm_'
DataNameStart := 'hand_eye/movingcam_'
*//設定標定影象採集次數
**//NumImages := 14
read_image (Image, 'E:/study/科研/機器視覺學習資料/halcon練習/標定實驗/標定影象/1.jpg')
dev_close_window ()
get_image_size (Image, Width, Height)
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)
dev_set_line_width (2)
dev_set_draw ('margin')
dev_display (Image)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
ParamName := ['color_0','color_1','color_2','color_3','color_4','color_5','color_6','alpha_6']
ParamValue := ['red','green','blue','red','green','blue','white',0.7]
* //初始視覺化機器人3D位姿中的標籤矩陣，Labels for the visualized 3D object models.
tuple_gen_const (7, '', Labels)
Labels[0] := 'Robot\'s Tool'
Labels[3] := 'Robot\'s Base'
* //初始視覺化機器人3D位姿中的操作指導資訊
Instructions[0] := 'Rotate: Left button'
Instructions[1] := 'Zoom: Shift + left button'
Instructions[2] := 'Move: Ctrl + left button'
*//設定機器人三維位姿示意圖中的座標軸箭頭樣式 Set size for 3D visualization in [m]
ArrowThickness := 0.005
ArrowLength := 0.05
*生成機器人3D模型，（Thickness of the arrows，Length of the arrows（輸入）；3D model of the robot's tool.3D model of the robot's Base（輸出））
*//OM3DToolOrigin:=
*//OM3DBase:=
gen_robot_tool_and_base_object_model_3d (ArrowThickness, ArrowLength, OM3DToolOrigin, OM3DBase)
* gen_robot_tool_and_base_object_model_3d (ArrowLength, ArrowLength, OM3DToolOrigin1, OM3DBase1)
* Load the calibration plate description file.
* Make sure that the file is in the current directory or
* //設定標定板描述檔案路徑，便於後面讀取標定板描述檔案in HALCONROOT/calib, or use an absolute path.
CalTabDescr:='E:/study/科研/機器視覺學習資料/halcon練習/標定實驗/caltab.descr'
CalTabFile := CalTabDescr
* 讀取相機初始引數或引數檔案Read the initial values for the internal camera parameters
*read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam)
*StartCamParam:=['area_scan_division', 0.0117685, -2541.7, 7.32909e-006, 7.4e-006, 323.6, 245.555, 646, 493]
StartCamParam:=['area_scan_polynomial', 0.0058132, 11113.2, 9.66687e+008, -5.98123e+014, 0.279616, 0.535812, 5.60089e-006, 5.6e-006, 322.43, 235.729, 640, 480]
* 建立手眼標定模型 Create the calibration model for the hand eye calibration
* Create the calibration model for the hand eye calibration
* 建立眼隨手動的手眼標定模型（標定型別、相機數、標定板數、標定資料代號）where the calibration object is observed with a camera
create_calib_data ('hand_eye_moving_cam', 1, 1, CalibDataID)
* 設定標定相機型別（標定代號、相機代號、相機型別、相機初始引數）Set the camera type used
set_calib_data_cam_param (CalibDataID, 0, [], StartCamParam)
* 設定標定板資訊Set the calibration object
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)
* Start the loop over the calibration images
*設定標定資料模型最優化方式Set the optimization method to be used
* ？？
set_calib_data (CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')
disp_message (WindowHandle, 'The calibration data model was created', 'window', 12, 12, 'black', 'true')
* disp_continue_message (WindowHandle, 'black', 'true')
stop ()
dev_open_window (0, Width + 10, Width, Height, 'black', WindowHandleR)
set_display_font (WindowHandleR, 14, 'mono', 'true', 'false')
*遍歷處理每個標定位姿的標定圖形
list_files ('E:/study/科研/機器視覺學習資料/halcon練習/標定實驗/標定影象', 'files', ImageFiles)
NumImages:=|ImageFiles|
for I := 0 to NumImages - 1 by 1
dev_set_window (WindowHandle)
dev_clear_window ()
*讀取不同相機位姿獲取的標定板影象
read_image (Image, ImageNameStart + I$'02d')
dev_display (Image)
* Search for the calibration plate, extract the marks and the
* pose of it, and store the results in the calibration data
* The poses are stored in the calibration data model for use by
* the hand eye calibration and do not have to be set explicitly
*搜尋標定區域並識別標定輪廓和標定點資訊
find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
*獲取標定板輪廓資訊
get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
*獲取標定板標定點資訊
get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, PoseForCalibrationPlate)
* Visualize the extracted calibration marks and the estimated pose (coordinate system)
dev_set_color ('green')
dev_display (Image)
dev_display (Caltab)
dev_set_color ('yellow')
*在標定點中心畫十字標記
disp_cross (WindowHandle, RCoord, CCoord, 6, 0)
dev_set_colored (3)
*在標定板影象中顯示標定板座標系
disp_3d_coord_system (WindowHandle, StartCamParam, PoseForCalibrationPlate, 0.01)
disp_message (WindowHandle, 'Extracting data from calibration image ' + (I + 1) + ' of ' + NumImages, 'window', 12, 12, 'black', 'true')
* 讀取機器人模型的工具座標系在機器人基座標系中的變換矩陣，可以從機器人模型中得出。Read pose of tool in robot base coordinates (ToolInBasePose)
read_pose (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
if (I == 0)
PoseIn := [-0.006,-0.296,12,178,2,270,0]
else
PoseIn := PoseOut
endif
*將工具初始座標OM3DToolOrigin值通過ToolInBasePose即工具座標在機器人基座標中的位姿轉換矩陣轉換為工具在基座標下的表示形式 OM3DTool
rigid_trans_object_model_3d (OM3DToolOrigin, ToolInBasePose, OM3DTool)
*create_pose (0.1, 0.2, 0.3, 40, 50, 60, 'R(p-T)', 'abg', 'point', Pose)
*（標準化3D互動顯示）視覺化互動顯示機器人蔘數的3D模型座標
visualize_object_model_3d (WindowHandleR, [OM3DTool,OM3DBase], [], PoseIn, ParamName, ParamValue, 'Position of robot tool coordinate system in robot base coordinate system', Labels, Instructions, PoseOut)
clear_object_model_3d (OM3DTool)
* Set the pose tool in robot base coordinates in the calibration data model
set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
endfor
dev_set_window (WindowHandleR)
dev_close_window ()
disp_message (WindowHandle, 'All relevant data has been set in the calibration data model', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
* Check the input poses for consistency
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
*
* Perform the hand eye calibration and store the results to file
* The calibration of the cameras is done internally prior
* to the hand eye calibration
dev_display (Image)
disp_message (WindowHandle, 'Performing the hand-eye calibration', 'window', 12, 12, 'black', 'true')
***利用前面獲取的標定資料進行手眼標定***
calibrate_hand_eye (CalibDataID, Errors)
***從手眼標定結果中獲取所需的引數具體數值***
* Query the error of the camera calibration
get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError)
* Query the camera parameters and the poses
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
* Get poses computed by the hand eye calibration
get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPose)
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_base_pose', CalObjInBasePose)
* Get the plane in base coordinate system pose by translating the
* CalObjInBasePose by the calibration object's thickness in the
* z-direction.
*//初始標定板位姿CalObjInBasePose在考慮標定板厚度後沿z軸平移變換的標定板在基座標系中位姿PlaneInBasePose
set_origin_pose (CalObjInBasePose, 0, 0, 0.005, PlaneInBasePose)
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
try
* Handle situation where user does not have the permission
* to write in the current directory.
*
* Store the camera parameters to file
write_cam_par (CamParam, DataNameStart + 'final_campar.dat')
* Save the hand eye calibration results to file
write_pose (ToolInCamPose, DataNameStart + 'final_pose_cam_tool.dat')
write_pose (CalObjInBasePose, DataNameStart + 'final_pose_base_calplate.dat')
write_pose (PlaneInBasePose, DataNameStart + 'final_pose_base_plane.dat')
catch (Exception)
* do nothing
endtry
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_display (Image)
* Display calibration errors
disp_results (WindowHandle, CamCalibError, Errors)
* disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*?? For the given camera, get the corresponding pose indices and calibration object indices
*//query_calib_data_observ_indices( : : CalibDataID, ItemType, ItemIdx : Index1, Index2)
*// Index1 returns a list of calibration object indices and Index2 returns a list of pose indices.
*//Each pair [Index1[I],Index2[I]] represents calibration object pose that are 'observed' by camera ItemIdx.
query_calib_data_observ_indices (CalibDataID, 'camera', 0, CalibObjIdx, PoseIds)
* Compute the pose of the calibration object in the camera coordinate
* system via calibrated poses and the ToolInBasePose and visualize it.
* Set sizes for 3D visualization in [m]
CameraSize := 0.05
CameraConeLength := 0.3
get_calib_data (CalibDataID, 'calib_obj', 0, 'x', PX)
get_calib_data (CalibDataID, 'calib_obj', 0, 'y', PY)
get_calib_data (CalibDataID, 'calib_obj', 0, 'z', PZ)
*//建立標定板標定點的點雲
gen_object_model_3d_from_points (PX, PY, PZ, OM3DObjectOrig)
*//將初始標定板位姿OM3DObjectOrig通過標定板與基座標系之間的轉換關係CalObjInBasePose（從標定資料中獲得）轉換到基座標系中表示 OM3DObject
rigid_trans_object_model_3d (OM3DObjectOrig, CalObjInBasePose, OM3DObject)
clear_object_model_3d (OM3DObjectOrig)
dev_open_window (0, Width + 10, Width, Height, 'black', WindowHandleR)
set_display_font (WindowHandleR, 14, 'mono', 'true', 'false')
ParamName := ['color_0','color_1','color_2','color_3','color_4','color_5','color_6','color_7','alpha_7','color_8','color_9','color_10','alpha_8','alpha_9','alpha_10','point_size']
ParamValue := ['red','red','green','blue','red','green','blue','white',0.7,'magenta','yellow','white',0.5,0.5,0.5,5]
* Labels for the visualized 3D object models.
tuple_gen_const (11, '', Labels)
Labels[0] := 'Calibration Object'
Labels[1] := 'Robot\'s Tool'
Labels[4] := 'Robot\'s Base'
Labels[8] := 'Camera'
for I := 0 to NumImages - 1 by 1
dev_set_window (WindowHandle)
dev_clear_window ()
read_image (Image, ImageNameStart + I$'02d')
dev_display (Image)
* Obtain the pose of the tool in robot base coordinates used in the calibration.
* The index corresponds to the index of the pose of the observation object.
get_calib_data (CalibDataID, 'tool', PoseIds[I], 'tool_in_base_pose', ToolInBasePose)
* Compute the pose of the calibration object relative to the camera
calc_calplate_pose_movingcam (CalObjInBasePose, ToolInCamPose, ToolInBasePose, CalObjInCamPose)
* Display the coordinate system
dev_set_colored (3)
disp_3d_coord_system (WindowHandle, CamParam, CalObjInCamPose, 0.01)
Message := 'Using the calibration results to display '
Message[1] := 'the coordinate system in image ' + (I + 1) + ' of ' + NumImages
disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
gen_camera_and_tool_moving_cam_object_model_3d (ToolInCamPose, ToolInBasePose, CameraSize, CameraConeLength, OM3DToolOrigin, CamParam, OM3DCamera, OM3DTool)
if (I == 0)
PoseIn := [-0.006,-0.296,12,178,2,270,0]
else
PoseIn := PoseOut
endif
visualize_object_model_3d (WindowHandleR, [OM3DObject,OM3DTool,OM3DBase,OM3DCamera], [], PoseIn, ParamName, ParamValue, [], Labels, Instructions, PoseOut)
clear_object_model_3d (OM3DTool)
clear_object_model_3d (OM3DCamera)
endfor
* Clear the data model
clear_calib_data (CalibDataID)
clear_object_model_3d (OM3DObject)
clear_object_model_3d (OM3DToolOrigin)
clear_object_model_3d (OM3DBase)
dev_set_window (WindowHandleR)
dev_close_window ()
*
* After the hand-eye calibration the computed pose
* ToolInCamPose can be used in robotic grasping applications.
* To grasp an object with the robot, typically, its pose
* with respect to the camera is determined (which
* is simulated here by setting the object's pose to the
* pose of the calibration object)
ObjInCamPose := CalObjInCamPose
* If the tool coordinate system is placed at the gripper
* and a detected object ObjInCamPose shall be grasped
* (here the calibration object),
* the pose of the detected object relative
* to the robot base coordinate system has to be computed.
pose_invert (ToolInCamPose, CamInToolPose)
pose_compose (ToolInBasePose, CamInToolPose, CamInBasePose)
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)
* This example explains how to use the hand eye calibration for the case where
* the camera is attached to the robot tool and the calibration object
* is stationary with respect to the robot. The robot positions the
* camera with respect to the calibration plate.
* In this case, the goal of the hand eye calibration is to determine two unknown poses:
* - the pose of the robot base in the coordinate system
* of the calibration object (CalObjInBasePose).
* - the pose of the camera in the coordinate system of the
* tool center point (ToolInCamPose).
* Theoretically, as input the method needs at least 3 poses of the
* calibration object in the camera coordinate system.
* However, it is recommended to use at least 10 Poses.
* The corresponding poses of the robot tool in the robot base coordinate system
* (ToolInBasePose) changes for each calibration image,
* because it describes the pose of the robot moving the camera.
* The poses of the calibration object are obtained from images of the
* calibration object recorded with the camera attached to the robot.
* To obtain good calibration results, it its essential to position
* the camera with respect to the calibration object so that the object appears
* tilted in the image.
* After the hand eye calibration, the computed transformations are
* extracted and used to compute the pose of the calibration object in the
* camera coordinate system.
dev_update_off ()
* Directories with calibration images and data files
*//設定標定影象初始影象名和初始引數檔名，便於遍歷讀取
ImageNameStart := '3d_machine_vision/hand_eye/movingcam_calib3cm_'
DataNameStart := 'hand_eye/movingcam_'
*//設定標定影象採集次數
**//NumImages := 14
read_image (Image, 'E:/study/科研/機器視覺學習資料/halcon練習/標定實驗/標定影象/1.jpg')
dev_close_window ()
get_image_size (Image, Width, Height)
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)
dev_set_line_width (2)
dev_set_draw ('margin')
dev_display (Image)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
ParamName := ['color_0','color_1','color_2','color_3','color_4','color_5','color_6','alpha_6']
ParamValue := ['red','green','blue','red','green','blue','white',0.7]
* //初始視覺化機器人3D位姿中的標籤矩陣，Labels for the visualized 3D object models.
tuple_gen_const (7, '', Labels)
Labels[0] := 'Robot\'s Tool'
Labels[3] := 'Robot\'s Base'
* //初始視覺化機器人3D位姿中的操作指導資訊
Instructions[0] := 'Rotate: Left button'
Instructions[1] := 'Zoom: Shift + left button'
Instructions[2] := 'Move: Ctrl + left button'
*//設定機器人三維位姿示意圖中的座標軸箭頭樣式 Set size for 3D visualization in [m]
ArrowThickness := 0.005
ArrowLength := 0.05
*生成機器人3D模型，（Thickness of the arrows，Length of the arrows（輸入）；3D model of the robot's tool.3D model of the robot's Base（輸出））
*//OM3DToolOrigin:=
*//OM3DBase:=
gen_robot_tool_and_base_object_model_3d (ArrowThickness, ArrowLength, OM3DToolOrigin, OM3DBase)
* gen_robot_tool_and_base_object_model_3d (ArrowLength, ArrowLength, OM3DToolOrigin1, OM3DBase1)
* Load the calibration plate description file.
* Make sure that the file is in the current directory or
* //設定標定板描述檔案路徑，便於後面讀取標定板描述檔案in HALCONROOT/calib, or use an absolute path.
CalTabDescr:='E:/study/科研/機器視覺學習資料/halcon練習/標定實驗/caltab.descr'
CalTabFile := CalTabDescr
* 讀取相機初始引數或引數檔案Read the initial values for the internal camera parameters
*read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam)
*StartCamParam:=['area_scan_division', 0.0117685, -2541.7, 7.32909e-006, 7.4e-006, 323.6, 245.555, 646, 493]
StartCamParam:=['area_scan_polynomial', 0.0058132, 11113.2, 9.66687e+008, -5.98123e+014, 0.279616, 0.535812, 5.60089e-006, 5.6e-006, 322.43, 235.729, 640, 480]
* 建立手眼標定模型 Create the calibration model for the hand eye calibration
* Create the calibration model for the hand eye calibration
* 建立眼隨手動的手眼標定模型（標定型別、相機數、標定板數、標定資料代號）where the calibration object is observed with a camera
create_calib_data ('hand_eye_moving_cam', 1, 1, CalibDataID)
* 設定標定相機型別（標定代號、相機代號、相機型別、相機初始引數）Set the camera type used
set_calib_data_cam_param (CalibDataID, 0, [], StartCamParam)
* 設定標定板資訊Set the calibration object
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)
* Start the loop over the calibration images
*設定標定資料模型最優化方式Set the optimization method to be used
* ？？
set_calib_data (CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')
disp_message (WindowHandle, 'The calibration data model was created', 'window', 12, 12, 'black', 'true')
* disp_continue_message (WindowHandle, 'black', 'true')
stop ()
dev_open_window (0, Width + 10, Width, Height, 'black', WindowHandleR)
set_display_font (WindowHandleR, 14, 'mono', 'true', 'false')
*遍歷處理每個標定位姿的標定圖形
list_files ('E:/study/科研/機器視覺學習資料/halcon練習/標定實驗/標定影象', 'files', ImageFiles)
NumImages:=|ImageFiles|
for I := 0 to NumImages - 1 by 1
dev_set_window (WindowHandle)
dev_clear_window ()
*讀取不同相機位姿獲取的標定板影象
read_image (Image, ImageNameStart + I$'02d')
dev_display (Image)
* Search for the calibration plate, extract the marks and the
* pose of it, and store the results in the calibration data
* The poses are stored in the calibration data model for use by
* the hand eye calibration and do not have to be set explicitly
*搜尋標定區域並識別標定輪廓和標定點資訊
find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
*獲取標定板輪廓資訊
get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
*獲取標定板標定點資訊
get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, PoseForCalibrationPlate)
* Visualize the extracted calibration marks and the estimated pose (coordinate system)
dev_set_color ('green')
dev_display (Image)
dev_display (Caltab)
dev_set_color ('yellow')
*在標定點中心畫十字標記
disp_cross (WindowHandle, RCoord, CCoord, 6, 0)
dev_set_colored (3)
*在標定板影象中顯示標定板座標系
disp_3d_coord_system (WindowHandle, StartCamParam, PoseForCalibrationPlate, 0.01)
disp_message (WindowHandle, 'Extracting data from calibration image ' + (I + 1) + ' of ' + NumImages, 'window', 12, 12, 'black', 'true')
* 讀取機器人模型的工具座標系在機器人基座標系中的變換矩陣，可以從機器人模型中得出。Read pose of tool in robot base coordinates (ToolInBasePose)
read_pose (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
if (I == 0)
PoseIn := [-0.006,-0.296,12,178,2,270,0]
else
PoseIn := PoseOut
endif
*將工具初始座標OM3DToolOrigin值通過ToolInBasePose即工具座標在機器人基座標中的位姿轉換矩陣轉換為工具在基座標下的表示形式 OM3DTool
rigid_trans_object_model_3d (OM3DToolOrigin, ToolInBasePose, OM3DTool)
*create_pose (0.1, 0.2, 0.3, 40, 50, 60, 'R(p-T)', 'abg', 'point', Pose)
*（標準化3D互動顯示）視覺化互動顯示機器人蔘數的3D模型座標
visualize_object_model_3d (WindowHandleR, [OM3DTool,OM3DBase], [], PoseIn, ParamName, ParamValue, 'Position of robot tool coordinate system in robot base coordinate system', Labels, Instructions, PoseOut)
clear_object_model_3d (OM3DTool)
* Set the pose tool in robot base coordinates in the calibration data model
set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
endfor
dev_set_window (WindowHandleR)
dev_close_window ()
disp_message (WindowHandle, 'All relevant data has been set in the calibration data model', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
* Check the input poses for consistency
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
*
* Perform the hand eye calibration and store the results to file
* The calibration of the cameras is done internally prior
* to the hand eye calibration
dev_display (Image)
disp_message (WindowHandle, 'Performing the hand-eye calibration', 'window', 12, 12, 'black', 'true')
***利用前面獲取的標定資料進行手眼標定***
calibrate_hand_eye (CalibDataID, Errors)
***從手眼標定結果中獲取所需的引數具體數值***
* Query the error of the camera calibration
get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError)
* Query the camera parameters and the poses
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
* Get poses computed by the hand eye calibration
get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPose)
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_base_pose', CalObjInBasePose)
* Get the plane in base coordinate system pose by translating the
* CalObjInBasePose by the calibration object's thickness in the
* z-direction.
*//初始標定板位姿CalObjInBasePose在考慮標定板厚度後沿z軸平移變換的標定板在基座標系中位姿PlaneInBasePose
set_origin_pose (CalObjInBasePose, 0, 0, 0.005, PlaneInBasePose)
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
try
* Handle situation where user does not have the permission
* to write in the current directory.
*
* Store the camera parameters to file
write_cam_par (CamParam, DataNameStart + 'final_campar.dat')
* Save the hand eye calibration results to file
write_pose (ToolInCamPose, DataNameStart + 'final_pose_cam_tool.dat')
write_pose (CalObjInBasePose, DataNameStart + 'final_pose_base_calplate.dat')
write_pose (PlaneInBasePose, DataNameStart + 'final_pose_base_plane.dat')
catch (Exception)
* do nothing
endtry
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_display (Image)
* Display calibration errors
disp_results (WindowHandle, CamCalibError, Errors)
* disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*?? For the given camera, get the corresponding pose indices and calibration object indices
*//query_calib_data_observ_indices( : : CalibDataID, ItemType, ItemIdx : Index1, Index2)
*// Index1 returns a list of calibration object indices and Index2 returns a list of pose indices.
*//Each pair [Index1[I],Index2[I]] represents calibration object pose that are 'observed' by camera ItemIdx.
query_calib_data_observ_indices (CalibDataID, 'camera', 0, CalibObjIdx, PoseIds)
* Compute the pose of the calibration object in the camera coordinate
* system via calibrated poses and the ToolInBasePose and visualize it.
* Set sizes for 3D visualization in [m]
CameraSize := 0.05
CameraConeLength := 0.3
get_calib_data (CalibDataID, 'calib_obj', 0, 'x', PX)
get_calib_data (CalibDataID, 'calib_obj', 0, 'y', PY)
get_calib_data (CalibDataID, 'calib_obj', 0, 'z', PZ)
*//建立標定板標定點的點雲
gen_object_model_3d_from_points (PX, PY, PZ, OM3DObjectOrig)
*//將初始標定板位姿OM3DObjectOrig通過標定板與基座標系之間的轉換關係CalObjInBasePose（從標定資料中獲得）轉換到基座標系中表示 OM3DObject
rigid_trans_object_model_3d (OM3DObjectOrig, CalObjInBasePose, OM3DObject)
clear_object_model_3d (OM3DObjectOrig)
dev_open_window (0, Width + 10, Width, Height, 'black', WindowHandleR)
set_display_font (WindowHandleR, 14, 'mono', 'true', 'false')
ParamName := ['color_0','color_1','color_2','color_3','color_4','color_5','color_6','color_7','alpha_7','color_8','color_9','color_10','alpha_8','alpha_9','alpha_10','point_size']
ParamValue := ['red','red','green','blue','red','green','blue','white',0.7,'magenta','yellow','white',0.5,0.5,0.5,5]
* Labels for the visualized 3D object models.
tuple_gen_const (11, '', Labels)
Labels[0] := 'Calibration Object'
Labels[1] := 'Robot\'s Tool'
Labels[4] := 'Robot\'s Base'
Labels[8] := 'Camera'
for I := 0 to NumImages - 1 by 1
dev_set_window (WindowHandle)
dev_clear_window ()
read_image (Image, ImageNameStart + I$'02d')
dev_display (Image)
* Obtain the pose of the tool in robot base coordinates used in the calibration.
* The index corresponds to the index of the pose of the observation object.
get_calib_data (CalibDataID, 'tool', PoseIds[I], 'tool_in_base_pose', ToolInBasePose)
* Compute the pose of the calibration object relative to the camera
calc_calplate_pose_movingcam (CalObjInBasePose, ToolInCamPose, ToolInBasePose, CalObjInCamPose)
* Display the coordinate system
dev_set_colored (3)
disp_3d_coord_system (WindowHandle, CamParam, CalObjInCamPose, 0.01)
Message := 'Using the calibration results to display '
Message[1] := 'the coordinate system in image ' + (I + 1) + ' of ' + NumImages
disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
gen_camera_and_tool_moving_cam_object_model_3d (ToolInCamPose, ToolInBasePose, CameraSize, CameraConeLength, OM3DToolOrigin, CamParam, OM3DCamera, OM3DTool)
if (I == 0)
PoseIn := [-0.006,-0.296,12,178,2,270,0]
else
PoseIn := PoseOut
endif
visualize_object_model_3d (WindowHandleR, [OM3DObject,OM3DTool,OM3DBase,OM3DCamera], [], PoseIn, ParamName, ParamValue, [], Labels, Instructions, PoseOut)
clear_object_model_3d (OM3DTool)
clear_object_model_3d (OM3DCamera)
endfor
* Clear the data model
clear_calib_data (CalibDataID)
clear_object_model_3d (OM3DObject)
clear_object_model_3d (OM3DToolOrigin)
clear_object_model_3d (OM3DBase)
dev_set_window (WindowHandleR)
dev_close_window ()
*
* After the hand-eye calibration the computed pose
* ToolInCamPose can be used in robotic grasping applications.
* To grasp an object with the robot, typically, its pose
* with respect to the camera is determined (which
* is simulated here by setting the object's pose to the
* pose of the calibration object)
ObjInCamPose := CalObjInCamPose
* If the tool coordinate system is placed at the gripper
* and a detected object ObjInCamPose shall be grasped
* (here the calibration object),
* the pose of the detected object relative
* to the robot base coordinate system has to be computed.
pose_invert (ToolInCamPose, CamInToolPose)
pose_compose (ToolInBasePose, CamInToolPose, CamInBasePose)
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)