halcon 手眼标定原理推导

记住一个很关键的点:我们最终求的是base 和Object之间的关系，无论是eye to hand 还是 eye in hand ，找不变的关系，推导中间量，最后推导base 和Object之间的关系。

一、eye to hand（Camera  Moving）

什么动，什么不动？

如下图：

tool 坐标系和object 坐标系 是相对静止的固定关系

camara 坐标系是发生移动的

数学推导

二、eye in hand(Stationary Camera)

什么动，什么不动？

如下图：

base 坐标系和object坐标系 是相对静止的固定关系

原理

halcon 手眼标定坐标系及坐标系之间的转换关系_WarGames_dc的博客-CSDN博客_halcon坐标系转换

三、手眼标定的坐标转换

这个是面试的时候经常问到的，我面了几家机器的还有新能源的，每次都问，要手推，还带说出为啥。

没事我们从头再来。这里涉及到的坐标系有四个：机器人基坐标系base、法兰上的工具坐标系tool、相机坐标系camera和标定板坐标系cal；此外，涉及到了四个关键的4x4齐次转换矩阵。

  eye  to  hand

* Calibration 01: Code generated by Calibration 01
CameraParameters := [0.038072,0.772143,3.75091e-006,3.75e-006,1042.63,770.207,2048,1536]
CameraPose := [-0.0116375,-0.0128823,0.491131,358.368,1.90647,39.3723,0]
*write_cam_par (CameraParameters,'D:/halcon/eyetohand/pose/CameraParameters.dat')

stop ()
* Image Acquisition 01: Code generated by Image Acquisition 01
list_files ('D:/halcon/eyetohand', ['files','follow_links','recursive'], ImageFiles)
tuple_regexp_select (ImageFiles, ['\\.(tif|tiff|gif|bmp|jpg|jpeg|jp2|png|pcx|pgm|ppm|pbm|xwd|ima|hobj)$','ignore_case'], ImageFiles)

poseFiles := []
poseFiles[0] := 'D:/halcon/eyetohand/pose/campose_01.dat'
poseFiles[1] := 'D:/halcon/eyetohand/pose/campose_02.dat'
poseFiles[2] := 'D:/halcon/eyetohand/pose/campose_03.dat'
poseFiles[3] := 'D:/halcon/eyetohand/pose/campose_04.dat'
poseFiles[4] := 'D:/halcon/eyetohand/pose/campose_05.dat'
poseFiles[5] := 'D:/halcon/eyetohand/pose/campose_06.dat'
poseFiles[6] := 'D:/halcon/eyetohand/pose/campose_07.dat'
poseFiles[7] := 'D:/halcon/eyetohand/pose/campose_08.dat'
poseFiles[8] := 'D:/halcon/eyetohand/pose/campose_09.dat'
poseFiles[9] := 'D:/halcon/eyetohand/pose/campose_10.dat'
poseFiles[10] :='D:/halcon/eyetohand/pose/campose_11.dat'
poseFiles[11] :='D:/halcon/eyetohand/pose/campose_12.dat'
poseFiles[12] :='D:/halcon/eyetohand/pose/campose_13.dat'
poseFiles[13] :='D:/halcon/eyetohand/pose/campose_14.dat'

TmpCtrl_ReferenceIndex := 0
TmpCtrl_PlateDescription := 'D:/halcon/eyetohand/caltab.descr'
TmpCtrl_FindCalObjParNames := ['gap_tolerance','alpha','skip_find_caltab']
TmpCtrl_FindCalObjParValues := [1,0.78,'false']
*以下是手眼标定
create_calib_data ('hand_eye_scara_stationary_cam', 1, 1, CalibHandle)
set_calib_data_cam_param (CalibHandle, 0, 'area_scan_division', CameraParameters)
set_calib_data_calib_object (CalibHandle, 0, TmpCtrl_PlateDescription)

