halcon例程:通过提取三个或者三个以上的参考点确定物体的姿态


这个halcon例程是:pose_of_known_3d_object.hdev



image.png


image.png


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算子接口

image.png

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的参数

image.png

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 ()


本文出自勇哥的网站《少有人走的路》wwww.skcircle.com,转载请注明出处!讨论可扫码加群:

发表评论:

◎欢迎参与讨论,请在这里发表您的看法、交流您的观点。

会员中心
搜索
«    2024年3月    »
123
45678910
11121314151617
18192021222324
25262728293031
网站分类
标签列表
最新留言
    热门文章 | 热评文章 | 随机文章
文章归档
友情链接
  • 订阅本站的 RSS 2.0 新闻聚合
  • 扫描加本站机器视觉QQ群,验证答案为:halcon勇哥的机器视觉
  • 点击查阅微信群二维码
  • 扫描加勇哥的非标自动化群,验证答案:C#/C++/VB勇哥的非标自动化群
  • 扫描加站长微信:站长微信:abc496103864
  • 扫描加站长QQ:
  • 扫描赞赏本站:
  • 留言板:

Powered By Z-BlogPHP 1.7.2

Copyright Your skcircle.com Rights Reserved.

鄂ICP备18008319号


站长QQ:496103864 微信:abc496103864