Halcon三 依据点关系计算物体三维位姿Halcon

1.set_origin_pose( : : PoseIn, DX, DY, DZ : PoseNewOrigin)

平移POSEIN的原点，输出为新的原点。注意，平移沿着OBJ的坐标新进行，而非沿着摄像机的坐标系进行。

2.disp_3d_coord_system (WindowHandle, CamParam, Pose, 0.02)

内部函数，显示世界坐系的坐标轴

3.determine_control_points (Image, Intersection, RowCenter, ColCenter)

内部函数，找到两个圆形洞，一个矩形洞的交点中心坐标。

4.gen_cross_contour_xld( : Cross : Row, Col, Size, Angle : )

在输出点处产生十字交叉的XLD轮廓。

5.dev_clear_window( : : : )

关闭活动窗口

6.affine_trans_point_3d( : : HomMat3D, Px, Py, Pz : Qx, Qy, Qz)

点到点的三维变换，输入控制为其次变换矩阵，输出为新坐标系下的点坐标。

7.project_3d_point( : : X, Y, Z, CameraParam : Row, Column)

把摄像机坐标下的点投影到图像坐标系，输出为图像坐标系下的行列坐标。

* Set image path and name
ImgPath := '3d_machine_vision/pose/'                          //设定读取图像的路径//
ImgName := 'metal_part'                           //设定图像名称//

* Read reference image
read_image (ImageRef, ImgPath+'calib') //读取图像//
* Define camera parameters (the exterior camera parameters define the world coordinate system)
CamParam := [1.222445e-002,-2.610410e+003,7.395958e-006,7.400000e-006,3.031241e+002,2.341259e+002,640,480]    //设定已知相机内参数//
* Reopen the window to get the WindowHandle
dev_close_window ()                   
get_image_size (ImageRef, Width, Height)    
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)
* Define WCS
find_caltab (ImageRef, Caltab, 'caltab_30mm.descr', 3, 112, 5) //输出标定板区域//
find_marks_and_pose (ImageRef, Caltab, 'caltab_30mm.descr', CamParam, 128, 10, 18, 0.9, 15, 100, RCoord, CCoord, PoseOfWCS) //输出标定点的图像坐标及标定板在摄像机坐标系下的初步位姿//
*
* Modify the pose of the WCS in order to modify the world coordinate system
*
if (true)
    * If the pose is only to be shifted, you can use set_origin_pose
    set_origin_pose (PoseOfWCS, -0.0568, 0.0372, 0, PoseOfWCS)
    * But you may also rotate the respective coordinate system, or apply other transformations.
    * This can be done based on homogenous transformation matrices
    pose_to_hom_mat3d (PoseOfWCS, camHwcs)//输出世界坐标相对于摄像机坐标的齐次变换矩阵//
    hom_mat3d_rotate_local (camHwcs, rad(180), 'x', camHwcs)//世界坐标绕着自身X轴转180，输出为

齐次变换矩阵//
    hom_mat3d_to_pose (camHwcs, PoseOfWCS)//输出世界坐标系在摄像机坐标系中的位姿//
endif
dev_display (ImageRef)
hom_mat3d_to_pose (camHwcs, Pose))//输出世界坐标系在摄像机坐标系中的位姿//
disp_3d_coord_system (WindowHandle, CamParam, Pose, 0.02)//显示世界坐系的坐标轴//
*
* Select an image (1, 2, or 3)
*
ImgNum := 1
*
read_image (Image, ImgPath+ImgName+'_'+ImgNum$'02d')
* Define object coordinates of control points
ShiftXObj := 5.5/1000.0    //世界坐标系中控制点距离原点X方向的距离//
ShiftYObj := 5.15/1000.0   //世界坐标系中控制点距离原点Y方向的距离//
ControlX := [18.73,-5.35,13.05, 0.00]/1000.0+ShiftXObj
ControlY := [27.52,27.68, 0.00, 0.00]/1000.0+ShiftYObj
ControlZ := [ 0.00, 0.00, 0.00, 0.00]        //控制点的世界坐标//          
dev_set_color ('green')
* Determine the image coordinates of the control points (here: via find_caltab and find_marks_and_pose)
determine_control_points (Image, Intersection, RowCenter, ColCenter)
* Visualize control point
disp_message (WindowHandle, 'Extracted control points', 'window', -1, -1, 'green', 'false')
gen_cross_contour_xld (Cross, RowCenter, ColCenter, 6, 0.785398)//在输出点坐标处输出XLD轮廓//
dev_display (Cross)       //显示XLD轮廓//               
disp_continue_message (WindowHandle, 'black', 'true')//窗口显示F5继续//   
stop ()                                   //程序暂停，等待继续执行按钮//          
* Determine the pose of the object ...
vector_to_pose (ControlX, ControlY, ControlZ, RowCenter, ColCenter, CamParam, 'iterative', 'error', PoseOfObject, Errors)  //根据4个控制点在世界坐标和图像坐

