def main():
#We need to initalize ROS environment for Robot and camera to connect/communicate
ROSEnvironment()
#Initalize camera
camera = Camera()
#start camera
focal_length = 640
camera.start()
robot = Robot()
#start robot
robot.start()
#initalize face detector
face_detector = FaceDetector()
predictor = dlib.shape_predictor('./shape_predictor_68_face_landmarks.dat')
#counter
cnt = 1
#loop
while True:
#get image
img = camera.getImage()
#gets face detections
dets = face_detector.detect(img)
#draw all face detections
for det in dets:
cv2.rectangle(img, (det.left(), det.top()),
(det.right(), det.bottom()), color_green, 3)
#we only use 1 face to estimate pose
if (len(dets) > 0):
det0 = dets[0]
#estimate pose
(success, rotation_vector, translation_vector,
image_points) = face_detector.estimate_pose(img, det0)
#draw pose
img = face_detector.draw_pose(img, rotation_vector,
translation_vector, image_points)
#converts 2d coordinates to 3d coordinates on camera axis
(x, y, z) = camera.convert2d_3d((det0.left() + det0.right()) / 2,
(det0.top() + det0.bottom()) / 2)
print(x, y, z, 'on camera axis')
#converts 3d coordinates on camera axis to 3d coordinates on robot axis
(x, y, z) = camera.convert3d_3d(x, y, z)
print(x, y, z, 'on robot axis')
#move robot
robot.lookatpoint(x, y, z, 4)
cv2.imshow("Frame", img[..., ::-1])
key = cv2.waitKey(1)
if key > 0:
break
def main():
# We need to initalize ROS environment for Robot and camera to connect/communicate
ROSEnvironment()
# Initalize robot
robot = Robot()
# Start robot
robot.start()
#The parameters are 3d coordinates in the real world
#TODO Change the values in lookatpoint
robot.lookatpoint(1, 1, 1)
def main():
# We need to initalize ROS environment for Robot and camera to connect/communicate
ROSEnvironment()
# Initalize robot
robot = Robot()
# Start robot
robot.start()
time.sleep(2) # wait a second
# TODO: change the values in lookatpoint to look at the red square.
robot.lookatpoint(1, 1, 1)
time.sleep(2) # wait a second
# TODO: change the values in lookatpoint to look at the green square.
robot.lookatpoint(1, -1, 1)
def main():
# We need to initalize ROS environment for Robot and camera to connect/communicate
ROSEnvironment()
# Initalize camera
camera = Camera()
# Start camera
camera.start()
# Initalize robot
robot = Robot()
# Start robot
robot.start()
# Initalize face detector
detector = dlib.get_frontal_face_detector()
# The variable for counting loop
cnt = 0
# Loop
while True:
# Get image
img = camera.getImage()
# Get face detections
dets = detector(img, 1)
for det in dets:
cv2.rectangle(img, (det.left(), det.top()),
(det.right(), det.bottom()), color_green, 3)
if (len(dets) > 0):
tracked_face = dets[0]
tracked_face_x = (tracked_face.left() + tracked_face.right()) / 2
tracked_face_y = (tracked_face.top() + tracked_face.bottom()) / 2
#TODO: convert 2d point to 3d point on the camera coordinates system
(x, y, z) = camera.convert2d_3d(tracked_face_x, tracked_face_y)
#TODO: convert the 3d point on the camera point system to the robot coordinates system
(x, y, z) = camera.convert3d_3d(x, y, z)
#TODO: move the robot so that it tracks the face
robot.lookatpoint(x, y, z)
# Show image
cv2.imshow("Frame", img[..., ::-1])
# Close if key is pressed
key = cv2.waitKey(1)
if key > 0:
break
def main():
ROSEnvironment()
camera = Camera()
camera.start()
robot = Robot()
robot.start()
#loops
while (True):
#gets image from camera
cam_image = camera.getImage()
#Gets width and height of image
input_image = cam_image
width = input_image.shape[1]
height = input_image.shape[0]
#TODO: check deep neural network with weight and configure file
net = cv2.dnn.readNet(weight_path, cfg_path)
#creates a "bob" that is the input image after mean subtraction, normalizing, channel swapping
#0.00392 is the scale factor
#(416,416) is the size of the output image
#(0,0,0) are the mean values that will be subtracted for each channel RGB
blob = cv2.dnn.blobFromImage(input_image,
0.00392, (416, 416), (0, 0, 0),
True,
crop=False)
#Inputs blob into the neural network
net.setInput(blob)
#gets the output layers "yolo_82', 'yolo_94', 'yolo_106"
#output layer contains the detection/prediction information
layer_names = net.getLayerNames()
#getUnconnectedOutLayers() returns indices of unconnected layers
#layer_names[i[0] - 1] gets the name of the layers of the indices
#net.getUnconnectedOutLayers() returns [[200], [227], [254]]
output_layers = [
layer_names[i[0] - 1] for i in net.getUnconnectedOutLayers()
]
#Runs forward pass to compute output of layer
#returns predictions/detections at 32, 16 and 8 scale
preds = net.forward(output_layers)
#Initialize list that contains class id, confidence values, bounding boxes
class_ids = []
confidence_values = []
bounding_boxes = []
#TODO: Change the values for conf_threshold and nms_threshold
#Initialize confidence threshold and threshold for non maximal suppresion
conf_threshold = 0.5
nms_threshold = 0.4
#for each scale, we go through the detections
for pred in preds:
for detection in pred:
#Use the max score as confidence
scores = detection[5:]
class_id = np.argmax(scores)
confidence = scores[class_id]
#Check if confidence is greather than threshold
if confidence > conf_threshold:
#Compute x,y, widht, height, class id, confidence value
center_x = int(detection[0] * width)
center_y = int(detection[1] * height)
w = int(detection[2] * width)
h = int(detection[3] * height)
x = center_x - w / 2
y = center_y - h / 2
class_ids.append(class_id)
confidence_values.append(float(confidence))
bounding_boxes.append([x, y, w, h])
#TODO: check your threshold for non maximal suppression
indices = cv2.dnn.NMSBoxes(bounding_boxes, confidence_values,
conf_threshold, nms_threshold)
#draw results
#tracked_object flag for if object is already tracked
tracked_object = 0
for i in indices:
i = i[0]
box = bounding_boxes[i]
x = box[0]
y = box[1]
w = box[2]
h = box[3]
center_x = x + w / 2.0
center_y = y + h / 2.0
classid = class_ids[i]
class_name = str(classes[classid])
#If detected object equals to the object tracked
#TODO: make if statement for selecting the object you want to track
if class_name == "bottle":
#TODO: Converts the 3d camera coordinates into 3d world coordinates
(x_3d, y_3d, z_3d) = camera.convert2d_3d(center_x, center_y)
(x_3d, y_3d, z_3d) = camera.convert3d_3d(x_3d, y_3d, z_3d)
#TODO: commands the robot to look
robot.lookatpoint(x_3d, y_3d, z_3d, 4)
tracked_object = 1
conf_value = confidence_values[i]
draw_boundingbox(input_image, classid, conf_value, round(x),
round(y), round(x + w), round(y + h))
print("Traking " + class_name, conf_value)
#show image
cv2.imshow("Object Detection Window", input_image[..., ::-1])
key = cv2.waitKey(1)
cv2.imwrite("detected_object.jpg", input_image)
cv2.destroyAllWindows()
def main():
faceInCenter_count = 0
current_pan = 0
current_tilt = 0
#We need to initalize ROS environment for Robot and camera to connect/communicate
ROSEnvironment()
#Initalize camera
camera = Camera()
#start camera
camera.start()
#Initalize robot
robot = Robot()
#start robot
robot.start()
#initalize face detector
face_detector = FaceDetector()
predictor = dlib.shape_predictor('./shape_predictor_68_face_landmarks.dat')
#face detection result
dets = None
# current tracking state
Tracking = False
# the time when motion for looking left/right runs
motion_start_time = None
cnt = 0
#loop
while True:
#get image
img = camera.getImage()
if frame_skip(img):
continue
#gets detect face
dets = face_detector.detect(img)
#If the number of face detected is greater than 0
if len(dets)>0:
#We select the first face detected
tracked_face = dets[0]
#Get the x, y position
tracked_face_X = (tracked_face.left()+tracked_face.right())/2
tracked_face_y = (tracked_face.top()+tracked_face.bottom())/2
# Estimate head pose
(success, rotation_vector, translation_vector, image_points) = face_detector.estimate_pose(img, tracked_face)
# Draw bounding box
cv2.rectangle(img,(tracked_face.left(), tracked_face.top()), (tracked_face.right(), tracked_face.bottom()), color_green, 3)
# Draw head pose
img = face_detector.draw_pose(img, rotation_vector, translation_vector, image_points)
#Check if head is in the center, returns how many times the head was in the center
faceInCenter_count =faceInCenter(camera, tracked_face_X, tracked_face_y, faceInCenter_count)
print faceInCenter_count
print ("{} in the center for {} times".format("Face as been", (faceInCenter_count)) )
#We track when the head is in the center for a certain period of time and there is not head motion activated
if(faceInCenter_count<5 and motion_start_time == None):
Tracking = True
else:
Tracking = False
#Start tracking
if Tracking:
print("Tracking the Person")
#TODO: converts 2d coordinates to 3d coordinates on camera axis
(x,y,z) = camera.convert2d_3d(tracked_face_X, tracked_face_y)
#TODO: converts 3d coordinates on camera axis to 3d coordinates on robot axis
(x,y,z) = camera.convert3d_3d(x,y,z)
#TODO: move robot to track your face
robot.lookatpoint(x,y,z, 1)
#When tracking is turned off, estimate the head pose and perform head motion if conditions meet
elif Tracking is False:
print "Stopped Tracking, Starting Head Pose Estimation"
# yaw is angle of face on z-axis
yaw = rotation_vector[2]
#Condition for user looking towards the right
if (yaw > 0.3 and motion_start_time == None):
print ('You are looking towards the right.')
#TODO: Remember the current position
current_position = robot.getPosition()
current_pan = current_position[0]
current_tilt = current_position[1]
print "Starting head motion to look right"
#TODO: add motion for looking right
robot.move(0.8,0.5)
motion_start_time = current_time()
#Condition for user looking towards the left
elif (yaw < -0.3 and motion_start_time == None):
print ('You are looking towards the left.')
#TODO: Remember the current position
current_position = robot.getPosition()
current_pan = current_position[0]
current_tilt = current_position[1]
print "Starting head motion to look left"
#TODO: add motion for looking left
robot.move(-0.8,0.5)
motion_start_time = current_time()
#When head motion is activated we start the counter
if(motion_start_time != None):
print ("{} and its been {} seconds".format("Look motion activated ", (current_time()-motion_start_time)) )
#After 3 seconds, we have to return to the current position
if(motion_start_time != None and ((current_time()-motion_start_time) > 3) ):
#Looking at the position that is stored.
print "Robot is going back "
#TODO: make the robot move to the stored current position
robot.move(current_pan, current_tilt)
motion_start_time = None
#Tracking = True
#Start tracking again
if (cnt>10 and motion_start_time == None):
Tracking = True
cnt = cnt+1
sleep(0.08)
#show image
cv2.imshow("Frame", img[...,::-1])
#Close if key is pressed
key = cv2.waitKey(1)
if key > 0:
break
