Skip to content

Commit e0befe3

Browse files
bredfeldtbredfeldt
committed
Now can live capture and create 3D locations
To do: clean up code, put repeated code into functions, put the raw images on the display rather than the blurred images, figure out a way to stream the images faster from the cameras (eliminate hard drive write and read if possible), add functionality to get 3D locations to the control server
1 parent 2e7c76d commit e0befe3

File tree

1 file changed

+244
-95
lines changed

1 file changed

+244
-95
lines changed

‎BlimpDetect-8-28_JB.py

Lines changed: 244 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,13 @@
22

33
# import system modules
44
import cv2.cv as cv
5-
#import cv
5+
import urllib
6+
import msvcrt
7+
import numpy as np
8+
from numpy import linalg as LA
9+
610
global imghsv
11+
global x_3d, y_3d, z_3d
712

813
# purpose: using HSV thresholds, detects blue, yellow and purple objects in a video stream in three new windows
914
# 1) a black/white stream showing objects matching threshold values (window "threshold")
@@ -21,6 +26,87 @@
2126

2227
# to modify color thresholds, change the cv.Scalar values in the InRange method in the gettresholdedimg function below
2328

29+
30+
#The following function takes coordinates from the images and convertes them to 3D spatial positions
31+
#The calibration constants are in R1, T1, R2, and T2 for cameras 1 (west) and 2 (east)
32+
#These constants are produced by matlab code that is available here:
33+
#http://www.vision.caltech.edu/bouguetj/calib_doc/
34+
def triang_3D(col_1, row_1, col_2, row_2) :
35+
global x_3d, y_3d, z_3d
36+
#R matrix for camera 1 (west side)
37+
R1 = np.array([[-74.2709, 637.41, -255.7461], [865.2027, 273.6518, -92.0415], [0.1602, 0.3172, -0.9347]])
38+
#T matrix for camera 1
39+
T1 = np.array([[1.3248e5], [4.1268e4], [505.0954]])
40+
P1 = np.hstack((R1, T1))
41+
42+
#R matrix for camera 2 (east side)
43+
R2 = np.array([[-20.0487, 179.5963, -666.7510], [751.5431, -397.57, -330.23], [-0.2329, -0.5675, -0.7898]])
44+
#T matrix for camera 2
45+
T2 = np.array([[3.7547e5], [3.3423e5], [907.6034]])
46+
P2 = np.hstack((R2, T2))
47+
48+
#blimp position from camera 1
49+
#col_1 = 411
50+
#row_1 = 382
51+
#m1 = np.array([
52+
#blimp position from camera 2
53+
#col_2 = 531
54+
#row_2 = 178
55+
56+
57+
#translated from matlab:
58+
59+
#Camera 1
60+
invR1 = LA.inv(R1)
61+
m1T1 = -1*T1
62+
C1 = np.dot(invR1, m1T1)
63+
x0 = C1[0]
64+
y0 = C1[1]
65+
z0 = C1[2]
66+
m1 = np.array([[col_1], [row_1], [1]]);
67+
M1 = np.dot(LA.pinv(P1), m1)
68+
x = M1[0]/M1[3]
69+
y = M1[1]/M1[3]
70+
z = M1[2]/M1[3]
71+
a = x-x0
72+
b = y-y0
73+
c = z-z0
74+
75+
#Camera 2
76+
invR2 = LA.inv(R2)
77+
m1T2 = -1*T2
78+
C2 = np.dot(invR2, m1T2)
79+
x1 = C2[0]
80+
y1 = C2[1]
81+
z1 = C2[2]
82+
m2 = np.array([[col_2], [row_2], [1]]);
83+
M2 = np.dot(LA.pinv(P2), m2)
84+
x = M2[0]/M2[3]
85+
y = M2[1]/M2[3]
86+
z = M2[2]/M2[3]
87+
d = x-x1
88+
e = y-y1
89+
f = z-z1
90+
91+
A11 = (a*a + b*b + c*c)
92+
A12 = -1*(a*d + e*b + f*c)
93+
A21 = -1*(a*d + e*b + f*c)
94+
A22 = d*d + e*e + f*f
95+
A = np.array([[A11, A12], [A21, A22]])
96+
A = np.squeeze(A) #get rid of 3rd dimension
97+
v = np.array([[(x1-x0)*a + (y1-y0)*b + (z1-z0)*c], [(x0-x1)*d + (y0-y1)*e + (z0-z1)*f]])
98+
v = np.squeeze(v) #get rid of 3rd dimension
99+
invA = LA.inv(A)
100+
r = np.dot(invA,v)
101+
x_coord = x0+a*r[0]
102+
y_coord = y0+b*r[0]
103+
z_coord = z0+c*r[0]
104+
105+
x_3d = x_coord
106+
y_3d = y_coord
107+
z_3d = z_coord
108+
109+
24110
def getthresholdedimg(im):
25111

