3d_position_of_circle.hdev

功能：提取圆轮廓并确定圆（钻孔）的3D位置，本地函数p_determine_ellipse_contours和p_cluster_normals的解析，附在主程序解析之后。
THIK：把大小圆分开检测，只是为了提升准确度嘛（？）

读取图像，设置窗口

dev_update_off ()
read_image (Image, 'rim')
get_image_size (Image, Width, Height)
dev_open_window_fit_image (Image, 0, 0, Width, Height, WindowHandle)
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
dev_display (Image)
dev_set_line_width (3)

提取钻孔的椭圆轮廓，输出分为大和小两种轮廓。

1
2
3

* （本地函数）提取椭圆轮廓，分大小两类
p_determine_ellipse_contours (Image, EllipseContoursLarge, EllipseContoursSmall, false, NumberLarge, NumberSmall)
concat_obj (EllipseContoursLarge, EllipseContoursSmall, EllipseContours)

计算孔的pose

RadiusLarge := 10.25 / 1000.0
RadiusSmall := 5.91 / 1000.0
gen_cam_par_area_scan_division (0.0122, -261.04, 7.39e-6, 7.4e-6, 303.12, 234.17, 652, 494, CamParam)
* 计算圆的位置和方向
get_circle_pose (EllipseContours, CamParam, [gen_tuple_const(NumberLarge,RadiusLarge),gen_tuple_const(NumberSmall,RadiusSmall)], 'pose', Pose1, Pose2)
* 计算圆心的法向量
get_circle_pose (EllipseContours, CamParam, [gen_tuple_const(NumberLarge,RadiusLarge),gen_tuple_const(NumberSmall,RadiusSmall)], 'center_normal', CenterNormal1, CenterNormal2)
* （本地函数）聚类。对于ClusterP1，它包含每个圆的位姿信息，通常包括位置（X、Y、Z）和旋转（例如，以四元数或欧拉角表示）
p_cluster_normals (Pose1, Pose2, CenterNormal1, CenterNormal2, ClusterP1, ClusterP2, ClusterCN1, ClusterCN2)

计算并展示圆在相机坐标系下的3D位置

dev_display (Image)
dev_set_colored (12)
dev_display (EllipseContours)
for i := 0 to NumberLarge + NumberSmall - 1 by 1
    * 将圆的位姿转换为3D齐次变换矩阵
    pose_to_hom_mat3d (ClusterP1[i * 7:i * 7 + 6], HomMat3D)
    affine_trans_point_3d (HomMat3D, 0, 0, 0, Qx, Qy, Qz)
    * 将3D点投影到2D图像平面上，得到行列坐标(Row, Column)
    project_3d_point (Qx, Qy, Qz, CamParam, Row, Column)
    Row := Row - 95
    Column := Column - 60
    disp_message (WindowHandle, ['X=' + ClusterCN1[i * 6]$'6.3f','Y=' + ClusterCN1[i * 6 + 1]$'6.3f','Z=' + ClusterCN1[i * 6 + 2]$'6.3f'], 'window', Row, Column, 'black', 'true')
endfor

p_determine_ellipse_contours函数解析

功能: 从输入图像中提取圆形或椭圆形的轮廓。通过图像预处理、形状选择、边缘检测、以及椭圆拟合等步骤，最终生成精确的椭圆轮廓。

threshold (Image, Dark, 0, 128)
if (visualization)
    dev_display (Image)
    dev_set_color ('green')
    dev_set_draw ('fill')
    dev_display (Dark)
    stop ()
endif
* 
* 提取连通块并通过圆度和面积进行筛选
connection (Dark, DarkRegions)
select_shape (DarkRegions, Circles, ['circularity', 'area'], 'and', [0.85, 50], [1.0, 99999])
if (visualization)
    dev_display (Image)
    dev_set_colored (12)
    dev_display (Circles)
    stop ()
endif
* 
dilation_circle (Circles, ROIOuter, 8.5)
erosion_circle (Circles, ROIInner, 8.5)
* 膨胀、腐蚀后相减，相当于从圆环中去掉中心，留下边缘区域
difference (ROIOuter, ROIInner, ROI)
union1 (ROI, ROIEdges)
if (visualization)
    dev_display (Image)
    dev_set_draw ('margin')
    dev_set_color ('green')
    dev_display (ROIEdges)
    stop ()
