1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
| dev_update_off ()
read_image (Image, 'C:/Users/19901/Desktop/DemoPic/九点标定.jpeg')
rgb1_to_gray (Image, GrayImage)
* 检测圆，以确定相机坐标系下的9点坐标
threshold (GrayImage, Regions, 13, 71)
fill_up_shape (Regions, WiresFilled, 'area', 1, 100)
opening_circle (WiresFilled, Balls, 15.5)
connection (Balls, SingleBalls)
select_shape (SingleBalls, IntermediateBalls, 'circularity', 'and', 0.85, 1.0)
smallest_circle (SingleBalls, Row, Column, Radius)
dev_clear_window ()
dev_display (Image)
* 生成虚拟机械坐标用以演示，实际应该为：确定的机械手坐标系下的行列坐标。
WorldRow := []
WorldCol :=[]
for Index := 0 to |Row|-1 by 1
WorldRow[Index] := Row[Index] + 40
WorldCol[Index] := Column[Index] - 10
gen_circle (Circle, WorldRow[Index], WorldCol[Index], 10)
dev_set_color('red')
dev_display (Circle)
endfor
* 生成标定，计算变换矩阵HomMat2D，(从相机坐标系到机械手坐标系的转换)
vector_to_hom_mat2d (Row, Column, WorldRow, WorldCol, HomMat2D)
* 测试标定效果
for Index := 0 to |Row|-1 by 1
r := WorldRow[Index]
c := WorldCol[Index]
* 逆矩阵
hom_mat2d_invert(HomMat2D, HomMat2D_Inverted)
affine_trans_point_2d (HomMat2D_Inverted, r, c, Qx, Qy)
gen_circle (Circle, Qx, Qy, 10)
dev_set_color('green')
dev_display (Circle)
endfor
|