26112
# this function take RGB image.Then convert it into HSV for easy colour detection
@@ -48,107 +134,170 @@ def getthresholdedimg(im):
48134

49135
return imgthreshold
50136

51-
# capture from m4v
52-
#capture = cv.CaptureFromFile('/Users/glenaronson/Desktop/blimp_large.m4v')
53-
capture = cv.CaptureFromFile('C:/Users/Jeremy/Documents/GitHub/blimp_blobs/blimp.m4v')
54-
#capture = cv.CaptureFromFile("C:\Users\Jeremy\Documents\GitHub\blimp_blobs\blimp.m4v")
55-
#capture = cv.CaptureFromFile('http://10.129.20.11/snapshot/view0.jpg')
56-
#capture = cv.CaptureFromFile('rtsp://10.129.20.11/video.sav')
57-
#http://10.129.20.11/goform/stream?cmd=get&channel=0
58-
#capture = cv.CaptureFromCAM(0)
59-
#capture = cv.CaptureFromFile("C:/Users/Jeremy/Documents/GitHub/blimp_blobs/Untitled 13.avi")
60-
#capture = cv.CaptureFromFile("Untitled 13.avi")
61-
frame = cv.QueryFrame(capture)
62-
frame_size = cv.GetSize(frame)
63-
64-
#frame = cv.LoadImageM("C:/Users/Jeremy/Documents/GitHub/blimp_blobs/test_images/SmallBlimp/(10.129.20.12)_1_2012_08_21_04_12_07.jpg")
65-
#frame_size = cv.GetSize(frame)
137+
138+
#!!Need to be on the local WID network to be able to grab images from the cameras
139+
#grab a frame from the east camera, store it to disk
140+
fname_east = 'C://Users//Jeremy//Documents//blimp//east.jpg'
141+
url_east = 'http://10.129.20.11/snapshot/view0.jpg'
142+
urllib.urlretrieve(url_east,fname_east)
143+
144+
#grab a frame from the west camera, store it to disk
145+
fname_west = 'C://Users//Jeremy//Documents//blimp//west.jpg'
146+
url_west = 'http://10.129.20.12/snapshot/view0.jpg'
147+
urllib.urlretrieve(url_west,fname_west)
148+
149+
frame_west = cv.LoadImageM(fname_west,cv.CV_LOAD_IMAGE_COLOR);
150+
frame_size = cv.GetSize(frame_west)
66151

67152
# blank images to which images are added later
68-
test = cv.CreateImage(cv.GetSize(frame),8,3)
69-
img2 = cv.CreateImage(cv.GetSize(frame),8,3)
153+
test = cv.CreateImage(cv.GetSize(frame_west),8,3)
154+
img2 = cv.CreateImage(cv.GetSize(frame_west),8,3)
70155

71156
# three windows that will open upon execution
72-
cv.NamedWindow("Real",0)
157+
cv.NamedWindow("west",cv.CV_WINDOW_AUTOSIZE)
158+
cv.NamedWindow("east",cv.CV_WINDOW_AUTOSIZE)
73159

74160
# blank lists to store coordinates of blue blob
75161
blue = []
76162

