def image_handler(self, channel, data):
msg = image_t.decode(data)
im = np.asarray(bytearray(msg.data), dtype=np.uint8).reshape(msg.height, msg.width)
bf_heatmap = im / 255.0
# get maximum value in heatmap, push to variable for plotting in local frame
el, az = np.unravel_index(im.argmax(), im.shape)
az_el = np.array([[az * np.pi / 180.0], [el * np.pi / 180.0]])
cv2.circle(im, (az, el), 5, (0, 0, 0))
lRb_val = self.prtObj.lRb()
self.imu_max = np.dot(lRb_val.T, np.matrix(self.prtObj.bRpe_pt(az_el)))
self.prtObj.upStateAcoustics_mod(bf_heatmap, im)
# print 'VICON azimuth elevation transforms:'
for i in range(0, self.N):
t = self.prtObj.peRb_pt(
np.dot(self.vicon_lRb.T, self.prtObj.particles_l[:, i : i + 1])
) # local all the way to plane
if t[0, 0] < 0:
t[0, 0] = t[0, 0] + 2 * np.pi
# print t[0,0]*180/np.pi, t[1,0]*180/np.pi
# cv2.circle(im, (int(t[0,0]*180/np.pi),int(t[1,0]*180/np.pi)), 3, (0,0,0))
cv2.imshow("img", im)
cv2.waitKey(10)
def decode(self, data):
msg = image_t.decode(data)
if msg.pixelformat == image_t.PIXEL_FORMAT_GRAY:
return im_resize(np.asarray(bytearray(msg.data), dtype=np.uint8).reshape(msg.height, msg.width), scale=self.scale)
elif msg.pixelformat == image_t.PIXEL_FORMAT_MJPEG:
im = cv2.imdecode(np.asarray(bytearray(msg.data), dtype=np.uint8), -1)
return im_resize(im, scale=self.scale)
else:
raise RuntimeError('Unknown pixelformat for ImageDecoder')
def convertNSend(channel, data):
global lc
msgIn = image_t.decode(data)
msgOut = images_t.images_t()
# Put the 1 image into a size-1 images_t message
msgOut.utime = msgIn.utime
msgOut.n_images = 1
msgOut.image_types = [msgOut.LEFT]
msgOut.images = [msgIn]
lc.publish("CAMERA", msgOut.encode())
print '.',
sys.stdout.flush()
def handle_image_t(self, channel, data):
msg = image_t.decode(data)
# conversion black magic...
if msg.pixelformat == image_t.PIXEL_FORMAT_MJPEG:
source = cv2.imdecode(np.asarray(bytearray(msg.data),
dtype="uint8"),
flags=cv2.CV_LOAD_IMAGE_COLOR)
frame = cv.CreateImageHeader((source.shape[1], source.shape[0]),
cv.IPL_DEPTH_8U, 3)
cv.SetData(frame, source.tostring(),
source.dtype.itemsize * 3 * source.shape[1])
self._frame = cv.CloneImage(frame)
else:
print "Don't know how to handle this image type: ", msg.pixelformat
self.update()
def image_handler(self, channel, data):
msg = image_t.decode(data)
im = np.asarray(bytearray(msg.data), dtype=np.uint8).reshape(msg.height, msg.width)
bf_heatmap = im/255.0;
# get maximum value in heatmap, push to variable for plotting in local frame
el,az = np.unravel_index(im.argmax(), im.shape)
az_el = np.array([[az*np.pi/180.0],[el*np.pi/180.0]])
cv2.circle(im, (az,el), 5, (0,0,0))
lRb_val = self.prtObj.lRb()
self.imu_max = np.dot(lRb_val.T, np.matrix(self.prtObj.bRpe_pt(az_el)))
self.prtObj.upStateAcoustics_mod(bf_heatmap, im)
# print 'VICON azimuth elevation transforms:'
for i in range(0,self.N):
t = self.prtObj.peRb_pt(np.dot(self.vicon_lRb.T, self.prtObj.particles_l[:,i:i+1])) # local all the way to plane
if t[0,0] < 0:
t[0,0] = t[0,0]+2*np.pi
# print t[0,0]*180/np.pi, t[1,0]*180/np.pi
# cv2.circle(im, (int(t[0,0]*180/np.pi),int(t[1,0]*180/np.pi)), 3, (0,0,0))
cv2.imshow('img', im)
cv2.waitKey(10)