本文实例为大家分享了python实现Nao机器人单目测距的具体代码，供大家参考，具体内容如下
此代码适于用做对Nao机器人做视觉识别和测距实验,只提供关键代码部分,尝试利用cv2去优化代码会更加简洁哟！
此代码的主要功能：
1.初始姿态下，通过更换摄像头和转头去寻找目标
2.通过颜色阈值识别目标，计算目标与Nao的距离和角度
可以扩展功能：
1.在运动过程中对方向和距离进行多次测量和校正，提高准确度
2.找到目标后，通过对目标的测量，选择使用哪个脚去踢目标
|
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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
|
#!/usr/bin/python2.7#-*- encoding: UTF-8 -*-import vision_definitions#----------------------单目测距--------------------------------#***********************************************#@函数名： DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)#@参数： (cxnum,rynum)是通过图像识别得到球心的像素点坐标# (colsum,rowsum)是图片总大小：640*480# cameraID=0，取上摄像头；cameraID=1，取下摄像头#@返回值： 无#@功能说明： 采用机器人的下摄像头进行测量球离机器人的相关角度与距离def DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID): distx=-(cxnum-colsum/2) disty=rynum-rowsu/2 print distx,disty Picture_angle=disty*47.64/480 if cameraID ==0: h=0.62 Camera_angle=12 else: h=0.57 Camera_angle=38 #Head_angle[0]机器人仰俯角度 Total_angle=math.pi*(Picture_angle+Camera_angle)/180+Head_angle[0] d1=h/math.tan(Total_angle) alpha=math.pi*(distx*60.92/640)/180 d2=d1/math.cos(alpha) #Head_angle[1]机器人左右角度 Forward_Distance=d2*math.cos(alpha+Head_angle[1]) Sideward_Distance=-d2*math.sin(alpha+Head_angle[1])#***********************************************#@函数名： GetNaoImage(IP,PORT,cameraID)#@参数： 略#@返回值： 无#@功能说明： 采调用机器人内置摄像头控制模块，对当前场景进行拍摄并保持。# 由于球距离机器人约小于0.6m时，机器人额头摄像头无法看到，# 所以需要变换摄像头，cameraID=0，取上摄像头；# cameraID=1，取下摄像头def Get NaoImage(IP,PORT,cameraID): camProxy=ALProxy("ALVideoDevice",IP,PORT) resolition=2 #VGA格式640*480 colorSpace=11#RGB #选择并启用摄像头 camProxy.setParam(vision_definitions.kCameraSelectID,cameraID) videoClient=camProxy.subscribe("python_client",resolition,colorSpace,5) #获取摄像机图像。 #image [6]包含以ASCII字符数组形式传递的图像数据。 naoImage=camProxy.getImageRemote(videoClient) camProxy.unsubscribe(videoClient) #获取图像大小和像素阵列。 imageWidth=naoImage[0] imageHeight=naoImage[1] array=naoImage[6] #从我们的像素阵列创建一个PIL图像。 im=Image.fromstring("RGB",(imageWidth,imageHeight),array) #保存图像。 im.save("temp.jpg","JPEG")#***********************************************#@函数名： findColorPattern(img,pattern)#@参数： 略#@返回值： 无#@功能说明： 将RGB图像转化为二值图：此方法用的是cv,可以尝试用cv2代码会更加简洁def findColorPattern(img,pattern): channels=[None,None,None] channels[0]=cv.CreateImage(cv.GetSize(img),8,1) channels[1]=cv.CreateImage(cv.GetSize(img),8,1) channels[2]=cv.CreateImage(cv.GetSize(img),8,1) ch0=cv.CreateImage(cv.GetSize(img),8,1) ch1=cv.CreateImage(cv.GetSize(img),8,1) ch2=cv.CreateImage(cv.GetSize(img),8,1) cv.Split(img,ch0,ch1,ch2,None) dest=[None,None,None,None] dest[0]=cv.CreateImage(cv.GetSize(img),8,1) dest[1]=cv.CreateImage(cv.GetSize(img),8,1) dest[2]=cv.CreateImage(cv.GetSize(img),8,1) dest[3]=cv.CreateImage(cv.GetSize(img),8,1) cv.Smooth(ch0,channels[0],cv.CV_GAUSSIAN,3,3,0) cv.Smooth(ch1,channels[1],cv.CV_GAUSSIAN,3,3,0) cv.Smooth(ch2,channels[2],cv.CV_GAUSSIAN,3,3,0) for i in range(3): k=2-i lower=pattern[k]-75#设置阈值 upper=pattern[k]+75 cv.InRangeS(channels[i],lower,upper,dest[i]) cv.And(dest[0],dest[1],dest[3]) temp=cv.CreateImage(cv.GetSize(img),8,1) cv.And(dest[2],dest[3],temp) ''' cv.NameWindow("result",cv.CV_WINDOW_AUTOSIZE) cv.ShowImage("result",temp) cv.WaitKey(0) ''' return temp#***********************************************#@函数名： xyProject(matrix,imgaesize)#@参数： matrix# imgaesize#@返回值： 无#@功能说明： 利用二值图，计算球的像素坐标。其原理是：遍历各行各列# 像素的数值的和，最大的组合即为球心坐标def xyProject(matrix,imagesize): #声明一个数据类型为8位型单通道的imagessize[1]*1/1*imagessize[0]矩阵(初始值为 0)。 colmask=cv.CreateMat(imagessize[1],1,cv.CV_8UC1) rowmask=cv.CreateMat(1,imagessize[0],cv.CV_8UC1) cv.Set(colmask,1) cv.Set(rowmask,1) colsum=[] for i in range(imagesize[0]): col=cv.GetCol(matrix,i) #计算向量点积 a=cv.DotProduct(colmask,col) colsum.append(a) rowsum=[] for i in range(imagesize[1]): row=cv.GetRow(matrix,i) a=cv.DotProduct(rowmask,row) rowsum.append(a) return(colsum,rowsum)#得到各行各列“1”值的和def crMax(colsum,rowsum): cx=max(colsum) ry=max(rowsum) for i in range(len(colsum)): if colsum[i]==cx: cxnum=i for i in range(len(rowsum)): if rowsum[i]==ry: rynum=i return(cxnum,rynum)#***********************************************#@函数名： GetHeadAngles(robotIP,PORT)#@参数： 略#@返回值： 无#@功能说明：def GetHeadAngles(robotIP,PORT): motionProxy=ALProxy("ALMotion",robotIP,PORT) names=["HeadPitch","HeadYaw"] useSensors=1 sensorAngles=motionProxy.getAngles(names,useSensors) return sensorAngles#***********************************************#@函数名： SetHeadAngles(robotIP,PORT,angles)#@参数： 略#@返回值： 无#@功能说明：def SetHeadAngles(robotIP,PORT,angles): motionProxy=ALProxy("ALMotion",robotIP,PORT) motionProxy.setStiffnesses("Head",1.0) names=["HeadPitch","HeadYaw"] fractionMaxSpeed=0.2 motionProxy.setAngles(names,angles,fractionMaxSpeed) time.sleep(2.0) motionProxy.setStiffnesses("Head",0.0)#***********************************************#@函数名： Capture_Picture(IP,PORT,cameraID,angles,pattern_colors)#@参数： angles# pattern_colors#@返回值： 无#@功能说明： 将上面的一系列函数整合起来def Capture_Picture(IP,PORT,cameraID,angles,pattern_colors): SetHeadAngles(IP,PORT,angles) GetNaoImage(IP,PORT,cameraID) image=cv.LoadImage("temp.jpg") imagesize=cv.GetSize(image) #返回数值，两个元素分别为列数和行数 matrix=findColorPattern(image,pattern_colors) (colsum,rowsum)=xyProject(matrix,imagesize) (cxnum,rynum)=crMax(colsum,rowsum) cv.SaveImage("result.jpg",matrix) return (cxnum,rynum,colsum,rowsum) #***********************************************#@函数名： Target_Detect_and_Distance(IP,PORT)#@参数：#@返回值： 无#@功能说明： 当上摄像头无法找到球时，切换到下摄像头，然后在左转右转。# 在这个过程中，如果发现目标，则计算距离并输出距离# 若始终未找到目标，则输出距离为0。def Target_Detect_and_Distance(IP,PORT): pattern_colors=(255,150,50) cameraID=0# 默认上摄像头 angles=[0,0] (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles) if(cxnum,rynum)==(639,479): cameraID=1 (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles) if(cxnum,rynum)==(639,479): cameraID=0 angles=[0.0.7] (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles) if(cxnum,rynum)==(639,479): cameraID=0 angles=[0,-0.7] (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles) HeadAngles-GetHeadAngles(IP,PORT) ############### (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID) if(cxnum,rynum)==(639,479): (Forward_Distance,Sideward_Distance)=(0,0) print "Forward_Distance=",Forward_Distance,"meters" print "Sideward_Distance="+Sideward_Distance+"meters"#***********************************************#@函数名： Target_Detect_and_Distance(IP,PORT)#@参数：#@返回值： 无#@功能说明： 当找到球后，可能会存在一定的误差。# 因此需要判断球位于机器人前方的哪一侧，再来确定用哪只脚踢球def Final_See(robotIP,PORT): pattern_colors=(255,150,50) angles=[0.5,0] SetHeadAngles(robotIP,PORT,angles) cameraID=1 GetNaoImage(robotIP,PORT,cameraID) image=cv.LoadImage("temp.jpg") imagesize=cv.GetNaoImage(image) matrix=findColorPattern(image,pattern_colors) (colsum,rowsum)=xyProject(matrix,imgaesize) (cxnum,rynum)=crMax(colsum,rowsum) cv.SaveImage("result.jpg",matrix) HeadAngles=GetHeadAngles(robotIP,PORT) ######################### (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID) if cxnum<len(colsum)/2: side=0#左脚 else: side=1#右脚 print "side=",side print "last distance=",Forward_Distance return (side,Forward_Distance) |
以上就是本文的全部内容，希望对大家的学习有所帮助，也希望大家多多支持服务器之家。
原文链接：https://blog.csdn.net/qazwsx0316/article/details/105164732