77-
78163
while(1):
79-
# captures feed from video in color
80-
color_image = cv.QueryFrame(capture)
81-
#color_image = frame
82-
83-
# ??
84-
imdraw = cv.CreateImage(cv.GetSize(frame), 8, 3)
85-
86-
# ??
87-
cv.SetZero(imdraw)
88-
cv.Flip(color_image,color_image, 1)
89-
cv.Smooth(color_image, color_image, cv.CV_GAUSSIAN, 3, 0)
90-
# ??
91-
imgbluethresh = getthresholdedimg(color_image)
92-
cv.Erode(imgbluethresh, imgbluethresh, None, 3)
93-
cv.Dilate(imgbluethresh, imgbluethresh, None, 10)
94-
# ??
95-
img2 = cv.CloneImage(imgbluethresh)
96-
# ??
97-
storage = cv.CreateMemStorage(0)
98-
contour = cv.FindContours(imgbluethresh, storage, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
99-
100-
# blank list into which points for bounding rectangles around blobs are appended
101-
points = []
102-
103-
# this is the new part here. ie use of cv.BoundingRect()
104-
while contour:
105-
106-
# Draw bounding rectangles
107-
bound_rect = cv.BoundingRect(list(contour))
108-
contour = contour.h_next()
109-
#print contour # not sure why print contour
110-
111-
# for more details about cv.BoundingRect,see documentation
112-
pt1 = (bound_rect[0], bound_rect[1])
113-
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
114-
points.append(pt1)
115-
points.append(pt2)
116-
cv.Rectangle(color_image, pt1, pt2, cv.CV_RGB(255,0,0), 1)
117-
118-
# calculating centroids
119-
centroidx = cv.Round((pt1[0]+pt2[0])/2)
120-
centroidy = cv.Round((pt1[1]+pt2[1])/2)
121-
122-
# identifying if blue blobs exist and adding centroids to corresponding lists.
123-
# note that the lower and upper bounds correspond to the the lower and upper bounds
124-
# in the getthresholdedimg(im): function earlier in the script.
125-
# e.g., yellow has a lowerbound of 95 and upper bound of 115 in both sections of code
126-
if (55 < cv.Get2D(imghsv,centroidy,centroidx)[0] < 155):
127-
blue.append((centroidx,centroidy))
128-
129-
# draw colors in windows; exception handling is used to avoid IndexError.
130-
# after drawing is over, centroid from previous part is removed from list by pop.
131-
# so in next frame, centroids in this frame become initial points of line to draw
132-
133-
# draw blue box around blue blimp blob
134-
try:
135-
cv.Circle(imdraw, blue[1], 5, (255,0,0))
136-
cv.Line(imdraw, blue[0], blue[1], (255,0,0), 3, 8, 60)
137-
blue.pop(0)
138-
print("centroid x:" + str(centroidx))
139-
print("centroid y:" + str(centroidy))
140-
print("")
141-
except IndexError:
142-
print "no blimp detected"
143-
144-
145-
# adds
146-
cv.Add(test,imdraw,test)
147-
148-
# display windows previously created
149-
cv.ShowImage("Real", color_image)
150-
151-
if cv.WaitKey(33) == 1048603:
152-
cv.DestroyWindow("Real")
153-
break
164+
urllib.urlretrieve(url_west,fname_west)
165+
urllib.urlretrieve(url_east,fname_east)
166+
167+
frame_west = cv.LoadImageM(fname_west,cv.CV_LOAD_IMAGE_COLOR);
168+
frame_east = cv.LoadImageM(fname_east,cv.CV_LOAD_IMAGE_COLOR);
169+
170+
imdraw_west = cv.CreateImage(cv.GetSize(frame_west), 8, 3)
171+
imdraw_east = cv.CreateImage(cv.GetSize(frame_east), 8, 3)
172+
173+
#process west image
174+
cv.SetZero(imdraw_west)
175+
cv.Smooth(frame_west, frame_west, cv.CV_GAUSSIAN, 3, 0)
176+
imgbluethresh = getthresholdedimg(frame_west)
177+
cv.Erode(imgbluethresh, imgbluethresh, None, 3)
178+
cv.Dilate(imgbluethresh, imgbluethresh, None, 10)
179+
img2 = cv.CloneImage(imgbluethresh)
180+
storage = cv.CreateMemStorage(0)
181+
contour = cv.FindContours(imgbluethresh, storage, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
182+
# blank list into which points for bounding rectangles around blobs are appended
183+
points = []
184+
while contour:
185+
186+
# Draw bounding rectangles
187+
bound_rect = cv.BoundingRect(list(contour))
188+
contour = contour.h_next()
189+
#print contour # not sure why print contour
190+
191+
# for more details about cv.BoundingRect,see documentation
192+
pt1 = (bound_rect[0], bound_rect[1])
193+
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
194+
points.append(pt1)
195+
points.append(pt2)
196+
cv.Rectangle(frame_west, pt1, pt2, cv.CV_RGB(255,0,0), 1)
197+
198+
# calculating centroids
199+
centroidx = cv.Round((pt1[0]+pt2[0])/2)
200+
centroidy = cv.Round((pt1[1]+pt2[1])/2)
201+
202+
# identifying if blue blobs exist and adding centroids to corresponding lists.
203+
# note that the lower and upper bounds correspond to the the lower and upper bounds
204+
# in the getthresholdedimg(im): function earlier in the script.
205+
# e.g., yellow has a lowerbound of 95 and upper bound of 115 in both sections of code
206+
if (55 < cv.Get2D(imghsv,centroidy,centroidx)[0] < 155):
207+
blue.append((centroidx,centroidy))
208+
209+
# draw colors in windows; exception handling is used to avoid IndexError.
210+
# after drawing is over, centroid from previous part is removed from list by pop.
211+
# so in next frame, centroids in this frame become initial points of line to draw
212+
213+
# draw blue box around blue blimp blob
214+
try:
215+
cv.Circle(imdraw_west, blue[1], 5, (255,0,0))
216+
cv.Line(imdraw_west, blue[0], blue[1], (255,0,0), 3, 8, 0)
217+
blue.pop(0)
218+
print("west centroid x:" + str(centroidx))
219+
print("west centroid y:" + str(centroidy))
220+
print("")
221+
except IndexError:
222+
print "no blimp detected"
223+
224+
225+
# adds
226+
cv.Add(test,imdraw_west,test)
227+
228+
centx_west = centroidx
229+
centy_west = centroidy
230+
231+
232+
#process east image
233+
cv.SetZero(imdraw_east)
234+
cv.Smooth(frame_east, frame_east, cv.CV_GAUSSIAN, 3, 0)
235+
imgbluethresh = getthresholdedimg(frame_east)
236+
cv.Erode(imgbluethresh, imgbluethresh, None, 3)
237+
cv.Dilate(imgbluethresh, imgbluethresh, None, 10)
238+
img2 = cv.CloneImage(imgbluethresh)
239+
storage = cv.CreateMemStorage(0)
240+
contour = cv.FindContours(imgbluethresh, storage, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
241+
# blank list into which points for bounding rectangles around blobs are appended
242+
points = []
243+
while contour:
244+
245+
# Draw bounding rectangles
246+
bound_rect = cv.BoundingRect(list(contour))
247+
contour = contour.h_next()
248+
#print contour # not sure why print contour
249+
250+
# for more details about cv.BoundingRect,see documentation
251+
pt1 = (bound_rect[0], bound_rect[1])
252+
pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
253+
points.append(pt1)
254+
points.append(pt2)
255+
cv.Rectangle(frame_east, pt1, pt2, cv.CV_RGB(255,0,0), 1)
256+
257+
# calculating centroids
258+
centroidx = cv.Round((pt1[0]+pt2[0])/2)
259+
centroidy = cv.Round((pt1[1]+pt2[1])/2)
260+
261+
# identifying if blue blobs exist and adding centroids to corresponding lists.
262+
# note that the lower and upper bounds correspond to the the lower and upper bounds
263+
# in the getthresholdedimg(im): function earlier in the script.
264+
# e.g., yellow has a lowerbound of 95 and upper bound of 115 in both sections of code
265+
if (55 < cv.Get2D(imghsv,centroidy,centroidx)[0] < 155):
266+
blue.append((centroidx,centroidy))
267+
268+
# draw colors in windows; exception handling is used to avoid IndexError.
269+
# after drawing is over, centroid from previous part is removed from list by pop.
270+
# so in next frame, centroids in this frame become initial points of line to draw
271+
272+
# draw blue box around blue blimp blob
273+
try:
274+
cv.Circle(imdraw_east, blue[1], 5, (255,0,0))
275+
cv.Line(imdraw_east, blue[0], blue[1], (255,0,0), 3, 8, 0)
276+
blue.pop(0)
277+
print("east centroid x:" + str(centroidx))
278+
print("east centroid y:" + str(centroidy))
279+
print("")
280+
except IndexError:
281+
print "no blimp detected"
282+
283+
284+
# adds
285+
cv.Add(test,imdraw_east,test)
286+
287+
centx_east = centroidx
288+
centy_east = centroidy
289+
290+
cv.ShowImage("west", frame_west)
291+
cv.WaitKey(100)
292+
293+
294+
cv.ShowImage("east", frame_east)
295+
cv.WaitKey(100)
296+
297+
triang_3D(centx_west, centy_west, centx_east, centy_east)
298+
299+
print("x_3d: " + str(x_3d))
300+
print("y_3d: " + str(y_3d))
301+
print("z_3d: " + str(z_3d))
302+
154303
######################################################

0 commit comments

Comments
 (0)