def paint(self, event=None):
dc = wx.PaintDC(self.panel)
dc.Clear()
dc.SetBrush(wx.TRANSPARENT_BRUSH)
dc.SetPen(wx.Pen(wx.RED, 4))
# print some basic information
existing_objs = 'existing objects: ' + str(self.obj_ids)
dc.DrawText(existing_objs, 0, 0)
pref_obj_id = 'pref obj id: ' + str(self.pref_obj_id)
dc.DrawText(pref_obj_id, 0, 20)
try:
for fly in self.flies:
# reproject fly onto camera...
if self.dummy:
xpos, ypos = DummyFlydra.reproject(fly.state_vecs[0:3])
# if xpos, ypos inside rectangle set pref obj id
if self.trigger_rectangle is not None and self.pref_obj_id is None:
if xpos >= self.trigger_rectangle[0][0] and xpos <= self.trigger_rectangle[1][0]:
if ypos >= self.trigger_rectangle[0][1] and ypos <= self.trigger_rectangle[1][1]:
self.set_pref_obj_id(fly.obj_id)
if fly.obj_id == self.pref_obj_id:
dc.DrawEllipse(xpos-7,ypos-2,30,20)
dc.DrawText(str(fly.obj_id), xpos, ypos)
if self.noflies is True:
print 'found a fly!'
self.noflies = False
except:
if self.noflies is False:
print 'no flies... waiting for a fly'
self.noflies = True
if self.trigger_rectangle is not None:
width = self.trigger_rectangle[1][0] - self.trigger_rectangle[0][0]
height = self.trigger_rectangle[1][1] - self.trigger_rectangle[0][1]
dc.DrawRectangle(self.trigger_rectangle[0][0], self.trigger_rectangle[0][1], width, height)
self.panel.Refresh()
def image_callback(self,data):
encoding = data.encoding
img = self.bridge.imgmsg_to_cv(data, desired_encoding="passthrough")
cv_image = cv.CreateImage(cv.GetSize(img),cv.IPL_DEPTH_8U,3)
if data.encoding == 'bayer_bggr8':
cv.CvtColor(img, cv_image,cv.CV_BayerRG2RGB)
else:
print 'warning: bayer type not recognized, using monochrome image instead'
cv_image = img
if self.parameters_loaded is False:
self.width = cv.GetSize(cv_image)[0]
self.height = cv.GetSize(cv_image)[1]
self.load_parameters()
##########################################################
# Drawing Functions go here, operate on cv_image
# print some basic (static) information
existing_objs = 'existing objects: ' + str(self.obj_ids)
cv.PutText(cv_image, existing_objs, (0, 15), self.font, self.color_green)
pref_obj_id = 'pref obj id: ' + str(self.pref_obj_id)
cv.PutText(cv_image, pref_obj_id, (0, 35), self.font, self.color_green)
if self.active == 1:
cv.PutText(cv_image, 'active', (self.width-100, 15), self.font, self.color_red)
elif self.active == -1:
cv.PutText(cv_image, 'dormant', (self.width-100, 15), self.font, self.color_green)
# black background for focus bar
cv.Rectangle(cv_image, (0,self.height-40), (self.width, self.height), self.color_black, thickness=-1)
# draw the flies
if len(self.objects) > 0:
if self.choose_object:
opt_dist = 1000 # initialization for finding nearest fly
choose_fly = self.obj_ids[0] # initialization for nearest fly
for fly in self.objects:
# reproject fly onto camera...
if self.dummy:
pos = [fly.position.x, fly.position.y, fly.position.z]
xpos, ypos = DummyFlydra.reproject(pos)
dist = linalg.norm(pos)
if not self.dummy:
pos = [fly.position.x, fly.position.y, fly.position.z]
xpos, ypos = self.camera_calibration.find2d(self.cam_id, pos, distorted=True)
xpos = int(xpos)
ypos = int(ypos)
cv.Circle(cv_image, (xpos,ypos), 1, self.color_red, thickness=1)
dist = linalg.norm(np.array(pos)-np.array(self.camera_center))
# if xpos, ypos inside rectangle set pref obj id
if self.trigger_rectangle is not None and self.pref_obj_id is None:
if xpos >= self.trigger_rectangle[0][0] and xpos <= self.trigger_rectangle[1][0]:
if ypos >= self.trigger_rectangle[0][1] and ypos <= self.trigger_rectangle[1][1]:
if dist <= self.dist_world_max and dist >= self.dist_world_min:
self.set_pref_obj_id(fly.obj_id)
# if objects out of range, use light green
if dist <= self.dist_world_max and dist >= self.dist_world_min:
# obj_id text
cv.PutText(cv_image, str(fly.obj_id), (xpos, ypos), self.font, self.color_green)
# distance text
cv.PutText(cv_image, str(dist)[0:4], (xpos, ypos+10), self.small_font, self.color_green)
else:
# obj_id text
cv.PutText(cv_image, str(fly.obj_id), (xpos, ypos), self.font, self.color_light_green)
# distance text
cv.PutText(cv_image, str(dist)[0:4], (xpos, ypos+10), self.small_font, self.color_light_green)
if fly.obj_id == self.pref_obj_id:
cv.PutText(cv_image, str(fly.obj_id), (xpos, ypos), self.font, self.color_red)
# circle for covariance
cov = np.sum(np.abs(fly.posvel_covariance_diagonal[0:3]))
cx = xpos+len(str(fly.obj_id))*5
cy = ypos-5
#radius = len(str(fly.obj_id))*5+5
cv.Circle(cv_image, (cx,cy), cov*10, self.color_red, thickness=1)
cv.PutText(cv_image, str(cov), (xpos, ypos+20), self.small_font, self.color_red)
# choose nearest object to last mouse click
if self.choose_object:
dist = np.abs(xpos-self.mousex)+np.abs(ypos-self.mousey)
if dist < opt_dist:
opt_dist = dist
choose_fly = fly.obj_id
if self.choose_object:
self.set_pref_obj_id(choose_fly)
self.choose_object = False
# trigger rectangle:
if self.trigger_rectangle is not None:
cv.Rectangle(cv_image, self.trigger_rectangle[0], self.trigger_rectangle[1], self.color_blue, thickness=1)
# distance scale:
cv.Line(cv_image, (self.dist_scale_x1,self.dist_scale_y), (self.dist_scale_x2,self.dist_scale_y), self.color_white, thickness=2)
dist_str = 'dist from cam, ' + self.dist_scale_units
cv.PutText(cv_image, dist_str, (self.dist_scale_x1,self.dist_scale_y+15), self.small_font, self.color_white)
for n in range(self.dist_scale_nticks+1):
y1 = self.dist_scale_y+5
y2 = self.dist_scale_y-5
x = self.dist_scale_x1+self.dist_scale_tick_interval*n
dist = self.dist_scale_min+self.dist_scale_interval*n
cv.Line(cv_image, (x,y1), (x,y2), self.color_white, thickness=2)
cv.PutText(cv_image, str(dist)[0:4], (x,y2), self.small_font, self.color_white)
# trigger distance rectangle:
if self.trigger_distance is not None:
cv.Rectangle(cv_image, self.trigger_distance[0], self.trigger_distance[1], self.color_blue, thickness=1)
# PTF position
if self.ptf_3d is not None:
if self.dummy:
xpos, ypos = DummyFlydra.reproject(self.ptf_3d)
xhome = 0
yhome = 0
if not self.dummy and self.ptf_3d is not None:
xpos, ypos = self.camera_calibration.find2d(self.cam_id, self.ptf_3d, distorted=True)
xhome, yhome = self.camera_calibration.find2d(self.cam_id, self.ptf_home, distorted=True)
#print '*'*80
#print self.ptf_3d, self.camera_center
dist = linalg.norm( np.array(self.ptf_3d) - np.array(self.camera_center))
home_dist = linalg.norm( np.array(self.ptf_home) - np.array(self.camera_center))
pix = self.dist_to_pixel(dist)
home_pix = self.dist_to_pixel(home_dist)
#print '**', pix, self.dist_scale_y
cv.Circle(cv_image, (int(pix),int(self.dist_scale_y)), 2, self.color_red, thickness=2)
cv.Circle(cv_image, (int(home_pix),int(self.dist_scale_y)), 2, self.color_blue, thickness=2)
cv.Circle(cv_image, (int(xpos),int(ypos)), 2, self.color_red, thickness=2)
cv.Circle(cv_image, (int(xhome),int(yhome)), 2, self.color_blue, thickness=2)
#print 'ptf_3d: ', self.ptf_3d, 'xpos, ypos: ', xpos, ypos
UL = (xpos-self.ptf_fov_in_gui_w, ypos-self.ptf_fov_in_gui_h)
LR = (xpos+self.ptf_fov_in_gui_w, ypos+self.ptf_fov_in_gui_h)
cv.Rectangle(cv_image, UL, LR, self.color_red, thickness=1)
# draw cursor:
xpos = int(self.cursor[0]*self.width)
ypos = int(self.cursor[1]*self.height)
cv.Circle(cv_image, (xpos,ypos), 2, self.color_purple, thickness=2)
cv.Line(cv_image, (xpos-10, ypos), (xpos+10,ypos), self.color_purple, thickness=1)
cv.Line(cv_image, (xpos, ypos-10), (xpos,ypos+10), self.color_purple, thickness=1)
##########################################################
cv.ShowImage(self.display_name, cv_image)
cv.WaitKey(3)
