Halcon 手眼标定（eye in hand ,eye to hand）

*眼在手上就是相机会固定在机械手的末端，或者说是工具坐标系上
*眼在手外就是相机固定在某个地方不动
*Scara与普通的机械手没什么大区别，注意一点是标定平面与实际产品平面差值，或者干脆把Z方向数值去掉，让机械手那边设置抓取高度，视觉这边只识别位置
*******************普通机械手  眼在手上*********************************************
*位姿(1,7)就是转换矩阵(4,4)的意思，只是欧拉角的表现形式,
*如果以相机为0序列，相机位姿（0,0,0,0），那么ToolInCamPose的位姿就是从相机转到Tool工具坐标系的转换矩阵，
*但是具体是左乘还是右乘()，一般如果A-->B，转换矩阵X， B=XA
*输入1 tool_in_base_pose工具坐标系相对于基座的位姿，输入2 CalObjInCamPose在相机坐标系下标定板的位姿
*输出1.相机的内参CamParam
*输出2.ToolInCamPose 相机转到工具坐标系的矩阵，位姿（眼在手上标定主要目的就是要求这个矩阵）
*输出3.CalObjInBasePose 标定板转到基座的矩阵，位姿
create_calib_data ('hand_eye_moving_cam', 1, 1, CalibDataID)
read_cam_par ('hand_eye/movingcam_start_campar.dat', StartCamParam)
set_calib_data_cam_param (CalibDataID, 0, [], StartCamParam)
set_calib_data_calib_object (CalibDataID, 0, 'caltab_30mm.descr')
for I:=0 to 13 by 1
    read_image (Image, '3d_machine_vision/hand_eye/movingcam_calib3cm_' + I$'02d')
    find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
    get_calib_data_observ_contours (Contours, CalibDataID, 'caltab', 0, 0, 0)
    
    read_pose ('hand_eye/movingcam_robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
    set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
endfor
check_hand_eye_calibration_input_poses (CalibDataID, 0.05, 0.005, Warnings)
if (|Warnings| != 0)
    stop ()
endif
calibrate_hand_eye (CalibDataID, Errors)
get_calib_data (CalibDataID, 'camera', 0, 'params', Camera_Params)
get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPose)
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_base_pose', CalObjInBasePose)
*消除标定板的高度，让其在平面上，抓取产品的时候应该用这个
set_origin_pose (CalObjInBasePose, 0, 0, 0.005, PlaneInBasePose)

write_cam_par (Camera_Params, 'Save_Param/MovingCamera_Internel.dat')
* 写出文件
write_pose (ToolInCamPose, 'Save_Param/MovingCamera_ToolInCamPose.dat')
write_pose (CalObjInBasePose, 'Save_Param/MovingCamera_CalObjInBasePose.dat')
write_pose (PlaneInBasePose, 'Save_Param/MovingCamera_pose_base_plane.dat')

*标定完成后，我要定位产品的位置，然后机械手去抓取，1.我会输入机械手末端的位姿 2.我相机识别产品位姿，然后返回ObjInBasePose产品相对于基座的位姿
*1.获取机械手末端的位姿 ToolInBasePose
*2.拍照，识别获取 ObjInCam 产品在相机的位姿，
*3.将产品相对于相机的位姿，转成产品相对于工具坐标系位姿，ObjInCam-->ObjInTool  ToolInCamPose取逆CamInToolPose 左乘ObjInCam
*4.将产品相对于工具坐标系位姿，转成产品相对于基座的位姿  
*
*现在我有ToolInBasePose : 工具坐标系在基座的位姿，理解为 基座转工具坐标系的矩阵（输入变量），
*       ToolInCamPose:工具坐标系跟相机的转换矩阵，这个是固定的，安装的时候就已经确定(主要用到的矩阵)
*       ObjInCamPose :相机识别到的位姿
*
*我要求  ObjInBasePose:产品在在基座看来的位姿，基座转产品的转换矩阵

*识别产品相对于相机位姿
read_image (Image, '3d_machine_vision/hand_eye/movingcam_calib3cm_01')
get_calib_data (CalibDataID, 'calib_obj_pose', [0,1], 'pose', ObjInCamPose)

*官方例程
pose_invert (ToolInCamPose, CamInToolPose)
pose_compose (ToolInBasePose, CamInToolPose, CamInBasePose)
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)

*2自己理解(位姿一样) TooloInBase*CamInTool*ObjInCam=ObjInBase
pose_invert (ToolInCamPose, CamInToolPose1)
pose_compose (CamInToolPose1, ObjInCamPose, ObjInToolPose1)
pose_compose (ToolInBasePose, ObjInToolPose1, ObjInBasePose1)


*********************普通机械手 眼在手外**********************************
read_cam_par ('hand_eye/stationarycam_start_campar.dat', StartCamParam)
create_calib_data ('hand_eye_stationary_cam', 1, 1, CalibDataID)
set_calib_data_cam_param (CalibDataID, 0, [], StartCamParam)
set_calib_data_calib_object (CalibDataID, 0, 'caltab_30mm.descr')

for I := 0 to 16  by 1
    read_image (Image,'3d_machine_vision/hand_eye/stationarycam_calib3cm_' + I$'02d')
    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, CalObjInCamPose)
    read_pose ('hand_eye/stationarycam_robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
    set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
endfor
check_hand_eye_calibration_input_poses (CalibDataID, 0.05, 0.005, Warnings)
if (|Warnings| != 0)
    stop ()
endif
*眼在手外标定的主要目的是找到相机与机械手基座的位姿，也就是BaseInCamPose
*相机识别到产品的位姿，(BaseInCamPose)-1*ObjInCamPose=CamInBasePose*ObjInCamPose=ObjInBasePose
calibrate_hand_eye (CalibDataID, Errors)
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPose)
*没有实际用途
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', CaliObjInToolPose)

write_cam_par (CamParam, 'Save_Param/CamParam_Internel.dat')
write_pose (BaseInCamPose,'Save_Param/BaseInCamPose.dat')
write_pose (CaliObjInToolPose, 'Save_Param/CaliObjInToolPose.dat')

*
pose_invert (BaseInCamPose, CamInBasePose)
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)