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 最後的窗口顯示結果