基于halcon的眼在手上（Eye-in-Hand）标定

前言

        前些时间写了一个关于python+opencv的眼在手上的标定方法，大致流程就是先做相机内参标定，再移动机械臂以不同的角度、姿态对标定板进行拍照，并且每次拍照都需要记录下机械臂的姿态。准确来说，是机械臂末端法兰坐标系在机械臂基座标系下的位姿和姿态。需要注意的是，如果在移动时，机械臂设置的是工具坐标系，那么读取到的结果也是工具坐标系在基座标系下的位置和姿态，如果用这个结果标定，获得的结果就是相机相对于工具坐标系下的结果。同样，halcon的眼在手上的标定用的也是这种方式，下面将进行叙述。

关于python+opencv的眼在手上的标定方法：python实现单目视觉手眼标定_手眼标定程序代码-CSDN博客

halcon示例代码

halcon示例代码

* 
* 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, 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')
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]
* Labels for the visualized 3D object models.
tuple_gen_const (7, '', Labels)
Labels[0] := 'Robot\'s Tool'
Labels[3] := 'Robot\'s Base'
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
gen_robot_tool_and_base_object_model_3d (ArrowThickness, ArrowLength, OM3DToolOrigin, OM3DBase)
* 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, [], 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')
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
    rigid_trans_object_model_3d (OM3DToolOrigin, ToolInBasePose, OM3DTool)
    visualize_object_model_3d (WindowHandleR, [OM3DTool,OM3DBase], [], PoseIn, ParamName, ParamValue, 'Position of robot tool coordinate system in robot base coordinate system', Labels, Instructions, PoseOut)
    * 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.
set_origin_pose (CalObjInBasePose, 0, 0, 0.005, PlaneInBasePose)
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_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, '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)
rigid_trans_object_model_3d (OM3DObjectOrig, CalObjInBasePose, OM3DObject)
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)
endfor
* Clear the data model
clear_calib_data (CalibDataID)
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)

解析

前面都是一些配置参数，这里就主要分析一下核心代码了

创建、设置标定模型

下面代码主要是用于创建标定模型

read_cam_par        读取相机内参；

create_calib_data        创建标定模型；

set_calib_data_cam_param        将相机内参设置到标定模型中；

set_calib_data_calib_object (CalibDataID, 0, CalTabFile)        这个设置的是标定板的描述文件“caltab_30mm.descr”，如真实的圆点大小、直径、、间距个数等；

set_calib_data (CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')        标定模型的一些设置，如通用模型、优化方式等；

* 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, [], 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')

 读取标定图像、显示结果

下面是for循环中的内容，主要功能为读取标定图像

read_image        读取图像；

find_calib_object        寻找标定板；

get_calib_data_observ_contours        找到标定板，然后画轮廓；

get_calib_data_observ_points        找到标定板，然后画标定点；

read_image (Image, ImageNameStart + I$'02d')
dev_display (Image)
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)

显示结果       

read_pose        读取机械臂的位姿，注意，这里halcon里面设置的就是工具坐标系在基座标中的位姿；

set_calib_data        设置机械臂工具坐标系在基座标中的位姿；

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
rigid_trans_object_model_3d (OM3DToolOrigin, ToolInBasePose, OM3DTool)
visualize_object_model_3d (WindowHandleR, [OM3DTool,OM3DBase], [], PoseIn, ParamName, ParamValue, 'Position of robot tool coordinate system in robot base coordinate system', Labels, Instructions, PoseOut)
set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)

PoseIn        是显示Robot‘s Base，相对于圆心的位姿和姿态；

rigid_trans_object_model_3d和visualize_object_model_3d是为了显示下面这个图：

手眼标定

calibrate_hand_eye        手眼标定，Errors为误差；

get_calib_data        获取一些标定数据，这个算子有点特别，里面包含很多参数选择

get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError)    获取相机内参标定误差；    

get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)        这里是获取相机内参数据，halcon在手眼标定的时候把相机内参也进行了优化，这个和一般的手眼标定有些不太一样。最开始的内参数据为：['area_scan_division', 0.0117685, -2541.7, 7.32909e-06, 7.4e-06, 323.6, 245.555, 646, 493]，优化之后为：['area_scan_division', 0.0124405, -2498.87, 7.39583e-06, 7.4e-06, 301.924, 244.786, 646, 493]；

get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPose)        获取工具坐标系在相机坐标系下的位姿。如果前面读取的是末端法兰坐标系相对于基座标，则这里获取的就是末端法兰坐标系在相机坐标系下的结果；

get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_base_pose', CalObjInBasePose)       获取标定板在机械臂基座标下的位置和姿态； 

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)

到这里标定就结束了。下面是保存参数。

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')
write_pose (PlaneInBasePose, DataNameStart + 'final_pose_base_plane.dat')

贴一个标定结果：

验证

        在这里就不对验证进行解释了，简单来说就是根据以下三个坐标系进行转换：标定板坐标系相对于基座标系的转换关系、工具坐标系相对于相机坐标系的转换关系、工具坐标系相对于机械臂基座标的转换关系。这里进行一些数学计算，就能获得标定板坐标系在相机坐标系下的位置和姿态。注意这里不是识别标定板的结果，而是根据结果进行验证。

下面是一个比较好的结果（halcon示例结果）：

可以看到坐标系和标定板的中心和姿态基本上是吻合的。这就是一个好的结果。 

--------------这里留个坑，过两天来填。主要是显示一个不太理想的结果（0519）------------------

 

        这里可以看到我在手眼标定结束后的平移部分的差值已经很大了，有7.7mm，最大有10.1mm。而我的标定板又很小，一共也就60，70mm，所以最后在验证的时候，差距就变的很大了，我放两张比较夸张的图。

 

 

        从上面的图片可以看出来，验证结果和手眼标定后的结果基本一致，在平移上差距较大，而在旋转上还是比较准确的。 

        填坑结束啦~~~如果有疑问，或者有错误请与我联系~~~