手眼标定(眼在手上)-halcon
此方法过于复杂,实际操作不友好
一 准备标定图像(大于15张)

二 准备标定板描述文件
三 准备相机内部参数
四 准备机械手工具坐标系下图像对应的位姿(变换)
五 创建手眼标定模型
create_calib_data ('hand_eye_moving_cam', 1, 1, CalibDataID)
六 对手眼标定模型设置相机内参
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_division', CamParam)
七 对手眼标定模型设置标定板描述文件
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)
八 校准姿态
set_calib_data (CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')
九 进行标定
1.读取标定图像
2.匹配标定板(标定图像信息录入)
3.获取标定板轮廓
4.获取标定板的mark圆坐标和标定板相当于相机的位姿
5.读取机械手基座坐标系下机械手工具位姿
6.设置机械手基座坐标系下机械手工具位姿设置到手眼标定模型里(机械手信息录入)
十 标定
calibrate_hand_eye (CalibDataID, Errors)
十一 查看标定效果
get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError)
十二 获取标定结果
1.获取相机内参
2.获取相机坐标系下机械手工具的位姿
3.获取机械手基础坐标系下标定板的位姿
十三 保存标定结果
十四 相机坐标系下标定板的位姿
1.读取图像
2.获取机械手基准坐标中机械手工具的姿态
3.计算相机坐标系下标定板的位姿
十五 获取机械手基座坐标系中标定板的位姿
1.获取相机坐标系下标定板的位姿
2.位姿反向,获取机械手工具坐标系下相机坐标系的位姿
3.机械手基础坐标系下机械手工具坐标系的位姿和机械手工具坐标系下相机坐标系的位姿进行乘积,从而获取机械手基础坐标系下相机坐标系的位姿。
4.机械手基础坐标系下相机坐标系的位姿和摄像机坐标系下校正对象坐标系的位姿进行乘积,从而获取机械手基础坐标系下标定板坐标系的位姿。
*关闭程序计数器,变量更新,图像窗口更新
dev_update_off ()
* 校正图像路径
ImageNameStart := '3d_machine_vision/hand_eye/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)
*设置区域填充方式为margin
dev_set_draw ('margin')
*显示图像
dev_display (Image)
*设置字体信息,字体大小为14,字体为mono,粗体
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
* 标定板描述文件
CalTabFile := 'caltab_30mm.descr'
* 读取摄像机内部参数
read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam)
* 创建手眼标定模型
create_calib_data ('hand_eye_moving_cam', 1, 1, CalibDataID)
* 对手眼标定模型设置摄像机内部参数
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_division', StartCamParam)
* 对手眼标定模型设置标定板描述文件
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)
*采用非线性算法获取精确校准姿态
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 ()
**************************************************************获取标定板MARK点坐标和标定板相对相机的位姿**************************************************************************************
for I := 0 to NumImages - 1 by 1
*读取含标定板的图像
read_image (Image, ImageNameStart + I$'02d')
*寻找标定板对象
find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
*获取标定板轮廓
get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
*获取标定板MARK点坐标和标定板相对相机的位姿
get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, PoseForCalibrationPlate)
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 (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
* 设置机器人基座坐标系下机器人工具位姿设置到手眼标定模型里
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 ()
**************************************************************执行手眼标定**************************************************************************************
*执行手眼标定
calibrate_hand_eye (CalibDataID, Errors)
* 查询手眼标定的错误情况
get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError)
* 获取摄像机内部参数
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
* 获取摄像机坐标系下机器人工具的位姿
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
* 保存相机内部参数参数到本地文件
write_cam_par (CamParam, DataNameStart + 'final_campar.dat')
* 将摄像机坐标系下的机器人工具位姿保存到本地
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
disp_results (WindowHandle, CamCalibError, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
**************************************************************计算摄像机坐标系下校正对象的位姿**************************************************************************************
* 查询摄像机,校正对象,校正对象位姿的相对关系
query_calib_data_observ_indices (CalibDataID, 'camera', 0, CalibObjIdx, PoseIds)
for I := 0 to NumImages - 1 by 1
read_image (Image, ImageNameStart + I$'02d')
dev_display (Image)
*获取机器人基准坐标中机器人工具的姿态
get_calib_data (CalibDataID, 'tool', PoseIds[I], 'tool_in_base_pose', ToolInBasePose)
*计算摄像机坐标系下校正对象的位姿
calc_calplate_pose_movingcam (CalObjInBasePose, ToolInCamPose, ToolInBasePose, CalObjInCamPose)
*显示坐标系统
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_calib_data (CalibDataID)
**************************************************************获取机器人基座坐标系中校正对象的位姿**************************************************************************************
ObjInCamPose := CalObjInCamPose
*位姿反向,获取机器人工具坐标系下摄像机坐标系的位姿
pose_invert (ToolInCamPose, CamInToolPose)
*机器人基础坐标系下机器人工具坐标系的位姿和机器人工具坐标系下摄像机坐标系的位姿进行乘积,从而获取机器人基础坐标系下摄像机坐标系的位姿。
pose_compose (ToolInBasePose, CamInToolPose, CamInBasePose)
*机器人基础坐标系下摄像机坐标系的位姿和摄像机坐标系下校正对象坐标系的位姿进行乘积,从而获取机器人基础坐标系下校正对象坐标系的位姿。
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)