手眼标定之相机随动eye-in-hand 示例：handeye_movingcam_calibration

*
* 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/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)
*读机械手基础坐标系下的末端工具的姿态，每张图只要机械手末端相对标定板有XYZ方向的平移或旋转，此姿态就会不一样。
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, Errors)
* Query the camera parameters and the poses
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
* Get poses computed by the hand eye calibration
*tool_in_cam_pose：在相机坐标系下工具坐标系的关系
get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPose)
*obj_in_base_pose：在机械手基础坐标系下标定板的姿态
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: ' + Errors[0]$'6.4f' + ' ' + Errors[2]$'6.4f'
Message[2] := 'Rotation part in degree: ' + Errors[1]$'6.4f' + ' ' + Errors[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)