* set_calib_data (CalibHandle, 'model', 'general', 'optimization_method', 'nonlinear')
Pose1:=[]
Num:=|ImageFiles|-1
for I := 0 to 13 by 1
    read_image (Image, ImageFiles[I])
    find_calib_object (Image, CalibHandle, 0, 0, I, TmpCtrl_FindCalObjParNames, TmpCtrl_FindCalObjParValues)
    get_calib_data_observ_contours (Caltab, CalibHandle, 'caltab', 0, 0, I)
    get_calib_data_observ_points (CalibHandle, 0, 0, I, RCoord, CCoord, Index, PoseForCalibrationPlate)
    dev_set_color ('green')
    dev_display (Image)
    dev_display (Caltab)
    dev_set_color ('yellow')
    disp_cross (3600, RCoord, CCoord, 6, 0)
    dev_set_colored (3)  
    disp_3d_coord_system (3600, CameraParameters, PoseForCalibrationPlate, 0.01)
    read_pose (poseFiles[I], Pose)
    Pose1:=[Pose1,Pose]
    set_calib_data (CalibHandle, 'tool',I, 'tool_in_base_pose',Pose)
endfor
stop ()
calibrate_hand_eye (CalibHandle, Errors)
get_calib_data (CalibHandle, 'camera', 0, 'base_in_cam_pose', BaseInCamPosePre)

* try
*     write_cam_par (CameraParameters,'C:/Users/86188/Desktop/Theendcamer3/CameraParameters.dat')
*     write_pose (ToolInCamPose, 'C:/Users/86188/Desktop/Theendcamer3/ToolInCamPose.dat')
*     write_pose (CalObjInBasePose,'C:/Users/86188/Desktop/Theendcamer3/CalObjInBasePose.dat')
* catch (Exception)
    * do nothing
* endtry
* get_calib_data (CalibHandle, 'calib_obj_pose', [0, TmpCtrl_ReferenceIndex], 'pose', CameraPose)
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
* 显示误差值
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_display (Image)
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 (3600, Message, 'window', 12, 12, 'black', 'true')
*识别
* Image Acquisition 01: Code generated by Image Acquisition 01

* open_framegrabber ('KsjCamera_MT_x86', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'default', '1', 0, -1, AcqHandle)
* grab_image_start (AcqHandle, -1)
* while (true)
*     grab_image_async (Image, AcqHandle, -1)
*     rgb1_to_gray (Image, GrayImage)
*     scale_image (GrayImage, ImageScaled, 3.86364, -62)    
*     threshold (ImageScaled, Regions, 84, 255)
*     connection (Regions, ConnectedRegions)   
*     select_shape (ConnectedRegions, SelectedRegions, 'area', 'and', 6261.18, 115385)
*     area_center (SelectedRegions, Area, Row, Column)
    *先转世界
*     image_points_to_world_plane  (CameraParameters,CameraPose, Row, Column, 'm', X, Y)
*     pose_invert (BaseInCamPosePre, CamInBasePosePre)
*     pose_compose (CamInBasePosePre, CameraPose, ObjInBase)
*     pose_to_hom_mat3d (ObjInBase, ObjInBaseHomMat3D)
*     affine_trans_point_3d (ObjInBaseHomMat3D, X, Y, 0, Qx, Qy, Qz)
    
    
* endwhile
* close_framegrabber (AcqHandle)

eye in hand:

CameraParameters := [0.00540942,-772.779,3.19882e-006,3.2e-006,995.629,706.461,2048,1536]
CameraPose := [-0.000755431,8.62376e-006,0.0876035,0.653457,359.717,272.888,0]