标之间的关系，得到物体坐标系在摄像机坐标系下的位姿//    
* Transform the poses into homogeneous transformation matrices
pose_to_hom_mat3d (PoseOfWCS, camHwcs)//世界坐标在摄像机坐标下的齐次变换矩阵//
pose_to_hom_mat3d (PoseOfObject, camHobj)//物体坐标系在摄像机坐标系下的齐次变换矩阵//
* Determine the transformation matrix from object coordinates into world coordinates
hom_mat3d_invert (camHwcs, wcsHcam) //求世界坐标在摄像机坐标坐标系下矩阵的逆矩阵//
hom_mat3d_compose (wcsHcam, camHobj, wcsHobj)//相机对世界坐标矩阵乘以物体系对世界坐标系

矩阵，得到物体对世界坐标系的齐次变换矩阵//

* Visualize the results
dev_clear_window ()                  //关闭活动窗口//
dev_display (Image)
dev_set_color ('green')              //设置接下来要显示部分的颜色//
disp_coordinate_system_3d (WindowHandle, CamParam, camHwcs, 'WCS') //显示世界坐标系，为绿色//
dev_set_color ('red')
disp_coordinate_system_3d (WindowHandle, CamParam, camHobj, 'OBJ')//显示物体坐标系，红色//
* Define some 3D object coordinates of points (here: the four corners of the rectangular hole)
CornersXObj := [0.89, 0.77, 12.12, 12.18]/1000.0+ShiftXObj     //角点物体坐标系下的X坐标//
CornersYObj := [21.63, 8.47, 8.45, 21.51]/1000.0+ShiftYObj//角点在物体坐标系下的Y坐标//
CornersZObj := [0,0,0,0]                                 //角点在物体坐标系下的Z坐标//
* Transform the 3D object coordinates into the world coordinate system
affine_trans_point_3d (wcsHobj, CornersXObj, CornersYObj, CornersZObj, CornersXWCS, CornersYWCS, CornersZWCS)       //转换物体坐标系中的点进入世界坐标系//    
* Transform the 3D object coordinates into the image coordinate system and display the respective points
affine_trans_point_3d (camHobj, CornersXObj, CornersYObj, CornersZObj, CornersXCam, CornersYCam, CornersZCam)        //转换物体坐标系中的点进入摄像机坐标系//    
project_3d_point (CornersXCam, CornersYCam, CornersZCam, CamParam, CornersRow, CornersCol)
 //把摄像机坐标系中的3D点投影到图像坐标//

dev_set_color ('blue')         
gen_cross_contour_xld (Cross, CornersRow, CornersCol, 6, 0.785398)//在图像中4个角点出产生交叉线// 
dev_display (Cross)
* Display the 3D object coordinates and the 3D coordinates in the world coordinate system
CornersXObjmm := CornersXObj*1000.0
CornersYObjmm := CornersYObj*1000.0
CornersZObjmm := CornersZObj*1000.0
CornersXWCSmm := CornersXWCS*1000.0
CornersYWCSmm := CornersYWCS*1000.0
CornersZWCSmm := CornersZWCS*1000.0   //米转化成毫米单位// 
disp_message (WindowHandle, 'Object coordinates:', 'window', 10, 10, 'red', 'false')
disp_message (WindowHandle, 'World coordinates:', 'window', 10, 200, 'green', 'false')

//显示信息// 
for i := 1 to |CornersRow| by 1
    disp_message (WindowHandle, i, 'window', CornersRow[i-1], CornersCol[i-1], 'blue', 'false')
    disp_message (WindowHandle, i+':', 'window', 30+i*20, 10, 'red', 'false')
    disp_message (WindowHandle, '('+CornersXObjmm[i-1]$'.2f'+','+CornersYObjmm[i-1]$'.2f'+','+CornersZObjmm[i-1]$'.2f'+') [mm]', 'window', 30+i*20, 30, 'red', 'false')
    dev_set_color ('green')
    disp_message (WindowHandle, i+':', 'window', 30+i*20, 200, 'green', 'false')
    disp_message (WindowHandle, '('+CornersXWCSmm[i-1]$'.2f'+','+CornersYWCSmm[i-1]$'.2f'+','+CornersZWCSmm[i-1]$'.2f'+') [mm]', 'window', 30+i*20, 220, 'green', 'false')
endfor                              //分别在物体坐标系和世界坐标系下显示4个角点的坐标//

8.1 绿色交叉线处为控制点

                   8.2 最后的窗口显示结果