关于Halcon 手在眼外3D取料引导案例心得记录：

概述：用机械手带着物料取图，记录图像相对于模版的姿态和当前机械手姿态，将两组姿态输入标定得到两个姿态，用这两个姿态可以将取图得到的姿态变换为机械手的姿态，从而可以取料。

读取3D点云模型，作为模版。

read_object_model_3d ('hand_eye/robot_gripper_3d_model.om3', 1, [], [], OM3DModel, Status)

create_surface_model (OM3DModel, 0.03, [], [], SurfaceModelID)

将点云重采样，简化点云，后续会将该点云变换观察3D配准结果。

sample_object_model_3d (OM3DModel, 'fast', 0.0009, [], [], SampledObjectModel3D)

创建标定句柄HECCalibDataID

create_calib_data ('hand_eye_stationary_cam', 0, 0, HECCalibDataID)

标定句柄添加参数，这个地方为非线性优化

set_calib_data (HECCalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')

循环给标定句柄添加姿态对

for I := 1 to NumCalibrationScenes by 1

    读取3D点云模型和对应的机械手坐标

    read_pose ('tool_in_base_pose_' + I$'02d' + '.dat', ToolInBasePose)

    filename := 'hand_eye/robot_gripper_3d_scene_' + I$'02d'

    read_object_model_3d (filename, 1, [], [], OM3DScene, Status1)

    3D点云配准得到点云姿态，相对于模版点云

find_surface_model (SurfaceModelID, OM3DScene, 0.05, 1, 0, 'false', [], [], ObjInCamPose, Score, SurfaceMatchingResultID)

再次优化姿态

refine_surface_model_pose (SurfaceModelID, OM3DScene, ObjInCamPose, 0, 'false', [], [], ObjInCamPose, Score, SurfaceMatchingResultID1)

去除未匹配到的数据

    if (|Score|)

        

        set_calib_data (HECCalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)

        set_calib_data_observ_pose (HECCalibDataID, 0, 0, I, ObjInCamPose)

    endif

Endfor

计算标定，HECPoseError包含标定结果

calibrate_hand_eye (HECCalibDataID, HECPoseError)

获取标定结果，有两个姿态

机械手基坐标系在相机坐标系中的姿态，可以记为B/C

get_calib_data (HECCalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInSensorPose)

物料在机械手工具坐标系中的姿态,可以记为O/T

get_calib_data (HECCalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', ObjInToolPose)

这个时候我在相机取到的图像通过配准的到姿态，通过变换就可以得到机械手工具坐标。

pose_invert (ObjInToolPose, PoseInvertTO)

pose_compose (ObjInCamPose, PoseInvertTO, PoseComposeTC)

将标定得到第一个Pose反转，再用取图得到的姿态乘以它，得到工具在相机的姿态

pose_invert (BaseInSensorPose, PoseInvertCB)

pose_compose (PoseInvertCB, PoseComposeTC, PoseComposeTB)