list_files ('D:/halcon/eyeinhand1205/images', ['files','follow_links','recursive'], ImageFiles)
tuple_regexp_select (ImageFiles, ['\\.(tif|tiff|gif|bmp|jpg|jpeg|jp2|png|pcx|pgm|ppm|pbm|xwd|ima|hobj)$','ignore_case'], ImageFiles)
num:=|ImageFiles|
* 将所有的pose文件都提出来放到  poseFiles 的数组中
poseFiles := []
poseFiles[0] := 'D:/halcon/eyeinhand1205/pose/campose_01.dat'
poseFiles[1] := 'D:/halcon/eyeinhand1205/pose/campose_02.dat'
poseFiles[2] := 'D:/halcon/eyeinhand1205/pose/campose_03.dat'
poseFiles[3] := 'D:/halcon/eyeinhand1205/pose/campose_04.dat'
poseFiles[4] := 'D:/halcon/eyeinhand1205/pose/campose_05.dat'
poseFiles[5] := 'D:/halcon/eyeinhand1205/pose/campose_06.dat'
poseFiles[6] := 'D:/halcon/eyeinhand1205/pose/campose_07.dat'
poseFiles[7] := 'D:/halcon/eyeinhand1205/pose/campose_08.dat'
poseFiles[8] := 'D:/halcon/eyeinhand1205/pose/campose_09.dat'
poseFiles[9] := 'D:/halcon/eyeinhand1205/pose/campose_10.dat'
poseFiles[10] :='D:/halcon/eyeinhand1205/pose/campose_11.dat'
poseFiles[11] :='D:/halcon/eyeinhand1205/pose/campose_12.dat'

* 设置参考位置 一般默认是第一张图的位置
TmpCtrl_ReferenceIndex:=0
* 设置第一步的标定板的描述文件
TmpCtrl_PlateDescription:='D:/halcon/eyeinhand1205/caltab.descr'
* 设置在标定的图像中找到标定板的参数
TmpCtrl_FindCalObjParNames := ['gap_tolerance','alpha','skip_find_caltab']
* TmpCtrl_FindCalObjParNames  对应的值
TmpCtrl_FindCalObjParValues := [1,1,'false']
* TmpCtrl_FindCalObjParNames 和TmpCtrl_FindCalObjParValues 在find_calib_object 的时候使用
* ==========================================================================================
* 创建手眼标定的模板
       * 第一个参数表示是手在眼上还是手在眼外  这个是手在眼上 所以 'hand_eye_scara_moving_cam'
       * 第二个参数：相机的个数
       * 第三个参数：标定板的个数
       * 最后一个参数表示这个的句柄
create_calib_data ('hand_eye_scara_moving_cam', 1, 1, CalibDataID)
* 设置标定模型中相机的参数
    * 第一个参数表示的是标定模型的句柄
    * 表示设定标定板中的相机的索引
    * 第三个参数：表示是相机的类型，是面阵相机还是线阵相机 area_scan_division 表示面阵相机
    * 最后一个参数表示 设置的标定模型中相机的什么参数：这里设定的是内参即CameraParameters
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_division', CameraParameters)
* 设置标定板的资料
    * 第一个参数表示的是标定模型的句柄
    * 表示设定标定板中的标定板的索引
    * 第三个参数：是标定板的描述文件