endif
* 
* 边缘检测
reduce_domain (Image, ROIEdges, RimReduced)
edges_sub_pix (RimReduced, Edges, 'canny', 2, 20, 40)
* 
* 筛选大圆的边缘
select_contours_xld (Edges, EdgesLarge, 'contour_length', 200, 300, 0, 0)
count_obj (EdgesLarge, NumberLarge)
* 对轮廓进行椭圆拟合，得到椭圆的参数
fit_ellipse_contour_xld (EdgesLarge, 'ftukey', -1, 2, 0, 200, 3, 2, Row, Column, Phi, Ra, Rb, StartPhi, EndPhi, PointOrder)
* 使用拟合参数生成椭圆轮廓
gen_ellipse_contour_xld (EllipseContoursLarge, Row, Column, gen_tuple_const(NumberLarge,0), Ra, Rb, gen_tuple_const(NumberLarge,0), gen_tuple_const(NumberLarge,rad(360)), gen_tuple_const(NumberLarge,'positive'), 1.5)
* 
* 筛选小圆的边缘，并生成对应轮廓
select_contours_xld (Edges, EdgesSmall, 'contour_length', 100, 200, 0, 0)
count_obj (EdgesSmall, NumberSmall)
fit_ellipse_contour_xld (EdgesSmall, 'ftukey', -1, 2, 0, 200, 3, 2, Row, Column, Phi, Ra, Rb, StartPhi, EndPhi, PointOrder)
gen_ellipse_contour_xld (EllipseContoursSmall, Row, Column, gen_tuple_const(NumberSmall,0), Ra, Rb, gen_tuple_const(NumberSmall,0), gen_tuple_const(NumberSmall,rad(360)), gen_tuple_const(NumberSmall,'positive'), 1.5)
* 
if (visualization)
    dev_display (Image)
    dev_set_color ('green')
    dev_display (EllipseContoursLarge)
    dev_set_color ('blue')
    dev_display (EllipseContoursSmall)
    stop ()
endif
return ()

p_cluster_normals函数解析

功能：对两组位姿和法向量进行聚类，确保方向一致的法向量被分到同一个聚类中。通过计算法向量的点积和偏差，最终选择偏差较小的聚类进行输出。

Number := |CenterNormal1| / 6
ClusterP1 := Pose1[0:6]
ClusterP2 := Pose2[0:6]
ClusterCN1 := CenterNormal1[0:5]
ClusterCN2 := CenterNormal2[0:5]
NA := ClusterCN1[3:5]
NB := ClusterCN2[3:5]
for i := 1 to Number - 1 by 1
    N1 := CenterNormal1[i * 6 + 3:i * 6 + 5]
    N2 := CenterNormal2[i * 6 + 3:i * 6 + 5]
    N1NA := sum(N1 * NA)
    N1NB := sum(N1 * NB)
    N2NA := sum(N2 * NA)
    N2NB := sum(N2 * NB)
    tuple_sort_index ([N1NA,N1NB,N2NA,N2NB] * -1, Indices)
    if (Indices[0] == 0 or Indices[0] == 3)
        ClusterP1 := [ClusterP1,Pose1[i * 7:i * 7 + 6]]
        ClusterP2 := [ClusterP2,Pose2[i * 7:i * 7 + 6]]
        ClusterCN1 := [ClusterCN1,CenterNormal1[i * 6:i * 6 + 5]]
        ClusterCN2 := [ClusterCN2,CenterNormal2[i * 6:i * 6 + 5]]
    else
        ClusterP1 := [ClusterP1,Pose2[i * 7:i * 7 + 6]]
        ClusterP2 := [ClusterP2,Pose1[i * 7:i * 7 + 6]]
        ClusterCN1 := [ClusterCN1,CenterNormal2[i * 6:i * 6 + 5]]
        ClusterCN2 := [ClusterCN2,CenterNormal1[i * 6:i * 6 + 5]]
    endif
endfor
* 
idx := []
for i := 0 to Number - 1 by 1
    idx := [idx,i * 6 + 3]
endfor
DeviationC1 := deviation(subset(ClusterCN1,idx)) + deviation(subset(ClusterCN1,idx + 1)) + deviation(subset(ClusterCN1,idx + 2))
DeviationC2 := deviation(subset(ClusterCN2,idx)) + deviation(subset(ClusterCN2,idx + 1)) + deviation(subset(ClusterCN2,idx + 2))
if (DeviationC2 < DeviationC1)
    Help := ClusterP1
    ClusterP1 := ClusterP2
    ClusterP2 := Help
    Help := ClusterCN1
    ClusterCN1 := ClusterCN2
    ClusterCN2 := Help
endif
return ()