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/‘ //設定讀取圖像的路徑//
* 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 綠色交叉線處為控制點
Halcon三 依據點關系計算物體三維位姿Halcon