set_calib_data_calib_object (CalibDataID, 0, TmpCtrl_PlateDescription)
Pose1:=[]
* 从标定的12张图像中找到相对于的标定的的轮廓和mark标定
for I := 0 to |ImageFiles|-1 by 1
      read_image (Image, ImageFiles[I])
       * 在采集的图像姿态中找到标定板
          * Image
          * 标定模型的句柄
          * 标定模型中相机的索引
          * 标定模型中标定板的索引
          * I 这个是读入图像姿态的索引
          * TmpCtrl_FindCalObjParNames  ：标定板的参数 的参数，是多个值可以根据不同的情况自己设定
          * TmpCtrl_FindCalObjParValues  TmpCtrl_FindCalObjParNames对应的值
    find_calib_object (Image, CalibDataID, 0, 0, I, TmpCtrl_FindCalObjParNames, TmpCtrl_FindCalObjParValues)
    * 从读入的标定图像中得到对应的轮廓信息
         * 得到标定的轮廓
         * 标定模型的句柄
         * marks 标定，像得到什么的， 这里是得到  标定板的
         * 相机索引
         * 标定板的索引
         * 读入标定图像姿态的索引
       get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
      * 得到 标定板中黑点对应的坐标:输出一个pose
         * 标定模型的句柄
         * 相机索引
         * 标定板的索引  
         * 读入标定图像姿态的索引
         * 黑点的横坐标 
         * 黑点的纵坐标
         * 黑点的索引
         * 输出每一张标定板姿态  [0.00826224, -0.0502468, 3.42284, 2.30472, 0.84261, 264.306, 0]
     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 (3600, RCoord, CCoord, 6, 0)
      dev_set_colored (3)  
       disp_3d_coord_system (3600, CameraParameters, PoseForCalibrationPlate, 0.01)
       *从生成的.dat文件即 poseFiles 读出每对应的pose文件
       read_pose (poseFiles[I], Pose) 
       * 将每一个新的pose 添加到新的数组中
       Pose1:=[Pose1,Pose]
       * 下面的这句是很重要的 表示的是要生产的姿态的方式：从***到*** 的姿态
      set_calib_data (CalibDataID, 'tool',I, 'tool_in_base_pose',Pose)

endfor
*  标定
calibrate_hand_eye (CalibDataID, Errors)
*  从标定模型 得到tc的关系，这个就是我们辛辛苦苦要得到的数据  tool->Carmer 的姿态
get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', Tool_CameraPose)
*  从标定模型中(获取参考（世界）坐标系)相对于机械手基础坐标系的关系
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_base_pose', Object_BasePose)
stop ()


dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
* 显示误差值
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_display (Image)
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 (3600, Message, 'window', 12, 12, 'black', 'true')

stop ()

create_pose (0.250595,0.05462,-0.058648,0,0,-80.111, 'Rp+T', 'gba', 'point', ToolInBasePose)
* 相机标定的外参
Obj_in_camera :=  [-0.000755431,8.62376e-006,0.0876035,0.653457,359.717,272.888,0]


pose_invert (Tool_CameraPose, CamInToolPose)
pose_compose (ToolInBasePose, CamInToolPose, CamerInBase)
pose_compose (CamerInBase, Obj_in_camera, ObjectInBase)
stop ()

* Image Acquisition 01: Code generated by Image Acquisition 01
open_framegrabber ('KsjCamera_MT_x64', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'default', '4', 0, -1, AcqHandle)
grab_image_start (AcqHandle, -1)

grab_image_async (Image, AcqHandle, -1)
* Image Acquisition 01: Do something
    rgb1_to_gray (Image, GrayImage)
    threshold (GrayImage, Regions, 9, 27)
    connection (Regions, ConnectedRegions)
    select_shape (ConnectedRegions, SelectedRegions, 'area', 'and', 207890, 425975)

    area_center (SelectedRegions, Area, Row, Column)
    * read_cam_par ('C:/Users/admin/Desktop/epson_eye_in_hand/param/cameraParam.cal', CameraParam)
* 通过相机的内外参，将特征点的像素坐标转换成相应的世界坐标
    image_points_to_world_plane  (CameraParameters,Obj_in_camera,  Row, Column, 'm', X, Y)
* write_cam_par (CameraParameters, 'C:/Users/86188/Desktop/campar.dat')
* write_pose (Obj_in_camera, 'C:/Users/86188/Desktop/campose.dat')
* 通过世界坐标系相对于机械手基础坐标系的关系，计算出变换矩阵
pose_to_hom_mat3d (Object_BasePose, base_H_obj)
* Z := [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
affine_trans_point_3d (base_H_obj, X, Y, 0, Qx, Qy, Qz)
close_framegrabber (AcqHandle)

* 识别
* Image Acquisition 01: Code generated by Image Acquisition 01

 上面这个是去年7月份在公司做的，现在没有机器人，等有了重新做一个标定，这个对三维理解太重要了