def compress_thread():
"""thread for compressing images"""
while True:
frame_time, im = state.compress_queue.get()
jpeg = scanner.jpeg_compress(im, int(opts.quality))
if opts.save:
state.save_queue.put((frame_time, jpeg, True))
def transmit_thread():
'''thread for image transmit to GCS'''
state = mpstate.camera_state
tx_count = 0
skip_count = 0
bsend = block_xmit.BlockSender(0, state.bandwidth)
while not state.unload.wait(0.02):
bsend.tick()
if state.transmit_queue.empty():
continue
(frame_time, regions, im_full, im_640) = state.transmit_queue.get()
pos = get_plane_position(frame_time)
latlon_list = log_joe_position(pos, frame_time, regions)
state.xmit_queue = bsend.sendq_size()
state.efficiency = bsend.get_efficiency()
state.bandwidth_used = bsend.get_bandwidth_used()
state.rtt_estimate = bsend.get_rtt_estimate()
jpeg = None
if len(regions) > 0 and bsend.sendq_size() < 2000:
# send a region message with thumbnails to the ground station
thumb = cuav_mosaic.CompositeThumbnail(im_full, regions, quality=state.quality, thumb_size=100)
bsend.set_bandwidth(state.bandwidth)
bsend.set_packet_loss(state.packet_loss)
pkt = ThumbPacket(frame_time, regions, thumb, latlon_list, state.frame_loss, state.xmit_queue)
# send matches with a higher priority
if state.transmit:
bsend.send(cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL),
dest=(state.gcs_address, state.gcs_view_port),
priority=1)
# Base how many images we send on the send queue size
send_frequency = state.xmit_queue // 3
if send_frequency == 0 or (tx_count+skip_count) % send_frequency == 0:
jpeg = scanner.jpeg_compress(im_640, state.quality)
if jpeg is None:
skip_count += 1
continue
# keep filtered image size
state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(jpeg)
tx_count += 1
if state.gcs_address is None:
continue
bsend.set_packet_loss(state.packet_loss)
bsend.set_bandwidth(state.bandwidth)
pkt = ImagePacket(frame_time, jpeg)
bsend.send(cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL),
dest=(state.gcs_address, state.gcs_view_port))
def CompositeThumbnail(img, regions, thumb_size=100, quality=75):
'''extract a composite thumbnail for the regions of an image
The composite will consist of N thumbnails side by side
return it as a compressed jpeg string
'''
composite = cv.CreateImage((thumb_size*len(regions), thumb_size),8,3)
for i in range(len(regions)):
(x1,y1,x2,y2) = regions[i].tuple()
midx = (x1+x2)/2
midy = (y1+y2)/2
x1 = midx - thumb_size/2
y1 = midy - thumb_size/2
thumb = cuav_util.SubImage(img, (x1, y1, thumb_size, thumb_size))
cv.SetImageROI(composite, (thumb_size*i, 0, thumb_size, thumb_size))
cv.Copy(thumb, composite)
cv.ResetImageROI(composite)
return scanner.jpeg_compress(numpy.ascontiguousarray(cv.GetMat(composite)), quality)
def transmit_thread():
'''thread for image transmit to GCS'''
state = mpstate.camera_state
tx_count = 0
skip_count = 0
bsend = block_xmit.BlockSender(0,
bandwidth=state.settings.bandwidth,
debug=False)
state.bsocket = MavSocket(mpstate.mav_master[0])
state.bsend2 = block_xmit.BlockSender(mss=96,
sock=state.bsocket,
dest_ip='mavlink',
dest_port=0,
backlog=5,
debug=False)
state.bsend2.set_bandwidth(state.settings.bandwidth2)
while not state.unload.wait(0.02):
bsend.tick(packet_count=1000, max_queue=state.settings.maxqueue1)
state.bsend2.tick(packet_count=1000,
max_queue=state.settings.maxqueue2)
check_commands()
if state.transmit_queue.empty():
continue
(frame_time, regions, im_full, im_640) = state.transmit_queue.get()
if state.settings.roll_stabilised:
roll = 0
else:
roll = None
pos = get_plane_position(frame_time, roll=roll)
# this adds the latlon field to the regions
log_joe_position(pos, frame_time, regions)
# filter out any regions outside the boundary
if state.boundary_polygon:
regions = cuav_region.filter_boundary(regions,
state.boundary_polygon, pos)
regions = cuav_region.filter_regions(
im_full, regions, min_score=state.settings.minscore)
state.xmit_queue = bsend.sendq_size()
state.xmit_queue2 = state.bsend2.sendq_size()
state.efficiency = bsend.get_efficiency()
state.bandwidth_used = bsend.get_bandwidth_used()
state.rtt_estimate = bsend.get_rtt_estimate()
jpeg = None
if len(regions) > 0:
lowscore = 0
highscore = 0
for r in regions:
lowscore = min(lowscore, r.score)
highscore = max(highscore, r.score)
if state.settings.transmit:
# send a region message with thumbnails to the ground station
thumb = None
if state.settings.send1:
thumb = cuav_mosaic.CompositeThumbnail(
cv.GetImage(cv.fromarray(im_full)),
regions,
quality=state.settings.quality,
thumb_size=state.settings.thumbsize)
pkt = ThumbPacket(frame_time, regions, thumb,
state.frame_loss, state.xmit_queue, pos)
buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL)
bsend.set_bandwidth(state.settings.bandwidth)
bsend.set_packet_loss(state.settings.packet_loss)
bsend.send(buf,
dest=(state.settings.gcs_address,
state.settings.gcs_view_port),
priority=1)
# also send thumbnails via 900MHz telemetry
if state.settings.send2 and highscore >= state.settings.minscore2:
if thumb is None or lowscore < state.settings.minscore2:
# remove some of the regions
regions = cuav_region.filter_regions(
im_full,
regions,
min_score=state.settings.minscore2)
thumb = cuav_mosaic.CompositeThumbnail(
cv.GetImage(cv.fromarray(im_full)),
regions,
quality=state.settings.quality,
thumb_size=state.settings.thumbsize)
pkt = ThumbPacket(frame_time, regions, thumb,
state.frame_loss, state.xmit_queue,
pos)
buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL)
state.bsend2.set_bandwidth(state.settings.bandwidth2)
state.bsend2.send(buf, priority=highscore)
# Base how many images we send on the send queue size
send_frequency = state.xmit_queue // 3
if send_frequency == 0 or (tx_count +
skip_count) % send_frequency == 0:
jpeg = scanner.jpeg_compress(im_640, state.settings.quality)
if jpeg is None:
skip_count += 1
continue
# keep filtered image size
state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(jpeg)
tx_count += 1
if state.settings.gcs_address is None:
continue
bsend.set_packet_loss(state.settings.packet_loss)
bsend.set_bandwidth(state.settings.bandwidth)
pkt = ImagePacket(frame_time, jpeg, state.xmit_queue, pos)
str = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL)
bsend.send(str,
dest=(state.settings.gcs_address,
state.settings.gcs_view_port))
def transmit_thread():
'''thread for image transmit to GCS'''
state = mpstate.camera_state
tx_count = 0
skip_count = 0
bsend = block_xmit.BlockSender(0, state.settings.bandwidth, debug=False)
state.bsocket = MavSocket(mpstate.mav_master[0])
state.bsend2 = block_xmit.BlockSender(mss=96, sock=state.bsocket, dest_ip='mavlink', dest_port=0, backlog=5, debug=False)
state.bsend2.set_bandwidth(state.settings.bandwidth2)
while not state.unload.wait(0.02):
bsend.tick(packet_count=1000, max_queue=state.settings.maxqueue1)
state.bsend2.tick(packet_count=1000, max_queue=state.settings.maxqueue2)
check_commands()
if state.transmit_queue.empty():
continue
(frame_time, regions, im_full, im_640) = state.transmit_queue.get()
if state.settings.roll_stabilised:
roll=0
else:
roll=None
pos = get_plane_position(frame_time, roll=roll)
# this adds the latlon field to the regions
log_joe_position(pos, frame_time, regions)
# filter out any regions outside the boundary
if state.boundary_polygon:
regions = cuav_region.filter_boundary(regions, state.boundary_polygon, pos)
state.xmit_queue = bsend.sendq_size()
state.xmit_queue2 = state.bsend2.sendq_size()
state.efficiency = bsend.get_efficiency()
state.bandwidth_used = bsend.get_bandwidth_used()
state.rtt_estimate = bsend.get_rtt_estimate()
jpeg = None
if len(regions) > 0:
lowscore = 0
highscore = 0
for r in regions:
lowscore = min(lowscore, r.score)
highscore = max(highscore, r.score)
if state.settings.transmit:
# send a region message with thumbnails to the ground station
thumb = None
if state.settings.send1:
thumb = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)),
regions, quality=state.settings.quality, thumb_size=state.settings.thumbsize)
pkt = ThumbPacket(frame_time, regions, thumb, state.frame_loss, state.xmit_queue, pos)
buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL)
bsend.set_bandwidth(state.settings.bandwidth)
bsend.set_packet_loss(state.settings.packet_loss)
bsend.send(buf,
dest=(state.settings.gcs_address, state.settings.gcs_view_port),
priority=1)
# also send thumbnails via 900MHz telemetry
if state.settings.send2 and highscore >= state.settings.minscore2:
if thumb is None or lowscore < state.settings.minscore2:
# remove some of the regions
regions = cuav_region.filter_regions(im_full, regions, min_score=state.settings.minscore2)
thumb = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)),
regions, quality=state.settings.quality, thumb_size=state.settings.thumbsize)
pkt = ThumbPacket(frame_time, regions, thumb, state.frame_loss, state.xmit_queue, pos)
buf = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL)
state.bsend2.set_bandwidth(state.settings.bandwidth2)
state.bsend2.send(buf, priority=highscore)
# Base how many images we send on the send queue size
send_frequency = state.xmit_queue // 3
if send_frequency == 0 or (tx_count+skip_count) % send_frequency == 0:
jpeg = scanner.jpeg_compress(im_640, state.settings.quality)
if jpeg is None:
skip_count += 1
continue
# keep filtered image size
state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(jpeg)
tx_count += 1
if state.settings.gcs_address is None:
continue
bsend.set_packet_loss(state.settings.packet_loss)
bsend.set_bandwidth(state.settings.bandwidth)
pkt = ImagePacket(frame_time, jpeg, state.xmit_queue, pos)
str = cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL)
bsend.send(str,
dest=(state.settings.gcs_address, state.settings.gcs_view_port))
continue
scanner.debayer(im, im_640)
regions = scanner.scan(im_640)
if len(regions) > 0:
print("Found %u regions" % len(regions))
for r in regions:
(minx, miny, maxx, maxy) = r
print(minx, miny, maxx, maxy)
mat = cv.fromarray(im_640)
for (x1, y1, x2, y2) in regions:
cv.Rectangle(mat, (x1, y1), (x2, y2), (255, 0, 0), 1)
im_marked = numpy.ascontiguousarray(mat)
# compress using neon-accelerated compressor, and write to a file
jpeg = scanner.jpeg_compress(im_marked, 30)
jfile = open("tmp/i%u.jpg" % i, "w")
jfile.write(jpeg)
jfile.close()
cv.ShowImage("Viewer", im_marked)
i += 1
if i % 10 == 0:
tdiff = time.time() - tstart
print("%.1f fps" % (10 / tdiff))
tstart = time.time()
key = cv.WaitKey(1)
if key == -1:
continue
def process(args):
"""process a set of files"""
global slipmap, mosaic
scan_count = 0
files = []
for a in args:
if os.path.isdir(a):
files.extend(glob.glob(os.path.join(a, "*.pgm")))
else:
files.append(a)
files.sort()
num_files = len(files)
print ("num_files=%u" % num_files)
region_count = 0
joes = []
if opts.mavlog:
mpos = mav_position.MavInterpolator(gps_lag=opts.gps_lag)
mpos.set_logfile(opts.mavlog)
else:
mpos = None
if opts.boundary:
boundary = cuav_util.polygon_load(opts.boundary)
else:
boundary = None
if opts.mosaic:
slipmap = mp_slipmap.MPSlipMap(service="GoogleSat", elevation=True, title="Map")
icon = slipmap.icon("planetracker.png")
slipmap.add_object(
mp_slipmap.SlipIcon("plane", (0, 0), icon, layer=3, rotation=0, follow=True, trail=mp_slipmap.SlipTrail())
)
C_params = cam_params.CameraParams(lens=opts.lens)
path = os.path.join(
os.path.dirname(os.path.realpath(__file__)), "..", "..", "cuav", "data", "chameleon1_arecont0.json"
)
C_params.load(path)
mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params)
if boundary is not None:
mosaic.set_boundary(boundary)
if opts.joe:
joes = cuav_util.polygon_load(opts.joe)
if boundary:
for i in range(len(joes)):
joe = joes[i]
if cuav_util.polygon_outside(joe, boundary):
print ("Error: joe outside boundary", joe)
return
icon = slipmap.icon("flag.png")
slipmap.add_object(mp_slipmap.SlipIcon("joe%u" % i, (joe[0], joe[1]), icon, layer=4))
joelog = cuav_joe.JoeLog("joe.log")
if opts.view:
viewer = mp_image.MPImage(title="Image")
for f in files:
frame_time = cuav_util.parse_frame_time(f)
if mpos:
try:
if opts.roll_stabilised:
roll = 0
else:
roll = None
pos = mpos.position(frame_time, opts.max_deltat, roll=roll)
slipmap.set_position("plane", (pos.lat, pos.lon), rotation=pos.yaw)
except mav_position.MavInterpolatorException as e:
print e
pos = None
else:
pos = None
# check for any events from the map
if opts.mosaic:
slipmap.check_events()
mosaic.check_events()
if f.endswith(".pgm"):
pgm = cuav_util.PGM(f)
im = pgm.array
if pgm.eightbit:
im_8bit = im
else:
im_8bit = numpy.zeros((960, 1280, 1), dtype="uint8")
if opts.gamma != 0:
scanner.gamma_correct(im, im_8bit, opts.gamma)
else:
scanner.reduce_depth(im, im_8bit)
im_full = numpy.zeros((960, 1280, 3), dtype="uint8")
scanner.debayer_full(im_8bit, im_full)
im_640 = numpy.zeros((480, 640, 3), dtype="uint8")
scanner.downsample(im_full, im_640)
else:
im_full = cv.LoadImage(f)
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
img_scan = im_640
t0 = time.time()
for i in range(opts.repeat):
if opts.fullres:
regions = scanner.scan_full(im_full)
else:
regions = scanner.scan(img_scan)
count += 1
regions = cuav_region.RegionsConvert(regions)
t1 = time.time()
if opts.filter:
regions = cuav_region.filter_regions(im_full, regions, frame_time=frame_time, min_score=opts.minscore)
scan_count += 1
# optionally link all the images with joe into a separate directory
# for faster re-running of the test with just joe images
if pos and opts.linkjoe and len(regions) > 0:
cuav_util.mkdir_p(opts.linkjoe)
if not cuav_util.polygon_outside((pos.lat, pos.lon), boundary):
joepath = os.path.join(opts.linkjoe, os.path.basename(f))
if os.path.exists(joepath):
os.unlink(joepath)
os.symlink(f, joepath)
if pos and len(regions) > 0:
joelog.add_regions(frame_time, regions, pos, f, width=1280, height=960, altitude=opts.altitude)
if boundary:
regions = cuav_region.filter_boundary(regions, boundary, pos)
region_count += len(regions)
if opts.mosaic and len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(
cv.GetImage(cv.fromarray(im_full)), regions, quality=opts.quality
)
chameleon.save_file("composite.jpg", composite)
thumbs = cuav_mosaic.ExtractThumbs(cv.LoadImage("composite.jpg"), len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if opts.compress:
jpeg = scanner.jpeg_compress(im_full, opts.quality)
jpeg_filename = f[:-4] + ".jpg"
if os.path.exists(jpeg_filename):
print ("jpeg %s already exists" % jpeg_filename)
continue
chameleon.save_file(jpeg_filename, jpeg)
if opts.view:
if opts.fullres:
img_view = im_full
else:
img_view = img_scan
mat = cv.fromarray(img_view)
for r in regions:
(x1, y1, x2, y2) = r.tuple()
if opts.fullres:
x1 *= 2
y1 *= 2
x2 *= 2
y2 *= 2
cv.Rectangle(mat, (max(x1 - 2, 0), max(y1 - 2, 0)), (x2 + 2, y2 + 2), (255, 0, 0), 2)
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
total_time += t1 - t0
print ("%s scan %.1f fps %u regions [%u/%u]" % (f, count / total_time, region_count, scan_count, num_files))
def transmit_thread():
'''thread for image transmit to GCS'''
state = mpstate.camera_state
tx_count = 0
skip_count = 0
bsend = block_xmit.BlockSender(0, state.bandwidth)
while not state.unload.wait(0.02):
bsend.tick()
if state.transmit_queue.empty():
continue
(frame_time, regions, im_full, im_640) = state.transmit_queue.get()
pos = get_plane_position(frame_time)
latlon_list = log_joe_position(pos, frame_time, regions)
state.xmit_queue = bsend.sendq_size()
state.efficiency = bsend.get_efficiency()
state.bandwidth_used = bsend.get_bandwidth_used()
state.rtt_estimate = bsend.get_rtt_estimate()
jpeg = None
if len(regions) > 0 and bsend.sendq_size() < 2000:
# send a region message with thumbnails to the ground station
thumb = cuav_mosaic.CompositeThumbnail(im_full,
regions,
quality=state.quality,
thumb_size=100)
bsend.set_bandwidth(state.bandwidth)
bsend.set_packet_loss(state.packet_loss)
pkt = ThumbPacket(frame_time, regions, thumb, latlon_list,
state.frame_loss, state.xmit_queue)
# send matches with a higher priority
bsend.send(cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL),
dest=(state.gcs_address, state.gcs_view_port),
priority=1)
# Base how many images we send on the send queue size
send_frequency = state.xmit_queue // 3
if send_frequency == 0 or (tx_count +
skip_count) % send_frequency == 0:
jpeg = scanner.jpeg_compress(im_640, state.quality)
if jpeg is None:
skip_count += 1
continue
# keep filtered image size
state.jpeg_size = 0.95 * state.jpeg_size + 0.05 * len(jpeg)
tx_count += 1
if state.gcs_address is None:
continue
bsend.set_packet_loss(state.packet_loss)
bsend.set_bandwidth(state.bandwidth)
pkt = ImagePacket(frame_time, jpeg)
bsend.send(cPickle.dumps(pkt, cPickle.HIGHEST_PROTOCOL),
dest=(state.gcs_address, state.gcs_view_port))
def process(filename):
'''process one file'''
pgm = cuav_util.PGM(filename)
img_full_grey = pgm.array
im_full = numpy.zeros((960,1280,3),dtype='uint8')
im_640 = numpy.zeros((480,640,3),dtype='uint8')
t0 = time.time()
for i in range(opts.repeat):
scanner.debayer(img_full_grey, im_640)
t1 = time.time()
print('debayer: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
for i in range(opts.repeat):
scanner.debayer_full(img_full_grey, im_full)
t1 = time.time()
print('debayer_full: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
im_full2 = cv.CreateImage((1280,960),8,3)
img_full_grey2 = cv.GetImage(cv.fromarray(img_full_grey))
for i in range(opts.repeat):
cv.CvtColor(img_full_grey2, im_full2, cv.CV_BayerBG2BGR)
t1 = time.time()
print('debayer_cv_full: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
for i in range(opts.repeat):
img = cv.GetImage(cv.fromarray(im_full))
cv.CvtColor(img, img, cv.CV_RGB2HSV)
t1 = time.time()
print('RGB2HSV_full: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
for i in range(opts.repeat):
img = cv.GetImage(cv.fromarray(im_640))
cv.CvtColor(img, img, cv.CV_RGB2HSV)
t1 = time.time()
print('RGB2HSV_640: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
for i in range(opts.repeat):
thumb = numpy.empty((100,100,3),dtype='uint8')
scanner.rect_extract(im_full, thumb, 120, 125)
t1 = time.time()
print('rect_extract: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
for i in range(opts.repeat):
thumb = cuav_util.SubImage(cv.GetImage(cv.fromarray(im_full)), (120,125,100,100))
t1 = time.time()
print('SubImage: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
for i in range(opts.repeat):
scanner.downsample(im_full, im_640)
t1 = time.time()
print('downsample: %.1f fps' % (opts.repeat/(t1-t0)))
t0 = time.time()
for i in range(opts.repeat):
scanner.scan(im_640)
t1 = time.time()
print('scan: %.1f fps' % (opts.repeat/(t1-t0)))
for quality in [30, 40, 50, 60, 70, 80, 90, 95]:
t0 = time.time()
for i in range(opts.repeat):
jpeg = cPickle.dumps(ImagePacket(time.time(), scanner.jpeg_compress(im_full, quality)),
protocol=cPickle.HIGHEST_PROTOCOL)
t1 = time.time()
print('jpeg full quality %u: %.1f fps %u bytes' % (quality, opts.repeat/(t1-t0), len(bytes(jpeg))))
for quality in [30, 40, 50, 60, 70, 80, 90, 95]:
t0 = time.time()
for i in range(opts.repeat):
jpeg = cPickle.dumps(ImagePacket(time.time(), scanner.jpeg_compress(im_640, quality)),
protocol=cPickle.HIGHEST_PROTOCOL)
t1 = time.time()
print('jpeg 640 quality %u: %.1f fps %u bytes' % (quality, opts.repeat/(t1-t0), len(bytes(jpeg))))
for thumb_size in [10, 20, 40, 60, 80, 100]:
thumb = numpy.zeros((thumb_size,thumb_size,3),dtype='uint8')
t0 = time.time()
for i in range(opts.repeat):
scanner.rect_extract(im_full, thumb, 0, 0)
jpeg = cPickle.dumps(ImagePacket(time.time(), scanner.jpeg_compress(thumb, 85)),
protocol=cPickle.HIGHEST_PROTOCOL)
t1 = time.time()
print('thumb %u quality 85: %.1f fps %u bytes' % (thumb_size, opts.repeat/(t1-t0), len(bytes(jpeg))))
