def get_h5_start_stop(filename,careful=True): h5 = tables.open_file(filename,mode='r') try: camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) except: sys.stderr.write('While getting caminfo from "%s" (traceback below)\n'% filename) raise cam_ids = cam_id2camns.keys() h5_start_quick = h5.root.data2d_distorted[0]['timestamp'] h5_stop_quick = h5.root.data2d_distorted[-1]['timestamp'] if careful: h5_start = np.inf h5_stop = -np.inf for row in h5.root.data2d_distorted: ts = row['timestamp'] h5_start = min(ts,h5_start) h5_stop = max(ts,h5_stop) if (h5_start != h5_start_quick) or (h5_stop != h5_stop_quick): warnings.warn('quickly computed timestamps are not exactly correct') #print 'h5_start,h5_start_quick',repr(h5_start),repr(h5_start_quick) #print 'h5_stop, h5_stop_quick',repr(h5_stop), repr(h5_stop_quick) else: h5_start = h5_start_quick h5_stop = h5_stop_quick h5.close() return h5_start, h5_stop
def make_montage(h5_filename, cfg_filename=None, ufmf_dir=None, dest_dir=None, save_ogv_movie=False, no_remove=False, max_n_frames=None, start=None, stop=None, movie_fnames=None, movie_cam_ids=None, caminfo_h5_filename=None, colormap=None, kalman_filename=None, candidate_index=0, nth_frame=1, verbose=False, reconstructor=None, **kwargs): config = get_config_defaults() if cfg_filename is not None: loaded_cfg = cherrypy.lib.reprconf.as_dict(cfg_filename) for section in loaded_cfg: config[section].update(loaded_cfg.get(section, {})) else: warnings.warn('no configuration file specified -- using defaults') orientation_3d_line_length = 0.1 if (config['what to show']['show_3d_smoothed_position'] or config['what to show']['show_3d_MLE_position'] or config['what to show']['show_3d_raw_orientation'] or config['what to show']['show_3d_raw_chosen_orientation'] or config['what to show']['show_3d_smoothed_orientation'] or config['what to show']['show_3d_obj_position_text']): if kalman_filename is None: raise ValueError('need kalman filename to show requested 3D data') if config['what to show']['obj_labels']: if kalman_filename is None: raise ValueError('need kalman filename to show object labels') if kalman_filename is not None: if (config['what to show']['show_3d_smoothed_orientation'] or config['what to show']['show_3d_raw_orientation'] or config['what to show']['show_3d_raw_chosen_orientation']): need_quality_data = True else: need_quality_data = False if need_quality_data: # need data about quality of tracking data3d, dataqual_3d = load_3d_data(kalman_filename, start=start, stop=stop, require_qual=True, **kwargs) else: data3d = load_3d_data(kalman_filename, start=start, stop=stop, require_qual=False, **kwargs) dataqual_3d = None if (config['what to show']['show_3d_MLE_position'] or config['what to show']['show_3d_raw_orientation']): if need_quality_data: data_raw_3d, dataqual_raw_3d = load_3d_raw_data( kalman_filename, **kwargs) else: data_raw_3d = load_3d_raw_data(kalman_filename, require_qual=False, **kwargs) dataqual_raw_3d = None else: data_raw_3d, dataqual_raw_3d = None, None if reconstructor is None: R = reconstruct.Reconstructor(kalman_filename) else: R = reconstruct.Reconstructor(reconstructor) else: data3d = R = data_raw_3d = None dataqual_raw_3d = None dataqual_3d = None min_ori_qual = config['what to show'][ 'minimum_display_orientation_quality'] if movie_fnames is None: # This works based on UUIDs movie_fnames = auto_discover_movies.find_movies( h5_filename, ufmf_dir=ufmf_dir, candidate_index=candidate_index, verbose=verbose) if verbose: print 'autodiscovery: found movie_fnames: %r' % (movie_fnames, ) else: if verbose: print 'autodiscovery: movie_fnames specified, not finding movies' if len(movie_fnames) == 0: if verbose: print 'autodiscovery: no FMF files found, looking for ufmfs' movie_fnames = auto_discover_ufmfs.find_ufmfs( h5_filename, ufmf_dir=ufmf_dir, careful=True, verbose=verbose, ) else: if verbose: print 'autodiscovery: prefixing directory' if ufmf_dir is not None: if verbose: print 'autodiscovery: prefixing movie names with directory %r' % ( ufmf_dir, ) movie_fnames = [os.path.join(ufmf_dir, f) for f in movie_fnames] if len(movie_fnames) == 0: raise ValueError('no input movies -- nothing to do') elif verbose: print 'movie_fnames:', movie_fnames if dest_dir is None: dest_dir = os.curdir else: if not os.path.exists(dest_dir): os.makedirs(dest_dir) # get name of data datetime_str = os.path.splitext(os.path.split(h5_filename)[-1])[0] if datetime_str.startswith('DATA'): datetime_str = datetime_str[4:19] workaround_ffmpeg2theora_bug = True if caminfo_h5_filename is None: caminfo_h5_filename = h5_filename if caminfo_h5_filename is not None: with open_file_safe(caminfo_h5_filename, mode='r') as h5: camn2cam_id, tmp = result_utils.get_caminfo_dicts(h5) del tmp else: camn2cam_id = None blank_images = {} all_frame_montages = [] for frame_enum, (frame_dict, frame) in enumerate( ufmf_tools.iterate_frames( h5_filename, movie_fnames, movie_cam_ids=movie_cam_ids, white_background=config['what to show']['white_background'], max_n_frames=max_n_frames, start=start, stop=stop, rgb8_if_color=True, camn2cam_id=camn2cam_id, )): if frame_enum % nth_frame != 0: continue tracker_data = frame_dict['tracker_data'] global_data = frame_dict['global_data'] if data3d is not None: this_frame_3d_data = data3d[data3d['frame'] == frame] if dataqual_3d is None: this_frame_dataqual = None else: this_frame_dataqual = dataqual_3d[data3d['frame'] == frame] else: this_frame_3d_data = None this_frame_dataqual = None if data_raw_3d is not None: this_frame_raw_3d_data = data_raw_3d[data_raw_3d['frame'] == frame] if dataqual_raw_3d is None: this_frame_raw_dataqual = None else: this_frame_raw_dataqual = dataqual_raw_3d[data_raw_3d['frame'] == frame] else: this_frame_raw_3d_data = None this_frame_raw_dataqual = None if config['what to show']['zoom_obj']: zoom_cond_3d = this_frame_3d_data['obj_id'] == config[ 'what to show']['zoom_obj'] if np.sum(zoom_cond_3d) == 0: # object not in this frame this_frame_this_obj_3d_data = None else: this_frame_this_obj_3d_data = this_frame_3d_data[zoom_cond_3d] if (frame_enum % 100) == 0: print '%s: frame %d' % (datetime_str, frame) saved_fnames = [] for movie_idx, ufmf_fname in enumerate(movie_fnames): try: frame_data = frame_dict[ufmf_fname] except KeyError: # no data saved (frame skip on Prosilica camera?) if movie_cam_ids is not None: cam_id = movie_cam_ids[movie_idx] else: cam_id = ufmf_tools.get_cam_id_from_ufmf_fname(ufmf_fname) camn = None if cam_id not in blank_images: im_w, im_h = global_data['width_heights'][cam_id] image = np.empty((im_h, im_w), dtype=np.uint8) image.fill(255) blank_images[cam_id] = image image = blank_images[cam_id] mean_image = None else: cam_id = frame_data['cam_id'] camn = frame_data['camn'] image = frame_data['image'] if config['what to show']['image_manipulation'] == 'absdiff': mean_image = frame_data['mean'] del frame_data save_fname = 'tmp_frame%07d_%s.png' % (frame, cam_id) save_fname_path = os.path.join(dest_dir, save_fname) pixel_aspect = config[cam_id].get('pixel_aspect', 1) transform = config[cam_id].get('transform', 'orig') border_pixels = config['what to show']['border_pixels'] if config['what to show']['max_resolution'] is not None: b2 = border_pixels * 2 fix_w, fix_h = config['what to show']['max_resolution'] fix_aspect = (fix_w - b2) / float(fix_h - b2) desire_aspect = image.shape[1] / float( image.shape[0] * pixel_aspect) if desire_aspect >= fix_aspect: # image is wider than resolution given device_w = fix_w - b2 device_h = (fix_w - b2) / desire_aspect device_x = border_pixels device_y = (fix_h - device_h + border_pixels) / 2.0 else: # image is taller than resolution given device_h = fix_h - b2 device_w = (fix_h - b2) * desire_aspect device_y = border_pixels device_x = (fix_w - device_w + border_pixels) / 2.0 user_rect = (0, 0, image.shape[1], image.shape[0]) elif config['what to show']['zoom_obj']: if border_pixels != 0: raise NotImplementedError() device_x = 0 device_y = 0 device_w = config['what to show']['zoom_orig_pixels'] * config[ 'what to show']['zoom_factor'] device_h = device_w fix_w = device_w fix_h = device_h if this_frame_this_obj_3d_data is not None: X = np.array([ this_frame_this_obj_3d_data['x'], this_frame_this_obj_3d_data['y'], this_frame_this_obj_3d_data['z'], np.ones_like(this_frame_this_obj_3d_data['x']) ]).T xarr, yarr = R.find2d(cam_id, X, distorted=True) assert len(xarr) == 1 x = xarr[0] y = yarr[0] r = config['what to show']['zoom_orig_pixels'] * 0.5 user_rect = (x - r, y - r, r * 2, r * 2) else: # we're not tracking object -- don't draw anything user_rect = (-1000, -1000, 10, 10) else: device_x = border_pixels device_y = border_pixels device_w = image.shape[1] device_h = int(image.shape[0] * pixel_aspect) # compensate for pixel_aspect fix_w = device_w + 2 * border_pixels fix_h = device_h + 2 * border_pixels user_rect = (0, 0, image.shape[1], image.shape[0]) canv = benu.Canvas(save_fname_path, fix_w, fix_h) device_rect = (device_x, device_y, device_w, device_h) with canv.set_user_coords(device_rect, user_rect, transform=transform): if config['what to show']['image_manipulation'] == 'raw': canv.imshow(image, 0, 0, cmap=colormap) if config['what to show']['image_manipulation'] == 'absdiff': if mean_image is not None: adsdiff_image = abs( image.astype(np.int16) - mean_image.astype(np.int16)) scaled_show = np.clip((5 * adsdiff_image) + 127, 0, 255).astype(np.uint8) canv.imshow(scaled_show, 0, 0, cmap=colormap) if config['what to show'][ 'show_2d_position'] and camn is not None: cond = tracker_data['camn'] == camn this_cam_data = tracker_data[cond] xarr = np.atleast_1d(this_cam_data['x']) yarr = np.atleast_1d(this_cam_data['y']) canv.scatter( xarr, yarr, color_rgba=(0, 0, 0, 1), radius=10, markeredgewidth=config['what to show']['linewidth'], ) # draw shadow canv.scatter( xarr + config['what to show']['linewidth'], yarr + config['what to show']['linewidth'], color_rgba=(1, 1, 1, 1), radius=10, markeredgewidth=config['what to show']['linewidth'], ) if config['what to show'][ 'show_2d_orientation'] and camn is not None: cond = tracker_data['camn'] == camn this_cam_data = tracker_data[cond] xarr = np.atleast_1d(this_cam_data['x']) yarr = np.atleast_1d(this_cam_data['y']) slope = np.atleast_1d(this_cam_data['slope']) thetaarr = np.arctan(slope) line_len = 30.0 xinc = np.cos(thetaarr) * line_len yinc = np.sin(thetaarr) * line_len / float(pixel_aspect) for x, y, xi, yi in zip(xarr, yarr, xinc, yinc): xarr = np.array([x - xi, x + xi]) yarr = np.array([y - yi, y + yi]) if np.any(np.isnan(xarr)) or np.any(np.isnan(yarr)): continue canv.plot( xarr, yarr, color_rgba=(0, 1, 0, 0.4), linewidth=config['what to show']['linewidth'], ) if config['what to show'][ 'show_3d_smoothed_position'] and camn is not None: if len(this_frame_3d_data): X = np.array([ this_frame_3d_data['x'], this_frame_3d_data['y'], this_frame_3d_data['z'], np.ones_like(this_frame_3d_data['x']) ]).T xarr, yarr = R.find2d(cam_id, X, distorted=True) canv.scatter( xarr, yarr, color_rgba=(0, 1, 1, 1), radius=10, markeredgewidth=config['what to show'] ['linewidth'], ) if config['what to show'][ 'show_3d_MLE_position'] and camn is not None: if len(this_frame_raw_3d_data): X = np.array([ this_frame_raw_3d_data['x'], this_frame_raw_3d_data['y'], this_frame_raw_3d_data['z'], np.ones_like(this_frame_raw_3d_data['x']) ]).T xarr, yarr = R.find2d(cam_id, X, distorted=True) canv.scatter( xarr, yarr, color_rgba=(0.2, 0.2, 0.5, 1), radius=8, markeredgewidth=config['what to show'] ['linewidth'], ) # draw shadow canv.scatter( xarr + config['what to show']['linewidth'], yarr + config['what to show']['linewidth'], color_rgba=(0.7, 0.7, 1, 1), # blue radius=8, markeredgewidth=config['what to show'] ['linewidth'], ) if config['what to show'][ 'show_3d_raw_orientation'] and camn is not None: if len(this_frame_raw_3d_data): hzs = np.array([ this_frame_raw_3d_data['hz_line0'], this_frame_raw_3d_data['hz_line1'], this_frame_raw_3d_data['hz_line2'], this_frame_raw_3d_data['hz_line3'], this_frame_raw_3d_data['hz_line4'], this_frame_raw_3d_data['hz_line5'] ]).T Xs = np.array([ this_frame_raw_3d_data['x'], this_frame_raw_3d_data['y'], this_frame_raw_3d_data['z'] ]).T cam_center = R.get_camera_center(cam_id)[:, 0] for (X, hz, this_dataqual) in zip(Xs, hzs, this_frame_raw_dataqual): if this_dataqual < min_ori_qual: continue cam_ray = geom.line_from_points( geom.ThreeTuple(cam_center), geom.ThreeTuple(X)) raw_ori_line = geom.line_from_HZline(hz) X_ = raw_ori_line.get_my_point_closest_to_line( cam_ray) ld = raw_ori_line.direction() dmag = abs(ld) du = ld * (1. / dmag ) # unit length direction (normalize) length = 0.5 # arbitrary, 0.5 meters N = 100 # n segments (to deal with distortion) X0 = X_.vals + du.vals * -length / 2.0 X = X0[:, np.newaxis] + np.linspace(0, length, N)[ np.newaxis, :] * du.vals[:, np.newaxis] Xh = np.vstack( (X, np.ones_like(X[0, np.newaxis, :]))).T xarr, yarr = R.find2d(cam_id, Xh, distorted=True) canv.plot( xarr, yarr, color_rgba=(0, 0, 1, 1), # blue linewidth=config['what to show']['linewidth'], ) if config['what to show'][ 'show_3d_smoothed_orientation'] and camn is not None: if len(this_frame_3d_data): for (row, ori_qual) in zip(this_frame_3d_data, this_frame_dataqual): if ori_qual < min_ori_qual: continue X0 = np.array([ row['x'], row['y'], row['z'], np.ones_like(row['x']) ]).T dx = np.array([ row['dir_x'], row['dir_y'], row['dir_z'], np.zeros_like(row['x']) ]).T X1 = X0 + dx * orientation_3d_line_length if np.any(np.isnan(X1)): continue pts = np.vstack([X0, X1]) xarr, yarr = R.find2d(cam_id, pts, distorted=True) canv.plot( xarr, yarr, color_rgba=(1, 0, 0, 1), # red linewidth=config['what to show']['linewidth'], ) if config['what to show'][ 'show_3d_raw_chosen_orientation'] and camn is not None: if len(this_frame_3d_data): for (row, ori_qual) in zip(this_frame_3d_data, this_frame_dataqual): if ori_qual < min_ori_qual: continue X0 = np.array([ row['x'], row['y'], row['z'], np.ones_like(row['x']) ]).T dx = np.array([ row['rawdir_x'], row['rawdir_y'], row['rawdir_z'], np.zeros_like(row['x']) ]).T X1 = X0 + dx * orientation_3d_line_length if np.any(np.isnan(X1)): continue pts = np.vstack([X0, X1]) xarr, yarr = R.find2d(cam_id, pts, distorted=True) canv.plot( xarr, yarr, color_rgba=(1, 159. / 255, 0, 1), # orange linewidth=config['what to show']['linewidth'], ) if config['what to show']['obj_labels'] and camn is not None: if len(this_frame_3d_data): X = np.array([ this_frame_3d_data['x'], this_frame_3d_data['y'], this_frame_3d_data['z'], np.ones_like(this_frame_3d_data['x']) ]).T xarr, yarr = R.find2d(cam_id, X, distorted=True) for i in range(len(xarr)): obj_id = this_frame_3d_data['obj_id'][i] canv.text('%d' % obj_id, xarr[i], yarr[i], font_size=14, color_rgba=(1, 0, 0, 1)) if config['what to show'][ 'show_3d_obj_position_text'] and camn is not None: if len(this_frame_3d_data): X = np.array([ this_frame_3d_data['x'], this_frame_3d_data['y'], this_frame_3d_data['z'], np.ones_like(this_frame_3d_data['x']) ]).T xarr, yarr = R.find2d(cam_id, X, distorted=True) for i in range(len(xarr)): canv.text('(%.1f, %.1f, %.1f) mm' % (X[i, 0] * 1000.0, X[i, 1] * 1000.0, X[i, 2] * 1000.0), xarr[i] + 10, yarr[i], font_size=14, color_rgba=(0, 1, 1, 1)) if config['what to show']['show_cam_id']: canv.text('%s' % cam_id, 0, 20, font_size=14, color_rgba=(1, 0, 0, 1)) if workaround_ffmpeg2theora_bug: # first frame should get a colored pixel so that # ffmpeg doesn't interpret the whole move as grayscale canv.plot( [0, 1], [0, 1], color_rgba=(1, 0, 0, 0.1), ) workaround_ffmpeg2theora_bug = False # Now we already did it. canv.save() saved_fnames.append(save_fname_path) target = os.path.join( dest_dir, 'movie%s_frame%07d.png' % (datetime_str, frame_enum + 1)) # All cameras saved for this frame, make montage title = '%s frame %d' % (datetime_str, frame) montage(saved_fnames, title, target) all_frame_montages.append(target) if not no_remove: for fname in saved_fnames: os.unlink(fname) print '%s: %d frames montaged' % ( datetime_str, len(all_frame_montages), ) if save_ogv_movie: orig_dir = os.path.abspath(os.curdir) os.chdir(dest_dir) try: CMD = 'ffmpeg2theora -v 10 movie%s_frame%%07d.png -o movie%s.ogv' % ( datetime_str, datetime_str) subprocess.check_call(CMD, shell=True) finally: os.chdir(orig_dir) if not no_remove: for fname in all_frame_montages: os.unlink(fname)
def plot_ori( kalman_filename=None, h5=None, obj_only=None, start=None, stop=None, output_filename=None, options=None, ): if output_filename is not None: import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt import matplotlib.ticker as mticker fps = None if h5 is not None: h5f = tables.open_file(h5, mode="r") camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5f) fps = result_utils.get_fps(h5f) h5f.close() else: camn2cam_id = {} use_kalman_smoothing = options.use_kalman_smoothing fps = options.fps dynamic_model = options.dynamic_model ca = core_analysis.get_global_CachingAnalyzer() (obj_ids, use_obj_ids, is_mat_file, data_file, extra) = ca.initial_file_load(kalman_filename) if dynamic_model is None: dynamic_model = extra["dynamic_model_name"] print('detected file loaded with dynamic model "%s"' % dynamic_model) if dynamic_model.startswith("EKF "): dynamic_model = dynamic_model[4:] print(' for smoothing, will use dynamic model "%s"' % dynamic_model) with open_file_safe(kalman_filename, mode="r") as kh5: if fps is None: fps = result_utils.get_fps(kh5, fail_on_error=True) kmle = kh5.root.ML_estimates[:] # load into RAM if start is not None: kmle = kmle[kmle["frame"] >= start] if stop is not None: kmle = kmle[kmle["frame"] <= stop] all_mle_obj_ids = kmle["obj_id"] # walk all tables to get all obj_ids all_obj_ids = {} parent = kh5.root.ori_ekf_qual for group in parent._f_iter_nodes(): for table in group._f_iter_nodes(): assert table.name.startswith("obj") obj_id = int(table.name[3:]) all_obj_ids[obj_id] = table if obj_only is None: use_obj_ids = all_obj_ids.keys() mle_use_obj_ids = list(np.unique(all_mle_obj_ids)) missing_objs = list(set(mle_use_obj_ids) - set(use_obj_ids)) if len(missing_objs): warnings.warn("orientation not fit for %d obj_ids" % (len(missing_objs), )) use_obj_ids.sort() else: use_obj_ids = obj_only # now, generate plots fig = plt.figure() ax1 = fig.add_subplot(511) ax2 = fig.add_subplot(512, sharex=ax1) ax3 = fig.add_subplot(513, sharex=ax1) ax4 = fig.add_subplot(514, sharex=ax1) ax5 = fig.add_subplot(515, sharex=ax1) min_frame_range = np.inf max_frame_range = -np.inf if options.print_status: print("%d object IDs in file" % (len(use_obj_ids), )) for obj_id in use_obj_ids: table = all_obj_ids[obj_id] rows = table[:] if start is not None: rows = rows[rows["frame"] >= start] if stop is not None: rows = rows[rows["frame"] <= stop] if options.print_status: print("obj_id %d: %d rows of EKF data" % (obj_id, len(rows))) frame = rows["frame"] # get camns camns = [] for colname in table.colnames: if colname.startswith("dist"): camn = int(colname[4:]) camns.append(camn) for camn in camns: label = camn2cam_id.get(camn, "camn%d" % camn) theta = rows["theta%d" % camn] used = rows["used%d" % camn] dist = rows["dist%d" % camn] frf = np.array(frame, dtype=np.float) min_frame_range = min(np.min(frf), min_frame_range) max_frame_range = max(np.max(frf), max_frame_range) (line, ) = ax1.plot(frame, theta * R2D, "o", mew=0, ms=2.0, label=label) c = line.get_color() ax2.plot(frame[used], dist[used] * R2D, "o", color=c, mew=0, label=label) ax2.plot(frame[~used], dist[~used] * R2D, "o", color=c, mew=0, ms=2.0) # plot 3D orientation mle_row_cond = all_mle_obj_ids == obj_id rows_this_obj = kmle[mle_row_cond] if options.print_status: print("obj_id %d: %d rows of ML data" % (obj_id, len(rows_this_obj))) frame = rows_this_obj["frame"] hz = [rows_this_obj["hz_line%d" % i] for i in range(6)] # hz = np.rec.fromarrays(hz,names=['hz%d'%for i in range(6)]) hz = np.vstack(hz).T orient = reconstruct.line_direction(hz) ax3.plot(frame, orient[:, 0], "ro", mew=0, ms=2.0, label="x") ax3.plot(frame, orient[:, 1], "go", mew=0, ms=2.0, label="y") ax3.plot(frame, orient[:, 2], "bo", mew=0, ms=2.0, label="z") qual = compute_ori_quality(kh5, rows_this_obj["frame"], obj_id) if 1: orinan = np.array(orient, copy=True) if options.ori_qual is not None and options.ori_qual != 0: orinan[qual < options.ori_qual] = np.nan try: sori = ori_smooth(orinan, frames_per_second=fps) except AssertionError: if options.print_status: print("not plotting smoothed ori for object id %d" % (obj_id, )) else: pass else: ax3.plot(frame, sori[:, 0], "r-", mew=0, ms=2.0) # ,label='x') ax3.plot(frame, sori[:, 1], "g-", mew=0, ms=2.0) # ,label='y') ax3.plot(frame, sori[:, 2], "b-", mew=0, ms=2.0) # ,label='z') ax4.plot(frame, qual, "b-") # , mew=0, ms=3 ) # -------------- kalman_rows = ca.load_data( obj_id, kh5, use_kalman_smoothing=use_kalman_smoothing, dynamic_model_name=dynamic_model, return_smoothed_directions=options.smooth_orientations, frames_per_second=fps, up_dir=options.up_dir, min_ori_quality_required=options.ori_qual, ) frame = kalman_rows["frame"] cond = np.ones(frame.shape, dtype=np.bool) if options.start is not None: cond &= options.start <= frame if options.stop is not None: cond &= frame <= options.stop kalman_rows = kalman_rows[cond] frame = kalman_rows["frame"] Dx = Dy = Dz = None if options.smooth_orientations: Dx = kalman_rows["dir_x"] Dy = kalman_rows["dir_y"] Dz = kalman_rows["dir_z"] elif "rawdir_x" in kalman_rows.dtype.fields: Dx = kalman_rows["rawdir_x"] Dy = kalman_rows["rawdir_y"] Dz = kalman_rows["rawdir_z"] if Dx is not None: ax5.plot(frame, Dx, "r-", label="dx") ax5.plot(frame, Dy, "g-", label="dy") ax5.plot(frame, Dz, "b-", label="dz") ax1.xaxis.set_major_formatter(mticker.FormatStrFormatter("%d")) ax1.set_ylabel("theta (deg)") ax1.legend() ax2.set_ylabel("z (deg)") ax2.legend() ax3.set_ylabel("ori") ax3.legend() ax4.set_ylabel("quality") ax5.set_ylabel("dir") ax5.set_xlabel("frame") ax5.legend() ax1.set_xlim(min_frame_range, max_frame_range) if output_filename is None: plt.show() else: plt.savefig(output_filename)
def retrack_movies( h5_filename, output_h5_filename=None, max_n_frames=None, start=None, stop=None, ufmf_dir=None, cfg_filename=None, ufmf_filenames=None, save_debug_images=False, ): # 2D data format for PyTables: Info2D = flydra_core.data_descriptions.Info2D if ufmf_filenames is None: ufmf_filenames = auto_discover_ufmfs.find_ufmfs(h5_filename, ufmf_dir=ufmf_dir, careful=True) print("ufmf_filenames: %r" % ufmf_filenames) if len(ufmf_filenames) == 0: raise RuntimeError( "nothing to do (autodetection of .ufmf files failed)") if ufmf_dir is not None: if (not ufmf_filenames[0].startswith("/")) and (not os.path.isfile( ufmf_filenames[0])): # filenames are not absolute and are not present, convert ufmf_filenames = [ os.path.join(ufmf_dir, fname) for fname in ufmf_filenames ] else: raise RuntimeError( "ufmf_dir given but ufmf_filenames exist without it") if os.path.exists(output_h5_filename): raise RuntimeError("will not overwrite old file '%s'" % output_h5_filename) # get name of data config = get_config_defaults() if cfg_filename is not None: loaded_cfg = cherrypy.lib.reprconf.as_dict(cfg_filename) for section in loaded_cfg: config[section].update(loaded_cfg.get(section, {})) default_camcfg = config["default"] for cam_id in config.keys(): if cam_id == "default": continue # ensure default key/value pairs in each cam_id for key, value in default_camcfg.iteritems(): if key not in config[cam_id]: config[cam_id][key] = value datetime_str = os.path.splitext(os.path.split(h5_filename)[-1])[0] datetime_str = datetime_str[4:19] retrack_cam_ids = [ ufmf_tools.get_cam_id_from_ufmf_fname(f) for f in ufmf_filenames ] with open_file_safe(h5_filename, mode="r") as h5: # Find camns in original data camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) retrack_camns = [] for cam_id in retrack_cam_ids: retrack_camns.extend(cam_id2camns[cam_id]) all_camns = camn2cam_id.keys() # Save results to temporary file. Copy to real location on success. tmpdir = tempfile.mkdtemp() tmp_output_h5_filename = os.path.join(tmpdir, "retrack.h5") with open_file_safe(tmp_output_h5_filename, mode="w", delete_on_error=True) as output_h5: out_data2d = output_h5.create_table( output_h5.root, "data2d_distorted", Info2D, "2d data", expectedrows=h5.root.data2d_distorted.nrows, ) # Are there any camns in original h5 that are not being retracked? if len(set(all_camns) - set(retrack_camns)): # Yes. # OK, exclude all camns to be retracked... orig_data2d = h5.root.data2d_distorted[:] # read all data for camn in retrack_camns: delete_cond = orig_data2d["camn"] == camn save_cond = ~delete_cond orig_data2d = orig_data2d[save_cond] # And save original data for untouched camns out_data2d.append(orig_data2d) for input_node in h5.root._f_iter_nodes(): if input_node._v_name not in [ "data2d_distorted", "kalman_estimates", "ML_estimates", "ML_estimates_2d_idxs", ]: print("copying", input_node._v_name) # copy everything from source to dest input_node._f_copy(output_h5.root, recursive=True) fpc = realtime_image_analysis.FitParamsClass( ) # allocate FitParamsClass count = 0 iterate_frames = ufmf_tools.iterate_frames # shorten notation for frame_enum, (frame_dict, frame) in enumerate( iterate_frames( h5_filename, ufmf_filenames, max_n_frames=max_n_frames, start=start, stop=stop, )): if (frame_enum % 100) == 0: print("%s: frame %d" % (datetime_str, frame)) for ufmf_fname in ufmf_filenames: try: frame_data = frame_dict[ufmf_fname] except KeyError: # no data saved (frame skip on Prosilica camera?) continue count += 1 camn = frame_data["camn"] cam_id = frame_data["cam_id"] camcfg = config.get(cam_id, default_camcfg) image = frame_data["image"] cam_received_timestamp = frame_data[ "cam_received_timestamp"] timestamp = frame_data["timestamp"] detected_points = True obj_slices = None if len(frame_data["regions"]) == 0: # no data this frame -- go to next camera or frame detected_points = False if detected_points: # print frame,cam_id,len(frame_data['regions']) absdiff_im = abs( frame_data["mean"].astype(np.float32) - image) thresh_val = (np.max(absdiff_im) * camcfg["absdiff_max_frac_thresh"]) thresh_val = max(camcfg["min_absdiff"], thresh_val) thresh_im = absdiff_im > thresh_val labeled_im, n_labels = scipy.ndimage.label(thresh_im) if not n_labels: detected_points = False else: obj_slices = scipy.ndimage.find_objects(labeled_im) detection = out_data2d.row if detected_points: height, width = image.shape if save_debug_images: xarr = [] yarr = [] frame_pt_idx = 0 detected_points = False # possible not to find any below for i in range(n_labels): y_slice, x_slice = obj_slices[i] # limit pixel operations to covering rectangle this_labeled_im = labeled_im[y_slice, x_slice] this_label_im = this_labeled_im == (i + 1) # calculate area (number of binarized pixels) xsum = np.sum(this_label_im, axis=0) pixel_area = np.sum(xsum) if pixel_area < camcfg["area_minimum_threshold"]: continue # calculate center xpos = np.arange(x_slice.start, x_slice.stop, x_slice.step) ypos = np.arange(y_slice.start, y_slice.stop, y_slice.step) xmean = np.sum((xsum * xpos)) / np.sum(xsum) ysum = np.sum(this_label_im, axis=1) ymean = np.sum((ysum * ypos)) / np.sum(ysum) if 1: if camcfg["pixel_aspect"] == 1: this_fit_im = this_label_im elif camcfg["pixel_aspect"] == 2: this_fit_im = np.repeat(this_label_im, 2, axis=0) else: raise ValueError("unknown pixel_aspect") fast_foreground = FastImage.asfastimage( this_fit_im.astype(np.uint8)) fail_fit = False try: ( x0_roi, y0_roi, weighted_area, slope, eccentricity, ) = fpc.fit(fast_foreground) except realtime_image_analysis.FitParamsError as err: fail_fit = True print("frame %d, ufmf %s: fit failed" % (frame, ufmf_fname)) print(err) else: if camcfg["pixel_aspect"] == 2: y0_roi *= 0.5 xmean = x_slice.start + x0_roi ymean = y_slice.start + y0_roi del weighted_area # don't leave room for confusion else: fail_fit = True if fail_fit: slope = np.nan eccentricity = np.nan detection["camn"] = camn detection["frame"] = frame detection["timestamp"] = timestamp detection[ "cam_received_timestamp"] = cam_received_timestamp detection["x"] = xmean detection["y"] = ymean detection["area"] = pixel_area detection["slope"] = slope detection["eccentricity"] = eccentricity detection["frame_pt_idx"] = frame_pt_idx # XXX These are not yet implemented: detection["cur_val"] = 0 detection["mean_val"] = np.nan detection["sumsqf_val"] = np.nan frame_pt_idx += 1 if save_debug_images: xarr.append(xmean) yarr.append(ymean) detection.append() detected_points = True if save_debug_images: save_dir = "debug" mkdir_p(save_dir) save_fname = "debug_%s_%d.png" % (cam_id, frame) save_fname_path = os.path.join( save_dir, save_fname) print("saving", save_fname_path) from . import benu canv = benu.Canvas(save_fname_path, width, height) maxlabel = np.max(labeled_im) fact = int(np.floor(255.0 / maxlabel)) canv.imshow((labeled_im * fact).astype(np.uint8), 0, 0) canv.scatter( xarr, yarr, color_rgba=(0, 1, 0, 1), radius=10, ) canv.save() if not detected_points: # If no point was tracked for this frame, # still save timestamp. detection["camn"] = camn detection["frame"] = frame detection["timestamp"] = timestamp detection[ "cam_received_timestamp"] = cam_received_timestamp detection["x"] = np.nan detection["y"] = np.nan detection["area"] = np.nan detection["slope"] = np.nan detection["eccentricity"] = np.nan detection["frame_pt_idx"] = 0 detection["cur_val"] = 0 detection["mean_val"] = np.nan detection["sumsqf_val"] = np.nan detection.append() if count == 0: raise RuntimeError("no frames processed") # move to correct location shutil.move(tmp_output_h5_filename, output_h5_filename)
def plot_latency(fname, do_3d_latency=False, do_2d_latency=False, end_idx=100000, save=False): if do_3d_latency==False and do_2d_latency==False: print('hmm, not plotting 3d or 2d data. nothing to do') return with tables.open_file(fname, mode='r') as h5: if do_2d_latency: d2d = h5.root.data2d_distorted[:end_idx] if do_3d_latency: dk = h5.root.kalman_estimates[:end_idx] camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) time_model=result_utils.get_time_model_from_data(h5) if do_2d_latency: df2d = pd.DataFrame(d2d) camn_list = list(df2d['camn'].unique()) camn_list.sort() figs = {} if do_3d_latency: dfk = pd.DataFrame(dk) fig = plt.figure() figs['3d'] = fig ax = fig.add_subplot(111) for obj_id, dfobj in dfk.groupby('obj_id'): frame = dfobj['frame'].values reconstruct_timestamp = dfobj['timestamp'].values trigger_timestamp = time_model.framestamp2timestamp(frame) latency = reconstruct_timestamp-trigger_timestamp latency[ latency < -1e8 ] = np.nan ax.plot(frame,latency,'b.-') ax.text(0,1,'3D reconstruction', va='top', ha='left', transform=ax.transAxes) ax.set_xlabel('frame') ax.set_ylabel('time (s)') if do_2d_latency: fig2 = plt.figure() figs['2'] = fig2 axn=None fig3 = plt.figure() figs['3'] = fig3 ax3n = None fig4 = plt.figure() figs['4'] = fig4 ax4n = None for camn, dfcam in df2d.groupby('camn'): cam_id = camn2cam_id[camn] df0 = dfcam[ dfcam['frame_pt_idx']==0 ] ts0s = df0['timestamp'].values tss = df0['cam_received_timestamp'].values frames = df0['frame'].values dts = tss-ts0s dframes = frames[1:] - frames[:-1] axn = fig2.add_subplot( len(camn_list), 1, camn_list.index(camn)+1,sharex=axn) axn.plot(frames,dts,'r.-',label='camnode latency' ) axn.plot( frames[:-1], (ts0s[1:]-ts0s[:-1])/dframes, 'g.-', label='mean inter-frame interval' ) axn.set_xlabel('frame') axn.set_ylabel('time (s)') axn.text(0,1,cam_id, va='top', ha='left', transform=axn.transAxes) if camn_list.index(camn)==0: axn.legend() ax3n = fig3.add_subplot( len(camn_list), 1, camn_list.index(camn)+1,sharex=ax3n) ax3n.plot(frames,ts0s,'g.-', label='calculated triggerbox timestamp') ax3n.set_xlabel('frame') ax3n.set_ylabel('time (s)') ax3n.text(0,1,cam_id, va='top', ha='left', transform=ax3n.transAxes) if camn_list.index(camn)==0: ax3n.legend() ax4n = fig4.add_subplot( len(camn_list), 1, camn_list.index(camn)+1,sharex=ax4n) ax4n.plot(frames[:-1],ts0s[1:]-ts0s[:-1],'g.-') ax4n.set_xlabel('frame') ax4n.set_ylabel('inter-frame-interval (s)') ax4n.text(0,1,cam_id, va='top', ha='left', transform=ax4n.transAxes) if save: for key in figs: fig = figs[key] fig.savefig('%s-latency-%s.png'%(fname,key)) else: plt.show()
def do_it( filename, efilename, use_nth_observation=None, h5_2d_data_filename=None, use_kalman_data=True, start=None, stop=None, options=None, ): if h5_2d_data_filename is None: h5_2d_data_filename = filename calib_dir = filename + ".recal" if not os.path.exists(calib_dir): os.makedirs(calib_dir) results = result_utils.get_results(filename, mode="r+") if use_kalman_data: mylocals = {} myglobals = {} execfile(efilename, myglobals, mylocals) use_obj_ids = mylocals["long_ids"] if "bad" in mylocals: use_obj_ids = set(use_obj_ids) bad = set(mylocals["bad"]) use_obj_ids = list(use_obj_ids.difference(bad)) kobs = results.root.ML_estimates kobs_2d = results.root.ML_estimates_2d_idxs h5_2d_data = result_utils.get_results(h5_2d_data_filename, mode="r+") camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5_2d_data) cam_ids = list(cam_id2camns.keys()) cam_ids.sort() data2d = h5_2d_data.root.data2d_distorted # use_idxs = numpy.arange(data2d.nrows) frames = data2d.cols.frame[:] qfi = result_utils.QuickFrameIndexer(frames) npoints_by_ncams = {} npoints_by_cam_id = {} for cam_id in cam_ids: npoints_by_cam_id[cam_id] = 0 IdMat = [] points = [] if use_kalman_data: row_keys = [] if start is not None or stop is not None: print("start, stop", start, stop) print( "WARNING: currently ignoring start/stop because Kalman data is being used" ) for obj_id_enum, obj_id in enumerate(use_obj_ids): row_keys.append((len(points), obj_id)) # print 'obj_id %d (%d of %d)'%(obj_id, obj_id_enum+1, len(use_obj_ids)) this_obj_id = obj_id k_use_idxs = kobs.get_where_list("obj_id==this_obj_id") obs_2d_idxs = kobs.read_coordinates(k_use_idxs, field="obs_2d_idx") kframes = kobs.read_coordinates(k_use_idxs, field="frame") kframes_use = kframes[::use_nth_observation] obs_2d_idxs_use = obs_2d_idxs[::use_nth_observation] widgets = [ "obj_id % 5d (% 3d of % 3d) " % (obj_id, obj_id_enum + 1, len(use_obj_ids)), progressbar.Percentage(), " ", progressbar.Bar(), " ", progressbar.ETA(), ] pbar = progressbar.ProgressBar(widgets=widgets, maxval=len(kframes_use)).start() for n_kframe, (kframe, obs_2d_idx) in enumerate( zip(kframes_use, obs_2d_idxs_use)): pbar.update(n_kframe) if 0: k_use_idx = k_use_idxs[n_kframe * use_nth_observation] print(kobs.read_coordinates(numpy.array([k_use_idx]))) if PT.__version__ <= "1.3.3": obs_2d_idx_find = int(obs_2d_idx) kframe_find = int(kframe) else: obs_2d_idx_find = obs_2d_idx kframe_find = kframe obj_id_save = int(obj_id) # convert from possible numpy scalar # sys.stdout.write(' reading frame data...') # sys.stdout.flush() obs_2d_idx_find_next = obs_2d_idx_find + numpy.uint64(1) kobs_2d_data = kobs_2d.read(start=obs_2d_idx_find, stop=obs_2d_idx_find_next) # sys.stdout.write('done\n') # sys.stdout.flush() assert len(kobs_2d_data) == 1 kobs_2d_data = kobs_2d_data[0] this_camns = kobs_2d_data[0::2] this_camn_idxs = kobs_2d_data[1::2] # sys.stdout.write(' doing frame selections...') # sys.stdout.flush() if 1: this_use_idxs = qfi.get_frame_idxs(kframe_find) elif 0: this_use_idxs = numpy.nonzero(frames == kframe_find)[0] else: this_use_idxs = data2d.get_where_list("frame==kframe_find") # sys.stdout.write('done\n') # sys.stdout.flush() if PT.__version__ <= "1.3.3": this_use_idxs = [int(t) for t in this_use_idxs] d2d = data2d.read_coordinates(this_use_idxs) if len(this_camns) < options.min_num_points: # not enough points to contribute to calibration continue npoints_by_ncams[len(this_camns)] = ( npoints_by_ncams.get(len(this_camns), 0) + 1) IdMat_row, points_row = create_new_row( d2d, this_camns, this_camn_idxs, cam_ids, camn2cam_id, npoints_by_cam_id, ) IdMat.append(IdMat_row) points.append(points_row) ## print 'running total of points','-'*20 ## for cam_id in cam_ids: ## print 'cam_id %s: %d points'%(cam_id,npoints_by_cam_id[cam_id]) ## print pbar.finish() if start is None: start = 0 if stop is None: stop = int(frames.max()) if not use_kalman_data: row_keys = None count = 0 for frameno in range(start, stop + 1, use_nth_observation): this_use_idxs = qfi.get_frame_idxs(frameno) d2d = data2d.read_coordinates(this_use_idxs) d2d = d2d[~numpy.isnan(d2d["x"])] this_camns = d2d["camn"] unique_camns = numpy.unique(this_camns) if len(this_camns) != len(unique_camns): # ambiguity - a camera has > 1 point continue this_camn_idxs = numpy.array([0] * len(this_camns)) if len(this_camns) < options.min_num_points: # not enough points to contribute to calibration continue npoints_by_ncams[len(this_camns)] = ( npoints_by_ncams.get(len(this_camns), 0) + 1) count += 1 IdMat_row, points_row = create_new_row(d2d, this_camns, this_camn_idxs, cam_ids, camn2cam_id, npoints_by_cam_id) IdMat.append(IdMat_row) points.append(points_row) print("%d points" % len(IdMat)) print("by camera id:") for cam_id in cam_ids: print(" %s: %d" % (cam_id, npoints_by_cam_id[cam_id])) print("by n points:") max_npoints = 0 for ncams in npoints_by_ncams: print(" %d: %d" % (ncams, npoints_by_ncams[ncams])) max_npoints = max(max_npoints, npoints_by_ncams[ncams]) print() if max_npoints < 10: print("not enough points, aborting", file=sys.stderr) results.close() h5_2d_data.close() sys.exit(1) IdMat = numpy.array(IdMat, dtype=numpy.uint8).T points = numpy.array(points, dtype=numpy.float32).T # resolution Res = [] for cam_id in cam_ids: image_table = results.root.images arr = getattr(image_table, cam_id) imsize = arr.shape[1], arr.shape[0] Res.append(imsize) Res = numpy.array(Res) cam_centers = [] cam_calibrations = [] if options.camera_center_reconstructor: creconstructor = flydra_core.reconstruct.Reconstructor( cal_source=options.camera_center_reconstructor) cam_centers = numpy.asarray([ creconstructor.get_camera_center(cam_id)[:, 0] for cam_id in cam_ids ]) flydra_core.reconstruct.save_ascii_matrix( cam_centers, os.path.join(calib_dir, "original_cam_centers.dat")) intrinsics_reconstructor = options.undistort_intrinsics_reconstructor if intrinsics_reconstructor and os.path.exists(intrinsics_reconstructor): tdir = tempfile.mkdtemp() reconstructor = flydra_core.reconstruct.Reconstructor( cal_source=intrinsics_reconstructor) for i, cam_id in enumerate(cam_ids): fname = os.path.join(tdir, "%s.rad" % cam_id) scc = reconstructor.get_SingleCameraCalibration(cam_id) scc.helper.save_to_rad_file(fname) cam_calibrations.append(fname) intrinsics_yaml = options.undistort_intrinsics_yaml if intrinsics_yaml and os.path.exists(intrinsics_yaml): for cam_id in cam_ids: fname = os.path.join(intrinsics_yaml, "%s.yaml" % cam_id) cam_calibrations.append(fname) undo_radial_distortion = len(cam_calibrations) == len(cam_ids) mcsc = MultiCamSelfCal(calib_dir) mcsc.create_calibration_directory( cam_ids=cam_ids, IdMat=IdMat, points=points, Res=Res, cam_calibrations=cam_calibrations, cam_centers=cam_centers, radial_distortion=undo_radial_distortion, square_pixels=1, num_cameras_fill=options.num_cameras_fill, ) results.close() h5_2d_data.close() if row_keys is not None: row_keys = numpy.array(row_keys) flydra_core.reconstruct.save_ascii_matrix( row_keys, os.path.join(calib_dir, "obj_ids_zero_indexed.dat"), isint=True) if options.run_mcsc: caldir = mcsc.execute(silent=False) print("\nfinished: result in ", caldir) if options.output_xml: fname = os.path.join(caldir, "reconstructor.xml") recon = flydra_core.reconstruct.Reconstructor(cal_source=caldir) recon.save_to_xml_filename(fname) print("\nfinished: new reconstructor in", fname)
def doit( filenames=None, start=None, stop=None, kalman_filename=None, fps=None, use_kalman_smoothing=True, dynamic_model=None, up_dir=None, options=None, ): if options.save_fig is not None: matplotlib.use('Agg') import pylab if not use_kalman_smoothing: if (fps is not None) or (dynamic_model is not None): print( 'WARNING: disabling Kalman smoothing ' '(--disable-kalman-smoothing) is ' 'incompatable with setting fps and ' 'dynamic model options (--fps and ' '--dynamic-model)', file=sys.stderr) ax = None ax_by_cam = {} fig = pylab.figure() assert len(filenames) >= 1, 'must give at least one filename!' n_files = 0 for filename in filenames: if options.show_source_name: figtitle = filename if kalman_filename is not None: figtitle += ' ' + kalman_filename else: figtitle = '' if options.obj_only is not None: figtitle += ' only showing objects: ' + ' '.join( map(str, options.obj_only)) if figtitle != '': pylab.figtext(0.01, 0.01, figtitle, verticalalignment='bottom') with PT.open_file(filename, mode='r') as h5: if options.spreadh5 is not None: h5spread = PT.open_file(options.spreadh5, mode='r') else: h5spread = None if fps is None: fps = result_utils.get_fps(h5) camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) cam_ids = cam_id2camns.keys() cam_ids.sort() if start is not None or stop is not None: frames = h5.root.data2d_distorted.read(field='frame') valid_cond = numpy.ones(frames.shape, dtype=numpy.bool) if start is not None: valid_cond = valid_cond & (frames >= start) if stop is not None: valid_cond = valid_cond & (frames <= stop) read_idxs = np.nonzero(valid_cond)[0] all_data = [] for start_stop in utils.iter_contig_chunk_idxs(read_idxs): (read_idx_start_idx, read_idx_stop_idx) = start_stop start_idx = read_idxs[read_idx_start_idx] stop_idx = read_idxs[read_idx_stop_idx - 1] these_rows = h5.root.data2d_distorted.read(start=start_idx, stop=stop_idx + 1) all_data.append(these_rows) if len(all_data) == 0: print('file %s has no frames in range %s - %s' % (filename, start, stop)) continue all_data = np.concatenate(all_data) del valid_cond, frames, start_idx, stop_idx, these_rows, read_idxs else: all_data = h5.root.data2d_distorted[:] tmp_frames = all_data['frame'] if len(tmp_frames) == 0: print('file %s has no frames, skipping.' % filename) continue n_files += 1 start_frame = tmp_frames.min() stop_frame = tmp_frames.max() del tmp_frames for cam_id_enum, cam_id in enumerate(cam_ids): if cam_id in ax_by_cam: ax = ax_by_cam[cam_id] else: n_subplots = len(cam_ids) if kalman_filename is not None: n_subplots += 1 if h5spread is not None: n_subplots += 1 ax = pylab.subplot(n_subplots, 1, cam_id_enum + 1, sharex=ax) ax_by_cam[cam_id] = ax ax.fmt_xdata = str ax.fmt_ydata = str camns = cam_id2camns[cam_id] cam_id_n_valid = 0 for camn in camns: this_idx = numpy.nonzero(all_data['camn'] == camn)[0] data = all_data[this_idx] xdata = data['x'] valid = ~numpy.isnan(xdata) data = data[valid] del valid if options.area_threshold > 0.0: area = data['area'] valid2 = area >= options.area_threshold data = data[valid2] del valid2 if options.likely_only: pt_area = data['area'] cur_val = data['cur_val'] mean_val = data['mean_val'] sumsqf_val = data['sumsqf_val'] p_y_x = some_rough_negative_log_likelihood( pt_area, cur_val, mean_val, sumsqf_val) valid3 = np.isfinite(p_y_x) data = data[valid3] n_valid = len(data) cam_id_n_valid += n_valid if options.timestamps: xdata = data['timestamp'] else: xdata = data['frame'] if n_valid >= 1: ax.plot(xdata, data['x'], 'ro', ms=2, mew=0) ax.plot(xdata, data['y'], 'go', ms=2, mew=0) ax.text( 0.1, 0, '%s %s: %d pts' % (cam_id, cam_id2camns[cam_id], cam_id_n_valid), horizontalalignment='left', verticalalignment='bottom', transform=ax.transAxes, ) ax.set_ylabel('pixels') if not options.timestamps: ax.set_xlim((start_frame, stop_frame)) ax.set_xlabel('frame') if options.timestamps: timezone = result_utils.get_tz(h5) df = DateFormatter(timezone) ax.xaxis.set_major_formatter( ticker.FuncFormatter(df.format_date)) else: ax.xaxis.set_major_formatter(ticker.FormatStrFormatter("%d")) ax.yaxis.set_major_formatter(ticker.FormatStrFormatter("%d")) if h5spread is not None: if options.timestamps: raise NotImplementedError( '--timestamps is currently incompatible with --spreadh5' ) ax_by_cam['h5spread'] = ax if kalman_filename is not None: # this is 2nd to last ax = pylab.subplot(n_subplots, 1, n_subplots - 1, sharex=ax) else: # this is last ax = pylab.subplot(n_subplots, 1, n_subplots, sharex=ax) frames = h5spread.root.framenumber[:] spread = h5spread.root.spread[:] valid_cond = numpy.ones(frames.shape, dtype=numpy.bool) if start is not None: valid_cond = valid_cond & (frames >= start) if stop is not None: valid_cond = valid_cond & (frames <= stop) spread_msec = spread[valid_cond] * 1000.0 ax.plot(frames[valid_cond], spread_msec, 'o', ms=2, mew=0) if spread_msec.max() < 1.0: ax.set_ylim((0, 1)) ax.set_yticks([0, 1]) ax.set_xlabel('frame') ax.set_ylabel('timestamp spread (msec)') ax.xaxis.set_major_formatter(ticker.FormatStrFormatter("%d")) ax.yaxis.set_major_formatter(ticker.FormatStrFormatter("%d")) h5spread.close() del frames del spread if options.timestamps: fig.autofmt_xdate() if kalman_filename is not None: if 1: ax = pylab.subplot(n_subplots, 1, n_subplots, sharex=ax) ax_by_cam['kalman pmean'] = ax ax.fmt_xdata = str ax.set_ylabel('3d error\nmeters') frame_start = start frame_stop = stop # copied from save_movies_overlay.py ca = core_analysis.get_global_CachingAnalyzer() (obj_ids, use_obj_ids, is_mat_file, data_file, extra) = ca.initial_file_load(kalman_filename) if options.timestamps: time_model = result_utils.get_time_model_from_data(data_file) if 'frames' in extra: frames = extra['frames'] valid_cond = np.ones((len(frames, )), dtype=np.bool) if start is not None: valid_cond &= frames >= start if stop is not None: valid_cond &= frames <= stop obj_ids = obj_ids[valid_cond] use_obj_ids = np.unique(obj_ids) print('quick found use_obj_ids', use_obj_ids) if is_mat_file: raise ValueError('cannot use .mat file for kalman_filename ' 'because it is missing the reconstructor ' 'and ability to get framenumbers') R = reconstruct.Reconstructor(data_file) if options.obj_only is not None: use_obj_ids = options.obj_only if dynamic_model is None and use_kalman_smoothing: dynamic_model = extra['dynamic_model_name'] print('detected file loaded with dynamic model "%s"' % dynamic_model) if dynamic_model.startswith('EKF '): dynamic_model = dynamic_model[4:] print(' for smoothing, will use dynamic model "%s"' % dynamic_model) if options.reproj_error: reproj_error = collections.defaultdict(list) max_reproj_error = {} kalman_rows = [] for obj_id in use_obj_ids: kalman_rows.append(ca.load_observations(obj_id, data_file)) kalman_rows = numpy.concatenate(kalman_rows) kalman_3d_frame = kalman_rows['frame'] if start is not None or stop is not None: if start is None: start = -numpy.inf if stop is None: stop = numpy.inf valid_cond = ((kalman_3d_frame >= start) & (kalman_3d_frame <= stop)) kalman_rows = kalman_rows[valid_cond] kalman_3d_frame = kalman_3d_frame[valid_cond] # modified from save_movies_overlay for this_3d_row_enum, this_3d_row in enumerate(kalman_rows): if this_3d_row_enum % 100 == 0: print('doing reprojection error for MLE 3d estimate for ' 'row %d of %d' % (this_3d_row_enum, len(kalman_rows))) vert = numpy.array( [this_3d_row['x'], this_3d_row['y'], this_3d_row['z']]) obj_id = this_3d_row['obj_id'] if numpy.isnan(vert[0]): # no observation this frame continue obs_2d_idx = this_3d_row['obs_2d_idx'] try: kobs_2d_data = data_file.root.ML_estimates_2d_idxs[int( obs_2d_idx)] except tables.exceptions.NoSuchNodeError, err: # backwards compatibility kobs_2d_data = data_file.root.kalman_observations_2d_idxs[ int(obs_2d_idx)] # parse VLArray this_camns = kobs_2d_data[0::2] this_camn_idxs = kobs_2d_data[1::2] # find original 2d data # narrow down search obs2d = all_data[all_data['frame'] == this_3d_row['frame']] for camn, this_camn_idx in zip(this_camns, this_camn_idxs): cam_id = camn2cam_id[camn] # do projection to camera image plane vert_image = R.find2d(cam_id, vert, distorted=True) new_cond = ((obs2d['camn'] == camn) & (obs2d['frame_pt_idx'] == this_camn_idx)) assert numpy.sum(new_cond) == 1 x = obs2d[new_cond]['x'][0] y = obs2d[new_cond]['y'][0] this_reproj_error = numpy.sqrt((vert_image[0] - x)**2 + (vert_image[1] - y)**2) if this_reproj_error > 100: print(' reprojection error > 100 (%.1f) at frame %d ' 'for camera %s, obj_id %d' % (this_reproj_error, this_3d_row['frame'], cam_id, obj_id)) if numpy.isnan(this_reproj_error): print('error:') print(this_camns, this_camn_idxs) print(cam_id) print(vert_image) print(vert) raise ValueError('nan at frame %d' % this_3d_row['frame']) reproj_error[cam_id].append(this_reproj_error) if cam_id in max_reproj_error: (cur_max_frame, cur_max_reproj_error, cur_obj_id) = max_reproj_error[cam_id] if this_reproj_error > cur_max_reproj_error: max_reproj_error[cam_id] = (this_3d_row['frame'], this_reproj_error, obj_id) else: max_reproj_error[cam_id] = (this_3d_row['frame'], this_reproj_error, obj_id) del kalman_rows, kalman_3d_frame, obj_ids print('mean reprojection errors:') cam_ids = reproj_error.keys() cam_ids.sort() for cam_id in cam_ids: errors = reproj_error[cam_id] mean_error = numpy.mean(errors) worst_frame, worst_error, worst_obj_id = max_reproj_error[ cam_id] print(' %s: %.1f (worst: frame %d, obj_id %d, error %.1f)' % (cam_id, mean_error, worst_frame, worst_obj_id, worst_error)) print() for kalman_smoothing in [True, False]: if use_kalman_smoothing == False and kalman_smoothing == True: continue print('loading frame numbers for kalman objects (estimates)') kalman_rows = [] for obj_id in use_obj_ids: try: my_rows = ca.load_data( obj_id, data_file, use_kalman_smoothing=kalman_smoothing, dynamic_model_name=dynamic_model, frames_per_second=fps, up_dir=up_dir, ) except core_analysis.NotEnoughDataToSmoothError, err: # OK, we don't have data from this obj_id continue else: kalman_rows.append(my_rows) if not len(kalman_rows): # no data continue kalman_rows = numpy.concatenate(kalman_rows) kalman_3d_frame = kalman_rows['frame'] if start is not None or stop is not None: if start is None: start = -numpy.inf if stop is None: stop = numpy.inf valid_cond = ((kalman_3d_frame >= start) & (kalman_3d_frame <= stop)) kalman_rows = kalman_rows[valid_cond] kalman_3d_frame = kalman_3d_frame[valid_cond] obj_ids = kalman_rows['obj_id'] use_obj_ids = numpy.unique(obj_ids) non_nan_rows = ~np.isnan(kalman_rows['x']) print('plotting %d Kalman objects' % (len(use_obj_ids), )) for obj_id in use_obj_ids: cond = obj_ids == obj_id cond &= non_nan_rows x = kalman_rows['x'][cond] y = kalman_rows['y'][cond] z = kalman_rows['z'][cond] w = numpy.ones(x.shape) X = numpy.vstack((x, y, z, w)).T frame = kalman_rows['frame'][cond] #print '%d %d %d'%(frame[0],obj_id, len(frame)) if options.timestamps: time_est = time_model.framestamp2timestamp(frame) if kalman_smoothing: kwprops = dict(lw=0.5) else: kwprops = dict(lw=1) for cam_id in cam_ids: if cam_id not in R.get_cam_ids(): print( 'no calibration for %s: not showing 3D projections' % (cam_id, )) continue ax = ax_by_cam[cam_id] x2d = R.find2d(cam_id, X, distorted=True) ## print '%d %d %s (%f,%f)'%( ## obj_id,frame[0],cam_id,x2d[0,0],x2d[1,0]) if options.timestamps: xdata = time_est else: xdata = frame ax.text(xdata[0], x2d[0, 0], '%d' % obj_id) thisline, = ax.plot(xdata, x2d[0, :], 'b-', picker=5, **kwprops) #5pt tolerance all_kalman_lines[thisline] = obj_id thisline, = ax.plot(xdata, x2d[1, :], 'y-', picker=5, **kwprops) #5pt tolerance all_kalman_lines[thisline] = obj_id ax.set_ylim([-100, 800]) if options.timestamps: ## ax.set_xlim( *time_model.framestamp2timestamp( ## (start_frame, stop_frame) )) pass else: ax.set_xlim((start_frame, stop_frame)) if 1: ax = ax_by_cam['kalman pmean'] P00 = kalman_rows['P00'][cond] P11 = kalman_rows['P11'][cond] P22 = kalman_rows['P22'][cond] Pmean = numpy.sqrt(P00**2 + P11**2 + P22**2) # variance std = numpy.sqrt(Pmean) # standard deviation (in meters) if options.timestamps: xdata = time_est else: xdata = frame ax.plot(xdata, std, 'k-', **kwprops) if options.timestamps: ax.set_xlabel('time (sec)') timezone = result_utils.get_tz(h5) df = DateFormatter(timezone) ax.xaxis.set_major_formatter( ticker.FuncFormatter(df.format_date)) for label in ax.get_xticklabels(): label.set_rotation(30) else: ax.set_xlabel('frame') ax.xaxis.set_major_formatter( ticker.FormatStrFormatter("%d")) ax.yaxis.set_major_formatter( ticker.FormatStrFormatter("%s")) if not kalman_smoothing: # plot 2D data contributing to 3D object # this is forked from flydra_analysis_plot_kalman_2d.py kresults = ca.get_pytables_file_by_filename(kalman_filename) try: kobs = kresults.root.ML_estimates except tables.exceptions.NoSuchNodeError: # backward compatibility kobs = kresults.root.kalman_observations kframes = kobs.read(field='frame') if frame_start is not None: k_after_start = numpy.nonzero(kframes >= frame_start)[0] else: k_after_start = None if frame_stop is not None: k_before_stop = numpy.nonzero(kframes <= frame_stop)[0] else: k_before_stop = None if k_after_start is not None and k_before_stop is not None: k_use_idxs = numpy.intersect1d(k_after_start, k_before_stop) elif k_after_start is not None: k_use_idxs = k_after_start elif k_before_stop is not None: k_use_idxs = k_before_stop else: k_use_idxs = numpy.arange(kobs.nrows) obs_2d_idxs = kobs.read(field='obs_2d_idx')[k_use_idxs] kframes = kframes[k_use_idxs] try: kobs_2d = kresults.root.ML_estimates_2d_idxs except tables.exceptions.NoSuchNodeError: # backwards compatibility kobs_2d = kresults.root.kalman_observations_2d_idxs # this will be slooow... used_cam_ids = collections.defaultdict(list) for obs_2d_idx, kframe in zip(obs_2d_idxs, kframes): obs_2d_row = kobs_2d[int(obs_2d_idx)] #print kframe,obs_2d_row for camn in obs_2d_row[::2]: try: cam_id = camn2cam_id[camn] except KeyError: cam_id = None if cam_id is not None: used_cam_ids[cam_id].append(kframe) for cam_id, kframes_used in used_cam_ids.iteritems(): kframes_used = numpy.array(kframes_used) yval = -99 * numpy.ones_like(kframes_used) ax = ax_by_cam[cam_id] if options.timestamps: ax.plot(time_model.framestamp2timestamp(kframes_used), yval, 'kx') else: ax.plot(kframes_used, yval, 'kx') ax.set_xlim((start_frame, stop_frame)) ax.set_ylim([-100, 800])
input_node._f_copy(output_h5.root,recursive=True) print 'done copying' # Clear values in destination table that we may overwrite. dest_table = output_h5.root.data2d_distorted for colname in ['x','y','area','slope','eccentricity','cur_val', 'mean_val','sumsqf_val']: if colname=='cur_val': fill_value = 0 else: fill_value = np.nan clear_col(dest_table,colname,fill_value=fill_value) dest_table.flush() print 'done clearing' camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) cam_id2fmfs = collections.defaultdict(list) cam_id2view = {} for ufmf_filename in ufmf_filenames: fmf = ufmf.FlyMovieEmulator(ufmf_filename, #darken=-50, allow_no_such_frame_errors=True) timestamps = fmf.get_all_timestamps() cam_id = get_cam_id_from_filename(fmf.filename, cam_id2camns.keys()) cam_id2fmfs[cam_id].append( (fmf,result_utils.Quick1DIndexer(timestamps))) cam_id2view[cam_id] = filename2view[fmf.filename]
from flydra_analysis.analysis.result_utils import get_caminfo_dicts import sys if sys.platform == "darwin": # for interactive use in IPython: import matplotlib matplotlib.use("TkAgg") if __name__ == "__main__": filename = "DATA20070214_192124.h5" print("filename", filename) n_cams = 4 print("n_cams", n_cams) kresults = tables.open_file(filename, mode="r") camn2cam_id, cam_id2camns = get_caminfo_dicts(kresults) data2d = kresults.root.data2d_distorted print("len(data2d)", len(data2d)) timestamp_vectors = [] framenumbers = [] if 1: # This is a slow but correct method that gets all data for a given frame try: # ipython speedup allframes uframes sortidx sortedframes except NameError: allframes = data2d.read(field="frame", flavor="numpy")
def show_it( self, fig, filename, kalman_filename=None, frame_start=None, frame_stop=None, show_nth_frame=None, obj_only=None, reconstructor_filename=None, options=None, ): if show_nth_frame == 0: show_nth_frame = None results = result_utils.get_results(filename, mode="r") opened_kresults = False kresults = None if kalman_filename is not None: if os.path.abspath(kalman_filename) == os.path.abspath(filename): kresults = results else: kresults = PT.open_file(kalman_filename, mode="r") opened_kresults = True # copied from plot_timeseries_2d_3d.py ca = core_analysis.get_global_CachingAnalyzer() ( xxobj_ids, xxuse_obj_ids, xxis_mat_file, xxdata_file, extra, ) = ca.initial_file_load(kalman_filename) fps = extra["frames_per_second"] dynamic_model_name = None if dynamic_model_name is None: dynamic_model_name = extra.get("dynamic_model_name", None) if dynamic_model_name is None: dynamic_model_name = dynamic_models.DEFAULT_MODEL warnings.warn('no dynamic model specified, using "%s"' % dynamic_model_name) else: print('detected file loaded with dynamic model "%s"' % dynamic_model_name) if dynamic_model_name.startswith("EKF "): dynamic_model_name = dynamic_model_name[4:] print(' for smoothing, will use dynamic model "%s"' % dynamic_model_name) if hasattr(results.root, "images"): img_table = results.root.images else: img_table = None reconstructor_source = None if reconstructor_filename is None: if kresults is not None: reconstructor_source = kresults elif hasattr(results.root, "calibration"): reconstructor_source = results else: reconstructor_source = None else: if os.path.abspath(reconstructor_filename) == os.path.abspath( filename): reconstructor_source = results elif (kalman_filename is not None) and (os.path.abspath(reconstructor_filename) == os.path.abspath(kalman_filename)): reconstructor_source = kresults else: reconstructor_source = reconstructor_filename if reconstructor_source is not None: self.reconstructor = flydra_core.reconstruct.Reconstructor( reconstructor_source) if options.stim_xml: file_timestamp = results.filename[4:19] stim_xml = xml_stimulus.xml_stimulus_from_filename( options.stim_xml, timestamp_string=file_timestamp) if self.reconstructor is not None: stim_xml.verify_reconstructor(self.reconstructor) if self.reconstructor is not None: self.reconstructor = self.reconstructor.get_scaled() camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(results) data2d = results.root.data2d_distorted # make sure we have 2d data table print("reading frames...") frames = data2d.read(field="frame") print("OK") if frame_start is not None: print("selecting frames after start") # after_start = data2d.get_where_list( 'frame>=frame_start') after_start = numpy.nonzero(frames >= frame_start)[0] else: after_start = None if frame_stop is not None: print("selecting frames before stop") # before_stop = data2d.get_where_list( 'frame<=frame_stop') before_stop = numpy.nonzero(frames <= frame_stop)[0] else: before_stop = None print("finding all frames") if after_start is not None and before_stop is not None: use_idxs = numpy.intersect1d(after_start, before_stop) elif after_start is not None: use_idxs = after_start elif before_stop is not None: use_idxs = before_stop else: use_idxs = numpy.arange(data2d.nrows) # OK, we have data coords, plot print("reading cameras") frames = frames[ use_idxs] # data2d.read_coordinates( use_idxs, field='frame') if len(frames): print("frame range: %d - %d (%d frames total)" % (frames[0], frames[-1], len(frames))) camns = data2d.read(field="camn") camns = camns[use_idxs] # camns = data2d.read_coordinates( use_idxs, field='camn') unique_camns = numpy.unique(camns) unique_cam_ids = list(set([camn2cam_id[camn] for camn in unique_camns])) unique_cam_ids.sort() print("%d cameras with data" % (len(unique_cam_ids), )) # plot all cameras, not just those with data all_cam_ids = cam_id2camns.keys() all_cam_ids.sort() unique_cam_ids = all_cam_ids if len(unique_cam_ids) == 1: n_rows = 1 n_cols = 1 elif len(unique_cam_ids) <= 6: n_rows = 2 n_cols = 3 elif len(unique_cam_ids) <= 12: n_rows = 3 n_cols = 4 else: n_rows = 4 n_cols = int(math.ceil(len(unique_cam_ids) / n_rows)) for i, cam_id in enumerate(unique_cam_ids): ax = auto_subplot(fig, i, n_rows=n_rows, n_cols=n_cols) ax.set_title("%s: %s" % (cam_id, str(cam_id2camns[cam_id]))) ## ax.set_xticks([]) ## ax.set_yticks([]) ax.this_minx = np.inf ax.this_maxx = -np.inf ax.this_miny = np.inf ax.this_maxy = -np.inf self.subplot_by_cam_id[cam_id] = ax for cam_id in unique_cam_ids: ax = self.subplot_by_cam_id[cam_id] if img_table is not None: bg_arr_h5 = getattr(img_table, cam_id) bg_arr = bg_arr_h5.read() ax.imshow(bg_arr.squeeze(), origin="lower", cmap=cm.pink) ax.set_autoscale_on(True) ax.autoscale_view() pylab.draw() ax.set_autoscale_on(False) if self.reconstructor is not None: if cam_id in self.reconstructor.get_cam_ids(): res = self.reconstructor.get_resolution(cam_id) ax.set_xlim([0, res[0]]) ax.set_ylim([res[1], 0]) if options.stim_xml is not None: stim_xml.plot_stim_over_distorted_image(ax, cam_id) for camn in unique_camns: cam_id = camn2cam_id[camn] ax = self.subplot_by_cam_id[cam_id] this_camn_idxs = use_idxs[camns == camn] xs = data2d.read_coordinates(this_camn_idxs, field="x") valid_idx = numpy.nonzero(~numpy.isnan(xs))[0] if not len(valid_idx): continue ys = data2d.read_coordinates(this_camn_idxs, field="y") if options.show_orientation: slope = data2d.read_coordinates(this_camn_idxs, field="slope") idx_first_valid = valid_idx[0] idx_last_valid = valid_idx[-1] tmp_frames = data2d.read_coordinates(this_camn_idxs, field="frame") ax.plot([xs[idx_first_valid]], [ys[idx_first_valid]], "ro", label="first point") ax.this_minx = min(np.min(xs[valid_idx]), ax.this_minx) ax.this_maxx = max(np.max(xs[valid_idx]), ax.this_maxx) ax.this_miny = min(np.min(ys[valid_idx]), ax.this_miny) ax.this_maxy = max(np.max(ys[valid_idx]), ax.this_maxy) ax.plot(xs[valid_idx], ys[valid_idx], "g.", label="all points") if options.show_orientation: angle = np.arctan(slope) r = 20.0 dx = r * np.cos(angle) dy = r * np.sin(angle) x0 = xs - dx x1 = xs + dx y0 = ys - dy y1 = ys + dy segs = [] for i in valid_idx: segs.append(((x0[i], y0[i]), (x1[i], y1[i]))) line_segments = collections.LineCollection( segs, linewidths=[1], colors=[(0, 1, 0)], ) ax.add_collection(line_segments) ax.plot([xs[idx_last_valid]], [ys[idx_last_valid]], "bo", label="first point") if show_nth_frame is not None: for i, f in enumerate(tmp_frames): if f % show_nth_frame == 0: ax.text(xs[i], ys[i], "%d" % (f, )) if 0: for x, y, frame in zip(xs[::5], ys[::5], tmp_frames[::5]): ax.text(x, y, "%d" % (frame, )) fig.canvas.mpl_connect("key_press_event", self.on_key_press) if options.autozoom: for cam_id in self.subplot_by_cam_id.keys(): ax = self.subplot_by_cam_id[cam_id] ax.set_xlim((ax.this_minx - 10, ax.this_maxx + 10)) ax.set_ylim((ax.this_miny - 10, ax.this_maxy + 10)) if options.save_fig: for cam_id in self.subplot_by_cam_id.keys(): ax = self.subplot_by_cam_id[cam_id] ax.set_xticks([]) ax.set_yticks([]) if kalman_filename is None: return if 0: # Do same as above for Kalman-filtered data kobs = kresults.root.ML_estimates kframes = kobs.read(field="frame") if frame_start is not None: k_after_start = numpy.nonzero(kframes >= frame_start)[0] else: k_after_start = None if frame_stop is not None: k_before_stop = numpy.nonzero(kframes <= frame_stop)[0] else: k_before_stop = None if k_after_start is not None and k_before_stop is not None: k_use_idxs = numpy.intersect1d(k_after_start, k_before_stop) elif k_after_start is not None: k_use_idxs = k_after_start elif k_before_stop is not None: k_use_idxs = k_before_stop else: k_use_idxs = numpy.arange(kobs.nrows) obj_ids = kobs.read(field="obj_id")[k_use_idxs] obs_2d_idxs = kobs.read(field="obs_2d_idx")[k_use_idxs] kframes = kframes[k_use_idxs] kobs_2d = kresults.root.ML_estimates_2d_idxs xys_by_obj_id = {} for obj_id, kframe, obs_2d_idx in zip(obj_ids, kframes, obs_2d_idxs): if obj_only is not None: if obj_id not in obj_only: continue obs_2d_idx_find = int( obs_2d_idx) # XXX grr, why can't pytables do this? obj_id_save = int(obj_id) # convert from possible numpy scalar xys_by_cam_id = xys_by_obj_id.setdefault(obj_id_save, {}) kobs_2d_data = kobs_2d.read(start=obs_2d_idx_find, stop=obs_2d_idx_find + 1) assert len(kobs_2d_data) == 1 kobs_2d_data = kobs_2d_data[0] this_camns = kobs_2d_data[0::2] this_camn_idxs = kobs_2d_data[1::2] this_use_idxs = use_idxs[frames == kframe] d2d = data2d.read_coordinates(this_use_idxs) for this_camn, this_camn_idx in zip(this_camns, this_camn_idxs): this_idxs_tmp = numpy.nonzero(d2d["camn"] == this_camn)[0] this_camn_d2d = d2d[d2d["camn"] == this_camn] found = False for this_row in this_camn_d2d: # XXX could be sped up if this_row["frame_pt_idx"] == this_camn_idx: found = True break if not found: if 1: print( "WARNING:point not found in data -- 3D data starts before 2D I guess." ) continue else: raise RuntimeError("point not found in data!?") this_cam_id = camn2cam_id[this_camn] xys = xys_by_cam_id.setdefault(this_cam_id, ([], [])) xys[0].append(this_row["x"]) xys[1].append(this_row["y"]) for obj_id in xys_by_obj_id: xys_by_cam_id = xys_by_obj_id[obj_id] for cam_id, (xs, ys) in xys_by_cam_id.iteritems(): ax = self.subplot_by_cam_id[cam_id] ax.plot(xs, ys, "x-", label="obs: %d" % obj_id) ax.text(xs[0], ys[0], "%d:" % (obj_id, )) ax.text(xs[-1], ys[-1], ":%d" % (obj_id, )) if 1: # do for core_analysis smoothed (or not) data for obj_id in xxuse_obj_ids: try: rows = ca.load_data( obj_id, kalman_filename, use_kalman_smoothing=True, frames_per_second=fps, dynamic_model_name=dynamic_model_name, ) except core_analysis.NotEnoughDataToSmoothError: warnings.warn( "not enough data to smooth obj_id %d, skipping." % (obj_id, )) if frame_start is not None: c1 = rows["frame"] >= frame_start else: c1 = np.ones((len(rows), ), dtype=np.bool) if frame_stop is not None: c2 = rows["frame"] <= frame_stop else: c2 = np.ones((len(rows), ), dtype=np.bool) valid = c1 & c2 rows = rows[valid] if len(rows) > 1: X3d = np.array((rows["x"], rows["y"], rows["z"], np.ones_like(rows["z"]))).T for cam_id in self.subplot_by_cam_id.keys(): ax = self.subplot_by_cam_id[cam_id] newx, newy = self.reconstructor.find2d(cam_id, X3d, distorted=True) ax.plot(newx, newy, "-", label="k: %d" % obj_id) results.close() if opened_kresults: kresults.close()
def iterate_frames(h5_filename, ufmf_fnames, # or fmfs white_background=False, max_n_frames = None, start = None, stop = None, rgb8_if_color=False, movie_cam_ids=None, camn2cam_id = None, ): """yield frame-by-frame data""" # First pass over .ufmf files: get intersection of timestamps first_ufmf_ts = -np.inf last_ufmf_ts = np.inf ufmfs = {} cam_ids = [] global_data = {'width_heights': {}} for movie_idx,ufmf_fname in enumerate(ufmf_fnames): if movie_cam_ids is not None: cam_id = movie_cam_ids[movie_idx] else: cam_id = get_cam_id_from_ufmf_fname(ufmf_fname) cam_ids.append( cam_id ) kwargs = {} extra = {} if ufmf_fname.lower().endswith('.fmf'): ufmf = fmf_mod.FlyMovie(ufmf_fname) bg_fmf_filename = os.path.splitext(ufmf_fname)[0] + '_mean.fmf' if os.path.exists(bg_fmf_filename): extra['bg_fmf'] = fmf_mod.FlyMovie(bg_fmf_filename) extra['bg_tss'] = extra['bg_fmf'].get_all_timestamps() extra['bg_fmf'].seek(0) else: ufmf = ufmf_mod.FlyMovieEmulator(ufmf_fname, white_background=white_background, **kwargs) global_data['width_heights'][cam_id] = ( ufmf.get_width(), ufmf.get_height() ) tss = ufmf.get_all_timestamps() ufmf.seek(0) ufmfs[ufmf_fname] = (ufmf, cam_id, tss, extra) min_ts = np.min(tss) max_ts = np.max(tss) if min_ts > first_ufmf_ts: first_ufmf_ts = min_ts if max_ts < last_ufmf_ts: last_ufmf_ts = max_ts assert first_ufmf_ts < last_ufmf_ts, ".ufmf files don't all overlap in time" ufmf_fnames.sort() cam_ids.sort() with open_file_safe( h5_filename, mode='r' ) as h5: if camn2cam_id is None: camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) parsed = result_utils.read_textlog_header(h5) flydra_version = parsed.get('flydra_version',None) if flydra_version is not None and flydra_version >= '0.4.45': # camnode.py saved timestamps into .ufmf file given by # time.time() (camn_receive_timestamp). Compare with # mainbrain's data2d_distorted column # 'cam_received_timestamp'. old_camera_timestamp_source = False timestamp_name = 'cam_received_timestamp' else: # camnode.py saved timestamps into .ufmf file given by # camera driver. Compare with mainbrain's data2d_distorted # column 'timestamp'. old_camera_timestamp_source = True timestamp_name = 'timestamp' h5_data = h5.root.data2d_distorted[:] if 1: # narrow search to local region of .h5 cond = ((first_ufmf_ts <= h5_data[timestamp_name]) & (h5_data[timestamp_name] <= last_ufmf_ts)) narrow_h5_data = h5_data[cond] narrow_camns = narrow_h5_data['camn'] narrow_timestamps = narrow_h5_data[timestamp_name] # Find the camn for each .ufmf file cam_id2camn = {} for cam_id in cam_ids: cam_id_camn_already_found = False for ufmf_fname in ufmfs.keys(): (ufmf, test_cam_id, tss, extra) = ufmfs[ufmf_fname] if cam_id != test_cam_id: continue assert not cam_id_camn_already_found cam_id_camn_already_found = True umin=np.min(tss) umax=np.max(tss) cond = (umin<=narrow_timestamps) & (narrow_timestamps<=umax) ucamns = narrow_camns[cond] ucamns = np.unique(ucamns) camns = [] for camn in ucamns: if camn2cam_id[camn]==cam_id: camns.append(camn) assert len(camns)<2, "can't handle multiple camns per cam_id" if len(camns): cam_id2camn[cam_id] = camns[0] ff = utils.FastFinder(narrow_h5_data['frame']) unique_frames = list(np.unique(narrow_h5_data['frame'])) unique_frames.sort() unique_frames = np.array( unique_frames ) if start is not None: unique_frames = unique_frames[ unique_frames >= start ] if stop is not None: unique_frames = unique_frames[ unique_frames <= stop ] if max_n_frames is not None: unique_frames = unique_frames[:max_n_frames] for frame_enum,frame in enumerate(unique_frames): narrow_idxs = ff.get_idxs_of_equal(frame) # trim data under consideration to just this frame this_h5_data = narrow_h5_data[narrow_idxs] this_camns = this_h5_data['camn'] this_tss = this_h5_data[timestamp_name] # a couple more checks if np.any( this_tss < first_ufmf_ts): continue if np.any( this_tss >= last_ufmf_ts): break per_frame_dict = {} for ufmf_fname in ufmf_fnames: ufmf, cam_id, tss, extra = ufmfs[ufmf_fname] if cam_id not in cam_id2camn: continue camn = cam_id2camn[cam_id] this_camn_cond = this_camns == camn this_cam_h5_data = this_h5_data[this_camn_cond] this_camn_tss = this_cam_h5_data[timestamp_name] if not len(this_camn_tss): # no h5 data for this cam_id at this frame continue this_camn_ts=np.unique(this_camn_tss) assert len(this_camn_ts)==1 this_camn_ts = this_camn_ts[0] if isinstance(ufmf, ufmf_mod.FlyMovieEmulator): is_real_ufmf = True else: is_real_ufmf = False # optimistic: get next frame. it's probably the one we want try: if is_real_ufmf: image,image_ts,more = ufmf.get_next_frame(_return_more=True) else: image,image_ts = ufmf.get_next_frame() more = fill_more_for( extra, image_ts ) except ufmf_mod.NoMoreFramesException: image_ts = None if this_camn_ts != image_ts: # It was not the frame we wanted. Find it. ufmf_frame_idxs = np.nonzero(tss == this_camn_ts)[0] if (len(ufmf_frame_idxs)==0 and old_camera_timestamp_source): warnings.warn( 'low-precision timestamp comparison in ' 'use due to outdated .ufmf timestamp ' 'saving.') # 2.5 msec precision required ufmf_frame_idxs = np.nonzero( abs( tss - this_camn_ts ) < 0.0025)[0] assert len(ufmf_frame_idxs)==1 ufmf_frame_no = ufmf_frame_idxs[0] if is_real_ufmf: image,image_ts,more = ufmf.get_frame(ufmf_frame_no, _return_more=True) else: image,image_ts = ufmf.get_frame(ufmf_frame_no) more = fill_more_for( extra, image_ts ) del ufmf_frame_no, ufmf_frame_idxs coding = ufmf.get_format() if imops.is_coding_color(coding): if rgb8_if_color: image = imops.to_rgb8(coding,image) else: warnings.warn('color image not converted to color') per_frame_dict[ufmf_fname] = { 'image':image, 'cam_id':cam_id, 'camn':camn, 'timestamp':this_cam_h5_data['timestamp'][0], 'cam_received_timestamp': this_cam_h5_data['cam_received_timestamp'][0], 'ufmf_frame_timestamp':this_cam_h5_data[timestamp_name][0], } if more is not None: per_frame_dict[ufmf_fname].update(more) per_frame_dict['tracker_data']=this_h5_data per_frame_dict['global_data']=global_data # on every iteration, pass our global data yield (per_frame_dict,frame)
def doit(output_h5_filename=None, kalman_filename=None, data2d_filename=None, start=None, stop=None, gate_angle_threshold_degrees=40.0, area_threshold_for_orientation=0.0, obj_only=None, options=None): gate_angle_threshold_radians = gate_angle_threshold_degrees * D2R if options.show: import matplotlib.pyplot as plt import matplotlib.ticker as mticker M = SymobolicModels() x = sympy.DeferredVector('x') G_symbolic = M.get_observation_model(x) dx_symbolic = M.get_process_model(x) if 0: print 'G_symbolic' sympy.pprint(G_symbolic) print G_linearized = [G_symbolic.diff(x[i]) for i in range(7)] if 0: print 'G_linearized' for i in range(len(G_linearized)): sympy.pprint(G_linearized[i]) print arg_tuple_x = (M.P00, M.P01, M.P02, M.P03, M.P10, M.P11, M.P12, M.P13, M.P20, M.P21, M.P22, M.P23, M.Ax, M.Ay, M.Az, x) xm = sympy.DeferredVector('xm') arg_tuple_x_xm = (M.P00, M.P01, M.P02, M.P03, M.P10, M.P11, M.P12, M.P13, M.P20, M.P21, M.P22, M.P23, M.Ax, M.Ay, M.Az, x, xm) eval_G = lambdify(arg_tuple_x, G_symbolic, 'numpy') eval_linG = lambdify(arg_tuple_x, G_linearized, 'numpy') # coord shift of observation model phi_symbolic = M.get_observation_model(xm) # H = G - phi H_symbolic = G_symbolic - phi_symbolic # We still take derivative wrt x (not xm). H_linearized = [H_symbolic.diff(x[i]) for i in range(7)] eval_phi = lambdify(arg_tuple_x_xm, phi_symbolic, 'numpy') eval_H = lambdify(arg_tuple_x_xm, H_symbolic, 'numpy') eval_linH = lambdify(arg_tuple_x_xm, H_linearized, 'numpy') if 0: print 'dx_symbolic' sympy.pprint(dx_symbolic) print eval_dAdt = drop_dims(lambdify(x, dx_symbolic, 'numpy')) debug_level = 0 if debug_level: np.set_printoptions(linewidth=130, suppress=True) if os.path.exists(output_h5_filename): raise RuntimeError("will not overwrite old file '%s'" % output_h5_filename) ca = core_analysis.get_global_CachingAnalyzer() with open_file_safe(output_h5_filename, mode='w') as output_h5: with open_file_safe(kalman_filename, mode='r') as kh5: with open_file_safe(data2d_filename, mode='r') as h5: for input_node in kh5.root._f_iter_nodes(): # copy everything from source to dest input_node._f_copy(output_h5.root, recursive=True) try: dest_table = output_h5.root.ML_estimates except tables.exceptions.NoSuchNodeError, err1: # backwards compatibility try: dest_table = output_h5.root.kalman_observations except tables.exceptions.NoSuchNodeError, err2: raise err1 for colname in ['hz_line%d' % i for i in range(6)]: clear_col(dest_table, colname) dest_table.flush() if options.show: fig1 = plt.figure() ax1 = fig1.add_subplot(511) ax2 = fig1.add_subplot(512, sharex=ax1) ax3 = fig1.add_subplot(513, sharex=ax1) ax4 = fig1.add_subplot(514, sharex=ax1) ax5 = fig1.add_subplot(515, sharex=ax1) ax1.xaxis.set_major_formatter( mticker.FormatStrFormatter("%d")) min_frame_range = np.inf max_frame_range = -np.inf reconst = reconstruct.Reconstructor(kh5) camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) fps = result_utils.get_fps(h5) dt = 1.0 / fps used_camn_dict = {} # associate framenumbers with timestamps using 2d .h5 file data2d = h5.root.data2d_distorted[:] # load to RAM if start is not None: data2d = data2d[data2d['frame'] >= start] if stop is not None: data2d = data2d[data2d['frame'] <= stop] data2d_idxs = np.arange(len(data2d)) h5_framenumbers = data2d['frame'] h5_frame_qfi = result_utils.QuickFrameIndexer(h5_framenumbers) ML_estimates_2d_idxs = (kh5.root.ML_estimates_2d_idxs[:]) all_kobs_obj_ids = dest_table.read(field='obj_id') all_kobs_frames = dest_table.read(field='frame') use_obj_ids = np.unique(all_kobs_obj_ids) if obj_only is not None: use_obj_ids = obj_only if hasattr(kh5.root.kalman_estimates.attrs, 'dynamic_model_name'): dynamic_model = kh5.root.kalman_estimates.attrs.dynamic_model_name if dynamic_model.startswith('EKF '): dynamic_model = dynamic_model[4:] else: dynamic_model = 'mamarama, units: mm' warnings.warn( 'could not determine dynamic model name, using "%s"' % dynamic_model) for obj_id_enum, obj_id in enumerate(use_obj_ids): # Use data association step from kalmanization to load potentially # relevant 2D orientations, but discard previous 3D orientation. if obj_id_enum % 100 == 0: print 'obj_id %d (%d of %d)' % (obj_id, obj_id_enum, len(use_obj_ids)) if options.show: all_xhats = [] all_ori = [] output_row_obj_id_cond = all_kobs_obj_ids == obj_id obj_3d_rows = ca.load_dynamics_free_MLE_position( obj_id, kh5) if start is not None: obj_3d_rows = obj_3d_rows[ obj_3d_rows['frame'] >= start] if stop is not None: obj_3d_rows = obj_3d_rows[obj_3d_rows['frame'] <= stop] try: smoothed_3d_rows = ca.load_data( obj_id, kh5, use_kalman_smoothing=True, frames_per_second=fps, dynamic_model_name=dynamic_model) except core_analysis.NotEnoughDataToSmoothError: continue smoothed_frame_qfi = result_utils.QuickFrameIndexer( smoothed_3d_rows['frame']) slopes_by_camn_by_frame = collections.defaultdict(dict) x0d_by_camn_by_frame = collections.defaultdict(dict) y0d_by_camn_by_frame = collections.defaultdict(dict) pt_idx_by_camn_by_frame = collections.defaultdict(dict) min_frame = np.inf max_frame = -np.inf start_idx = None for this_idx, this_3d_row in enumerate(obj_3d_rows): # iterate over each sample in the current camera framenumber = this_3d_row['frame'] if not np.isnan(this_3d_row['hz_line0']): # We have a valid initial 3d orientation guess. if framenumber < min_frame: min_frame = framenumber assert start_idx is None, "frames out of order?" start_idx = this_idx max_frame = max(max_frame, framenumber) h5_2d_row_idxs = h5_frame_qfi.get_frame_idxs( framenumber) frame2d = data2d[h5_2d_row_idxs] frame2d_idxs = data2d_idxs[h5_2d_row_idxs] obs_2d_idx = this_3d_row['obs_2d_idx'] kobs_2d_data = ML_estimates_2d_idxs[int(obs_2d_idx)] # Parse VLArray. this_camns = kobs_2d_data[0::2] this_camn_idxs = kobs_2d_data[1::2] # Now, for each camera viewing this object at this # frame, extract images. for camn, camn_pt_no in zip(this_camns, this_camn_idxs): # find 2D point corresponding to object cam_id = camn2cam_id[camn] cond = ((frame2d['camn'] == camn) & (frame2d['frame_pt_idx'] == camn_pt_no)) idxs = np.nonzero(cond)[0] if len(idxs) == 0: continue assert len(idxs) == 1 ## if len(idxs)!=1: ## raise ValueError('expected one (and only one) frame, got %d'%len(idxs)) idx = idxs[0] orig_data2d_rownum = frame2d_idxs[idx] frame_timestamp = frame2d[idx]['timestamp'] row = frame2d[idx] assert framenumber == row['frame'] if ((row['eccentricity'] < reconst.minimum_eccentricity) or (row['area'] < area_threshold_for_orientation)): slopes_by_camn_by_frame[camn][ framenumber] = np.nan x0d_by_camn_by_frame[camn][ framenumber] = np.nan y0d_by_camn_by_frame[camn][ framenumber] = np.nan pt_idx_by_camn_by_frame[camn][ framenumber] = camn_pt_no else: slopes_by_camn_by_frame[camn][ framenumber] = row['slope'] x0d_by_camn_by_frame[camn][framenumber] = row[ 'x'] y0d_by_camn_by_frame[camn][framenumber] = row[ 'y'] pt_idx_by_camn_by_frame[camn][ framenumber] = camn_pt_no if start_idx is None: warnings.warn("skipping obj_id %d: " "could not find valid start frame" % obj_id) continue obj_3d_rows = obj_3d_rows[start_idx:] # now collect in a numpy array for all cam assert int(min_frame) == min_frame assert int(max_frame + 1) == max_frame + 1 frame_range = np.arange(int(min_frame), int(max_frame + 1)) if debug_level >= 1: print 'frame range %d-%d' % (frame_range[0], frame_range[-1]) camn_list = slopes_by_camn_by_frame.keys() camn_list.sort() cam_id_list = [camn2cam_id[camn] for camn in camn_list] n_cams = len(camn_list) n_frames = len(frame_range) save_cols = {} save_cols['frame'] = [] for camn in camn_list: save_cols['dist%d' % camn] = [] save_cols['used%d' % camn] = [] save_cols['theta%d' % camn] = [] # NxM array with rows being frames and cols being cameras slopes = np.ones((n_frames, n_cams), dtype=np.float) x0ds = np.ones((n_frames, n_cams), dtype=np.float) y0ds = np.ones((n_frames, n_cams), dtype=np.float) for j, camn in enumerate(camn_list): slopes_by_frame = slopes_by_camn_by_frame[camn] x0d_by_frame = x0d_by_camn_by_frame[camn] y0d_by_frame = y0d_by_camn_by_frame[camn] for frame_idx, absolute_frame_number in enumerate( frame_range): slopes[frame_idx, j] = slopes_by_frame.get( absolute_frame_number, np.nan) x0ds[frame_idx, j] = x0d_by_frame.get(absolute_frame_number, np.nan) y0ds[frame_idx, j] = y0d_by_frame.get(absolute_frame_number, np.nan) if options.show: frf = np.array(frame_range, dtype=np.float) min_frame_range = min(np.min(frf), min_frame_range) max_frame_range = max(np.max(frf), max_frame_range) ax1.plot(frame_range, slope2modpi(slopes[:, j]), '.', label=camn2cam_id[camn]) if options.show: ax1.legend() if 1: # estimate orientation of initial frame row0 = obj_3d_rows[: 1] # take only first row but keep as 1d array hzlines = np.array([ row0['hz_line0'], row0['hz_line1'], row0['hz_line2'], row0['hz_line3'], row0['hz_line4'], row0['hz_line5'] ]).T directions = reconstruct.line_direction(hzlines) q0 = PQmath.orientation_to_quat(directions[0]) assert not np.isnan( q0.x), "cannot start with missing orientation" w0 = 0, 0, 0 # no angular rate init_x = np.array( [w0[0], w0[1], w0[2], q0.x, q0.y, q0.z, q0.w]) Pminus = np.zeros((7, 7)) # angular rate part of state variance is .5 for i in range(0, 3): Pminus[i, i] = .5 # quaternion part of state variance is 1 for i in range(3, 7): Pminus[i, i] = 1 if 1: # setup of noise estimates Q = np.zeros((7, 7)) # angular rate part of state variance for i in range(0, 3): Q[i, i] = Q_scalar_rate # quaternion part of state variance for i in range(3, 7): Q[i, i] = Q_scalar_quat preA = np.eye(7) ekf = kalman_ekf.EKF(init_x, Pminus) previous_posterior_x = init_x if options.show: _save_plot_rows = [] _save_plot_rows_used = [] for frame_idx, absolute_frame_number in enumerate( frame_range): # Evaluate the Jacobian of the process update # using previous frame's posterior estimate. (This # is not quite the same as this frame's prior # estimate. The difference this frame's prior # estimate is _after_ the process update # model. Which we need to get doing this.) if options.show: _save_plot_rows.append(np.nan * np.ones( (n_cams, ))) _save_plot_rows_used.append(np.nan * np.ones( (n_cams, ))) this_dx = eval_dAdt(previous_posterior_x) A = preA + this_dx * dt if debug_level >= 1: print print 'frame', absolute_frame_number, '-' * 40 print 'previous posterior', previous_posterior_x if debug_level > 6: print 'A' print A xhatminus, Pminus = ekf.step1__calculate_a_priori(A, Q) if debug_level >= 1: print 'new prior', xhatminus # 1. Gate per-camera orientations. this_frame_slopes = slopes[frame_idx, :] this_frame_theta_measured = slope2modpi( this_frame_slopes) this_frame_x0d = x0ds[frame_idx, :] this_frame_y0d = y0ds[frame_idx, :] if debug_level >= 5: print 'this_frame_slopes', this_frame_slopes save_cols['frame'].append(absolute_frame_number) for j, camn in enumerate(camn_list): # default to no detection, change below save_cols['dist%d' % camn].append(np.nan) save_cols['used%d' % camn].append(0) save_cols['theta%d' % camn].append( this_frame_theta_measured[j]) all_data_this_frame_missing = False gate_vector = None y = [] # observation (per camera) hx = [] # expected observation (per camera) C = [] # linearized observation model (per camera) N_obs_this_frame = 0 cams_without_data = np.isnan(this_frame_slopes) if np.all(cams_without_data): all_data_this_frame_missing = True smoothed_pos_idxs = smoothed_frame_qfi.get_frame_idxs( absolute_frame_number) if len(smoothed_pos_idxs) == 0: all_data_this_frame_missing = True smoothed_pos_idx = None smooth_row = None center_position = None else: try: assert len(smoothed_pos_idxs) == 1 except: print 'obj_id', obj_id print 'absolute_frame_number', absolute_frame_number if len(frame_range): print 'frame_range[0],frame_rang[-1]', frame_range[ 0], frame_range[-1] else: print 'no frame range' print 'len(smoothed_pos_idxs)', len( smoothed_pos_idxs) raise smoothed_pos_idx = smoothed_pos_idxs[0] smooth_row = smoothed_3d_rows[smoothed_pos_idx] assert smooth_row['frame'] == absolute_frame_number center_position = np.array( (smooth_row['x'], smooth_row['y'], smooth_row['z'])) if debug_level >= 2: print 'center_position', center_position if not all_data_this_frame_missing: if expected_orientation_method == 'trust_prior': state_for_phi = xhatminus # use a priori elif expected_orientation_method == 'SVD_line_fits': # construct matrix of planes P = [] for camn_idx in range(n_cams): this_x0d = this_frame_x0d[camn_idx] this_y0d = this_frame_y0d[camn_idx] slope = this_frame_slopes[camn_idx] plane, ray = reconst.get_3D_plane_and_ray( cam_id, this_x0d, this_y0d, slope) if np.isnan(plane[0]): continue P.append(plane) if len(P) < 2: # not enough data to do SVD... fallback to prior state_for_phi = xhatminus # use a priori else: Lco = reconstruct.intersect_planes_to_find_line( P) q = PQmath.pluecker_to_quat(Lco) state_for_phi = cgtypes_quat2statespace(q) cams_with_data = ~cams_without_data possible_cam_idxs = np.nonzero(cams_with_data)[0] if debug_level >= 6: print 'possible_cam_idxs', possible_cam_idxs gate_vector = np.zeros((n_cams, ), dtype=np.bool) ## flip_vector = np.zeros( (n_cams,), dtype=np.bool) for camn_idx in possible_cam_idxs: cam_id = cam_id_list[camn_idx] camn = camn_list[camn_idx] # This ignores distortion. To incorporate # distortion, this would require # appropriate scaling of orientation # vector, which would require knowing # target's size. In which case we should # track head and tail separately and not # use this whole quaternion mess. ## theta_measured=slope2modpi( ## this_frame_slopes[camn_idx]) theta_measured = this_frame_theta_measured[ camn_idx] if debug_level >= 6: print 'cam_id %s, camn %d' % (cam_id, camn) if debug_level >= 3: a = reconst.find2d(cam_id, center_position) other_position = get_point_on_line( xhatminus, center_position) b = reconst.find2d(cam_id, other_position) theta_expected = find_theta_mod_pi_between_points( a, b) print(' theta_expected,theta_measured', theta_expected * R2D, theta_measured * R2D) P = reconst.get_pmat(cam_id) if 0: args_x = (P[0, 0], P[0, 1], P[0, 2], P[0, 3], P[1, 0], P[1, 1], P[1, 2], P[1, 3], P[2, 0], P[2, 1], P[2, 2], P[2, 3], center_position[0], center_position[1], center_position[2], xhatminus) this_y = theta_measured this_hx = eval_G(*args_x) this_C = eval_linG(*args_x) else: args_x_xm = (P[0, 0], P[0, 1], P[0, 2], P[0, 3], P[1, 0], P[1, 1], P[1, 2], P[1, 3], P[2, 0], P[2, 1], P[2, 2], P[2, 3], center_position[0], center_position[1], center_position[2], xhatminus, state_for_phi) this_phi = eval_phi(*args_x_xm) this_y = angle_diff(theta_measured, this_phi, mod_pi=True) this_hx = eval_H(*args_x_xm) this_C = eval_linH(*args_x_xm) if debug_level >= 3: print(' this_phi,this_y', this_phi * R2D, this_y * R2D) save_cols['dist%d' % camn][-1] = this_y # save # gate if abs(this_y) < gate_angle_threshold_radians: save_cols['used%d' % camn][-1] = 1 gate_vector[camn_idx] = 1 if debug_level >= 3: print ' good' if options.show: _save_plot_rows_used[-1][ camn_idx] = this_y y.append(this_y) hx.append(this_hx) C.append(this_C) N_obs_this_frame += 1 # Save which camn and camn_pt_no was used. if absolute_frame_number not in used_camn_dict: used_camn_dict[ absolute_frame_number] = [] camn_pt_no = (pt_idx_by_camn_by_frame[camn] [absolute_frame_number]) used_camn_dict[ absolute_frame_number].append( (camn, camn_pt_no)) else: if options.show: _save_plot_rows[-1][camn_idx] = this_y if debug_level >= 6: print ' bad' if debug_level >= 1: print 'gate_vector', gate_vector #print 'flip_vector',flip_vector all_data_this_frame_missing = not bool( np.sum(gate_vector)) # 3. Construct observations model using all # gated-in camera orientations. if all_data_this_frame_missing: C = None R = None hx = None else: C = np.array(C) R = R_scalar * np.eye(N_obs_this_frame) hx = np.array(hx) if 0: # crazy observation error scaling for i in range(N_obs_this_frame): beyond = abs(y[i]) - 10 * D2R beyond = max(0, beyond) # clip at zero R[i:i] = R_scalar * (1 + 10 * beyond) if debug_level >= 6: print 'full values' print 'C', C print 'hx', hx print 'y', y print 'R', R if debug_level >= 1: print 'all_data_this_frame_missing', all_data_this_frame_missing xhat, P = ekf.step2__calculate_a_posteriori( xhatminus, Pminus, y=y, hx=hx, C=C, R=R, missing_data=all_data_this_frame_missing) if debug_level >= 1: print 'xhat', xhat previous_posterior_x = xhat if center_position is not None: # save output_row_frame_cond = all_kobs_frames == absolute_frame_number output_row_cond = output_row_frame_cond & output_row_obj_id_cond output_idxs = np.nonzero(output_row_cond)[0] if len(output_idxs) == 0: pass else: assert len(output_idxs) == 1 idx = output_idxs[0] hz = state_to_hzline(xhat, center_position) for row in dest_table.iterrows(start=idx, stop=(idx + 1)): for i in range(6): row['hz_line%d' % i] = hz[i] row.update() ## xhat_results[ obj_id ][absolute_frame_number ] = ( ## xhat,center_position) if options.show: all_xhats.append(xhat) all_ori.append(state_to_ori(xhat)) # save to H5 file names = [colname for colname in save_cols] names.sort() arrays = [] for name in names: if name == 'frame': dtype = np.int64 elif name.startswith('dist'): dtype = np.float32 elif name.startswith('used'): dtype = np.bool elif name.startswith('theta'): dtype = np.float32 else: raise NameError('unknown name %s' % name) arr = np.array(save_cols[name], dtype=dtype) arrays.append(arr) save_recarray = np.rec.fromarrays(arrays, names=names) h5group = core_analysis.get_group_for_obj(obj_id, output_h5, writeable=True) output_h5.create_table(h5group, 'obj%d' % obj_id, save_recarray, filters=tables.Filters( 1, complib='lzo')) if options.show: all_xhats = np.array(all_xhats) all_ori = np.array(all_ori) _save_plot_rows = np.array(_save_plot_rows) _save_plot_rows_used = np.array(_save_plot_rows_used) ax2.plot(frame_range, all_xhats[:, 0], '.', label='p') ax2.plot(frame_range, all_xhats[:, 1], '.', label='q') ax2.plot(frame_range, all_xhats[:, 2], '.', label='r') ax2.legend() ax3.plot(frame_range, all_xhats[:, 3], '.', label='a') ax3.plot(frame_range, all_xhats[:, 4], '.', label='b') ax3.plot(frame_range, all_xhats[:, 5], '.', label='c') ax3.plot(frame_range, all_xhats[:, 6], '.', label='d') ax3.legend() ax4.plot(frame_range, all_ori[:, 0], '.', label='x') ax4.plot(frame_range, all_ori[:, 1], '.', label='y') ax4.plot(frame_range, all_ori[:, 2], '.', label='z') ax4.legend() colors = [] for i in range(n_cams): line, = ax5.plot(frame_range, _save_plot_rows_used[:, i] * R2D, 'o', label=cam_id_list[i]) colors.append(line.get_color()) for i in range(n_cams): # loop again to get normal MPL color cycling ax5.plot(frame_range, _save_plot_rows[:, i] * R2D, 'o', mec=colors[i], ms=1.0) ax5.set_ylabel('observation (deg)') ax5.legend()
def doit( h5_filename=None, output_h5_filename=None, ufmf_filenames=None, kalman_filename=None, start=None, stop=None, view=None, erode=0, save_images=False, save_image_dir=None, intermediate_thresh_frac=None, final_thresh=None, stack_N_images=None, stack_N_images_min=None, old_sync_timestamp_source=False, do_rts_smoothing=True, ): """ Copy all data in .h5 file (specified by h5_filename) to a new .h5 file in which orientations are set based on image analysis of .ufmf files. Tracking data to associate 2D points from subsequent frames is read from the .h5 kalman file specified by kalman_filename. """ if view is None: view = ["orig" for f in ufmf_filenames] else: assert len(view) == len(ufmf_filenames) if intermediate_thresh_frac is None or final_thresh is None: raise ValueError("intermediate_thresh_frac and final_thresh must be " "set") filename2view = dict(zip(ufmf_filenames, view)) ca = core_analysis.get_global_CachingAnalyzer() obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load( kalman_filename) try: ML_estimates_2d_idxs = data_file.root.ML_estimates_2d_idxs[:] except tables.exceptions.NoSuchNodeError as err1: # backwards compatibility try: ML_estimates_2d_idxs = data_file.root.kalman_observations_2d_idxs[:] except tables.exceptions.NoSuchNodeError as err2: raise err1 if os.path.exists(output_h5_filename): raise RuntimeError("will not overwrite old file '%s'" % output_h5_filename) with open_file_safe(output_h5_filename, delete_on_error=True, mode="w") as output_h5: if save_image_dir is not None: if not os.path.exists(save_image_dir): os.mkdir(save_image_dir) with open_file_safe(h5_filename, mode="r") as h5: fps = result_utils.get_fps(h5, fail_on_error=True) for input_node in h5.root._f_iter_nodes(): # copy everything from source to dest input_node._f_copy(output_h5.root, recursive=True) print("done copying") # Clear values in destination table that we may overwrite. dest_table = output_h5.root.data2d_distorted for colname in [ "x", "y", "area", "slope", "eccentricity", "cur_val", "mean_val", "sumsqf_val", ]: if colname == "cur_val": fill_value = 0 else: fill_value = np.nan clear_col(dest_table, colname, fill_value=fill_value) dest_table.flush() print("done clearing") camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) cam_id2fmfs = collections.defaultdict(list) cam_id2view = {} for ufmf_filename in ufmf_filenames: fmf = ufmf.FlyMovieEmulator( ufmf_filename, # darken=-50, allow_no_such_frame_errors=True, ) timestamps = fmf.get_all_timestamps() cam_id = get_cam_id_from_filename(fmf.filename, cam_id2camns.keys()) cam_id2fmfs[cam_id].append( (fmf, result_utils.Quick1DIndexer(timestamps))) cam_id2view[cam_id] = filename2view[fmf.filename] # associate framenumbers with timestamps using 2d .h5 file data2d = h5.root.data2d_distorted[:] # load to RAM data2d_idxs = np.arange(len(data2d)) h5_framenumbers = data2d["frame"] h5_frame_qfi = result_utils.QuickFrameIndexer(h5_framenumbers) fpc = realtime_image_analysis.FitParamsClass( ) # allocate FitParamsClass for obj_id_enum, obj_id in enumerate(use_obj_ids): print("object %d of %d" % (obj_id_enum, len(use_obj_ids))) # get all images for this camera and this obj_id obj_3d_rows = ca.load_dynamics_free_MLE_position( obj_id, data_file) this_obj_framenumbers = collections.defaultdict(list) if save_images: this_obj_raw_images = collections.defaultdict(list) this_obj_mean_images = collections.defaultdict(list) this_obj_absdiff_images = collections.defaultdict(list) this_obj_morphed_images = collections.defaultdict(list) this_obj_morph_failures = collections.defaultdict(list) this_obj_im_coords = collections.defaultdict(list) this_obj_com_coords = collections.defaultdict(list) this_obj_camn_pt_no = collections.defaultdict(list) for this_3d_row in obj_3d_rows: # iterate over each sample in the current camera framenumber = this_3d_row["frame"] if start is not None: if not framenumber >= start: continue if stop is not None: if not framenumber <= stop: continue h5_2d_row_idxs = h5_frame_qfi.get_frame_idxs(framenumber) frame2d = data2d[h5_2d_row_idxs] frame2d_idxs = data2d_idxs[h5_2d_row_idxs] obs_2d_idx = this_3d_row["obs_2d_idx"] kobs_2d_data = ML_estimates_2d_idxs[int(obs_2d_idx)] # Parse VLArray. this_camns = kobs_2d_data[0::2] this_camn_idxs = kobs_2d_data[1::2] # Now, for each camera viewing this object at this # frame, extract images. for camn, camn_pt_no in zip(this_camns, this_camn_idxs): # find 2D point corresponding to object cam_id = camn2cam_id[camn] movie_tups_for_this_camn = cam_id2fmfs[cam_id] cond = (frame2d["camn"] == camn) & ( frame2d["frame_pt_idx"] == camn_pt_no) idxs = np.nonzero(cond)[0] assert len(idxs) == 1 idx = idxs[0] orig_data2d_rownum = frame2d_idxs[idx] if not old_sync_timestamp_source: # Change the next line to 'timestamp' for old # data (before May/June 2009 -- the switch to # fview_ext_trig) frame_timestamp = frame2d[idx][ "cam_received_timestamp"] else: # previous version frame_timestamp = frame2d[idx]["timestamp"] found = None for fmf, fmf_timestamp_qi in movie_tups_for_this_camn: fmf_fnos = fmf_timestamp_qi.get_idxs( frame_timestamp) if not len(fmf_fnos): continue assert len(fmf_fnos) == 1 # should only be one .ufmf with this frame and cam_id assert found is None fmf_fno = fmf_fnos[0] found = (fmf, fmf_fno) if found is None: print( "no image data for frame timestamp %s cam_id %s" % (repr(frame_timestamp), cam_id)) continue fmf, fmf_fno = found image, fmf_timestamp = fmf.get_frame(fmf_fno) mean_image = fmf.get_mean_for_timestamp(fmf_timestamp) coding = fmf.get_format() if imops.is_coding_color(coding): image = imops.to_rgb8(coding, image) mean_image = imops.to_rgb8(coding, mean_image) else: image = imops.to_mono8(coding, image) mean_image = imops.to_mono8(coding, mean_image) xy = ( int(round(frame2d[idx]["x"])), int(round(frame2d[idx]["y"])), ) maxsize = (fmf.get_width(), fmf.get_height()) # Accumulate cropped images. Note that the region # of the full image that the cropped image # occupies changes over time as the tracked object # moves. Thus, averaging these cropped-and-shifted # images is not the same as simply averaging the # full frame. roiradius = 25 warnings.warn( "roiradius hard-coded to %d: could be set " "from 3D tracking" % roiradius) tmp = clip_and_math(image, mean_image, xy, roiradius, maxsize) im_coords, raw_im, mean_im, absdiff_im = tmp max_absdiff_im = absdiff_im.max() intermediate_thresh = intermediate_thresh_frac * max_absdiff_im absdiff_im[absdiff_im <= intermediate_thresh] = 0 if erode > 0: morphed_im = scipy.ndimage.grey_erosion(absdiff_im, size=erode) ## morphed_im = scipy.ndimage.binary_erosion(absdiff_im>1).astype(np.float32)*255.0 else: morphed_im = absdiff_im y0_roi, x0_roi = scipy.ndimage.center_of_mass( morphed_im) x0 = im_coords[0] + x0_roi y0 = im_coords[1] + y0_roi if 1: morphed_im_binary = morphed_im > 0 labels, n_labels = scipy.ndimage.label( morphed_im_binary) morph_fail_because_multiple_blobs = False if n_labels > 1: x0, y0 = np.nan, np.nan # More than one blob -- don't allow image. if 1: # for min flattening morphed_im = np.empty(morphed_im.shape, dtype=np.uint8) morphed_im.fill(255) morph_fail_because_multiple_blobs = True else: # for mean flattening morphed_im = np.zeros_like(morphed_im) morph_fail_because_multiple_blobs = True this_obj_framenumbers[camn].append(framenumber) if save_images: this_obj_raw_images[camn].append( (raw_im, im_coords)) this_obj_mean_images[camn].append(mean_im) this_obj_absdiff_images[camn].append(absdiff_im) this_obj_morphed_images[camn].append(morphed_im) this_obj_morph_failures[camn].append( morph_fail_because_multiple_blobs) this_obj_im_coords[camn].append(im_coords) this_obj_com_coords[camn].append((x0, y0)) this_obj_camn_pt_no[camn].append(orig_data2d_rownum) if 0: fname = "obj%05d_%s_frame%07d_pt%02d.png" % ( obj_id, cam_id, framenumber, camn_pt_no, ) plot_image_subregion( raw_im, mean_im, absdiff_im, roiradius, fname, im_coords, view=filename2view[fmf.filename], ) # Now, all the frames from all cameras for this obj_id # have been gathered. Do a camera-by-camera analysis. for camn in this_obj_absdiff_images: cam_id = camn2cam_id[camn] image_framenumbers = np.array(this_obj_framenumbers[camn]) if save_images: raw_images = this_obj_raw_images[camn] mean_images = this_obj_mean_images[camn] absdiff_images = this_obj_absdiff_images[camn] morphed_images = this_obj_morphed_images[camn] morph_failures = np.array(this_obj_morph_failures[camn]) im_coords = this_obj_im_coords[camn] com_coords = this_obj_com_coords[camn] camn_pt_no_array = this_obj_camn_pt_no[camn] all_framenumbers = np.arange(image_framenumbers[0], image_framenumbers[-1] + 1) com_coords = np.array(com_coords) if do_rts_smoothing: # Perform RTS smoothing on center-of-mass coordinates. # Find first good datum. fgnz = np.nonzero(~np.isnan(com_coords[:, 0])) com_coords_smooth = np.empty(com_coords.shape, dtype=np.float) com_coords_smooth.fill(np.nan) if len(fgnz[0]): first_good = fgnz[0][0] RTS_com_coords = com_coords[first_good:, :] # Setup parameters for Kalman filter. dt = 1.0 / fps A = np.array( [ [1, 0, dt, 0], # process update [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1], ], dtype=np.float, ) C = np.array( [[1, 0, 0, 0], [0, 1, 0, 0] ], # observation matrix dtype=np.float, ) Q = 0.1 * np.eye(4) # process noise R = 1.0 * np.eye(2) # observation noise initx = np.array( [ RTS_com_coords[0, 0], RTS_com_coords[0, 1], 0, 0 ], dtype=np.float, ) initV = 2 * np.eye(4) initV[0, 0] = 0.1 initV[1, 1] = 0.1 y = RTS_com_coords xsmooth, Vsmooth = adskalman.adskalman.kalman_smoother( y, A, C, Q, R, initx, initV) com_coords_smooth[first_good:] = xsmooth[:, :2] # Now shift images image_shift = com_coords_smooth - com_coords bad_cond = np.isnan(image_shift[:, 0]) # broadcast zeros to places where no good tracking image_shift[bad_cond, 0] = 0 image_shift[bad_cond, 1] = 0 shifted_morphed_images = [ shift_image(im, xy) for im, xy in zip(morphed_images, image_shift) ] results = flatten_image_stack( image_framenumbers, shifted_morphed_images, im_coords, camn_pt_no_array, N=stack_N_images, ) else: results = flatten_image_stack( image_framenumbers, morphed_images, im_coords, camn_pt_no_array, N=stack_N_images, ) # The variable fno (the first element of the results # tuple) is guaranteed to be contiguous and to span # the range from the first to last frames available. for ( fno, av_im, lowerleft, orig_data2d_rownum, orig_idx, orig_idxs_in_average, ) in results: # Clip image to reduce moment arms. av_im[av_im <= final_thresh] = 0 fail_fit = False fast_av_im = FastImage.asfastimage( av_im.astype(np.uint8)) try: (x0_roi, y0_roi, area, slope, eccentricity) = fpc.fit(fast_av_im) except realtime_image_analysis.FitParamsError as err: fail_fit = True this_morph_failures = morph_failures[ orig_idxs_in_average] n_failed_images = np.sum(this_morph_failures) n_good_images = stack_N_images - n_failed_images if n_good_images >= stack_N_images_min: n_images_is_acceptable = True else: n_images_is_acceptable = False if fail_fit: x0_roi = np.nan y0_roi = np.nan area, slope, eccentricity = np.nan, np.nan, np.nan if not n_images_is_acceptable: x0_roi = np.nan y0_roi = np.nan area, slope, eccentricity = np.nan, np.nan, np.nan x0 = x0_roi + lowerleft[0] y0 = y0_roi + lowerleft[1] if 1: for row in dest_table.iterrows( start=orig_data2d_rownum, stop=orig_data2d_rownum + 1): row["x"] = x0 row["y"] = y0 row["area"] = area row["slope"] = slope row["eccentricity"] = eccentricity row.update() # save data if save_images: # Display debugging images fname = "av_obj%05d_%s_frame%07d.png" % ( obj_id, cam_id, fno, ) if save_image_dir is not None: fname = os.path.join(save_image_dir, fname) raw_im, raw_coords = raw_images[orig_idx] mean_im = mean_images[orig_idx] absdiff_im = absdiff_images[orig_idx] morphed_im = morphed_images[orig_idx] raw_l, raw_b = raw_coords[:2] imh, imw = raw_im.shape[:2] n_ims = 5 if 1: # increase contrast contrast_scale = 2.0 av_im_show = np.clip(av_im * contrast_scale, 0, 255) margin = 10 scale = 3 # calculate the orientation line yintercept = y0 - slope * x0 xplt = np.array([ lowerleft[0] - 5, lowerleft[0] + av_im_show.shape[1] + 5, ]) yplt = slope * xplt + yintercept if 1: # only send non-nan values to plot plt_good = ~np.isnan(xplt) & ~np.isnan(yplt) xplt = xplt[plt_good] yplt = yplt[plt_good] top_row_width = scale * imw * n_ims + ( 1 + n_ims) * margin SHOW_STACK = True if SHOW_STACK: n_stack_rows = 4 rw = scale * imw * stack_N_images + ( 1 + n_ims) * margin row_width = max(top_row_width, rw) col_height = (n_stack_rows * scale * imh + (n_stack_rows + 1) * margin) stack_margin = 20 else: row_width = top_row_width col_height = scale * imh + 2 * margin stack_margin = 0 canv = benu.Canvas( fname, row_width, col_height + stack_margin, color_rgba=(1, 1, 1, 1), ) if SHOW_STACK: for (stacki, s_orig_idx ) in enumerate(orig_idxs_in_average): (s_raw_im, s_raw_coords) = raw_images[s_orig_idx] s_raw_l, s_raw_b = s_raw_coords[:2] s_imh, s_imw = s_raw_im.shape[:2] user_rect = (s_raw_l, s_raw_b, s_imw, s_imh) x_display = (stacki + 1) * margin + ( scale * imw) * stacki for show in ["raw", "absdiff", "morphed"]: if show == "raw": y_display = scale * imh + 2 * margin elif show == "absdiff": y_display = 2 * scale * imh + 3 * margin elif show == "morphed": y_display = 3 * scale * imh + 4 * margin display_rect = ( x_display, y_display + stack_margin, scale * raw_im.shape[1], scale * raw_im.shape[0], ) with canv.set_user_coords( display_rect, user_rect, transform=cam_id2view[cam_id], ): if show == "raw": s_im = s_raw_im.astype( np.uint8) elif show == "absdiff": tmp = absdiff_images[ s_orig_idx] s_im = tmp.astype(np.uint8) elif show == "morphed": tmp = morphed_images[ s_orig_idx] s_im = tmp.astype(np.uint8) canv.imshow(s_im, s_raw_l, s_raw_b) sx0, sy0 = com_coords[s_orig_idx] X = [sx0] Y = [sy0] # the raw coords in red canv.scatter(X, Y, color_rgba=(1, 0.5, 0.5, 1)) if do_rts_smoothing: sx0, sy0 = com_coords_smooth[ s_orig_idx] X = [sx0] Y = [sy0] # the RTS smoothed coords in green canv.scatter( X, Y, color_rgba=(0.5, 1, 0.5, 1)) if s_orig_idx == orig_idx: boxx = np.array([ s_raw_l, s_raw_l, s_raw_l + s_imw, s_raw_l + s_imw, s_raw_l, ]) boxy = np.array([ s_raw_b, s_raw_b + s_imh, s_raw_b + s_imh, s_raw_b, s_raw_b, ]) canv.plot( boxx, boxy, color_rgba=(0.5, 1, 0.5, 1), ) if show == "morphed": canv.text( "morphed %d" % (s_orig_idx - orig_idx, ), display_rect[0], (display_rect[1] + display_rect[3] + stack_margin - 20), font_size=font_size, color_rgba=(1, 0, 0, 1), ) # Display raw_im display_rect = ( margin, margin, scale * raw_im.shape[1], scale * raw_im.shape[0], ) user_rect = (raw_l, raw_b, imw, imh) with canv.set_user_coords( display_rect, user_rect, transform=cam_id2view[cam_id], ): canv.imshow(raw_im.astype(np.uint8), raw_l, raw_b) canv.plot( xplt, yplt, color_rgba=(0, 1, 0, 0.5)) # the orientation line canv.text( "raw", display_rect[0], display_rect[1] + display_rect[3], font_size=font_size, color_rgba=(0.5, 0.5, 0.9, 1), shadow_offset=1, ) # Display mean_im display_rect = ( 2 * margin + (scale * imw), margin, scale * mean_im.shape[1], scale * mean_im.shape[0], ) user_rect = (raw_l, raw_b, imw, imh) with canv.set_user_coords( display_rect, user_rect, transform=cam_id2view[cam_id], ): canv.imshow(mean_im.astype(np.uint8), raw_l, raw_b) canv.text( "mean", display_rect[0], display_rect[1] + display_rect[3], font_size=font_size, color_rgba=(0.5, 0.5, 0.9, 1), shadow_offset=1, ) # Display absdiff_im display_rect = ( 3 * margin + (scale * imw) * 2, margin, scale * absdiff_im.shape[1], scale * absdiff_im.shape[0], ) user_rect = (raw_l, raw_b, imw, imh) absdiff_clip = np.clip(absdiff_im * contrast_scale, 0, 255) with canv.set_user_coords( display_rect, user_rect, transform=cam_id2view[cam_id], ): canv.imshow(absdiff_clip.astype(np.uint8), raw_l, raw_b) canv.text( "absdiff", display_rect[0], display_rect[1] + display_rect[3], font_size=font_size, color_rgba=(0.5, 0.5, 0.9, 1), shadow_offset=1, ) # Display morphed_im display_rect = ( 4 * margin + (scale * imw) * 3, margin, scale * morphed_im.shape[1], scale * morphed_im.shape[0], ) user_rect = (raw_l, raw_b, imw, imh) morphed_clip = np.clip(morphed_im * contrast_scale, 0, 255) with canv.set_user_coords( display_rect, user_rect, transform=cam_id2view[cam_id], ): canv.imshow(morphed_clip.astype(np.uint8), raw_l, raw_b) if 0: canv.text( "morphed", display_rect[0], display_rect[1] + display_rect[3], font_size=font_size, color_rgba=(0.5, 0.5, 0.9, 1), shadow_offset=1, ) # Display time-averaged absdiff_im display_rect = ( 5 * margin + (scale * imw) * 4, margin, scale * av_im_show.shape[1], scale * av_im_show.shape[0], ) user_rect = ( lowerleft[0], lowerleft[1], av_im_show.shape[1], av_im_show.shape[0], ) with canv.set_user_coords( display_rect, user_rect, transform=cam_id2view[cam_id], ): canv.imshow( av_im_show.astype(np.uint8), lowerleft[0], lowerleft[1], ) canv.plot( xplt, yplt, color_rgba=(0, 1, 0, 0.5)) # the orientation line canv.text( "stacked/flattened", display_rect[0], display_rect[1] + display_rect[3], font_size=font_size, color_rgba=(0.5, 0.5, 0.9, 1), shadow_offset=1, ) canv.text( "%s frame % 7d: eccentricity % 5.1f, min N images %d, actual N images %d" % ( cam_id, fno, eccentricity, stack_N_images_min, n_good_images, ), 0, 15, font_size=font_size, color_rgba=(0.6, 0.7, 0.9, 1), shadow_offset=1, ) canv.save() # Save results to new table if 0: recarray = np.rec.array(list_of_rows_of_data2d, dtype=Info2DCol_description) dest_table.append(recarray) dest_table.flush() dest_table.attrs.has_ibo_data = True data_file.close()
def show_it( fig, filename, frame_start=None, frame_stop=None, ): results = result_utils.get_results(filename, mode='r') reconstructor = flydra_core.reconstruct.Reconstructor(results) camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(results) data2d = results.root.data2d_distorted # make sure we have 2d data table print 'reading frames...' frames = data2d.read(field='frame') print 'OK' if frame_start is not None: print 'selecting frames after start' after_start = numpy.nonzero(frames >= frame_start)[0] else: after_start = None if frame_stop is not None: print 'selecting frames before stop' before_stop = numpy.nonzero(frames <= frame_stop)[0] else: before_stop = None print 'finding all frames' if after_start is not None and before_stop is not None: use_idxs = numpy.intersect1d(after_start, before_stop) elif after_start is not None: use_idxs = after_start elif before_stop is not None: use_idxs = before_stop else: use_idxs = numpy.arange(data2d.nrows) # OK, we have data coords, plot print 'reading cameras' frames = frames[use_idxs] camns = data2d.read(field='camn') camns = camns[use_idxs] unique_camns = numpy.unique1d(camns) unique_cam_ids = list( sets.Set([camn2cam_id[camn] for camn in unique_camns])) unique_cam_ids.sort() print '%d cameras with data' % (len(unique_cam_ids), ) if len(unique_cam_ids) == 1: n_rows = 1 n_cols = 1 else: n_rows = 2 n_cols = 3 subplot_by_cam_id = {} for i, cam_id in enumerate(unique_cam_ids): ax = auto_subplot(fig, i, n_rows=n_rows, n_cols=n_cols) ax.text( 0.5, 0.95, '%s: %s' % (cam_id, str(cam_id2camns[cam_id])), horizontalalignment='center', verticalalignment='top', transform=ax.transAxes, ) ## ax.set_xticks([]) ## ax.set_yticks([]) subplot_by_cam_id[cam_id] = ax cdict = { 'red': ((0, 0, 0), (1, 1, 1)), 'green': ((0, 0, 0), (0, 0, 0)), 'blue': ((0, 0, 0), (1, 1, 1)), } # black2magenta = matplotlib.colors.LinearSegmentedColormap('black2magenta',cdict,256) for camn in unique_camns: cam_id = camn2cam_id[camn] ax = subplot_by_cam_id[cam_id] this_camn_idxs = use_idxs[camns == camn] w, h = reconstructor.get_resolution(cam_id) xbins = numpy.linspace(0, w, 40) ybins = numpy.linspace(0, h, 30) xs = data2d.read_coordinates(this_camn_idxs, field='x') ys = data2d.read_coordinates(this_camn_idxs, field='y') hist, xedges, yedges = numpy.histogram2d(xs, ys, bins=(xbins, ybins)) hist = numpy.ma.masked_where(hist == 0, hist) im = getattr(results.root.images, cam_id) ax.imshow(im, origin='lower', extent=[0, w - 1, 0, h - 1], cmap=pylab.cm.gray) pcolor_im = ax.pcolor( xbins, ybins, hist.T, alpha=0.5, ) # cmap = black2magenta ) #fig.colorbar( pcolor_im, cax ) res = reconstructor.get_resolution(cam_id) ax.set_xlim([0, res[0]]) #ax.set_ylim([0,res[1]]) ax.set_ylim([res[1], 0]) if 1: return # Old code below: # Do same as above for Kalman-filtered data if kalman_filename is None: return kresults = PT.open_file(kalman_filename, mode='r') kobs = kresults.root.ML_estimates kframes = kobs.read(field='frame') if frame_start is not None: k_after_start = numpy.nonzero(kframes >= frame_start)[0] #k_after_start = kobs.read_coordinates(idxs) #k_after_start = kobs.get_where_list( # 'frame>=frame_start') else: k_after_start = None if frame_stop is not None: k_before_stop = numpy.nonzero(kframes <= frame_stop)[0] #k_before_stop = kobs.read_coordinates(idxs) #k_before_stop = kobs.get_where_list( # 'frame<=frame_stop') else: k_before_stop = None if k_after_start is not None and k_before_stop is not None: k_use_idxs = numpy.intersect1d(k_after_start, k_before_stop) elif k_after_start is not None: k_use_idxs = k_after_start elif k_before_stop is not None: k_use_idxs = k_before_stop else: k_use_idxs = numpy.arange(kobs.nrows) obj_ids = kobs.read(field='obj_id')[k_use_idxs] #obj_ids = kobs.read_coordinates( k_use_idxs, # field='obj_id') obs_2d_idxs = kobs.read(field='obs_2d_idx')[k_use_idxs] #obs_2d_idxs = kobs.read_coordinates( k_use_idxs, # field='obs_2d_idx') kframes = kframes[k_use_idxs] #kobs.read_coordinates( k_use_idxs, # field='frame') kobs_2d = kresults.root.ML_estimates_2d_idxs xys_by_obj_id = {} for obj_id, kframe, obs_2d_idx in zip(obj_ids, kframes, obs_2d_idxs): obs_2d_idx_find = int( obs_2d_idx) # XXX grr, why can't pytables do this? obj_id_save = int(obj_id) # convert from possible numpy scalar xys_by_cam_id = xys_by_obj_id.setdefault(obj_id_save, {}) kobs_2d_data = kobs_2d.read(start=obs_2d_idx_find, stop=obs_2d_idx_find + 1) assert len(kobs_2d_data) == 1 kobs_2d_data = kobs_2d_data[0] this_camns = kobs_2d_data[0::2] this_camn_idxs = kobs_2d_data[1::2] this_use_idxs = use_idxs[frames == kframe] if debugADS: print print kframe, '===============' print 'this_use_idxs', this_use_idxs d2d = data2d.read_coordinates(this_use_idxs) if debugADS: print 'd2d ---------------' for row in d2d: print row for this_camn, this_camn_idx in zip(this_camns, this_camn_idxs): this_idxs_tmp = numpy.nonzero(d2d['camn'] == this_camn)[0] this_camn_d2d = d2d[d2d['camn'] == this_camn] found = False for this_row in this_camn_d2d: # XXX could be sped up if this_row['frame_pt_idx'] == this_camn_idx: found = True break if not found: if 0: print 'WARNING:point not found in data!?' continue else: raise RuntimeError('point not found in data!?') #this_row = this_camn_d2d[this_camn_idx] this_cam_id = camn2cam_id[this_camn] xys = xys_by_cam_id.setdefault(this_cam_id, ([], [])) xys[0].append(this_row['x']) xys[1].append(this_row['y']) for obj_id in xys_by_obj_id: xys_by_cam_id = xys_by_obj_id[obj_id] for cam_id, (xs, ys) in xys_by_cam_id.iteritems(): ax = subplot_by_cam_id[cam_id] if 0: ax.plot(xs, ys, label='obs: %d' % obj_id) else: ax.plot(xs, ys, 'x-', label='obs: %d' % obj_id) ax.text(xs[0], ys[0], '%d:' % (obj_id, )) ax.text(xs[-1], ys[-1], ':%d' % (obj_id, )) for cam_id in subplot_by_cam_id.keys(): ax = subplot_by_cam_id[cam_id] ax.legend() print 'note: could/should also plot re-projection of Kalman filtered/smoothed data'
def main(): args = docopt(__doc__) filename = args['FILENAME'] results = tables.open_file(filename, mode='r') time_model = result_utils.get_time_model_from_data(results, debug=False) worst_sync_dict = get_clock_sync.get_worst_sync_dict(results) camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(results) hostnames = worst_sync_dict.keys() hostnames.sort() cam_ids = cam_id2camns.keys() cam_ids.sort() camns = camn2cam_id.keys() # read all data end_idx = int(args['--end-idx']) d2d = results.root.data2d_distorted[:end_idx] cam_info = results.root.cam_info results.close() if 1: for cam_id_enum, cam_id in enumerate(cam_ids): camns = cam_id2camns[cam_id] if not len(camns) == 1: raise NotImplementedError camn = camns[0] cond1 = cam_info['cam_id'] == cam_id assert np.sum(cond1) == 1 hostname = str(cam_info[cond1]['hostname'][0]) cond = d2d['camn'] == camn mydata = d2d[cond] frame = mydata['frame'] if 1: # Find frames that we never received. Note that # frames we received but with no detections are saved # with a single feature point at coords (nan,nan). frame_sorted = np.sort( frame) # We can get out-of-order frames. frame_min = frame_sorted[0] frame_max = frame_sorted[-1] fdiff = frame_sorted[1:] - frame_sorted[:-1] skips = ( fdiff - 1 ) # IFI of 1 is not a skip, but normal. (We also have IFIs of 0.) skips = skips[skips > 0] n_skipped = np.sum(skips) n_total = frame_max - frame_min frac_skipped = n_skipped / float(n_total) trigger_timestamp = time_model.framestamp2timestamp(frame) # on camera computer: cam_received_timestamp = mydata['cam_received_timestamp'] latency_sec = cam_received_timestamp - trigger_timestamp median_latency_sec = np.median(latency_sec) mean_latency_sec = latency_sec.mean() max_latency_sec = np.max(latency_sec) err_est = worst_sync_dict.get(hostname, np.nan) print '%s (on %s): median: %.1f, mean: %.1f, worst: %.1f (estimate error: %.1f msec). %.2f%% skipped' % ( cam_id, hostname, median_latency_sec * 1000.0, mean_latency_sec * 1000.0, max_latency_sec * 1000.0, err_est * 1000.0, frac_skipped * 100.0, )
def find_ufmfs(filename,ufmf_dir=None,careful=True,verbose=False): ufmf_template = 'small_%(date_time)s_%(cam_id)s.ufmf$' date_time_re = '([0-9]{8}_[0-9]{6})' ufmf_template_re = ufmf_template.replace('%(date_time)s',date_time_re) if ufmf_dir is None: ufmf_dir = os.path.split( os.path.abspath( filename ))[0] all_ufmfs = glob.glob(os.path.join(ufmf_dir,'*.ufmf')) all_ufmfs.sort() h5_start, h5_stop = get_h5_start_stop(filename,careful=careful) if verbose: print 'h5_start, h5_stop',h5_start, h5_stop possible_ufmfs = [] with tables.open_file(filename,mode='r') as h5: camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) cam_ids = cam_id2camns.keys() for cam_id in cam_ids: # find likely ufmfs for each cam_id in .h5 file this_cam_re = ufmf_template_re.replace('%(cam_id)s',cam_id) prog = re.compile(this_cam_re) approx_starts = [] this_cam_id_fnames = [] for ufmf_filename in all_ufmfs: dir,node = os.path.split(ufmf_filename) match_object = prog.match(node) if match_object is None: continue date_time = match_object.group(1) struct_time = time.strptime(date_time,'%Y%m%d_%H%M%S') ufmf_approx_start = time.mktime(struct_time) approx_starts.append( ufmf_approx_start ) this_cam_id_fnames.append( ufmf_filename ) if len(this_cam_id_fnames)==0: continue approx_stops = approx_starts[1:] approx_stops.append(np.inf) ufmf_approx_starts = np.array( approx_starts ) ufmf_approx_stops = np.array( approx_stops ) eps = 1.0 # 1 second slop bad_cond = (ufmf_approx_stops-eps) < h5_start bad_cond |= (ufmf_approx_starts+eps) > h5_stop good_cond = ~bad_cond good_idx = np.nonzero(good_cond)[0] possible_ufmfs.extend( [ this_cam_id_fnames[idx] for idx in good_idx] ) results = [] for ufmf_filename in possible_ufmfs: try: ufmf = motmot.ufmf.ufmf.FlyMovieEmulator(ufmf_filename) except Exception, err: warnings.warn('auto_discover_ufmfs: error while reading %s: %s, ' 'skipping'%(ufmf_filename,err)) continue ufmf_timestamps = ufmf.get_all_timestamps() ufmf_start = ufmf_timestamps[0] ufmf_stop = ufmf_timestamps[-1] if h5_start <= ufmf_start <= h5_stop: results.append( ufmf_filename ) elif h5_start <= ufmf_stop <= h5_stop: results.append( ufmf_filename ) elif ufmf_start <= h5_start <= ufmf_stop: results.append( ufmf_filename ) ufmf.close()
def kalmanize( src_filename, do_full_kalmanization=True, dest_filename=None, reconstructor=None, reconstructor_filename=None, start_frame=None, stop_frame=None, exclude_cam_ids=None, exclude_camns=None, dynamic_model_name=None, debug=False, frames_per_second=None, area_threshold=0, min_observations_to_save=0, options=None, ): if options is None: # get default options parser = get_parser() (options, args) = parser.parse_args([]) if debug: numpy.set_printoptions(precision=3, linewidth=120, suppress=False) if exclude_cam_ids is None: exclude_cam_ids = [] if exclude_camns is None: exclude_camns = [] use_existing_filename = True if reconstructor is not None: assert isinstance(reconstructor, flydra_core.reconstruct.Reconstructor) assert reconstructor_filename is None with open_file_safe(src_filename, mode='r') as results: camn2cam_id, cam_id2camns = get_caminfo_dicts(results) if do_full_kalmanization: if dynamic_model_name is None: if hasattr(results.root, 'kalman_estimates'): if hasattr(results.root.kalman_estimates.attrs, 'dynamic_model_name'): dynamic_model_name = (results.root.kalman_estimates. attrs.dynamic_model_name) warnings.warn('dynamic model not specified. ' 'using "%s"' % dynamic_model_name) if dynamic_model_name is None: dynamic_model_name = 'EKF mamarama, units: mm' warnings.warn('dynamic model not specified. ' 'using "%s"' % dynamic_model_name) else: print 'using dynamic model "%s"' % dynamic_model_name if reconstructor_filename is not None: if reconstructor_filename.endswith('h5'): with PT.open_file(reconstructor_filename, mode='r') as fd: reconstructor = flydra_core.reconstruct.Reconstructor( fd, minimum_eccentricity=options. force_minimum_eccentricity) else: reconstructor = flydra_core.reconstruct.Reconstructor( reconstructor_filename, minimum_eccentricity=options.force_minimum_eccentricity ) else: # reconstructor_filename is None if reconstructor is None: reconstructor = flydra_core.reconstruct.Reconstructor( results, minimum_eccentricity=options.force_minimum_eccentricity ) if options.force_minimum_eccentricity is not None: if (reconstructor.minimum_eccentricity != options.force_minimum_eccentricity): raise ValueError('could not force minimum_eccentricity') if dest_filename is None: dest_filename = os.path.splitext( results.filename)[0] + '.kalmanized.h5' else: use_existing_filename = False dest_filename = tempfile.mktemp(suffix='.h5') if reconstructor is not None and reconstructor.cal_source_type == 'pytables': save_reconstructor_filename = reconstructor.cal_source.filename else: warnings.warn('unable to determine reconstructor source ' 'filename for %r' % reconstructor.cal_source_type) save_reconstructor_filename = None if frames_per_second is None: frames_per_second = get_fps(results) if do_full_kalmanization: print 'read frames_per_second from file', frames_per_second dt = 1.0 / frames_per_second if options.sync_error_threshold_msec is None: # default is IFI/2 sync_error_threshold = (0.5 * dt) else: sync_error_threshold = options.sync_error_threshold_msec / 1000.0 if os.path.exists(dest_filename): if use_existing_filename: raise ValueError('%s already exists. Will not ' 'overwrite.' % dest_filename) else: os.unlink(dest_filename) with open_file_safe(dest_filename, mode="w", title="tracked Flydra data file", delete_on_error=True) as h5file: if 'experiment_info' in results.root: results.root.experiment_info._f_copy(h5file.root, recursive=True) if do_full_kalmanization: parsed = read_textlog_header(results) if 'trigger_CS3' not in parsed: parsed['trigger_CS3'] = 'unknown' textlog_save_lines = [ 'kalmanize running at %s fps, (top %s, trigger_CS3 %s, flydra_version %s)' % (str(frames_per_second), str(parsed.get( 'top', 'unknown')), str(parsed['trigger_CS3']), flydra_core.version.__version__), 'original file: %s' % (src_filename, ), 'dynamic model: %s' % (dynamic_model_name, ), 'reconstructor file: %s' % (save_reconstructor_filename, ), ] kalman_model = dynamic_models.get_kalman_model( name=dynamic_model_name, dt=dt) h5saver = KalmanSaver( h5file, reconstructor, cam_id2camns=cam_id2camns, min_observations_to_save=min_observations_to_save, textlog_save_lines=textlog_save_lines, dynamic_model_name=dynamic_model_name, dynamic_model=kalman_model, debug=debug, fake_timestamp=options.fake_timestamp, ) tracker = Tracker( reconstructor, kalman_model=kalman_model, save_all_data=True, area_threshold=area_threshold, area_threshold_for_orientation=options. area_threshold_for_orientation, disable_image_stat_gating=options. disable_image_stat_gating, orientation_consensus=options.orientation_consensus, fake_timestamp=options.fake_timestamp, ) tracker.set_killed_tracker_callback(h5saver.save_tro) # copy timestamp data into newly created kalmanized file if hasattr(results.root, 'trigger_clock_info'): results.root.trigger_clock_info._f_copy(h5file.root) data2d = results.root.data2d_distorted frame_count = 0 last_frame = None frame_data = collections.defaultdict(list) time_frame_all_cam_timestamps = [] time_frame_all_camns = [] if 1: time1 = time.time() if do_full_kalmanization: print 'loading all frame numbers...' frames_array = numpy.asarray(data2d.read(field='frame')) time2 = time.time() if do_full_kalmanization: print 'done in %.1f sec' % (time2 - time1) if (not options.disable_image_stat_gating and 'cur_val' in data2d.colnames): warnings.warn( 'No pre-filtering of data based on zero ' 'probability -- more data association work is ' 'being done than necessary') if len(frames_array) == 0: # no data print 'No 2D data. Nothing to do.' return if do_full_kalmanization: print '2D data range: approximately %d<frame<%d' % ( frames_array[0], frames_array[-1]) if do_full_kalmanization: accum_frame_spread = None else: accum_frame_spread = [] accum_frame_spread_fno = [] accum_frame_all_timestamps = [] accum_frame_all_camns = [] max_all_check_times = -np.inf for row_start, row_stop in utils.iter_non_overlapping_chunk_start_stops( frames_array, min_chunk_size=500000, size_increment=1000, status_fd=sys.stdout): print 'Doing initial scan of approx frame range %d-%d.' % ( frames_array[row_start], frames_array[row_stop - 1]) this_frames_array = frames_array[row_start:row_stop] if start_frame is not None: if this_frames_array.max() < start_frame: continue if stop_frame is not None: if this_frames_array.min() > stop_frame: continue data2d_recarray = data2d.read(start=row_start, stop=row_stop) this_frames = data2d_recarray['frame'] print 'Examining frames %d-%d in detail.' % (this_frames[0], this_frames[-1]) this_row_idxs = np.argsort(this_frames) for ii in range(len(this_row_idxs) + 1): if ii >= len(this_row_idxs): finish_frame = True else: finish_frame = False this_row_idx = this_row_idxs[ii] row = data2d_recarray[this_row_idx] new_frame = row['frame'] if start_frame is not None: if new_frame < start_frame: continue if stop_frame is not None: if new_frame > stop_frame: continue if last_frame != new_frame: if new_frame < last_frame: print 'new_frame', new_frame print 'last_frame', last_frame raise RuntimeError( "expected continuously increasing " "frame numbers") finish_frame = True if finish_frame: # new frame ######################################## # Data for this frame is complete if last_frame is not None: this_frame_spread = 0.0 if len(time_frame_all_cam_timestamps) > 1: check_times = np.array( time_frame_all_cam_timestamps) check_times -= check_times.min() this_frame_spread = check_times.max() if accum_frame_spread is not None: accum_frame_spread.append( this_frame_spread) accum_frame_spread_fno.append(last_frame) accum_frame_all_timestamps.append( time_frame_all_cam_timestamps) accum_frame_all_camns.append( time_frame_all_camns) max_all_check_times = max( this_frame_spread, max_all_check_times) if this_frame_spread > sync_error_threshold: if this_frame_spread == max_all_check_times: print '%s frame %d: sync diff: %.1f msec' % ( os.path.split( results.filename)[-1], last_frame, this_frame_spread * 1000.0) if debug > 5: print print 'frame_data for frame %d' % ( last_frame, ) pprint.pprint(dict(frame_data)) print if do_full_kalmanization: if this_frame_spread > sync_error_threshold: if debug > 5: print( 'frame sync error (spread %.1f msec), ' 'skipping' % (this_frame_spread * 1e3, )) print warnings.warn( 'Synchronization error detected, ' 'but continuing analysis without ' 'potentially bad data.') else: process_frame(reconstructor, tracker, last_frame, frame_data, camn2cam_id, debug=debug) frame_count += 1 if do_full_kalmanization and frame_count % 1000 == 0: time2 = time.time() dur = time2 - time1 fps = frame_count / dur print 'frame % 10d, kalmanization/data association speed: % 8.1f fps' % ( last_frame, fps) time1 = time2 frame_count = 0 ######################################## frame_data = collections.defaultdict(list) time_frame_all_cam_timestamps = [] # clear values time_frame_all_camns = [] # clear values last_frame = new_frame camn = row['camn'] try: cam_id = camn2cam_id[camn] except KeyError: # This will happen if cameras were re-synchronized (and # thus gain new cam_ids) immediately before saving was # turned on in MainBrain. The reason is that the network # buffers are still full of old data coming in from the # cameras. warnings.warn('WARNING: no cam_id for camn ' '%d, skipping this row of data' % camn) continue if cam_id in exclude_cam_ids: # exclude this camera continue if camn in exclude_camns: # exclude this camera continue time_frame_all_cam_timestamps.append(row['timestamp']) time_frame_all_camns.append(row['camn']) if do_full_kalmanization: x_distorted = row['x'] if numpy.isnan(x_distorted): # drop point -- not found continue y_distorted = row['y'] (x_undistorted, y_undistorted) = reconstructor.undistort( cam_id, (x_distorted, y_distorted)) (area, slope, eccentricity, frame_pt_idx) = (row['area'], row['slope'], row['eccentricity'], row['frame_pt_idx']) if 'cur_val' in row.dtype.fields: cur_val = row['cur_val'] else: cur_val = None if 'mean_val' in row.dtype.fields: mean_val = row['mean_val'] else: mean_val = None if 'sumsqf_val' in row.dtype.fields: sumsqf_val = row['sumsqf_val'] else: sumsqf_val = None # FIXME: cache this stuff? pmat_inv = reconstructor.get_pmat_inv(cam_id) camera_center = reconstructor.get_camera_center(cam_id) camera_center = numpy.hstack((camera_center[:, 0], [1])) camera_center_meters = reconstructor.get_camera_center( cam_id) camera_center_meters = numpy.hstack( (camera_center_meters[:, 0], [1])) helper = reconstructor.get_reconstruct_helper_dict( )[cam_id] rise = slope run = 1.0 if np.isinf(rise): if rise > 0: rise = 1.0 run = 0.0 else: rise = -1.0 run = 0.0 (p1, p2, p3, p4, ray0, ray1, ray2, ray3, ray4, ray5) = do_3d_operations_on_2d_point( helper, x_undistorted, y_undistorted, pmat_inv, camera_center, x_distorted, y_distorted, rise, run) line_found = not numpy.isnan(p1) pluecker_hz_meters = (ray0, ray1, ray2, ray3, ray4, ray5) # Keep in sync with kalmanize.py and data_descriptions.py pt_undistorted = (x_undistorted, y_undistorted, area, slope, eccentricity, p1, p2, p3, p4, line_found, frame_pt_idx, cur_val, mean_val, sumsqf_val) projected_line_meters = geom.line_from_HZline( pluecker_hz_meters) frame_data[camn].append( (pt_undistorted, projected_line_meters)) if do_full_kalmanization: tracker.kill_all_trackers() # done tracking if not do_full_kalmanization: os.unlink(dest_filename) if accum_frame_spread is not None: # save spread data to file for analysis accum_frame_spread = np.array(accum_frame_spread) accum_frame_spread_fno = np.array(accum_frame_spread_fno) if options.dest_file is not None: accum_frame_spread_filename = options.dest_file else: accum_frame_spread_filename = src_filename + '.spreadh5' cam_ids = cam_id2camns.keys() cam_ids.sort() camn_order = [] for cam_id in cam_ids: camn_order.extend(cam_id2camns[cam_id]) camn_order = np.array(camn_order) cam_id_array = np.array(cam_ids) N_cams = len(camn_order) N_frames = len(accum_frame_spread_fno) all_timestamps = np.empty((N_frames, N_cams), dtype=np.float) all_timestamps.fill(np.nan) for i, (timestamps, camns) in enumerate( zip(accum_frame_all_timestamps, accum_frame_all_camns)): for j, camn in enumerate(camn_order): try: idx = camns.index(camn) except ValueError: continue # not found, skip timestamp = timestamps[idx] all_timestamps[i, j] = timestamp h5 = tables.open_file(accum_frame_spread_filename, mode='w') h5.create_array(h5.root, 'spread', accum_frame_spread, 'frame timestamp spreads (sec)') h5.create_array(h5.root, 'framenumber', accum_frame_spread_fno, 'frame number') h5.create_array(h5.root, 'all_timestamps', all_timestamps, 'all timestamps') h5.create_array(h5.root, 'camn_order', camn_order, 'camn_order') h5.create_array(h5.root, 'cam_id_array', cam_id_array, 'cam_id_array') h5.close() print 'saved %s' % accum_frame_spread_filename if max_all_check_times > sync_error_threshold: if not options.keep_sync_errors: if do_full_kalmanization: print 'max_all_check_times %.2f msec' % (max_all_check_times * 1000.0) handle, target = tempfile.mkstemp( os.path.split(dest_filename)[1]) os.unlink(target) # remove original file there shutil.move(dest_filename, target) raise ValueError( 'Synchonization errors exist in the data. Moved result file' ' to ensure it is not confused with valid data. The new ' 'location is: %s' % (target, )) else: sys.exit(1) # sync error else: if not do_full_kalmanization: print '%s no sync differences greater than %.1f msec' % ( os.path.split(src_filename)[-1], sync_error_threshold * 1000.0, )
def plot_latency(fname, do_3d_latency=False, do_2d_latency=False, end_idx=100000, save=False): if do_3d_latency == False and do_2d_latency == False: print("hmm, not plotting 3d or 2d data. nothing to do") return with tables.open_file(fname, mode="r") as h5: if do_2d_latency: d2d = h5.root.data2d_distorted[:end_idx] if do_3d_latency: dk = h5.root.kalman_estimates[:end_idx] camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5) time_model = result_utils.get_time_model_from_data(h5) if do_2d_latency: df2d = pd.DataFrame(d2d) camn_list = list(df2d["camn"].unique()) camn_list.sort() figs = {} if do_3d_latency: dfk = pd.DataFrame(dk) fig = plt.figure() figs["3d"] = fig ax = fig.add_subplot(111) for obj_id, dfobj in dfk.groupby("obj_id"): frame = dfobj["frame"].values reconstruct_timestamp = dfobj["timestamp"].values trigger_timestamp = time_model.framestamp2timestamp(frame) latency = reconstruct_timestamp - trigger_timestamp latency[latency < -1e8] = np.nan ax.plot(frame, latency, "b.-") ax.text(0, 1, "3D reconstruction", va="top", ha="left", transform=ax.transAxes) ax.set_xlabel("frame") ax.set_ylabel("time (s)") if do_2d_latency: fig2 = plt.figure() figs["2"] = fig2 axn = None fig3 = plt.figure() figs["3"] = fig3 ax3n = None fig4 = plt.figure() figs["4"] = fig4 ax4n = None for camn, dfcam in df2d.groupby("camn"): cam_id = camn2cam_id[camn] df0 = dfcam[dfcam["frame_pt_idx"] == 0] ts0s = df0["timestamp"].values tss = df0["cam_received_timestamp"].values frames = df0["frame"].values dts = tss - ts0s dframes = frames[1:] - frames[:-1] axn = fig2.add_subplot(len(camn_list), 1, camn_list.index(camn) + 1, sharex=axn) axn.plot(frames, dts, "r.-", label="camnode latency") axn.plot( frames[:-1], (ts0s[1:] - ts0s[:-1]) / dframes, "g.-", label="mean inter-frame interval", ) axn.set_xlabel("frame") axn.set_ylabel("time (s)") axn.text(0, 1, cam_id, va="top", ha="left", transform=axn.transAxes) if camn_list.index(camn) == 0: axn.legend() ax3n = fig3.add_subplot(len(camn_list), 1, camn_list.index(camn) + 1, sharex=ax3n) ax3n.plot(frames, ts0s, "g.-", label="calculated triggerbox timestamp") ax3n.set_xlabel("frame") ax3n.set_ylabel("time (s)") ax3n.text(0, 1, cam_id, va="top", ha="left", transform=ax3n.transAxes) if camn_list.index(camn) == 0: ax3n.legend() ax4n = fig4.add_subplot(len(camn_list), 1, camn_list.index(camn) + 1, sharex=ax4n) ax4n.plot(frames[:-1], ts0s[1:] - ts0s[:-1], "g.-") ax4n.set_xlabel("frame") ax4n.set_ylabel("inter-frame-interval (s)") ax4n.text(0, 1, cam_id, va="top", ha="left", transform=ax4n.transAxes) if save: for key in figs: fig = figs[key] fig.savefig("%s-latency-%s.png" % (fname, key)) else: plt.show()