eye-in-hand手眼標定系統
阿新 • • 發佈:2019-02-11
上一個博文說了一種手眼標定系統的特殊情況,就是相機隨著機械手一起移動的情況。雖然前面簡單介紹了一種辦法,就是內次拍照時讓相機回到之前標定的位置,但是這樣處理機器人的工作效率就會大大降低,產生很多重複動作,所以這篇博文就來講一種更簡單的標定方案。
下面是halcon官方給的手眼標定系統的案例程式碼。
這個例子主要應用於相機固定在機械手末端的手眼系統中,而且標定板相對於機器人也是靜止的。
在這種情況下,手眼校準的目標是確定兩個未知姿勢:
校準物件(CalObjInBasePose)的座標系中機器人基座的姿態。
相機在工具中心點座標系中的姿態。
理論上,作為輸入,該方法在攝像機座標系中至少需要3個校準物件的姿勢。但是我們在實際工程應用中需要至少10個姿態。
校準物件的姿態是從連線到機器人的攝像頭記錄的校準物件的影象中獲得的。為了獲得更好的結果,將攝像機相對於校準物件放置以使物件在影象中傾斜是至關重要的。
在手眼校準之後,提取計算出的變換並用於計算攝像機座標系中校準物件的姿態。
dev_update_off ()
* Directories with calibration images and data files
ImageNameStart := '3d_machine_vision/handeye/movingcam_calib3cm_'
DataNameStart := 'handeye/movingcam_'
NumImages := 14
read_image (Image, ImageNameStart + '00')
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')
* 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.
CalTabFile := 'caltab_30mm.descr'
* Read the initial values for the internal camera parameters
read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam)
* 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, 'area_scan_division', StartCamParam)
* Set the calibration object
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)
* Start the loop over the calibration images
* Set the opitmization 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 ()
for I := 0 to NumImages - 1 by 1
read_image (Image, ImageNameStart + I$'02d')
* 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)
* Read pose of tool in robot base coordinates (ToolInBasePose)
read_pose (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
* Set the pose tool in robot base coordinates in the calibration data model
set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
* Uncomment for inspection of visualization
* disp_message (WindowHandle, 'Extracting data from calibration image ' + (I + 1) + ' of ' + NumImages, 'window', 12, 12, 'black', 'true')
* disp_continue_message (WindowHandle, 'black', 'true')
* stop ()
endfor
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 ()
* 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, PoseErrors)
* 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)
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')
catch (Exception)
* do nothing
endtry
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_display (Image)
* Display calibration errors
Message := 'Quality of the results: root mean square maximum'
Message[1] := 'Translation part in meter: ' + PoseErrors[0]$'6.4f' + ' ' + PoseErrors[2]$'6.4f'
Message[2] := 'Rotation part in degree: ' + PoseErrors[1]$'6.4f' + ' ' + PoseErrors[3]$'6.4f'
disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
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, '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.
for I := 0 to NumImages - 1 by 1
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')
if (I < NumImages - 1)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
endif
endfor
* Clear the data model
clear_calib_data (CalibDataID)
*
* After the hand-eye calibration the computed pose
* ToolInCamPose can be used in robotic grasping applications.
* 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, CalObjInCamPose, ObjInBasePose)