class Camera(object):
def __init__(self, calib_img, resolution, framerate):
self.stream = PiVideoStream(resolution=resolution, framerate=framerate)
calib = undistort(cv2.imread(calib_img))
self.transform = birdseye_transform(calib)
def start(self):
self.stream.start()
def stop(self):
self.stream.stop()
def get_line(self):
frame = self.stream.read()
test = undistort(frame)
bird = birdseye(test, transform=self.transform)
u = u_channel(bird)
filtered = sobel(u)
thresh = threshold(filtered)
lines, line_coeff = detect_lines(thresh)
# print("line:", p)
# print("curvature: {}, slope: {}, offset: {}".format(*line_coeff))
return line_coeff
class PiCam(BaseCamera):
def __init__(self, image_w=160, image_h=120, image_d=3, framerate=20):
from imutils.video.pivideostream import PiVideoStream #settings Pi camera
from imutils.video import FPS
self.image_d = image_d
self.frame = None
self.running = True
resolution = (image_w, image_h)
self.cap = PiVideoStream(resolution=resolution,
framerate=20) #rpi camera
self.cap.start()
self.cap.camera.iso = 0 # 0:auto, 100-200: sunny day
self.cap.camera.awb_mode = 'auto' # sunlight,cloudy,auto
self.cap.camera.hflip = False
self.cap.camera.vflip = False
print('PiCamera loaded.. .warming camera')
time.sleep(2.0)
print("analog_gain: ", float(self.cap.camera.analog_gain))
print("exposure_speed: ", self.cap.camera.exposure_speed)
print("iso: ", self.cap.camera.iso)
def poll(self):
self.frame = self.cap.read()
if self.image_d == 1:
self.frame = rgb2gray(self.frame)
def update(self):
'''
poll the camera for a frame
'''
while (self.running):
self.poll()
def run(self):
self.poll()
return self.frame
# def run_threaded(self):
# return self.frame
def shutdown(self):
self.running = False
time.sleep(0.2)
self.cap.stop()
def acquire_numpy_array_old(duration=8):
from imutils.video.pivideostream import PiVideoStream
fps = 90
vs = PiVideoStream(resolution=(640, 480), framerate=fps).start()
vs.camera.shutter_speed = 4000
sleep(2)
vs.start()
save_dir = '/home/pi/NumpyData/' + time_stamp()
os.makedirs(save_dir)
print("Acquiring + saving uncompressed video ... ")
example_frame = vs.read()
assert example_frame.size
frames_shape = [fps * duration] + list(example_frame.shape)
frames = np.empty(frames_shape, dtype=np.uint8)
for i in range(fps * duration):
last_frame_time = time()
frames[i] = vs.read()
sleep(1.0 / fps - (time() - last_frame_time))
vs.stop()
np.save(save_dir + '/data.npy', frames)
class StingGui(object):
buttonPins = {p: i for i, p in enumerate((17, 22, 23, 27))}
framerate = 90
screenWidth = 320
screenHeight = 240
blue = (0xff, 0, 0)
green = (0, 0xa0, 0)
red = (0, 0, 0xff)
faceTimeout = 1.0
logoTimeout = 5.0
collectionId = 'sturdy-sting'
def __init__(self, showLogo=True):
self.ready = False
self.menu = generateMenus(self)
self._oldMenu = None
self.toast = None
self._oldToast = None
self._oldButton = None
self.running = True
self.fireEnable = True
self.addWhiteListFlag = False
self.removeWhiteListFlag = False
self.showLogo = showLogo
self.startTime = time.time()
self.logoImage = cv2.imread('logo.png')
self.faceCascade = cv2.CascadeClassifier('face_cascade.xml')
self.videoStream = VideoStream(framerate=self.framerate,
resolution=(self.screenWidth,
self.screenHeight))
self.isEnemy = False
self.rekClient = boto3.client('rekognition')
def __enter__(self):
self.ready = True
self.videoStream.start()
GPIO.setmode(GPIO.BCM)
for buttonPin in self.buttonPins.keys():
GPIO.setup(buttonPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(buttonPin,
GPIO.FALLING,
callback=self.buttonPressed,
bouncetime=50)
cv2.namedWindow('disp', cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty('disp', cv2.WND_PROP_FULLSCREEN,
cv2.WINDOW_FULLSCREEN)
return self
def __exit__(self, *args):
self.videoStream.stop()
for buttonPin in self.buttonPins.keys():
GPIO.remove_event_detect(buttonPin)
GPIO.cleanup(buttonPin)
def buttonPressed(self, buttonPin):
# The built-in debounce in rpi.GPIO isn't very good,
# so we add our own
time.sleep(.010)
pressed = not GPIO.input(buttonPin)
if pressed:
buttonIndex = self.buttonPins[buttonPin]
print 'button', buttonIndex
self.menu = self.menu.select(buttonIndex)
def stop(self):
self.running = False
def addOverlay(self, frame):
if self.showLogo:
delta = time.time() - self.startTime
if delta > self.logoTimeout:
self.showLogo = False
else:
weight = (self.logoTimeout - delta) / self.logoTimeout
return cv2.addWeighted(frame, 1.0, self.logoImage, weight, 0)
if self.toast:
self.toast = self.toast.checkTimeout()
if self.isEnemy and (not self.toast or not self.toast.priority):
self.toast = Toast('TARGET ACQUIRED',
color=self.red,
timeout=1,
priority=True)
self.screenShotFrames = 5
elif not self.toast:
self.toast = Toast(
'Weapon Hot' if self.fireEnable else 'Weapon Safe', 0)
menuChanged = self.menu != self._oldMenu
toastChanged = self.toast != self._oldToast
if menuChanged or toastChanged:
self._oldMenu = self.menu
self._oldToast = self.toast
texts = ([x[0] for x in self.menu.options] + ['', '', '', ''])[:4]
heightStep = self.screenHeight / len(texts)
yOffset = heightStep / 2
thickness = 1
fontFace = cv2.FONT_HERSHEY_DUPLEX
fontScale = getattr(self.menu, 'fontScale', 1.0)
img = np.zeros((self.screenHeight, self.screenWidth, 3), np.uint8)
for i, text in enumerate(texts):
size, dummy = cv2.getTextSize(text, fontFace, fontScale,
thickness)
w, h = size
x = self.screenWidth - w - 5
y = i * heightStep + h + yOffset
cv2.putText(img, text, (x, y), fontFace, fontScale,
(255, 255, 255), thickness)
thickness = 2
if self.menu.title:
size, dummy = cv2.getTextSize(self.menu.title, fontFace,
fontScale, thickness)
w, h = size
x = 0
y = h
cv2.putText(img, self.menu.title, (x, y), fontFace, fontScale,
(255, 255, 255), thickness)
if self.toast:
fontScale = getattr(self.toast, 'fontScale', 1.0)
size, dummy = cv2.getTextSize(self.toast.text, fontFace,
fontScale, thickness)
x = 0
y = int(self.screenHeight - size[1] * 1.1)
cv2.putText(img, self.toast.text, (x, y), fontFace, fontScale,
self.toast.color, thickness)
self._menuImage = img
return cv2.add(frame, self._menuImage)
def getFrame(self):
frame = self.videoStream.read()
frame = cv2.flip(frame, -1) # rotate 180
return frame
def getFaces(self, frame):
''' Returns an array of tuples which are (faceImage, (x1, y1, x2, y2)) '''
ret = []
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
faceLocations = self.faceCascade.detectMultiScale(
gray,
scaleFactor=1.1,
minNeighbors=5,
minSize=(40, 40),
flags=cv2.CASCADE_SCALE_IMAGE)
for (x, y, w, h) in faceLocations:
cx, cy = int(x + w / 2), int(y + h / 2)
hw, hh = [int(q * 2) / 2 for q in (w, h)]
x1, x2 = cx - hw, cx + hw
y1, y2 = cy - hh, cy + hh
imgH, imgW = frame.shape[0:2]
x1, y1 = [max(q, 0) for q in (x1, y1)]
x2, y2 = [min(q, r) for q, r in ((x2, imgW), (y2, imgH))]
if (x2 - x1) > 0 and (y2 - y1) > 0:
faceImage = copy.deepcopy(gray[y1:y2, x1:x2])
ret.append((faceImage, (x1, y1, x2, y2)))
return ret
def drawBoxesAroundFaces(self, frame, faces):
color = self.red if self.isEnemy else self.blue
if self.isEnemy:
color = self.red
for face, box in faces:
x1, y1, x2, y2 = box
w = x2 - x1
h = y2 - y1
centerX, centerY = center = (x1 + w / 2, y1 + h / 2)
radius = int(max([q * .4 for q in (w, h)]))
cv2.circle(frame, center, radius, color, 2)
cv2.line(frame, (x1, centerY), (x2, centerY), color, 2)
cv2.line(frame, (centerX, y1), (centerX, y2), color, 2)
else:
color = self.blue
for face, box in faces:
cv2.rectangle(frame, box[0:2], box[2:4], color, 2)
def mainLoop(self):
if not self.ready:
raise RuntimeError('Class not in ready state use "with" statement')
frameTries = 5
lastFaceFoundTime = 0
faceFound = False
faceId = None
frameTriesRemaining = 0
faceRek = None
loopStart = time.time()
with Sting() as sting:
while self.running:
loopElapsed = time.time() - loopStart
if loopElapsed > .2:
print 'loop took %d milliseconds' % (loopElapsed * 1000)
loopStart = time.time()
frame = self.getFrame()
faces = self.getFaces(frame)
self.drawBoxesAroundFaces(frame, faces)
faceThisFrame = len(faces) > 0
if faceThisFrame:
if faceFound == False:
if not self.addWhiteListFlag:
if faceRek:
faceRek.destroy()
faceRek = FaceRekognizer(self.collectionId)
frameTriesRemaining = frameTries
faceId = None
self.isEnemy = False
faceFound = True
lastFaceFoundTime = time.time()
faceImage = faces[0][0]
# Identify, Friend or Foe?
if frameTriesRemaining and faceRek:
frameTriesRemaining -= 1
faceRek.addImage(faceImage, frameTriesRemaining > 0)
if self.addWhiteListFlag:
jpg = imgToJpg(faceImage)
args = {
'CollectionId': self.collectionId,
'Image': {
'Bytes': jpg
}
}
try:
ret = self.rekClient.index_faces(**args)
self.toast = Toast('Friend added', 3)
self.menu = self.menu.select(GoBack())
faceId = None
faceFound = False
isEnemy = False
except:
pass
elif self.removeWhiteListFlag and faceId:
ret = self.rekClient.delete_faces(
CollectionId=self.collectionId, FaceIds=[faceId])
print 'called delete_faces'
self.toast = Toast('Friend removed', 3)
self.menu = self.menu.select(GoBack())
faceId = None
faceFound = False
isEnemy = True
# Coast for a bit if we don't find a faces for a few frames
elif faceFound and time.time(
) - lastFaceFoundTime > self.faceTimeout:
self.isEnemy = False
faceFound = False
faceId = None
if faceRek and faceRek.finished:
faceId = faceRek.getResult()
self.isEnemy = not faceId
faceRek.destroy()
faceRek = None
if faceRek:
print 'Checking \r',
else:
if faceFound:
if self.isEnemy:
print 'Enemy \r',
else:
print 'Friend \r',
else:
print 'idle \r',
sys.stdout.flush()
# Where the rubber meets the road
if self.fireEnable and faceFound:
if self.isEnemy or (faceRek and not faceRek.finished):
sting.flywheel = True
if self.isEnemy:
sting.trigger = True
else:
sting.trigger = False
sting.flywheel = False
else:
sting.trigger = False
sting.flywheel = False
# Update display
frame = self.addOverlay(frame)
cv2.imshow('disp', frame)
cv2.waitKey(2)
for i in range(LCD_WIDTH):
lcd_byte(ord(message[i]), LCD_CHR)
lcd_string(" No Intruder", LCD_LINE_1)
#################################################
recognizer = cv2.createLBPHFaceRecognizer()
recognizer.load('trainner/trainner.yml')
cascadePath = "haarcascade_frontalface_default.xml"
faceCascade = cv2.CascadeClassifier(cascadePath)
ap = argparse.ArgumentParser()
ap.add_argument("-p", "--picamera", type=int, default=1, help="jffj")
args = vars(ap.parse_args())
cam = PiVideoStream()
cam = cam.start()
time.sleep(.5)
font = cv2.cv.InitFont(cv2.cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 1, 1)
while (True):
i = 0
if i == 1: # When output from motion sensor is HIGH
print("Intruder Detected")
GPIO.output(4, 1) # Turn ON LED
time.sleep(.5)
GPIO.output(4, 0)
lcd_string("Intruder Detect", LCD_LINE_1)
lcd_string(" Room 1", LCD_LINE_2)
img = cam.read()
img = imutils.resize(img, width=450)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
faces = faceCascade.detectMultiScale(gray, 1.2, 5)
def ai():
cap = PiVideoStream(resolution=(208, 208)) #rpi camera
cap.start()
time.sleep(2.0)
cap.camera.hflip = True
cap.camera.vflip = True
print("[INFO] video recording")
## out = cv2.VideoWriter('avcnet.avi',cv2.VideoWriter_fourcc(*'XVID'), 10, (208,208))
# load the trained convolutional neural network
print("[INFO] loading network...")
## model = load_model("./avcnet_v1.model")
model = load_model("/home/pi/drone_exe/avcnet_best_5.hdf5",
custom_objects={"tf": tf})
# default parameters
orient = 0
tim_old = 0.1
state = 'fly'
dist = 510
global velocity_y
velocity_y = 0
fly_1 = 0.9
k = 0.83 # k=(tau/T)/(1+tau/T) tau time constant LPF, T period
while True:
start = time.time()
frame = cap.read()
orig = frame.copy()
frame = cv2.resize(frame, (64, 64))
frame = frame.astype("float") / 255.0
frame = img_to_array(frame)
frame = np.expand_dims(frame, axis=0)
# classify the input image
(stop, left, right, fly) = model.predict(frame)[0]
# build the label
## my_dict = {'stop':stop, 'left':left, 'right':right,'fly':fly}
my_dict = {'stop': stop, 'left': left, 'right': right}
maxPair = max(my_dict.iteritems(), key=itemgetter(1))
fly_f = k * fly_1 + (1 - k) * fly
fly_1 = fly_f
if state == 'avoid':
## label=maxPair[0]
## proba=maxPair[1]
if fly_f * 100 >= 60:
dist = 510
state = 'fly'
print >> f, state
else:
label = 'forward'
proba = fly
if fly_f * 100 <= 50:
dist = 180
label = maxPair[0]
proba = maxPair[1]
print >> f, my_dict
state = 'avoid'
label_1 = "{} {:.1f}% {}".format(label, proba * 100, state)
# draw the label on the image
output = imutils.resize(orig, width=208)
cv2.putText(output, label_1, (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
(0, 255, 0), 2)
# Write the frame into the file 'output.avi'
## out.write(output)
if vehicle.channels['8'] > 1700:
event.clear()
if vehicle.channels['8'] > 1400 and vehicle.channels['8'] < 1700:
if state == "fly":
event.clear()
if state == "avoid":
event.set()
if label == 'left':
velocity_y = -0.5
if label == 'right':
velocity_y = 0.5
if label == 'stop':
velocity_y = 0
if vehicle.channels['8'] < 1400:
event.clear()
msg_sensor(dist, orient)
# show the output frame
cv2.imshow("Frame", output)
key = cv2.waitKey(10) & 0xFF
# if the `Esc` key was pressed, break from the loop
if key == 27:
break
# do a bit of cleanup
f.close()
print('end')
cv2.destroyAllWindows()
cap.stop()
class DroidVisionThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.running = True
self.fps_counter = FPS().start()
self.camera = PiVideoStream(resolution=(config.RAW_FRAME_WIDTH, config.RAW_FRAME_HEIGHT))
self.camera.start()
time.sleep(config.CAMERA_WARMUP_TIME) # wait for camera to initialise
self.frame = None
self.frame_chroma = None
self.centre = config.CENTRE
self.can_see_lines = False
self.last_yellow_mean = config.WIDTH * 0.8
self.last_blue_mean = config.WIDTH * 0.2
self.desired_steering = config.NEUTRAL_STEERING # 0 = left, 0.5 = center, 1 = right
self.desired_throttle = config.NEUTRAL_THROTTLE # 0 = stop, 0.5 = medium speed, 1 = fastest
def run(self):
debug("DroidVisionThread: Thread started")
self.vision_processing()
self.camera.stop()
cv2.destroyAllWindows()
debug("DroidVisionThread: Thread stopped")
def stop(self):
self.running = False
debug("DroidVisionThread: Stopping thread")
def vision_processing(self):
while self.running:
self.grab_frame()
# colour mask
blue_mask = cv2.inRange(self.frame_chroma, config.BLUE_CHROMA_LOW, config.BLUE_CHROMA_HIGH)
yellow_mask = cv2.inRange(self.frame_chroma, config.YELLOW_CHROMA_LOW, config.YELLOW_CHROMA_HIGH)
colour_mask = cv2.bitwise_or(blue_mask, yellow_mask)
colour_mask = cv2.erode(colour_mask, config.ERODE_KERNEL)
colour_mask = cv2.dilate(colour_mask, config.DILATE_KERNEL)
# lines
lines = cv2.HoughLinesP(colour_mask, config.HOUGH_LIN_RES, config.HOUGH_ROT_RES, config.HOUGH_VOTES, config.HOUGH_MIN_LEN, config.HOUGH_MAX_GAP)
blue_lines = np.array([])
yellow_lines = np.array([])
if lines != None:
for line in lines:
x1,y1,x2,y2 = line[0]
angle = np.rad2deg(np.arctan2(y2-y1, x2-x1))
if config.MIN_LINE_ANGLE < abs(angle) < config.MAX_LINE_ANGLE:
if config.IMSHOW:
cv2.line(self.frame, (x1,y1), (x2,y2), (0,0,255), 1)
if angle > 0:
yellow_lines = np.append(yellow_lines, [x1, x2])
elif angle < 0:
blue_lines = np.append(blue_lines, [x1, x2])
# find centre
blue_mean = self.last_blue_mean
yellow_mean = self.last_yellow_mean
if len(blue_lines):
blue_mean = int(np.mean(blue_lines))
if len(yellow_lines):
yellow_mean = int(np.mean(yellow_lines))
self.centre = (blue_mean + yellow_mean) / 2.0
self.last_blue_mean = blue_mean
self.last_yellow_mean = yellow_mean
self.can_see_lines = (len(blue_lines) or len(yellow_lines))
# set steering and throttle
self.desired_steering = (1.0 - (self.centre / config.WIDTH))
if len(blue_lines) or len(yellow_lines):
self.desired_throttle = 0.22
else:
self.desired_throttle = 0
if config.IMSHOW:
cv2.circle(self.frame, (int(self.centre), config.HEIGHT - 20), 10, (0,0,255), -1)
cv2.imshow("colour_mask without noise", colour_mask)
cv2.imshow("raw frame", self.frame)
cv2.waitKey(1)
def grab_frame(self):
self.fps_counter.update()
# grab latest frame and index the ROI
self.frame = self.camera.read()
self.frame = self.frame[config.ROI_YMIN:config.ROI_YMAX, config.ROI_XMIN:config.ROI_XMAX]
# convert to chromaticity colourspace
B = self.frame[:, :, 0].astype(np.uint16)
G = self.frame[:, :, 1].astype(np.uint16)
R = self.frame[:, :, 2].astype(np.uint16)
Y = 255.0 / (B + G + R)
b = (B * Y).astype(np.uint8)
g = (G * Y).astype(np.uint8)
r = (R * Y).astype(np.uint8)
self.frame_chroma = cv2.merge((b,g,r))
def get_fps(self):
self.fps_counter.stop()
return self.fps_counter.fps()
def get_error(self):
return (config.CENTRE - self.centre)
def get_steering_throttle(self):
return self.desired_steering, self.desired_throttle
from imutils.video.pivideostream import PiVideoStream
from imutils.video import FPS # Pour mesurer les FPS
import cv2 # Librairie OpenCV
import time # Fonctions temporelles
# Definition des seuils HSV pour le vert
greenLower = (85, 126, 60)
greenUpper = (95, 255, 255)
# Creation de la fenetre d'affichage
cv2.namedWindow('Tracking', cv2.WINDOW_NORMAL)
# Demarage du flux video sur un thread different
vs = PiVideoStream()
vs.camera.video_stabilization = True
vs.start()
# Laisse le temps de chauffer a la camera
time.sleep(1.0)
# Demarre le compteur de FPS
fps = FPS().start()
# Boucle principale
while True :
frame = vs.read()
#frame = imutils.resize(frame, width = 400)
# Convertis la frame en HSV
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# Construis un masque pour la couleur définie
mask = cv2.inRange(hsv, greenLower, greenUpper)
# Effectue 2 passages d'erosion pour retirer les petits éléments
mask = cv2.erode(mask, None, iterations = 2)
# Effectue 2 passages de dilatation pour conserver la taille
mask = cv2.dilate(mask, None, iterations = 2)
# Detecte les contours dans le masque
def ai():
cap = PiVideoStream(resolution=(208, 208), framerate=20) #rpi camera
cap.start()
cap.camera.iso = 0 # 0:auto, 100-200: sunny day
cap.camera.awb_mode = 'sunlight' # sunlight,cloudy,auto
cap.camera.hflip = True
cap.camera.vflip = True
time.sleep(2.0)
print("analog_gain: ", float(cap.camera.analog_gain), file=f)
print("exposure_speed: ", cap.camera.exposure_speed, file=f)
print("iso: ", cap.camera.iso, file=f)
print("[INFO] video recording")
out = cv2.VideoWriter('avcnet.avi', cv2.VideoWriter_fourcc(*'XVID'), 4,
(208, 208))
# load the trained convolutional neural network
print("[INFO] loading network...")
print('inference:', inference)
print('inference:', inference, file=f)
if inference == 'k':
from keras.models import load_model
import tensorflow as tf
from keras.preprocessing.image import img_to_array
model = load_model("/home/pi/drone_exe/TF_model/fly1.h5",
custom_objects={"tf": tf})
if inference == 'tf':
from tensorflow.python.platform import gfile
import tensorflow as tf
from keras.preprocessing.image import img_to_array
f1 = gfile.FastGFile("/home/pi/drone_exe/TF_model/tf_model_cv.pb",
'rb')
graph_def = tf.GraphDef()
# Parses a serialized binary message into the current message.
graph_def.ParseFromString(f1.read())
f1.close()
sess = tf.Session()
sess.graph.as_default()
# Import a serialized TensorFlow `GraphDef` protocol buffer
# and place into the current default `Graph`.
tf.import_graph_def(graph_def)
softmax_tensor = sess.graph.get_tensor_by_name(
'import/activation_5/Softmax:0')
if inference == 'dnn':
net = cv2.dnn.readNetFromTensorflow(
'/home/pi/drone_exe/TF_model/fly1.pb')
## net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) # if no NCS stick
## net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) # if no NCS stick
net.setPreferableTarget(cv2.dnn.DNN_TARGET_MYRIAD)
# default parameters
orient = 0
distmax = 600
tim_old = 0.1
state = 'fly'
dist = 510
global velocity_y
velocity_y = 0
fly_1 = 0.9
k = 0.66 # k=(tau/T)/(1+tau/T) tau time constant LPF, T period
## k=0 # no filtering
fps = FPS().start()
while True:
start = time.time()
frame = cap.read()
orig = frame.copy()
##
#===========================dnn============================================
if inference == 'dnn':
# use cv2.dnn module for inference
resized = cv2.resize(frame, (64, 64))
scale = 1 / 255.0 # AI trained with scaled images image/255
blob = cv2.dnn.blobFromImage(resized,
scale, (64, 64), (0, 0, 0),
swapRB=True,
ddepth=5)
net.setInput(blob)
predictions = net.forward()
#===========================tf=============================================
if inference == 'tf':
#use tf for inference
frame = cv2.resize(frame, (64, 64))
frame = cv2.cvtColor(
frame, cv2.COLOR_RGB2BGR) # @ training images RGB->BGR
frame = frame.astype("float") / 255.0
frame = img_to_array(frame)
frame = np.expand_dims(frame, axis=0)
predictions = sess.run(softmax_tensor,
{'import/img_input:0': frame})
#===========================k==============================================
if inference == 'k':
#use k for inference
frame = cv2.resize(frame, (64, 64))
frame = cv2.cvtColor(
frame, cv2.COLOR_RGB2BGR) # @ training images RGB->BGR
frame = frame.astype("float") / 255.0
frame = img_to_array(frame)
frame = np.expand_dims(frame, axis=0)
predictions = model.predict(frame)
#===========================================================================
# classify the input image
fly = predictions[0][0]
# build the label
my_dict = {
'stop': predictions[0][3],
'left': predictions[0][1],
'right': predictions[0][2]
}
maxPair = max(my_dict.items(), key=itemgetter(1))
fly_f = k * fly_1 + (1 - k) * fly
fly_1 = fly_f
if state == 'avoid':
## label=maxPair[0]
## proba=maxPair[1]
if fly_f * 100 >= 60:
dist = 510
state = 'fly'
print(state, file=f)
else:
label = 'forward'
proba = fly
if fly_f * 100 <= 50:
dist = 180
label = maxPair[0]
proba = maxPair[1]
print(my_dict, file=f)
state = 'avoid'
label_1 = "{} {:.1f}% {}".format(label, proba * 100, state)
# draw the label on the image
output = imutils.resize(orig, width=208)
cv2.putText(output, label_1, (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
(0, 255, 0), 2)
# Write the frame into the file 'output.avi'
out.write(output)
if vehicle.channels['8'] > 1700:
event.clear()
if vehicle.channels['8'] > 1400 and vehicle.channels['8'] < 1700:
if state == "fly":
event.clear()
if state == "avoid":
event.set()
if label == 'left':
velocity_y = -0.8
if label == 'right':
velocity_y = 0.8
if label == 'stop':
velocity_y = 0
if vehicle.channels['8'] < 1400:
event.clear()
msg_sensor(dist, 0, 600)
# show the output frame
cv2.imshow("Frame", output)
key = cv2.waitKey(10) & 0xFF
# update the FPS counter
fps.update()
# if the `Esc` key was pressed, break from the loop
if key == 27:
break
# do a bit of cleanup
# stop the timer and save FPS information
fps.stop()
print("[INFO] elapsed time: {:.2f}".format(fps.elapsed()), file=f)
print("[INFO] approx. FPS: {:.2f}".format(fps.fps()), file=f)
f.close()
f.close()
print('end')
cv2.destroyAllWindows()
cap.stop()
out.release()
def get_video(): array,_ = freenect.sync_get_video() array = cv2.cvtColor(array,cv2.COLOR_RGB2BGR) return array #function to get depth image from kinect def get_depth(): array,_ = freenect.sync_get_depth() array = array.astype(np.uint8) #array = array * 32 return array #nitialize the camera and grab a reference to the raw camera capture #res = '(640, 480)' stream = PiVideoStream(resolution=(640,480)).start() stream.start() #camera = PiCamera() #camera.resolution = (640, 480) #camera.framerate = 30 # allow the camera to warmup time.sleep(1) if __name__ == "__main__": while 1: #get a frame from RGB camera frame = stream.read() #get a frame from depth sensor depth = get_depth()
class DroidVisionThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.running = True
self.fps_counter = FPS().start()
self.camera = PiVideoStream(resolution=(config.RAW_FRAME_WIDTH,
config.RAW_FRAME_HEIGHT))
self.camera.start()
time.sleep(config.CAMERA_WARMUP_TIME) # wait for camera to initialise
self.frame = None
self.frame_chroma = None
self.centre = config.CENTRE
self.can_see_lines = False
self.last_yellow_mean = config.WIDTH * 0.8
self.last_blue_mean = config.WIDTH * 0.2
self.desired_steering = config.NEUTRAL_STEERING # 0 = left, 0.5 = center, 1 = right
self.desired_throttle = config.NEUTRAL_THROTTLE # 0 = stop, 0.5 = medium speed, 1 = fastest
def run(self):
debug("DroidVisionThread: Thread started")
self.vision_processing()
self.camera.stop()
cv2.destroyAllWindows()
debug("DroidVisionThread: Thread stopped")
def stop(self):
self.running = False
debug("DroidVisionThread: Stopping thread")
def vision_processing(self):
while self.running:
self.grab_frame()
# colour mask
blue_mask = cv2.inRange(self.frame_chroma, config.BLUE_CHROMA_LOW,
config.BLUE_CHROMA_HIGH)
yellow_mask = cv2.inRange(self.frame_chroma,
config.YELLOW_CHROMA_LOW,
config.YELLOW_CHROMA_HIGH)
colour_mask = cv2.bitwise_or(blue_mask, yellow_mask)
colour_mask = cv2.erode(colour_mask, config.ERODE_KERNEL)
colour_mask = cv2.dilate(colour_mask, config.DILATE_KERNEL)
# lines
lines = cv2.HoughLinesP(colour_mask, config.HOUGH_LIN_RES,
config.HOUGH_ROT_RES, config.HOUGH_VOTES,
config.HOUGH_MIN_LEN, config.HOUGH_MAX_GAP)
blue_lines = np.array([])
yellow_lines = np.array([])
if lines != None:
for line in lines:
x1, y1, x2, y2 = line[0]
angle = np.rad2deg(np.arctan2(y2 - y1, x2 - x1))
if config.MIN_LINE_ANGLE < abs(
angle) < config.MAX_LINE_ANGLE:
if config.IMSHOW:
cv2.line(self.frame, (x1, y1), (x2, y2),
(0, 0, 255), 1)
if angle > 0:
yellow_lines = np.append(yellow_lines, [x1, x2])
elif angle < 0:
blue_lines = np.append(blue_lines, [x1, x2])
# find centre
blue_mean = self.last_blue_mean
yellow_mean = self.last_yellow_mean
if len(blue_lines):
blue_mean = int(np.mean(blue_lines))
if len(yellow_lines):
yellow_mean = int(np.mean(yellow_lines))
self.centre = (blue_mean + yellow_mean) / 2.0
self.last_blue_mean = blue_mean
self.last_yellow_mean = yellow_mean
self.can_see_lines = (len(blue_lines) or len(yellow_lines))
# set steering and throttle
self.desired_steering = (1.0 - (self.centre / config.WIDTH))
if len(blue_lines) or len(yellow_lines):
self.desired_throttle = 0.22
else:
self.desired_throttle = 0
if config.IMSHOW:
cv2.circle(self.frame, (int(self.centre), config.HEIGHT - 20),
10, (0, 0, 255), -1)
cv2.imshow("colour_mask without noise", colour_mask)
cv2.imshow("raw frame", self.frame)
cv2.waitKey(1)
def grab_frame(self):
self.fps_counter.update()
# grab latest frame and index the ROI
self.frame = self.camera.read()
self.frame = self.frame[config.ROI_YMIN:config.ROI_YMAX,
config.ROI_XMIN:config.ROI_XMAX]
# convert to chromaticity colourspace
B = self.frame[:, :, 0].astype(np.uint16)
G = self.frame[:, :, 1].astype(np.uint16)
R = self.frame[:, :, 2].astype(np.uint16)
Y = 255.0 / (B + G + R)
b = (B * Y).astype(np.uint8)
g = (G * Y).astype(np.uint8)
r = (R * Y).astype(np.uint8)
self.frame_chroma = cv2.merge((b, g, r))
def get_fps(self):
self.fps_counter.stop()
return self.fps_counter.fps()
def get_error(self):
return (config.CENTRE - self.centre)
def get_steering_throttle(self):
return self.desired_steering, self.desired_throttle
class Detector:
"""カメラ映像を読み込み、人体検知を用いたスクワット回数の計測を行う"""
def __init__(self,
model_path,
threshold=0.8,
resolution=(512, 352),
framerate=5,
head_line=80):
"""初期化処理"""
# True: 立っている状態, False: かがんでいる状態
self.is_standing = True
# is_standing の判定に使うしきい値
self.head_line = head_line
# カメラへの接続
self.camera = PiVideoStream(resolution=resolution, framerate=framerate)
# どのくらいの尤度で人として検知するか
self.threshold = threshold
# TFLiteモデルの読み込み
self.detector = tf.lite.Interpreter(model_path=model_path)
# TFLiteの初期化
self.detector.allocate_tensors()
self.detector.set_num_threads(4)
# モデルの入出力情報の取得
self.input_details = self.detector.get_input_details()
self.output_details = self.detector.get_output_details()
self.input_height = self.input_details[0]['shape'][1]
self.input_width = self.input_details[0]['shape'][2]
def start(self):
"""カメラ映像の読み込み開始"""
self.camera.start()
def stop(self):
"""カメラ映像の読み込みを終了"""
self.camera.stop()
def process_frame(self, count):
squatted = False
# 映像フレームの読み込み
frame = self.camera.read()
# 人体検知の実行
detection = self._detect(frame)
if detection:
# 検知結果をフレームに描画
self._draw_box(frame, detection)
# スクワットの判定
squatted = self._update_state(head_position=detection[4])
# head_line の位置を描画
cv2.line(frame, (0, self.head_line), (frame.shape[1], self.head_line),
(0, 255, 255), 2)
# is_standing 状態の描画
cv2.putText(frame, f'standing: {self.is_standing}', (20, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
# スクワット回数を描画
cv2.putText(frame, str(count), (5, 332), cv2.FONT_HERSHEY_SIMPLEX, 4,
(0, 255, 255), 2)
return squatted, frame
def _detect(self, frame):
"""人体検知の実行"""
# 映像フレームのサイズを取得
height, width, channels = frame.shape
# 入力データの用意
resized = cv2.resize(frame, (self.input_width, self.input_height))
data = np.expand_dims(resized, axis=0)
self.detector.set_tensor(self.input_details[0]['index'], data)
# 推論の実行
self.detector.invoke()
# 推論結果の取得
boxes = self.detector.get_tensor(self.output_details[0]['index'])
scores = self.detector.get_tensor(self.output_details[2]['index'])
num_boxes = self.detector.get_tensor(self.output_details[3]['index'])
if int(num_boxes) < 1:
return
# 最も尤度の高かった物体の情報のみ取得
top, left, bottom, right = boxes[0][0]
score = scores[0][0]
# 尤度が threshold 以下の場合は無視する
if score < self.threshold:
return
# 矩形座標をオリジナル画像用にスケール
xmin = int(left * width)
ymin = int(bottom * height)
xmax = int(right * width)
ymax = int(top * height)
return (score, xmin, ymin, xmax, ymax)
def _draw_box(self, frame, detection):
"""推論結果を映像フレームに描画"""
score, xmin, ymin, xmax, ymax = detection
# 検知した人の位置を描画
cv2.rectangle(frame, (xmin, ymin), (xmax, ymax), (0, 255, 0), 2)
# スクワットを行う際の目印となる点を描画
xcent = int(((xmax - xmin) // 2) + xmin)
cv2.circle(frame, (xcent, ymax), 3, (0, 0, 255), 5, 8, 0)
# 尤度の描画
label = 'person: {:.2f}%'.format(score * 100)
y = ymin - 15 if ymin - 15 > 15 else ymin + 15
cv2.putText(frame, label, (xmin, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 1)
def _update_state(self, head_position):
"""
立ち・屈みの状態を更新し、
屈みから立ちの状態になった場合はスクワットした判定を返す
"""
current = head_position < self.head_line
# もし以前と状態が違うなら
if current is not self.is_standing:
# 新しい状態に更新する
self.is_standing = current
# 屈みから立ちの状態になった場合は
if current:
# Trueを返す
return True
# そうでない場合はFalseを返す
return False
