def main():
real_data = read_training_data(TL_REAL_IMAGES_RELATIVE_PATH)
clf = TLClassifier()
labels = []
predictions = []
print "Real"
for image, image_id, color_label in real_data:
color_prediction = clf.get_classification(image)
labels.append(color_label)
predictions.append(color_prediction)
print("Image: ", image_id, " Label: ", get_color_name(color_label),
" Prediction: ", get_color_name(color_prediction))
sim_data = read_training_data(TL_SIM_IMAGES_RELATIVE_PATH)
print "Simulation"
for image, image_id, color_label in sim_data:
color_prediction = clf.get_classification(image)
labels.append(color_label)
predictions.append(color_prediction)
print("Image: ", image_id, " Label: ", get_color_name(color_label),
" Prediction: ", get_color_name(color_prediction))
print("Accuracy: ", accuracy_score(labels, predictions))
print("Total: ", len(labels), " Correct: ",
accuracy_score(labels, predictions, normalize=False))
print("Confusion Matrix: ", confusion_matrix(labels, predictions))
return True
return False
DEBUG = False
if DEBUG == True:
tl = TLIdentifier(1, 'ALL', './traffic_light_identifier.h5')
tlc = TLClassifier('./traffic_light_classifier.h5')
# TL.resize_images('./images/unknown/','./images/resizeunknown/')
images = tl.read_image_files(
'/home/student/catkin_ws/src/CarND-Capstone/ros/src/tl_detector/light_classification/images/training/misses/'
)
tl.set_window(0, 800, 100, 600)
for image in images:
fimage = mpimg.imread(image)
plt.imshow(fimage)
plt.show()
fnd, result = tl.process_image(fimage)
if fnd == True:
plt.imshow(result)
plt.show()
state = tlc.get_classification(result)
if state == TrafficLight.RED:
print("red")
elif state == TrafficLight.GREEN:
print("green")
else:
print("yellow")
parentdir = os.path.dirname(currentdir)
sys.path.insert(0, parentdir)
from tl_classifier import TLClassifier
# path to the frozen tensorflow model (protobuf)
model_file = currentdir + "/../../models/frozen_classifier_model.pb"
# create classifier by reading the frozen model file
light_classifier = TLClassifier(model_file)
# test images (cutout of a detected traffic light)
image_files = [
"sample_tl_cutout_yellow_1.png", "traffic_light_red_1.png",
"traffic_light_green_1.png", "traffic_light_red_2.png"
]
# classify every sample image and print the results
for image_file in image_files:
image = cv2.imread(currentdir + "/" + image_file)
print("----")
print("classifying image " + image_file)
print("shape of input image: " + str(image.shape))
# classify cutout image
detected_color_id = light_classifier.get_classification(image)
# print result
print("detected color-id is: " + str(detected_color_id))
# xmin = int(round(top_xmin * img.shape[1]))
# ymin = int(round(top_ymin * img.shape[0]))
# xmax = int(round(top_xmax * img.shape[1]))
# ymax = int(round(top_ymax * img.shape[0]))
# coords = (xmin, ymin), xmax - xmin + 1, ymax - ymin + 1
# color = tld_color[label]
# currentAxis.add_patch(plt.Rectangle(*coords, fill=False, edgecolor=color, linewidth=1))
#except:
# pass
#print(img.shape)
## test (1096, 1368, 3)
## impl (1096, 1368, 3)
label, score, top_xmin, top_ymin, top_xmax, top_ymax = light_classifier.get_classification(
img)
#label, score, top_xmin, top_ymin, top_xmax, top_ymax = 4, 0, 0, 0, 0, 0
#display_txt = '{:0.2f}, {}'.format(score, label)
#print(label, display_txt)
xmin = int(round(top_xmin * img.shape[1]))
ymin = int(round(top_ymin * img.shape[0]))
xmax = int(round(top_xmax * img.shape[1]))
ymax = int(round(top_ymax * img.shape[0]))
if label != 4:
coords = (xmin, ymin), xmax - xmin + 1, ymax - ymin + 1
display_txt = '{:0.2f}, {}:{}'.format(score, label,
tld_classes[label])
color = tld_color[label]
def all_files(folder):
files = list(glob(path.join(folder, "*.jpg")))
return files
model = TLClassifier("models/tl_model")
green = [
misc.imread(f)
for f in all_files("../../data/camera_training/validation/green/")
]
red = [
misc.imread(f)
for f in all_files(".../../data/camera_training/validation/red/")
]
yellow = [
misc.imread(f)
for f in all_files("../../data/camera_training/validation/yellow/")
]
nolight = [
misc.imread(f)
for f in all_files("../../data/camera_training/validation/nolight/")
]
images = [green, red, yellow, nolight]
labels = ["green", "red", "yellow", "nolight"]
for subset, label in zip(images, labels):
for img in subset:
assert model.get_classification(img) == label
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.config = yaml.load(rospy.get_param("/traffic_light_config"))
self.is_simulator = not self.config["is_site"]
#self.process_image_fn = self.process_image_test()
self.process_image_fn = self.process_image_sim if self.is_simulator else self.process_image
self.wp_control = WPControl(self.config)
self.light_classifier = TLClassifier(self.is_simulator)
self.pose = None
self.camera_image = None
self.lights = []
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.wp_control.waypoints_cb)
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb)
rospy.Subscriber('/image_color', Image, self.image_cb)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.bridge = CvBridge()
self.state = TrafficLight.UNKNOWN
self.last_wp_i = -1
self.state_count = 0
self.stop_line_i = 0 #test
self.i_count = -1
self.start_classification = False
self.check_count = I_COUNT_CHECK
self.sl_distance = 1000
self.diff = -10
self.loop()
def loop(self):
rate = rospy.Rate(10)
while not self.wp_control.is_ready():
#print("is not ready")
rate.sleep()
while not rospy.is_shutdown():
'''Publish upcoming red lights at camera frequency.'''
if self.pose is not None:
light_wp_i = self.get_next_traffic_light()
if not self.start_classification:
self.state_count = 0
self.last_wp_i = -1
self.upcoming_red_light_pub.publish(Int32(self.last_wp_i))
else:
if self.state_count >= STATE_COUNT_THRESHOLD:
if self.state != TrafficLight.RED: light_wp_i = -1
self.last_wp_i = light_wp_i
self.upcoming_red_light_pub.publish(Int32(self.last_wp_i))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp_i))
#print("SP: State: ", self.state, " Count: ", self.state_count, " Last wp i:" , self.last_wp_i, " stop line i: ", self.stop_line_i)
rate.sleep()
def pose_cb(self, msg):
self.pose = msg
def traffic_cb(self, msg):
self.lights = msg.lights
def image_cb(self, msg):
""" Camera images receiver """
#self.start_classification = True
self.camera_image = msg
if not self.start_classification:
self.state = TrafficLight.UNKNOWN
self.i_count = -1
else:
self.process_image_fn()
def process_image_test(self):
cur_img_state = self.get_test_light_state()
if self.state != cur_img_state:
self.state_count = 0
self.state = cur_img_state
else:
self.state_count += 1
def process_image_sim(self):
self.i_count += 1
#process every `self.check_count` image
if ( self.i_count % self.check_count == 0 ) :
cur_img_state = self.classify_light_image()
self.process_classified_image_state(cur_img_state)
def process_image(self):
cur_img_state = self.classify_light_image()
self.process_classified_image_state(cur_img_state)
def classify_test_light_image(self):
""" Determines the current color of the traffic light """
if( len(self.lights) == 0 ): return TrafficLight.UNKNOWN
# test detection: /vehicle/traffic_lights topic
return self.lights[self.stop_line_i].state
def classify_light_image(self):
""" Determines the current color of the traffic light """
if( self.camera_image is None ): return TrafficLight.UNKNOWN
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# Classification
return self.light_classifier.get_classification(cv_image)
def process_classified_image_state(self, cur_img_state):
""" Process classified light image state to find current state"""
#self.print_img_color(cur_img_state)
#default is red
if self.state == TrafficLight.UNKNOWN and cur_img_state == TrafficLight.UNKNOWN:
self.state = TrafficLight.RED
cur_img_state = TrafficLight.RED
if self.state != TrafficLight.UNKNOWN and cur_img_state == TrafficLight.UNKNOWN:
cur_img_state = self.state
elif self.state != cur_img_state:
self.state_count = 0
self.state = cur_img_state
else:
self.state_count += 1
def print_img_color(self, cur_img_state):
r_map = { TrafficLight.GREEN: "Green", TrafficLight.RED: "Red", TrafficLight.YELLOW: "Yellow" }
print("Classification Result: ", r_map.get(cur_img_state, "Unknown"))
def get_next_traffic_light(self):
""" Finds closest visible traffic light, if one exists """
current_wp_i = self.wp_control.get_closest_wp([self.pose.pose.position.x, self.pose.pose.position.y])[1]
self.sl_distance, self.stop_line_i = self.wp_control.get_closest_stop_line(self.pose.pose.position)
stop_line_wp_i = self.wp_control.get_stop_line_wp_i(self.stop_line_i)
self.diff = stop_line_wp_i - current_wp_i
#print("Closst light Diff: ", self.diff, " stop line i:", self.stop_line_i, " sl-distance: ", self.sl_distance)
self.start_classification = ( self.diff > -5 and self.sl_distance < 50 )
return stop_line_wp_i
from tl_classifier import TLClassifier
import cv2
import sys
import os
classifier = TLClassifier("sim_model.h5")
print(sys.argv[1])
image = cv2.imread(sys.argv[1], cv2.IMREAD_COLOR)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
print(classifier.get_classification(image, "out.jpg"))
if __name__ == '__main__':
classifier = TLClassifier()
img = cv.imread('image.png')
# hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
# cv.imshow('test image', img)
# h, s, v = cv.split(hsv)
# h[h > 240] = 255
# h[h < 15] = 255
# h[h != 255] = 0
# # cv.imshow('test h', h)
# s[s > 150] = 255
# s[s != 255] = 0
# # cv.imshow('test s', s)
# v[v > 200] = 255
# v[v != 255] = 0
# # cv.imshow('test v', v)
# thres = cv.bitwise_and(cv.bitwise_and(h, s), v)
# cv.imshow('test thres', thres)
# nonzero = cv.countNonZero(thres)
# print(nonzero)
print(classifier.get_classification(img, 0.5))
cv.waitKey()
cv.destroyAllWindows()
path.mkdir(exist_ok=True, parents=True)
cnt_error = 0
cnt_ok = 0
idx = 0
for root, dirs, files in os.walk("images", topdown=False):
for filename in files:
if filename.startswith('.DS_Store'):
continue
path = root + '/' + filename
print('start processing...{}'.format(path))
image = load_img(root + '/' + filename) # this is a PIL image
image_np = load_image_into_numpy_array(image)
start = time.time()
color, _ = tl_classifier.get_classification(image_np, idx)
elapsed = time.time() - start
draw = ImageDraw.Draw(image)
font = ImageFont.truetype('/Library/Fonts/Arial.ttf', 55)
print('elapsed time:', elapsed, ' s')
print(path, tf_colors[color])
# output path either base dir or subdir for color
# output_path = output_base_path + filename
output_path = output_base_path + tf_colors[color] + '/' + filename
# draw text and write file in main output dir
draw.text(
(10, 10),
tf_colors[color],
font=font,
fill=(255, 255, 0),
)
print('GREEN', state)
print('Testing GREEN')
state = clf.get_classification_batch_argmax(image_list=test_green)
if (state == 0):
print('RED', state)
elif (state == 1):
print('YELLOW', state)
else:
print('GREEN', state)
print('Testing YELLOW SINGLE')
state = clf.get_classification(image=test_yellow_single)
if (state == 0):
print('RED', state)
elif (state == 1):
print('YELLOW', state)
else:
print('GREEN', state)
print('Testing RED MIX YELLOW, should return RED')
state = clf.get_classification_batch_argmax(image_list=test_red_yellow)
if (state == 0):
print('RED', state)
elif (state == 1):
from tl_classifier import TLClassifier
from matplotlib import pyplot as plt
classifier = TLClassifier(False) #run in sim mode
import cv2
image = cv2.imread('data/test_images_sim/left0988.jpg')
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
plt.figure(figsize=(12, 8))
plt.imshow(image)
plt.show()
result = classifier.get_classification(image)
print('result: ', result)
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.has_image = False
self.lights = []
self.bridge = CvBridge()
self.light_classifier = None
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.waypoints_2d = None
self.waypoint_tree = None
self.img_count = 0
# Set if using real car
self.is_site = False
self.light_classifier = TLClassifier(self.is_site)
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
# May want to use image_raw instead for classifier?
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
detector_rate = rospy.Rate(5)
while not rospy.is_shutdown():
self.find_traffic_lights()
detector_rate.sleep()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
# Setup the Kd Tree which has log(n) complexity
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoint_tree = KDTree(self.waypoints_2d)
def traffic_cb(self, msg):
self.lights = msg.lights
def save_img(self, light, state):
# Thanks to https://github.com/ericlavigne/CarND-Capstone-Wolf-Pack for the idea to save images in this way
# To build up the training set
if SAVE_TRAFFIC_LIGHT_IMG and self.has_image: # self.img_count to reduce image save
file_name = "IMG_" + str(time.time()).replace('.', '') + '.jpg'
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
img_path = '/home/student/Udacity/tl_classifier/training_data/light_classification/IMGS/'
if state == 0:
if self.img_count % 10 == 0:
img_path = img_path + "RED/" + file_name
cv2.imwrite(img_path, cv_image,
[int(cv2.IMWRITE_JPEG_QUALITY), 100])
rospy.loginfo("Path: {0}".format(img_path))
elif state == 1:
img_path = img_path + "YELLOW/" + file_name
cv2.imwrite(img_path, cv_image,
[int(cv2.IMWRITE_JPEG_QUALITY), 100])
rospy.loginfo("Path: {0}".format(img_path))
elif state == 2:
if self.img_count % 10 == 0:
img_path = img_path + "GREEN/" + file_name
cv2.imwrite(img_path, cv_image,
[int(cv2.IMWRITE_JPEG_QUALITY), 100])
rospy.loginfo("Path: {0}".format(img_path))
else:
img_path = img_path + "UNKNOWN/" + file_name
cv2.imwrite(img_path, cv_image,
[int(cv2.IMWRITE_JPEG_QUALITY), 100])
rospy.loginfo("Path: {0}".format(img_path))
self.img_count += 1
def image_cb(self, msg):
""" updates the current image
Args:
msg (Image): image from car-mounted camera
"""
self.has_image = True
self.camera_image = msg
# Process traffic lights?
def find_traffic_lights(self):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
#if self.light_classifier and self.has_image == True:
# if self.is_site:# site package
# save real track images.
# self.save_img(self.camera_image, 4)
light_wp, state = self.process_traffic_lights()
# rospy.logwarn("Closest light wp: {0} \n And light state: {1}".format(light_wp, state))
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def get_closest_waypoint(self, x, y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
closest_idx = None
if self.waypoint_tree:
closest_idx = self.waypoint_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
# Assume RED
TLstate = 0
if (self.has_image):
try:
# Convert image into something usable
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
image = cv2.resize(image, (32, 64))
image_array = np.asarray(image)
# Get classification
TLstate = self.light_classifier.get_classification(
image_array[None, :, :, :])
#rospy.loginfo("TL State: {0}, Actual State: {1}".format(TLstate, light.state))
except:
#rospy.loginfo("Could not identify TL State - assuming RED")
TLstate = 0
# Return light state
return TLstate
# for testing we return the light state from the simulator
#return light.state
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
# light = None
# List of positions that correspond to the line to stop in front of for a given intersection
# stop_line_positions = self.config['stop_line_positions']
# if(self.pose):
# car_position = self.get_closest_waypoint(self.pose.pose)
# TODO find the closest visible traffic light (if one exists)
closest_light = None
line_wp_idx = None
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions = self.config['stop_line_positions']
if self.pose and self.waypoints:
car_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
# if can not get the car pose , break
if not car_wp_idx:
return -1, TrafficLight.UNKNOWN
min_diff = min(len(self.waypoints.waypoints),
TL_DETECTION_DISTANCE)
for i, light in enumerate(self.lights):
# Get stop line waypoint index
line = stop_line_positions[i]
temp_wp_idx = self.get_closest_waypoint(line[0],
line[1]) # x,y
# Find closest step line waypoint index
d = temp_wp_idx - car_wp_idx
if d >= 0 and d < min_diff:
min_diff = d
closest_light = light
line_wp_idx = temp_wp_idx
# DEBUG
if d < 70: # when close traffic light , save image
# To save training image
self.save_img(self.camera_image, light.state)
if closest_light:
state = self.get_light_state(closest_light)
#rospy.loginfo('closest light wp %i with color %i', line_wp_idx, state )
return line_wp_idx, state
else:
return -1, TrafficLight.UNKNOWN