手眼标定 halcon代码

以下是Halcon中手眼标定的示例代码：

1. 初始化机器人和相机

dev_open_window(0,0,512,512,'black','',$WinID)
dev_open_window(513,0,512,512,'black','',$WinID2)

open_framegrabber('GenICamTL',1,1,0,0,0,0,'default','',-1,'false','default','default','false',$CamID)
set_framegrabber_param($CamID,'PixelFormat','Mono12')
set_framegrabber_param($CamID,'ExposureTime','1000.0')
set_framegrabber_param($CamID,'Gain','0.0')

open_serial(1,'/dev/ttyUSB0','38400','8E1','RTSCTS',$RobotID)

gen_cam_parfoc(1.0,0.0,0.0,0.0,0.0,0.0,$CamParam)

2. 获取机器人和相机的位姿

get_robot_pos($RobotID, $RobotPose)
get_cam_par($CamParam,'focus',$CamFocus)
get_cam_par($CamParam,'ccd_width',$CamWidth)
get_cam_par($CamParam,'ccd_height',$CamHeight)
get_cam_par($CamParam,'image_width',$CamImgWidth)
get_cam_par($CamParam,'image_height',$CamImgHeight)
get_cam_par($CamParam,'principal_point',$CamPrincipalPoint)
get_cam_par($CamParam,'distortion',$CamDistortion)
get_cam_par($CamParam,'camera_type',$CamType)

3. 设置标定板参数

$ChessboardRows:=9
$ChessboardColumns:=7
$ChessboardSize:=30.0
$CalibrationPose:=[-0.40, 0.20, 0.35, 0.0, 0.0, 0.0]
$CalibrationPoseInv:=pose_inv($CalibrationPose)
$CalibrationPoseInCam:=pose_compose($CamPose,$CalibrationPoseInv)
$CalibrationPoseInRobot:=pose_compose($RobotPose,$CalibrationPose)
$CalibrationPoseInRobotInv:=pose_inv($CalibrationPoseInRobot)

4. 获取标定板角点在机器人参考系下的坐标

gen_circle_contour_xld(ChessboardCenterRow,ChessboardCenterColumn,$ChessboardSize/2.0,0,6.28318,0,20,$ChessboardCircle)
find_calib_object(ChessboardCircle,$ChessboardRows,$ChessboardColumns,0,$ChessboardPose)
hom_mat3d_to_pose($ChessboardPose, $ChessboardPoseInRobot)

5. 获取标定板角点在相机图像中的坐标

project_object_model_3d($ChessboardPoseInCam,$CamParam,$ChessboardCircle_Model,$ChessboardCircle_Image)

6. 手眼标定

find_pose_of_camera($ChessboardPoseInRobot,$ChessboardCircle,$CamParam,$CamPose,$CamPoseInRobot,$Errors)

7. 保存标定结果

write_cam_par($CamParam,'calibration_data','calibration_cam_param.cam_par')
write_pose($CamPoseInRobot,'calibration_data','calibration_cam_pose.robot_pose')