def view_imagefile(self, filename): '''view an image in a zoomable window''' img = cuav_util.LoadImage(filename, rotate180=self.camera_settings.rotate180) (w,h) = cuav_util.image_shape(img) for i in range(len(self.images)): if filename == self.images[i].filename: self.current_view = i self.images[i].shape = (w,h) for r in self.regions: if r.filename == filename: r.region.draw_rectangle(img, colour=(255,0,0), linewidth=min(max(w/600,1),3), offset=max(w/200,1)) if self.view_image is None or not self.view_image.is_alive(): import wx self.view_image = mp_image.MPImage(title='View', mouse_events=True, key_events=True, can_zoom=True, can_drag=True) vmenu = MPMenuSubMenu('View', items=[ MPMenuItem('Next Image\tCtrl+N', 'Next Image', 'nextImage'), MPMenuItem('Previous Image\tCtrl+P', 'Previous Image', 'previousImage'), MPMenuItem('Fit Window\tCtrl+F', 'Fit Window', 'fitWindow'), MPMenuItem('Full Zoom\tCtrl+Z', 'Full Zoom', 'fullSize'), MPMenuItem('Brightness +\tCtrl+B', 'Increase Brightness', 'increaseBrightness'), MPMenuItem('Brightness -\tCtrl+Shift+B', 'Decrease Brightness', 'decreaseBrightness')]) self.view_menu = MPMenuTop([vmenu]) self.view_image.set_menu(self.view_menu) self.view_image.set_popup_menu(vmenu) self.view_filename = filename self.view_image.set_image(img, bgr=True) self.view_image.set_title('View: ' + os.path.basename(filename))
def __init__(self, mpstate): super(CameraGroundModule, self).__init__(mpstate, "camera_ground", "cuav camera control (ground)", public=True, multi_vehicle=True) self.unload_event = threading.Event() self.unload_event.clear() self.transmit_queue = Queue.Queue() self.decoder = video_play.VideoReader() self.is_armed = False self.capture_count = 0 self.image = None self.last_capture_count = None self.handled_timestamps = {} self.viewer = mp_image.MPImage(title='Image', width=200, height=200, auto_size=True) from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting self.camera_settings = MPSettings([ MPSetting('m_bandwidth', int, 500, 'max bandwidth on mavlink', increment=1, tab='GCS'), MPSetting('m_maxqueue', int, 5, 'Maximum images queue for mavlink', tab='GCS'), ], title='Camera Settings') self.msocket = None self.msend = None self.last_heartbeat = time.time() self.transmit_thread = self.start_thread(self.transmit_threadfunc) self.add_command('camera', self.cmd_camera, 'camera control', ['<status>', 'set (CAMERASETTING)']) self.add_completion_function('(CAMERASETTING)', self.camera_settings.completion) # prevent loopback of messages for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']: self.module('link').no_fwd_types.add(mtype) print("camera ground initialised")
pgm = cuav_util.PGM(filename) im_640 = numpy.zeros((480, 640, 3), dtype='uint8') raw_image = pgm.array show_mask(raw_image) scanner.thermal_convert(pgm.array, im_640, threshold, blue_threshold, green_threshold) color_img = cv.CreateImageHeader((640, 480), 8, 3) cv.SetData(color_img, im_640) return color_img view_image = mp_image.MPImage(title='ThermalView', width=640, height=480, mouse_events=True, key_events=True, can_zoom=True, can_drag=True) menu = MPMenuTop([]) view_menu = MPMenuSubMenu('View', [ MPMenuItem('Next Image\tCtrl+N', 'Next Image', 'nextImage'), MPMenuItem('Previous Image\tCtrl+P', 'Previous Image', 'previousImage') ]) menu.add(view_menu) view_image.set_menu(menu) settings = MPSettings([ MPSetting('threshold', int,
import argparse import airsim from MAVProxy.modules.lib import multiproc from MAVProxy.modules.lib import mp_image import time if __name__ == '__main__': multiproc.freeze_support() ap = argparse.ArgumentParser() ap.add_argument("--delay", type=float, default=0.5) ap.add_argument("--port", type=int, default=19721) ap.add_argument("--title", type=str, default='FPV Camera Feed') args = ap.parse_args() viewer = mp_image.MPImage(title=args.title, width=320, height=240, auto_size=True) client = airsim.MultirotorClient() lastt = time.time() dt_list = [] j = 1 while True: apiRequest = client.SimGetImages( [airsim.ImageRequest("0", airsim.ImageType.Scene, False, False)]) apiSingle = apiRequest[0] encodedImage = apiSingle.image_data_uint8 timestamp = apiSingle.time_stamp now = time.time()
def process(args): '''process a set of files''' global slipmap, mosaic scan_count = 0 files = [] if os.path.isdir(args.directory): files.extend(file_list(args.directory, ['jpg', 'pgm', 'png'])) else: if args.directory.find('*') != -1: files.extend(glob.glob(args.directory)) else: files.append(args.directory) files.sort() num_files = len(files) print("num_files=%u" % num_files) region_count = 0 slipmap = mp_slipmap.MPSlipMap(service=args.service, elevation=True, title='Map') icon = slipmap.icon('redplane.png') slipmap.add_object( mp_slipmap.SlipIcon('plane', (0, 0), icon, layer=3, rotation=0, follow=True, trail=mp_slipmap.SlipTrail())) for flag in args.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = slipmap.icon(icon) slipmap.add_object( mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False)) if args.mission: from pymavlink import mavwp wp = mavwp.MAVWPLoader() wp.load(args.mission.name) plist = wp.polygon_list() if len(plist) > 0: for i in range(len(plist)): slipmap.add_object( mp_slipmap.SlipPolygon('Mission-%s-%u' % (args.mission.name, i), plist[i], layer='Mission', linewidth=2, colour=(255, 255, 255))) if args.mavlog: mpos = mav_position.MavInterpolator() mpos.set_logfile(args.mavlog.name) else: mpos = None if args.gammalog is not None: gamma = parse_gamma_log(args.gammalog) else: gamma = None if args.kmzlog: kmzpos = mav_position.KmlPosition(args.kmzlog.name) else: kmzpos = None if args.triggerlog: triggerpos = mav_position.TriggerPosition(args.triggerlog.name) else: triggerpos = None # create a simple lens model using the focal length if args.camera_params: C_params = cam_params.CameraParams.fromfile(args.camera_params.name) else: C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=args.xresolution, yresolution=args.yresolution) if args.target: target = args.target.split(',') else: target = [0, 0, 0] camera_settings = MPSettings([ MPSetting('roll_stabilised', bool, args.roll_stabilised, 'Roll Stabilised'), MPSetting('altitude', int, args.altitude, 'Altitude', range=(0, 10000), increment=1), MPSetting( 'minalt', int, 30, 'MinAltitude', range=(0, 10000), increment=1), MPSetting('mpp100', float, 0.0977, 'MPPat100m', range=(0, 10000), increment=0.001), MPSetting('rotate180', bool, args.rotate_180, 'rotate180'), MPSetting('filter_type', str, 'compactness', 'Filter Type', choice=['simple', 'compactness']), MPSetting('target_lattitude', float, float(target[0]), 'target latitude', increment=1.0e-7), MPSetting('target_longitude', float, float(target[1]), 'target longitude', increment=1.0e-7), MPSetting('target_radius', float, float(target[2]), 'target radius', increment=1), MPSetting('quality', int, 75, 'Compression Quality', range=(1, 100), increment=1), MPSetting('thumbsize', int, args.thumbsize, 'Thumbnail Size', range=(10, 200), increment=1), MPSetting('minscore', int, args.minscore, 'Min Score', range=(0, 1000), increment=1, tab='Scoring'), MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1, digits=2, tab='Display'), ], title='Camera Settings') image_settings = MPSettings([ MPSetting('MinRegionArea', float, 0.05, range=(0, 100), increment=0.05, digits=2, tab='Image Processing'), MPSetting('MaxRegionArea', float, 4.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MinRegionSize', float, 0.02, range=(0, 100), increment=0.05, digits=2), MPSetting('MaxRegionSize', float, 3.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MaxRarityPct', float, 0.02, range=(0, 100), increment=0.01, digits=2), MPSetting('RegionMergeSize', float, 1.0, range=(0, 100), increment=0.1, digits=1), MPSetting('BlueEmphasis', bool, args.blue_emphasis), MPSetting('SaveIntermediate', bool, args.debug) ], title='Image Settings') mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params, camera_settings=camera_settings, image_settings=image_settings, start_menu=True, classify=args.categories, thumb_size=args.mosaic_thumbsize) joelog = cuav_joe.JoeLog(None) if args.view: viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True) start_time = time.time() for f in files: if not mosaic.started(): print("Waiting for startup") if args.start: mosaic.has_started = True while not mosaic.started(): mosaic.check_events() time.sleep(0.01) if mpos: # get the position by interpolating telemetry data from the MAVLink log file # this assumes that the filename contains the timestamp if gamma is not None: frame_time = parse_gamma_time(f, gamma) else: frame_time = cuav_util.parse_frame_time(f) frame_time += args.time_offset if camera_settings.roll_stabilised: roll = 0 else: roll = None try: pos = mpos.position(frame_time, roll=roll) except Exception: print("No position available for %s" % frame_time) # skip this frame continue elif kmzpos is not None: pos = kmzpos.position(f) elif triggerpos is not None: pos = triggerpos.position(f) else: # get the position using EXIF data pos = mav_position.exif_position(f) pos.time += args.time_offset # update the plane icon on the map if pos is not None: slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw) if camera_settings.altitude > 0: pos.altitude = camera_settings.altitude # check for any events from the map slipmap.check_events() mosaic.check_events() im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180) if im_orig is None: continue (w, h) = cuav_util.image_shape(im_orig) if False: im_640 = cv.CreateImage((640, 480), 8, 3) cv.Resize(im_full, im_640, cv.CV_INTER_NN) im_640 = numpy.ascontiguousarray(cv.GetMat(im_640)) im_full = im_orig im_full = numpy.ascontiguousarray(cv.GetMat(im_full)) count = 0 total_time = 0 t0 = time.time() img_scan = im_full scan_parms = {} for name in image_settings.list(): scan_parms[name] = image_settings.get(name) scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate']) scan_parms['BlueEmphasis'] = float(scan_parms['BlueEmphasis']) if pos is not None: (sw, sh) = cuav_util.image_shape(img_scan) altitude = pos.altitude if altitude < camera_settings.minalt: altitude = camera_settings.minalt scan_parms[ 'MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0 regions = scanner.scan(img_scan, scan_parms) else: regions = scanner.scan(img_scan) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full)) count += 1 t1 = time.time() frame_time = pos.time if pos: for r in regions: r.latlon = cuav_util.gps_position_from_image_region( r, pos, w, h, altitude=altitude, C=C_params) if camera_settings.target_radius > 0 and pos is not None: regions = cuav_region.filter_radius( regions, (camera_settings.target_lattitude, camera_settings.target_longitude), camera_settings.target_radius) regions = cuav_region.filter_regions( im_full, regions, frame_time=frame_time, min_score=camera_settings.minscore, filter_type=camera_settings.filter_type) scan_count += 1 if pos and len(regions) > 0: altitude = camera_settings.altitude if altitude <= 0: altitude = None joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=altitude) mosaic.add_image(pos.time, f, pos) region_count += len(regions) if len(regions) > 0: composite = cuav_mosaic.CompositeThumbnail( cv.GetImage(cv.fromarray(im_full)), regions) thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions)) mosaic.add_regions(regions, thumbs, f, pos) if args.view: img_view = img_scan (wview, hview) = cuav_util.image_shape(img_view) mat = cv.fromarray(img_view) for r in regions: r.draw_rectangle(mat, (255, 0, 0)) cv.CvtColor(mat, mat, cv.CV_BGR2RGB) viewer.set_image(mat) viewer.set_title('Image: ' + os.path.basename(f)) if args.saveview: cv.CvtColor(mat, mat, cv.CV_RGB2BGR) cv.SaveImage('view-' + os.path.basename(f), mat) total_time += (t1 - t0) if t1 != t0: print('%s scan %.1f fps %u regions [%u/%u]' % (os.path.basename(f), count / total_time, region_count, scan_count, num_files)) #raw_input("hit ENTER when ready") print("All images processed (%u seconds)" % (time.time() - start_time)) while True: # check for any events from the map slipmap.check_events() mosaic.check_events() time.sleep(0.2)
def check_commands(self, bsend): '''check for remote commands''' if bsend is None: return buf = bsend.recv(0) if buf is None: return try: obj = cPickle.loads(str(buf)) if obj is None: return except Exception as e: return if isinstance(obj, cuav_command.StampedCommand): bidx = None for i in range(len(self.bsend)): if bsend == self.bsend[i]: bidx = i + 1 if bidx is None and bsend == self.msend: bidx = 0 if bidx is not None: now = time.time() self.mcount[bidx] += 1 self.last_msg_stamp[bidx] = now if obj.timestamp in self.handled_timestamps: # we've seen this packet before, discard return self.handled_timestamps[obj.timestamp] = time.time() if isinstance(obj, cuav_command.HeartBeat): self.image_count = obj.icount self.console.set_status('Images', 'Images %u' % self.image_count, row=6) if isinstance(obj, cuav_command.ThumbPacket): # we've received a set of thumbnails from the plane for a positive hit if obj.frame_time not in self.thumbs_received: self.thumbs_received.add(obj.frame_time) self.thumb_total_bytes += len(buf) # add the thumbnails to the mosaic thumbdec = cv2.imdecode(obj.thumb, 1) if thumbdec is None: pass thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions)) thumbsRGB = [] #colour space conversion for thumb in thumbs: thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB)) # log the joe positions # note the filename is where the fullsize image would be downloaded # to, if requested filename = os.path.join( self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg" self.log_joe_position(obj.pos, obj.frame_time, obj.regions, filename, None) # update the mosaic and map self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos) # update console display self.region_count += len(obj.regions) self.thumb_count += 1 self.console.set_status('Regions', 'Regions %u' % self.region_count, row=6) self.console.set_status('Thumbs', 'Thumbs %u' % self.thumb_count, row=7) self.console.set_status( 'ThumbSize', 'ThumbSize %.0f' % (self.thumb_total_bytes / self.thumb_count), row=7) if isinstance(obj, cuav_command.ImagePacket): # we have an image from the plane self.image_total_bytes += len(buf) #save to file imagedec = cv2.imdecode(obj.jpeg, 1) ff = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg" write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99] cv2.imwrite(ff, imagedec, write_param) self.mosaic.tag_image(obj.frame_time) if obj.pos is not None: self.mosaic.add_image(obj.frame_time, ff, obj.pos) # update console self.image_count += 1 color = 'black' self.console.set_status('Images', 'Images %u' % self.image_count, row=6, fg=color) if isinstance(obj, cuav_command.CommandPacket): # ground doesn't accept command packets from air pass if isinstance(obj, cuav_command.CommandResponse): # reply to CommandPacket print('REMOTE: %s' % obj.response) if isinstance(obj, cuav_command.CameraMessage): print('CUAV AIR REMOTE: %s' % obj.msg) if isinstance(obj, cuav_landingregion.LandingZoneDisplay): lzresult = obj # display on all maps for m in self.module_matching('map?'): m.map.add_object( mp_slipmap.SlipCircle('LZ', 'LZ', lzresult.latlon, lzresult.maxrange, linewidth=3, color=(0, 255, 0))) m.map.add_object( mp_slipmap.SlipCircle('LZMid', 'LZMid', lzresult.latlon, 2.0, linewidth=3, color=(0, 255, 0))) lztext = 'LZ: %.6f %.6f E:%.1f AS:%.0f N:%u' % ( lzresult.latlon[0], lzresult.latlon[1], lzresult.maxrange, lzresult.avgscore, lzresult.numregions) m.map.add_object(mp_slipmap.SlipInfoText( 'landingzone', lztext)) # assume map2 is the search map map2 = self.module('map2') if map2 is not None: #map2.map.set_zoom(250) map2.map.set_center(lzresult.latlon[0], lzresult.latlon[1]) map2.map.set_follow(0) # assume map3 is the lz map map3 = self.module('map3') if map3 is not None: map3.map.set_zoom(max(50, 2 * lzresult.maxrange)) map3.map.set_center(lzresult.latlon[0], lzresult.latlon[1]) map3.map.set_follow(0) try: cuav = self.module('CUAV') cuav.show_JoeZone() except Exception as ex: print("err: ", ex) return if isinstance(obj, cuav_command.FilePacket): print("got file %s" % obj.filename) try: open(obj.filename, "w").write(obj.contents) except Exception as ex: print("file save failed", ex) return if obj.filename == "newwp.txt": try: wpmod = self.module('wp') wpmod.wploader.load(obj.filename) except Exception as ex: print("wp load failed", ex) if isinstance(obj, cuav_command.PreviewPacket): # we have a preview image from the plane img = cv2.imdecode(obj.jpeg, 1) if self.preview_window is None or not self.preview_window.is_alive( ): self.preview_window = mp_image.MPImage(title='Preview', width=410, height=308, auto_size=True) if self.preview_window is not None: self.preview_window.set_image(img, bgr=True) self.preview_window.poll()
if __name__ == '__main__': import argparse import sys from MAVProxy.modules.lib import mp_image # construct the argument parse and parse the arguments ap = argparse.ArgumentParser() ap.add_argument("infile", type=str) args = ap.parse_args() decoder = VideoReader() counter = 0 viewer = mp_image.MPImage(title='Decoded', width=200, height=200, auto_size=True) f = open(args.infile, "rb") while True: time.sleep(0.1) blk = f.read(512) if blk is None or len(blk) == 0: break print("Block %u" % counter) counter += 1 img = decoder.get_image(blk) while img is not None: viewer.set_image(img) img = decoder.get_image('')
import video_decode_dec265 encoder = video_encode_x265.VideoWriter() decoder = video_decode_dec265.VideoReader() else: import video_encode import video_play encoder = video_encode.VideoWriter() decoder = video_play.VideoReader() if args.crop: encoder.set_cropstr(args.crop) viewer_in = mp_image.MPImage(title='Original', width=200, height=200, auto_size=True) viewer_out = mp_image.MPImage(title='Decoded', width=200, height=200, auto_size=True) idx = 0 timestamp_ms = 0 enc_saved = open("enc.dat", "wb") total_bytes = 0 total_frames = 0 while True:
def process(args): '''process a set of files''' global slipmap, mosaic scan_count = 0 files = scan_image_directory(args.imagedir) files.sort() num_files = len(files) print("num_files=%u" % num_files) region_count = 0 joes = [] if args.mavlog: mpos = mav_position.MavInterpolator(gps_lag=args.gps_lag) mpos.set_logfile(args.mavlog) else: mpos = None if args.boundary: boundary = cuav_util.polygon_load(args.boundary) else: boundary = None if args.mosaic: slipmap = mp_slipmap.MPSlipMap(service='GoogleSat', elevation=True, title='Map') icon = slipmap.icon('redplane.png') slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0, follow=True, trail=mp_slipmap.SlipTrail())) if args.camera_params: C_params = cam_params.CameraParams.fromfile(args.camera_params.name) else: im_orig = cv2.imread(files[0]) (w,h) = cuav_util.image_shape(im_orig) C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=w, yresolution=h) mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params) if boundary is not None: mosaic.set_boundary(boundary) if args.joe: joes = cuav_util.polygon_load(args.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 args.view: viewer = mp_image.MPImage(title='Image') frame_time = 0 scan_parms = { 'MinRegionArea' : args.min_region_area, 'MaxRegionArea' : args.max_region_area, 'MinRegionSize' : args.min_region_size, 'MaxRegionSize' : args.max_region_size, 'MaxRarityPct' : args.max_rarity_pct, 'RegionMergeSize' : args.region_merge, 'SaveIntermediate' : float(0), #'SaveIntermediate' : float(args.debug), 'MetersPerPixel' : args.meters_per_pixel100 * args.altitude / 100.0 } filenum = 0 for f in files: filenum += 1 if mpos: frame_time = cuav_util.parse_frame_time(f) try: if args.roll_stabilised: roll = 0 else: roll = None pos = mpos.position(frame_time, args.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 args.mosaic: slipmap.check_events() mosaic.check_events() im_orig = cv2.imread(f) (w,h) = cuav_util.image_shape(im_orig) im_full = im_orig im_half = cv2.resize(im_orig, (0,0), fx=0.5, fy=0.5) im_half = numpy.ascontiguousarray(im_half) im_full = numpy.ascontiguousarray(im_full) count = 0 total_time = 0 if args.fullres: img_scan = im_full else: img_scan = im_half t0=time.time() for i in range(args.repeat): regions = scanner.scan(img_scan, scan_parms) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full)) count += 1 t1=time.time() if args.filter: regions = cuav_region.filter_regions(im_full, regions, min_score=args.minscore, filter_type=args.filter_type) if len(regions) > 0 and args.debug: composite = cuav_region.CompositeThumbnail(im_full, regions, thumb_size=args.thumb_size) thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions)) thumb_num = 0 for thumb in thumbs: print("thumb %u score %f" % (thumb_num, regions[thumb_num].score)) cv2.imwrite('%u_thumb%u.jpg' % (filenum,thumb_num), thumb) thumb_num += 1 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 args.linkjoe and len(regions) > 0: cuav_util.mkdir_p(args.linkjoe) if not cuav_util.polygon_outside((pos.lat, pos.lon), boundary): joepath = os.path.join(args.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=w, height=h, altitude=args.altitude, C=C_params) if boundary: regions = cuav_region.filter_boundary(regions, boundary, pos) region_count += len(regions) if args.mosaic and len(regions) > 0 and pos: composite = cuav_region.CompositeThumbnail(im_full, regions) thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions)) mosaic.add_regions(regions, thumbs, f, pos) if args.view: if args.fullres: img_view = im_full else: img_view = img_scan #mat = cv.fromarray(img_view) for r in regions: r.draw_rectangle(img_view, colour=(255,0,0), linewidth=min(max(w/600,1),3), offset=max(w/200,1)) img_view = cv2.cvtColor(img_view, cv2.COLOR_BGR2RGB) viewer.set_image(img_view) total_time += (t1-t0) if t1 != t0: print('%s scan %.1f fps %u regions [%u/%u]' % ( f, count/total_time, region_count, scan_count, num_files))
def __init__(self, slipmap, grid_width=20, grid_height=20, thumb_size=35, C=CameraParams(), camera_settings = None, image_settings = None, start_menu=False, classify=None): self.thumb_size = thumb_size self.width = grid_width * thumb_size self.height = grid_height * thumb_size self.mosaic = cv.CreateImage((self.height,self.width),8,3) cuav_util.zero_image(self.mosaic) self.display_regions = grid_width*grid_height self.regions = [] self.regions_sorted = [] self.regions_hidden = set() self.mouse_region = None self.ridx_by_frame_time = {} self.page = 0 self.sort_type = 'Time' self.images = [] self.current_view = 0 self.last_view_latlon = None self.view_filename = None self.full_res = False self.boundary = [] self.displayed_image = None self.last_click_position = None self.c_params = C self.camera_settings = camera_settings self.image_settings = image_settings self.start_menu = start_menu self.classify = classify self.has_started = not start_menu import wx self.image_mosaic = mp_image.MPImage(title='Mosaic', mouse_events=True, key_events=True, auto_size=False, report_size_changes=True) self.slipmap = slipmap self.selected_region = 0 self.view_image = None self.brightness = 1 # dictionary of image requests, contains True if fullres image is wanted self.image_requests = {} self.slipmap.add_callback(functools.partial(self.map_callback)) if classify: import lxml.objectify, lxml.etree with open(classify) as f: categories = lxml.objectify.fromstring(f.read()) cat_names = set() self.categories = [] try: for c in categories.category: self.categories.append((c.get('shortcut') or '', c.text)) if c.text in cat_names: print 'WARNING: category name',c.text,'used more than once' else: cat_names.add(c.text) except AttributeError as ex: print ex print 'failed to load any categories for classification' self.region_class = lxml.objectify.E.regions() self.add_menus()
pgm = cuav_util.PGM(filename) im2 = numpy.zeros((opts.height, opts.width, 3), dtype='uint8') raw_image = pgm.array show_mask(raw_image) scanner.thermal_convert(pgm.array, im2, threshold, blue_threshold, green_threshold) color_img = cv.CreateImageHeader((opts.width, opts.height), 8, 3) cv.SetData(color_img, im2) return color_img view_image = mp_image.MPImage(title='ThermalView', width=opts.width, height=opts.height, mouse_events=True, key_events=True, can_zoom=True, can_drag=True) menu = MPMenuTop([]) view_menu = MPMenuSubMenu('View', [ MPMenuItem('Next Image\tCtrl+N', 'Next Image', 'nextImage'), MPMenuItem('Previous Image\tCtrl+P', 'Previous Image', 'previousImage') ]) menu.add(view_menu) view_image.set_menu(menu) settings = MPSettings([ MPSetting('threshold', int,
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') frame_time = 0 scan_parms = { 'MinRegionArea': opts.min_region_area, 'MaxRegionArea': opts.max_region_area, 'MinRegionSize': opts.min_region_size, 'MaxRegionSize': opts.max_region_size, 'MaxRarityPct': opts.max_rarity_pct, 'RegionMergeSize': opts.region_merge, 'SaveIntermediate': float(opts.debug), 'MetersPerPixel': opts.meters_per_pixel } if opts.filter_type == 'compactness': calculate_compactness = True print("Using compactness filter") else: calculate_compactness = False for f in files: if mpos: frame_time = cuav_util.parse_frame_time(f) 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(im_8bit, im_full) im_640 = numpy.zeros((480, 640, 3), dtype='uint8') scanner.downsample(im_full, im_640) else: im_orig = cv.LoadImage(f) (w, h) = cuav_util.image_shape(im_orig) im_full = im_orig im_640 = cv.CreateImage((640, 480), 8, 3) cv.Resize(im_full, im_640, cv.CV_INTER_NN) im_640 = numpy.ascontiguousarray(cv.GetMat(im_640)) im_full = numpy.ascontiguousarray(cv.GetMat(im_full)) count = 0 total_time = 0 if opts.fullres: img_scan = im_full else: img_scan = im_640 t0 = time.time() for i in range(opts.repeat): regions = scanner.scan(img_scan, scan_parms) regions = cuav_region.RegionsConvert( regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full), calculate_compactness) count += 1 t1 = time.time() if opts.filter: regions = cuav_region.filter_regions(im_full, regions, frame_time=frame_time, min_score=opts.minscore, filter_type=opts.filter_type) 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) thumbs = cuav_mosaic.ExtractThumbs(composite, 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() (w, h) = cuav_util.image_shape(img_view) x1 = x1 * w // 1280 x2 = x2 * w // 1280 y1 = y1 * h // 960 y2 = y2 * h // 960 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) if t1 != t0: print('%s scan %.1f fps %u regions [%u/%u]' % (f, count / total_time, region_count, scan_count, num_files))
def __init__(self, slipmap, grid_width=20, grid_height=20, thumb_size=35, map_thumb_size=None, C=None, camera_settings=None, image_settings=None, start_menu=False, classify=None, image_view_width=700, search_map=None, lz_map=None): if C is None: raise ValueError("camera parameters must be supplied") self.thumb_size = thumb_size if map_thumb_size is not None: self.map_thumb_size = map_thumb_size else: self.map_thumb_size = self.thumb_size self.width = grid_width * thumb_size self.height = grid_height * thumb_size self.mosaic = numpy.zeros((self.height, self.width, 3), dtype=numpy.uint32) self.display_regions = grid_width * grid_height self.regions = [] self.regions_sorted = [] self.regions_hidden = set() self.mouse_region = None self.ridx_by_frame_time = {} self.page = 0 self.sort_type = 'Score' self.images = [] self.current_view = 0 self.last_view_latlon = None self.view_filename = None self.full_res = False self.autorefresh = True self.topfifty = False self.boundary = [] self.displayed_image = None self.last_click_position = None self.c_params = C self.camera_settings = camera_settings self.image_settings = image_settings self.start_menu = start_menu self.classify = classify self.has_started = not start_menu self.image_view_width = image_view_width # for image viewer import wx self.image_mosaic = mp_image.MPImage(title='Mosaic', mouse_events=True, key_events=True, auto_size=False, report_size_changes=True) self.slipmap = slipmap self.search_map = search_map self.lz_map = lz_map self.allmaps = [slipmap] if self.search_map: self.allmaps.append(self.search_map) if self.lz_map: self.allmaps.append(self.lz_map) print("allmaps: ", self.allmaps) self.selected_region = 0 self.have_selected_region = False self.view_image = None self.brightness = 0 # dictionary of image requests, contains True if fullres image is wanted self.image_requests = {} for m in self.allmaps: m.add_callback(functools.partial(self.map_callback)) if classify: import lxml.objectify, lxml.etree with open(classify) as f: categories = lxml.objectify.fromstring(f.read()) cat_names = set() self.categories = [] try: for c in categories.category: self.categories.append((c.get('shortcut') or '', c.text)) if c.text in cat_names: print 'WARNING: category name', c.text, 'used more than once' else: cat_names.add(c.text) except AttributeError as ex: print ex print 'failed to load any categories for classification' self.region_class = lxml.objectify.E.regions() self.add_menus()
def view_imagefile(self, filename, focus_region=None): '''view an image in a zoomable window''' img = cv2.imread(filename) img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) (w, h) = cuav_util.image_shape(img) for i in range(len(self.images)): if filename == self.images[i].filename: self.current_view = i self.last_view_latlon = None self.images[i].shape = (w, h) for r in self.regions: if r.filename == filename: r.region.draw_rectangle(img, colour=(255, 0, 0), linewidth=min(max(w / 600, 1), 3), offset=max(w / 200, 1)) if self.view_image is None or not self.view_image.is_alive(): import wx viewwidth = w viewheight = h if self.image_view_width is not None: if viewwidth > self.image_view_width: viewwidth = self.image_view_width viewheight = (h * viewwidth) / w self.view_image = mp_image.MPImage(title='View', mouse_events=True, key_events=True, can_zoom=True, can_drag=True, width=viewwidth, height=viewheight) vmenu = MPMenuSubMenu('View', items=[ MPMenuItem('Next Image\tCtrl+N', 'Next Image', 'nextImage'), MPMenuItem('Previous Image\tCtrl+P', 'Previous Image', 'previousImage'), MPMenuItem('Fit Window\tCtrl+F', 'Fit Window', 'fitWindow'), MPMenuItem('Full Zoom\tCtrl+Z', 'Full Zoom', 'fullSize'), MPMenuItem('Brightness +\tCtrl+B', 'Increase Brightness', 'increaseBrightness'), MPMenuItem('Brightness -\tCtrl+Shift+B', 'Decrease Brightness', 'decreaseBrightness'), MPMenuItem('Refresh Image\tCtrl+R', 'Refresh Image', 'refreshImage'), MPMenuItem('Place Marker\tCtrl+M', 'Place Marker', 'placeMarker') ]) self.view_menu = MPMenuTop([vmenu]) self.view_image.set_menu(self.view_menu) self.view_image.set_popup_menu(vmenu) self.view_filename = filename self.view_image.set_image(img) if focus_region is not None: try: self.view_image.center(focus_region.center()) except Exception: pass self.view_image.set_title('View: ' + os.path.basename(filename)) self.view_image.fit_to_window()
def process(folderfile): '''process a file''' file_index = 0 files = [] if os.path.isdir(folderfile): files = file_list(folderfile, ['jpg', 'jpeg', 'png']) else: files.append(folderfile) fname = files[file_index] settings = MPSettings([ MPSetting('MetersPerPixel', float, 0.2, range=(0, 10), increment=0.01, digits=2, tab='Image Processing'), MPSetting('MinRegionArea', float, 0.15, range=(0, 100), increment=0.05, digits=2), MPSetting('MaxRegionArea', float, 2.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MinRegionSize', float, 0.1, range=(0, 100), increment=0.05, digits=2), MPSetting( 'MaxRegionSize', float, 2, range=(0, 100), increment=0.1, digits=1), MPSetting('MaxRarityPct', float, 0.02, range=(0, 100), increment=0.01, digits=2), MPSetting('RegionMergeSize', float, 3.0, range=(0, 100), increment=0.1, digits=1), MPSetting('minscore', int, 0, 'Min Score', range=(0, 1000), increment=1, tab='Scoring'), MPSetting( 'filter_type', str, 'simple', 'Filter Type', choice=['simple']), MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1, digits=2, tab='Display') ], title='Settings') menu = MPMenuSubMenu( 'View', items=[ MPMenuItem('Original Image', 'Original Image', '_1original.pnm'), MPMenuItem('Unquantised Image', 'Unquantised Image', '_1unquantised.pnm'), MPMenuItem('Thresholded Image', 'Thresholded Image', '_2thresholded.pnm'), MPMenuItem('Neighbours Image', 'Neighbours Image', '_3neighbours.pnm'), MPMenuItem('Regions Image', 'Regions Image', '_4regions.pnm'), MPMenuItem('Prune Large Image', 'Prune Large Image', '_5prunelarge.pnm'), MPMenuItem('Merged Image', 'Merged Large', '_6merged.pnm'), MPMenuItem('Pruned Image', 'Pruned Image', '_7pruned.pnm'), MPMenuItem('Fit Window', 'Fit Window', 'fitWindow'), MPMenuItem('Full Zoom', 'Full Zoom', 'fullSize'), MPMenuItem('Next Image', 'Next Image', 'nextImage'), MPMenuItem('Previous Image', 'Previous Image', 'previousImage') ]) im_orig = cv2.imread(fname, -1) im_numpy = numpy.ascontiguousarray(im_orig) # create the various views view = mp_image.MPImage(title='FullImage', can_zoom=True, can_drag=True) view.set_popup_menu(menu) dlg = wxsettings.WXSettings(settings) selected_image = "_1original.pnm" while dlg.is_alive() and view.is_alive(): last_change = settings.last_change() scan_parms = { 'MinRegionArea': settings.MinRegionArea, 'MaxRegionArea': settings.MaxRegionArea, 'MinRegionSize': settings.MinRegionSize, 'MaxRegionSize': settings.MaxRegionSize, 'MaxRarityPct': settings.MaxRarityPct, 'RegionMergeSize': settings.RegionMergeSize, 'SaveIntermediate': 1.0, 'MetersPerPixel': settings.MetersPerPixel } t0 = time.time() regions = scanner.scan(im_numpy, scan_parms) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(im_orig), cuav_util.image_shape(im_orig), False) t1 = time.time() print("Processing %s took %.2f seconds" % (fname, t1 - t0)) show_image(view, str(file_index + 1) + selected_image, im_orig, fname) while last_change == settings.last_change() and dlg.is_alive(): new_index = file_index for event in view.events(): if isinstance(event, MPMenuItem): if event.returnkey == 'nextImage': new_index = (file_index + 1) % len(files) elif event.returnkey == 'previousImage': new_index = (file_index - 1) % len(files) elif event.returnkey.endswith("pnm"): selected_image = event.returnkey show_image(view, str(file_index + 1) + selected_image, im_orig, fname) if new_index != file_index: file_index = new_index fname = files[file_index] im_orig = cv2.imread(fname, -1) im_numpy = numpy.ascontiguousarray(im_orig) break time.sleep(0.1) #remove all the pnm's directory = os.getcwd() lst = os.listdir(directory) for item in lst: if item.endswith(".pnm"): os.remove(os.path.join(directory, item))
def process(args): '''process a file''' file_index = 0 if os.path.isdir(args[0]): files = file_list(args[0], ['jpg', 'pgm', 'png']) else: files = args fname = files[file_index] settings = MPSettings([ MPSetting('MetersPerPixel', float, 0.2, range=(0, 10), increment=0.01, digits=2, tab='Image Processing'), MPSetting('MinRegionArea', float, 0.15, range=(0, 100), increment=0.05, digits=2), MPSetting('MaxRegionArea', float, 2.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MinRegionSize', float, 0.1, range=(0, 100), increment=0.05, digits=2), MPSetting( 'MaxRegionSize', float, 2, range=(0, 100), increment=0.1, digits=1), MPSetting('MaxRarityPct', float, 0.02, range=(0, 100), increment=0.01, digits=2), MPSetting('RegionMergeSize', float, 3.0, range=(0, 100), increment=0.1, digits=1), MPSetting('minscore', int, 0, 'Min Score', range=(0, 1000), increment=1, tab='Scoring'), MPSetting('filter_type', str, 'simple', 'Filter Type', choice=['simple', 'compactness']), MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1, digits=2, tab='Display') ], title='Settings') menu = MPMenuSubMenu( 'View', items=[ MPMenuItem('Original Image', 'Original Image', 'originalImage'), MPMenuItem('Unquantised Image', 'Unquantised Image', 'unquantisedImage'), MPMenuItem('Thresholded Image', 'Thresholded Image', 'thresholdedImage'), MPMenuItem('Neighbours Image', 'Neighbours Image', 'neighboursImage'), MPMenuItem('Regions Image', 'Regions Image', 'regionsImage'), MPMenuItem('Prune Large Image', 'Prune Large Image', 'prunelargeImage'), MPMenuItem('Merged Image', 'Merged Large', 'mergedImage'), MPMenuItem('Pruned Image', 'Pruned Image', 'prunedImage'), MPMenuItem('Fit Window', 'Fit Window', 'fitWindow'), MPMenuItem('Full Zoom', 'Full Zoom', 'fullSize'), MPMenuItem('Next Image', 'Next Image', 'nextImage'), MPMenuItem('Previous Image', 'Previous Image', 'previousImage') ]) im_orig = cuav_util.LoadImage(fname) im_numpy = numpy.ascontiguousarray(cv.GetMat(im_orig)) im_rgb = cv.fromarray(im_numpy) cv.CvtColor(im_rgb, im_rgb, cv.CV_BGR2RGB) # create the various views view = mp_image.MPImage(title='FullImage', can_zoom=True, can_drag=True) view.set_popup_menu(menu) dlg = wxsettings.WXSettings(settings) selected_image = "original" while dlg.is_alive() and view.is_alive(): last_change = settings.last_change() scan_parms = { 'MinRegionArea': settings.MinRegionArea, 'MaxRegionArea': settings.MaxRegionArea, 'MinRegionSize': settings.MinRegionSize, 'MaxRegionSize': settings.MaxRegionSize, 'MaxRarityPct': settings.MaxRarityPct, 'RegionMergeSize': settings.RegionMergeSize, 'SaveIntermediate': 1.0, 'MetersPerPixel': settings.MetersPerPixel } t0 = time.time() regions = scanner.scan(im_numpy, scan_parms) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(im_orig), cuav_util.image_shape(im_orig), False) t1 = time.time() print("Processing %s took %.2f seconds" % (fname, t1 - t0)) show_image(view, selected_image, im_rgb, fname) while last_change == settings.last_change() and dlg.is_alive(): new_index = file_index for event in view.events(): if isinstance(event, MPMenuItem): if event.returnkey == 'nextImage': new_index = (file_index + 1) % len(files) elif event.returnkey == 'previousImage': new_index = (file_index - 1) % len(files) elif event.returnkey.endswith("Image"): selected_image = event.returnkey[:-5] show_image(view, selected_image, im_rgb, fname) if new_index != file_index: file_index = new_index fname = files[file_index] im_orig = cuav_util.LoadImage(fname) im_numpy = numpy.ascontiguousarray(cv.GetMat(im_orig)) im_rgb = cv.fromarray(im_numpy) cv.CvtColor(im_rgb, im_rgb, cv.CV_BGR2RGB) break time.sleep(0.1)