这个halcon例程是:pose_of_known_3d_object.hdev
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
determine_control_points算子接口
threshold (Image, Region, 128, 255) connection (Region, ConnectedRegions) select_shape_std (ConnectedRegions, SelectedRegions, 'max_area', 70) fill_up (SelectedRegions, RegionFillUp) difference (RegionFillUp, SelectedRegions, Holes) connection (Holes, SeparateHoles) area_center (SeparateHoles, Area, Row1, Column) IndicesArea := sort_index(Area) * Determine the coordinates of the centers of the three holes RowCenter := [] ColCenter := [] * Get the corners by intersecting the detected sides fill_up (SelectedRegions, ElementFull) dilation_circle (ElementFull, RegionDilation, 3.5) erosion_circle (ElementFull, RegionErosion, 3.5) difference (RegionDilation, RegionErosion, ROI) reduce_domain (Image, ROI, ImageReduced) edges_sub_pix (ImageReduced, Edges, 'lanser2', 0.5, 10, 40) segment_contours_xld (Edges, Contours, 'lines', 3, 7, 7) fit_line_contour_xld (Contours, 'tukey', -1, 10, 5, 1, RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist) intersection_lines (RowBegin[0], ColBegin[0], RowEnd[0], ColEnd[0], RowBegin[2], ColBegin[2], RowEnd[2], ColEnd[2], RowC1, ColC1, IsOverlappingS1) intersection_lines (RowBegin[4], ColBegin[4], RowEnd[4], ColEnd[4], RowBegin[2], ColBegin[2], RowEnd[2], ColEnd[2], RowC2, ColC2, IsOverlappingS1) * Prepare visualisation of the intersection gen_empty_obj (Intersections) calc_directions_norm (RowEnd[0] - RowBegin[0], ColEnd[0] - ColBegin[0], DRow, DCol) gen_contour_polygon_xld (LineFragment, RowEnd[0] + DRow * [0,-60], ColEnd[0] + DCol * [0,-60]) concat_obj (Intersections, LineFragment, Intersections) gen_contour_polygon_xld (LineFragment, RowEnd[0] + DRow * [40,70], ColEnd[0] + DCol * [40,70]) concat_obj (Intersections, LineFragment, Intersections) calc_directions_norm (RowEnd[2] - RowBegin[2], ColEnd[2] - ColBegin[2], DRow, DCol) gen_contour_polygon_xld (LineFragment, RowBegin[2] + DRow * [-40,-70], ColBegin[2] + DCol * [-40,-70]) concat_obj (Intersections, LineFragment, Intersections) gen_contour_polygon_xld (LineFragment, [RowBegin[2],RowEnd[2]], [ColBegin[2],ColEnd[2]]) concat_obj (Intersections, LineFragment, Intersections) gen_contour_polygon_xld (LineFragment, RowEnd[2] + DRow * [40,70], ColEnd[2] + DCol * [40,70]) concat_obj (Intersections, LineFragment, Intersections) calc_directions_norm (RowEnd[4] - RowBegin[4], ColEnd[4] - ColBegin[4], DRow, DCol) gen_contour_polygon_xld (LineFragment, RowBegin[4] + DRow * [-40,-70], ColBegin[4] + DCol * [-40,-70]) concat_obj (Intersections, LineFragment, Intersections) gen_contour_polygon_xld (LineFragment, RowBegin[4] + DRow * [0,60], ColBegin[4] + DCol * [0,60]) concat_obj (Intersections, LineFragment, Intersections) RowCenter := [RowCenter,RowC1,RowC2] ColCenter := [ColCenter,ColC1,ColC2] * Get the second and third largest hole for i := |IndicesArea| - 2 to |IndicesArea| - 3 by -1 select_obj (SeparateHoles, CircularHole, IndicesArea[i] + 1) dilation_circle (CircularHole, RegionDilation, 3.5) erosion_circle (CircularHole, RegionErosion, 3.5) difference (RegionDilation, RegionErosion, ROI) reduce_domain (Image, ROI, ImageReduced) edges_sub_pix (ImageReduced, Edges, 'lanser2', 0.7, 20, 40) longest_closed_contour (Edges, LongestClosedContour) fit_ellipse_contour_xld (LongestClosedContour, 'fitzgibbon', -1, 1, 0, 200, 3, 1, Row, Col, Phi, Radius1, Radius2, StartPhi, EndPhi, PointOrder) RowCenter := [RowCenter,Row] ColCenter := [ColCenter,Col] endfor gen_contour_polygon_xld (ThreeCenterPoints, RowCenter[1:3], ColCenter[1:3]) area_center_xld (ThreeCenterPoints, Area1, Row2, Column1, PointOrder) if (PointOrder == 'positive') RowCenter := subset(RowCenter,[0,1,3,2]) ColCenter := subset(ColCenter,[0,1,3,2]) endif * return ()
算子disp_coordinate_system_3d的参数
dev_set_window (WindowHandle) ArrowLength := 0.02 ArrowX_WCS := [0,ArrowLength,0,0] ArrowY_WCS := [0,0,ArrowLength,0] ArrowZ_WCS := [0,0,0,ArrowLength] affine_trans_point_3d (HomMat_WCS_to_CCS, ArrowX_WCS, ArrowY_WCS, ArrowZ_WCS, ArrowX_CCS, ArrowY_CCS, ArrowZ_CCS) project_3d_point (ArrowX_CCS, ArrowY_CCS, ArrowZ_CCS, CamPar, ArrowRow, ArrowColumn) disp_arrow (WindowHandle, ArrowRow[0], ArrowColumn[0], ArrowRow[1], ArrowColumn[1], 1) disp_arrow (WindowHandle, ArrowRow[0], ArrowColumn[0], ArrowRow[2], ArrowColumn[2], 1) disp_arrow (WindowHandle, ArrowRow[0], ArrowColumn[0], ArrowRow[3], ArrowColumn[3], 1) set_tposition (WindowHandle, ArrowRow[0], ArrowColumn[0]) write_string (WindowHandle, NameCS) set_tposition (WindowHandle, ArrowRow[1], ArrowColumn[1]) write_string (WindowHandle, 'x') set_tposition (WindowHandle, ArrowRow[2], ArrowColumn[2]) write_string (WindowHandle, 'y') set_tposition (WindowHandle, ArrowRow[3], ArrowColumn[3]) write_string (WindowHandle, 'z') return ()

