class MainThread(threading.Thread):
def __init__(self, queues, options, parameters):
threading.Thread.__init__(self)
im_list, order_list = queues
self.im_list = im_list
print "the size of the list is ", im_list.qsize()
time.sleep(1)
self.order_list = order_list
self.image_to_show = "main_contour"
self.options = options
self.parameters = parameters
## THINGS FOR KALMAN FILTER
## EXPERIMENTAL
#~ dt = 1/fps_max
#~ A = np.mat([[1,dt,0,0],[0,1,0,0],[0,0,1,dt],[0,0,0,1]])
#~ B = np.mat([[dt,0,0,0],[0,1,0,0],[0,0,dt,0],[0,0,0,1]])
#~ H = np.identity(4)#*step_2_pixel # Which should we choose here?...
#~ P = np.identity(4) # This changes over time so we are not too worried.
#~ Q = np.identity(4)*0.01 # We are pretty confident it our model.. maybe foolishly so!
#~ R = np.identity(4)*0.010 # we are a fair bit more uncertain about this...
#~ I = np.identity(4)
def run(self):
## Short explanatory text re parameters, options and state.
## PARAMETERS
## Parameters are numbers where there is some optimal or obvious value, they might be tweaked between runs
## but ultimately we want to stick with one.
##
## OPTIONS
## Options are typically yes or no questions, but more over it is simple things that you might want to change
## between runs simply depending on what you want to do. Are you debugging? If so, printing output is nice.
##
## STATE
## State values are things that change during a run, it's all the stuff like the position of the particle
## the speed of the step engine, etc etc.
# parameters holding object
self.kalman = Kalman(fps_max=10,
going_right=True,
middle=self.parameters.middle)
# option holding object
options = self.options
options.testing_disconnected = False
options.printing_output = True
# options.starting_left = True
# options.needs_average = False
options.saving_directory = "E:/staffana/Feb 26/"
print "starting left is ", options.starting_left
# state holding object. Could be replaced if we implement a Kalman filter
#self.state = DetectorState(self.parameters, options)
# Object stores the times taken for various computations in this class.
timedata = TimeData(
) # The "class" that handles all the times we meassure
stepfile = open("MetaData/step_data.txt", 'w')
stepfile.write(
"i \t re_rod_pos \t rod vel \t step vel \t\t correction_vel \t\t new velocity \n"
)
#### CONNECT TO THE STEP MOTOR AND LET THE USER FIND A PARTICLE
if not options.testing_disconnected:
aver_im, self.state = InitializeStepEngine(self.order_list,
self.im_list, options,
self.parameters,
timedata)
#~ state.step_vel, aver_im, state.speed_error, state.vel_fac, state.going_right = outputs
state = self.state # I think this is safe.
### TRY TO OPEN A SEPARATE WINDOW WHERE THE USER CAN PRESS STOP MAYBE BUT FIRST JUST SHOW IT
else:
# dirt_im = Image.open("Images/Averaging/average_image.jpg")
dirt_im = Img.open("C:/test.jpg")
dirt_arr = np.array(dirt_im)
aver_im = dirt_arr.astype(np.float32)
## NOW WE START THE (POSSIBLY) NEVER ENDING THE LOOP OF TRACKING THE PARTICLE
for j in range(1, 30000):
timedata.iteration_start = time.clock()
timedata.pos_save_total = 0
#print "before running SaveStep pos our timedata pos save total was ", timedata.pos_save_total
ignorePosition = SaveStepPos(options, timedata)
#print "After running SaveStep pos our timedata pos save total is ", timedata.pos_save_total
if j % 2 == 0:
i = j / 2
else:
continue
print "\n our current frame is ", i, "\n"
#### ACQUIRE AND CROP THE IMAGE
timedata.image_old = timedata.image_start
timedata.image_start = time.clock()
im = ImageGrab.grab(self.parameters.box)
if options.printing_output:
im.save("Images/cropped image" + str(i) + ".jpg")
if self.image_to_show == "original":
self.im_list.put(im)
timedata.image_end = time.clock()
#### PERFORM EDGE DETECTION AND CONTOUR DETECTION
timedata.contour_start = time.clock()
contours, pix = GetContours(im, aver_im, 190,
options.printing_output, i)
timedata.contour_end = time.clock()
best_contour_nr, positions = DetectParticle(
state, contours, i, timedata, pix, self.parameters,
options) # i is the current nr of runs
# To save space we could possibly merge contours with state? It seems a bit weird either way...
## Updates the state based on the particle we have, or have not, found.
state.update(best_contour_nr, contours, positions, self.parameters,
i, timedata, options)
self.kalman.predict(state, self.parameters.step_2_pixel)
self.kalman.update(state)
print " the found position we think was ", state.pos_approx
print "Kalman predicted the position to be ", [
self.kalman.x_pred.item(0),
self.kalman.x_pred.item(2)
]
print "Kalman thinks our actual position is ", [
self.kalman.x_est.item(0),
self.kalman.x_est.item(2)
],
if best_contour_nr < 0:
continue
## If we are printing output we save a picture of the chosen contour
if options.printing_output:
try:
cv2.drawContours(pix, [contours[best_contour_nr]], -1,
(0, 255, 0), 3)
except IndexError as ie:
print "contours is ", contours, " and the best contour number is ", best_contour_nr
raise
im = Image.fromarray(pix)
if self.image_to_show == "main_contour":
self.im_list.put(im)
#print "WE ADDED AN IMAGEHOORAY HOORAY \n \n"
im.save("Images/main_contour" + str(i) + ".jpg")
## Saves the time now that we are done with detecting the particle
timedata.detection_end
#### MEASURE THE POSITION AND VELOCITY OF THE STEP ENGINE
if not options.testing_disconnected:
state.currentPosition = SaveStepPos(options, timedata)
## I HAVE CHAGED SAVE STEP POS NEED TO CHANGE THE DEFINITION
state.update_correction_vector(state,
self.parameters,
options,
current_frame=j)
state.corr_vec *= 0
# this should probably also be a state operation.. it would really finish cutting down on the main loop!
#### AND THEN DO THE REQUIRED CORRECTIONS
timedata.step_control_start = time.clock()
timedata.step_control_end = time.clock()
### IF WE ARE TOO CLOSE TO EITHER INLET WE WANT TO REVERSE THE PUMP
state.check_pump()
#### Check the image queue and the order queue if we should stop or something
self.check_queues()
#### SLEEP ANY TIME THAT IS LEFT
timedata.iteration_end = time.clock()
dt = timedata.iteration_end - timedata.iteration_start
time_to_sleep = max(0, (1 / self.parameters.fps_max) - dt)
timedata.write_times(
) ## Think of a better name for this function...
# print "dt = ", dt, "time to sleep is ", time_to_sleep
# print "we are taking approximately ", (dt*fps_max)*100 ,"% of the CPU"
time.sleep(time_to_sleep)
print " \n" # just to separate the iterations more.
def check_queues(self):
if not self.order_list.empty():
order = self.order_list.get()
if order == "stop":
print "WE HAVE ORDERED TO STOP!!!!"
## close connection to step engine
StopAxes()
Disconnect()
## close connection to the pump
self.state.pump.CloseConnection()
## Should close various files as well? I think opening them next time will solve that.
thread.exit()
# import sys
# sys.exit()
image_commands = [
"nothing", "main_contour", "original", "all_contours"
]
if order in image_commands:
self.image_to_show = order
option_commands = [
"calibrate", "average", "starting_left", "starting_right"
]
if order in option_commands:
pass
else:
pass
