def test_pymvg(): R1 = reconstruct.Reconstructor(sample_cal) R2 = R1.convert_to_pymvg() pts_3d = [ (0.0, 0.0, 0.0), (0.1, 0.0, 0.0), (0.0, 0.1, 0.0), (0.0, 0.0, 0.1), (0.1, 0.1, 0.0), (0.1, 0.0, 0.1), (0.0, 0.1, 0.1), ] pts_3d = np.array(pts_3d) # make homogeneous coords pts_3d_h = np.ones( (len(pts_3d),4) ) pts_3d_h[:, :3] = pts_3d # check 3D->2D projection for cam_id in R1.get_cam_ids(): fpix = R1.find2d( cam_id, pts_3d_h ) cpix = R2.find2d( cam_id, pts_3d ) assert np.allclose( fpix, cpix) # check 2D->3D triangulation for X in pts_3d: inputs = [] for cam_id in R1.get_cam_ids(): inputs.append( (cam_id, R1.find2d(cam_id,X)) ) tri1 = R1.find3d( inputs, return_line_coords=False ) tri2 = R2.find3d( inputs ) assert np.allclose(tri1, tri2)
def doit(calsource, options=None): r = reconstruct.Reconstructor(calsource) if options.scaled: r = r.get_scaled() root = ET.Element("root") r.add_element(root) child = root[0] result = reconstruct.pretty_dump(child, ind=' ') if options.dest: with open(options.dest, 'wb') as the_file: the_file.write(result) print 'saved calibration to %s' % options.dest else: print result
def test_find_line(): R = reconstruct.Reconstructor(sample_cal) # define a line by pair of 3D points pts_3d = np.array([ (0.0, 0.0, 0.0), (0.1, 0.0, 0.0), ]) ptA, ptB = pts_3d pluecker_expected = reconstruct.pluecker_from_verts(ptA, ptB) # build 3D->2D projection of line inputs = [] for cam_id in R.get_cam_ids(): hlper = R.get_reconstruct_helper_dict()[cam_id] pmat_inv = R.get_pmat_inv(cam_id) cc = R.get_camera_center(cam_id)[:, 0] cc = np.array([cc[0], cc[1], cc[2], 1.0]) a = R.find2d(cam_id, ptA) b = R.find2d(cam_id, ptB) x0_abs, y0_abs = a x0u, y0u = hlper.undistort(x0_abs, y0_abs) x, y = a x1, y1 = b rise = y1 - y run = x1 - x slope = rise / run area = 1.0 eccentricity = 4.0 if 1: # ugly. :( (p1, p2, p3, p4, ray0, ray1, ray2, ray3, ray4, ray5) = reconstruct.do_3d_operations_on_2d_point( hlper, x0u, y0u, pmat_inv, cc, x0_abs, y0_abs, rise, run) value_tuple = x, y, area, slope, eccentricity, p1, p2, p3, p4 inputs.append((cam_id, value_tuple)) X, Lcoords = R.find3d(inputs, return_X_coords=True, return_line_coords=True) assert np.allclose(X, ptA) norm = reconstruct.norm_vec assert np.allclose(norm(pluecker_expected), norm(Lcoords))
def check_homography(distorted=True): """check that realignment of calibration doesn't shift 2D projections """ srcR = reconstruct.Reconstructor(sample_cal) cam_ids = srcR.cam_ids s = 0.180548337471 R = [[-0.90583342, -0.41785822, 0.06971599], [0.42066911, -0.90666599, 0.03153224], [0.05003311, 0.05789032, 0.9970684]] t = [0.0393599, -0.01713819, 0.71691032] M = align.build_xform(s, R, t) alignedR = srcR.get_aligned_copy(M) srcX = [[1, 2, 3], [4, 5, 6], [7, 8, 9], [10, 11, 12]] for pt in srcX: all_src_pts = [] for cam_id in cam_ids: uv = srcR.find2d(cam_id, pt, distorted=distorted) all_src_pts.append((cam_id, uv)) pth = np.ones((4, 1)) pth[:3, 0] = pt expected_pth = np.dot(M, pth) expected_pt = expected_pth[:3, 0] / expected_pth[3] aligned_pt = alignedR.find3d(all_src_pts, undistort=not distorted, return_line_coords=False) assert np.allclose(expected_pt, aligned_pt) all_reproj_pts = [] for cam_id in cam_ids: uv = alignedR.find2d(cam_id, aligned_pt, distorted=distorted) all_reproj_pts.append((cam_id, uv)) for i in range(len(all_src_pts)): orig2d = all_src_pts[i][1] new2d = all_reproj_pts[i][1] assert np.allclose(orig2d, new2d)
def test_xml(self): caldir = os.path.join(os.path.split(__file__)[0], "sample_calibration") x = reconstruct.Reconstructor(caldir) root = ET.Element("xml") x.add_element(root) tree = ET.ElementTree(root) fd = StringIO.StringIO() tree.write(fd) if 0: tree.write("test.xml") fd.seek(0) root = ET.parse(fd).getroot() assert root.tag == 'xml' assert len(root) == 1 y = reconstruct.Reconstructor_from_xml(root[0]) assert x == y
def main(): parser = argparse.ArgumentParser() parser.add_argument('filename', nargs='+', help='name of flydra .hdf5 file', ) parser.add_argument("--stim-xml", type=str, default=None, help="name of XML file with stimulus info", required=True, ) parser.add_argument("--align-json", type=str, default=None, help="previously exported json file containing s,R,T", ) parser.add_argument("--radius", type=float, help="radius of line (in meters)", default=0.002, metavar="RADIUS") parser.add_argument("--obj-only", type=str) parser.add_argument("--obj-filelist", type=str, help="use object ids from list in text file", ) parser.add_argument( "-r", "--reconstructor", dest="reconstructor_path", type=str, help=("calibration/reconstructor path (if not specified, " "defaults to FILE)")) args = parser.parse_args() options = args # optparse OptionParser backwards compatibility reconstructor_path = args.reconstructor_path fps = None ca = core_analysis.get_global_CachingAnalyzer() by_file = {} for h5_filename in args.filename: assert(tables.is_hdf5_file(h5_filename)) obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load( h5_filename) this_fps = result_utils.get_fps( data_file, fail_on_error=False ) if fps is None: if this_fps is not None: fps = this_fps if reconstructor_path is None: reconstructor_path = data_file by_file[h5_filename] = (use_obj_ids, data_file) del h5_filename del obj_ids, use_obj_ids, is_mat_file, data_file, extra if options.obj_only is not None: obj_only = core_analysis.parse_seq(options.obj_only) else: obj_only = None if reconstructor_path is None: raise RuntimeError('must specify reconstructor from CLI if not using .h5 files') R = reconstruct.Reconstructor(reconstructor_path) if fps is None: fps = 100.0 warnings.warn('Setting fps to default value of %f'%fps) else: fps = 1.0 if options.stim_xml is None: raise ValueError( 'stim_xml must be specified (how else will you align the data?') if 1: stim_xml = xml_stimulus.xml_stimulus_from_filename( options.stim_xml, ) try: fanout = xml_stimulus.xml_fanout_from_filename( options.stim_xml ) except xml_stimulus.WrongXMLTypeError: pass else: include_obj_ids, exclude_obj_ids = fanout.get_obj_ids_for_timestamp( timestamp_string=file_timestamp ) if include_obj_ids is not None: use_obj_ids = include_obj_ids if exclude_obj_ids is not None: use_obj_ids = list( set(use_obj_ids).difference( exclude_obj_ids ) ) print('using object ids specified in fanout .xml file') if stim_xml.has_reconstructor(): stim_xml.verify_reconstructor(R) x = [] y = [] z = [] speed = [] if options.obj_filelist is not None: obj_filelist=options.obj_filelist else: obj_filelist=None if obj_filelist is not None: obj_only = 1 if obj_only is not None: if len(by_file) != 1: raise RuntimeError("specifying obj_only can only be done for a single file") if obj_filelist is not None: data = np.loadtxt(obj_filelist,delimiter=',') obj_only = np.array(data[:,0], dtype='int') print(obj_only) use_obj_ids = numpy.array(obj_only) h5_filename = by_file.keys()[0] (prev_use_ob_ids, data_file) = by_file[h5_filename] by_file[h5_filename] = (use_obj_ids, data_file) for h5_filename in by_file: (use_obj_ids, data_file) = by_file[h5_filename] for obj_id_enum,obj_id in enumerate(use_obj_ids): rows = ca.load_data( obj_id, data_file, use_kalman_smoothing=False, #dynamic_model_name = dynamic_model_name, #frames_per_second=fps, #up_dir=up_dir, ) verts = numpy.array( [rows['x'], rows['y'], rows['z']] ).T if len(verts)>=3: verts_central_diff = verts[2:,:] - verts[:-2,:] dt = 1.0/fps vels = verts_central_diff/(2*dt) speeds = numpy.sqrt(numpy.sum(vels**2,axis=1)) # pad end points speeds = numpy.array([speeds[0]] + list(speeds) + [speeds[-1]]) else: speeds = numpy.zeros( (verts.shape[0],) ) if verts.shape[0] != len(speeds): raise ValueError('mismatch length of x data and speeds') x.append( verts[:,0] ) y.append( verts[:,1] ) z.append( verts[:,2] ) speed.append(speeds) data_file.close() del h5_filename, use_obj_ids, data_file if 0: # debug if stim_xml is not None: v = None for child in stim_xml.root: if child.tag == 'cubic_arena': info = stim_xml._get_info_for_cubic_arena(child) v=info['verts4x4'] if v is not None: for vi in v: print('adding',vi) x.append( [vi[0]] ) y.append( [vi[1]] ) z.append( [vi[2]] ) speed.append( [100.0] ) x = np.concatenate(x) y = np.concatenate(y) z = np.concatenate(z) w = np.ones_like(x) speed = np.concatenate(speed) # homogeneous coords verts = np.array([x,y,z,w]) ####################################################### # Create the MayaVi engine and start it. e = Engine() # start does nothing much but useful if someone is listening to # your engine. e.start() # Create a new scene. from tvtk.tools import ivtk #viewer = ivtk.IVTK(size=(600,600)) viewer = IVTKWithCalGUI(size=(800,600)) viewer.open() e.new_scene(viewer) viewer.cal_align.set_data(verts,speed,R,args.align_json) if 0: # Do this if you need to see the MayaVi tree view UI. ev = EngineView(engine=e) ui = ev.edit_traits() # view aligned data e.add_source(viewer.cal_align.source) v = Vectors() v.glyph.scale_mode = 'data_scaling_off' v.glyph.color_mode = 'color_by_scalar' v.glyph.glyph_source.glyph_position='center' v.glyph.glyph_source.glyph_source = tvtk.SphereSource( radius=options.radius, ) e.add_module(v) if stim_xml is not None: if 0: stim_xml.draw_in_mayavi_scene(e) else: actors = stim_xml.get_tvtk_actors() viewer.scene.add_actors(actors) gui = GUI() gui.start_event_loop()
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 generate_calibration( n_cameras=5, return_full_info=False, radial_distortion=False, ): pi = numpy.pi sccs = [] # 1. extrinsic parameters: if 1: # method 1: # arrange cameras in circle around common point common_point = numpy.array((0, 0, 0), dtype=numpy.float64) r = 10.0 theta = numpy.linspace(0, 2 * pi, n_cameras, endpoint=False) x = numpy.cos(theta) y = numpy.sin(theta) z = numpy.zeros(y.shape) cc = numpy.c_[x, y, z] #cam_up = numpy.array((0,0,1)) #cam_ups = numpy.resize(cam_up,cc.shape) #cam_forwads = -cc cam_centers = r * cc + common_point # Convert up/forward into rotation matrix. if 1: Rs = [] for i, th in enumerate(theta): pos = cam_centers[i] target = common_point up = (0, 0, 1) if 0: print 'pos', pos print 'target', target print 'up', up R = cgtypes.mat4().lookAt(pos, target, up) #print 'R4',R R = R.getMat3() #print 'R3',R R = numpy.asarray(R).T #print 'R',R #print Rs.append(R) else: # (Camera coords: looking forward -z, up +y, right +x) R = cgtypes.mat3().identity() if 1: # (looking forward -z, up +x, right -y) R = R.rotation(-pi / 2, (0, 0, 1)) # (looking forward +x, up +z, right -y) R = R.rotation(-pi / 2, (0, 1, 0)) # rotate to point -theta (with up +z) Rs = [R.rotation(float(th) + pi, (0, 0, 1)) for th in theta] #Rs = [ R for th in theta ] else: Rs = [R.rotation(pi / 2.0, (1, 0, 0)) for th in theta] #Rs = [ R for th in theta ] Rs = [numpy.asarray(R).T for R in Rs] print 'Rs', Rs # 2. intrinsic parameters resolutions = {} for cam_no in range(n_cameras): cam_id = 'fake_%d' % (cam_no + 1) # resolution of image res = (1600, 1200) resolutions[cam_id] = res # principal point cc1 = res[0] / 2.0 cc2 = res[1] / 2.0 # focal length fc1 = 1.0 fc2 = 1.0 alpha_c = 0.0 # R = numpy.asarray(Rs[cam_no]).T # conversion between cgkit and numpy R = Rs[cam_no] C = cam_centers[cam_no][:, numpy.newaxis] K = numpy.array(((fc1, alpha_c * fc1, cc1), (0, fc2, cc2), (0, 0, 1))) t = numpy.dot(-R, C) Rt = numpy.concatenate((R, t), axis=1) P = numpy.dot(K, Rt) if 0: print 'cam_id', cam_id print 'P' print P print 'K' print K print 'Rt' print Rt print KR = numpy.dot(K, R) print 'KR', KR K3, R3 = reconstruct.my_rq(KR) print 'K3' print K3 print 'R3' print R3 K3R3 = numpy.dot(K3, R3) print 'K3R3', K3R3 print '*' * 60 if radial_distortion: f = 1000.0 r1 = 0.8 r2 = -0.2 helper = reconstruct_utils.ReconstructHelper( f, f, # focal length cc1, cc2, # image center r1, r2, # radial distortion 0, 0) # tangential distortion scc = reconstruct.SingleCameraCalibration_from_basic_pmat( P, cam_id=cam_id, res=res, ) sccs.append(scc) if 1: # XXX test K2, R2 = scc.get_KR() if 0: print 'C', C print 't', t print 'K', K print 'K2', K2 print 'R', R print 'R2', R2 print 'P', P print 'KR|t', numpy.dot(K, Rt) t2 = scc.get_t() print 't2', t2 Rt2 = numpy.concatenate((R2, t2), axis=1) print 'KR2|t', numpy.dot(K2, Rt2) print KR2 = numpy.dot(K2, R2) KR = numpy.dot(K, R) if not numpy.allclose(KR2, KR): if not numpy.allclose(KR2, -KR): raise ValueError('expected KR2 and KR to be identical') else: print 'WARNING: weird sign error in calibration math FIXME!' recon = reconstruct.Reconstructor(sccs) full_info = { 'reconstructor': recon, 'center': common_point, # where all the cameras are looking 'camera_dist_from_center': r, 'resolutions': resolutions, } if return_full_info: return full_info return recon
good_cond = (real_frames >= options.start) obj_verts = obj_verts[good_cond] speeds = speeds[good_cond] real_frames = real_frames[good_cond] if not len(use_obj_ids) == 1: obj_verts = numpy.concatenate(obj_verts, axis=0) speeds = numpy.concatenate(speeds, axis=0) real_frames = numpy.concatenate(real_frames, axis=0) ####################### start draw permanently on stuff ############################ if options.stim_xml is not None: if not is_mat_file: R = reconstruct.Reconstructor(data_file) stim_xml.verify_reconstructor(R) if not is_mat_file: assert (data_file.filename.startswith('DATA') and (data_file.filename.endswith('.h5') or data_file.filename.endswith('.kh5'))) file_timestamp = data_file.filename[4:19] actors = stim_xml.get_tvtk_actors() for actor in actors: ren.add_actor(actor) if 1: if 0: # Inspired by pyface.tvtk.decorated_scene marker = tvtk.OrientationMarkerWidget()
def plot_top_and_side_views( subplot=None, options=None, obs_mew=None, scale=1.0, units="m", ): """ inputs ------ subplot - a dictionary of matplotlib axes instances with keys 'xy' and/or 'xz' fps - the framerate of the data """ assert subplot is not None assert options is not None if not hasattr(options, "show_track_ends"): options.show_track_ends = False if not hasattr(options, "unicolor"): options.unicolor = False if not hasattr(options, "show_landing"): options.show_landing = False kalman_filename = options.kalman_filename fps = options.fps dynamic_model = options.dynamic_model use_kalman_smoothing = options.use_kalman_smoothing if not hasattr(options, "ellipsoids"): options.ellipsoids = False if not hasattr(options, "show_observations"): options.show_observations = False if not hasattr(options, "markersize"): options.markersize = 0.5 if options.ellipsoids and use_kalman_smoothing: warnings.warn( "plotting ellipsoids while using Kalman smoothing does not reveal original error estimates" ) assert kalman_filename is not None start = options.start stop = options.stop obj_only = options.obj_only if not use_kalman_smoothing: if dynamic_model is not None: print( "ERROR: disabling Kalman smoothing (--disable-kalman-smoothing) is incompatable with setting dynamic model options (--dynamic-model)", file=sys.stderr, ) sys.exit(1) ca = core_analysis.get_global_CachingAnalyzer() if kalman_filename is not None: obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load( kalman_filename ) if not is_mat_file: mat_data = None if fps is None: fps = result_utils.get_fps(data_file, fail_on_error=False) if fps is None: fps = 100.0 warnings.warn("Setting fps to default value of %f" % fps) reconstructor = reconstruct.Reconstructor(data_file) else: reconstructor = None if options.stim_xml: file_timestamp = data_file.filename[4:19] stim_xml = xml_stimulus.xml_stimulus_from_filename( options.stim_xml, timestamp_string=file_timestamp, ) try: fanout = xml_stimulus.xml_fanout_from_filename(options.stim_xml) except xml_stimulus.WrongXMLTypeError: walking_start_stops = [] else: include_obj_ids, exclude_obj_ids = fanout.get_obj_ids_for_timestamp( timestamp_string=file_timestamp ) walking_start_stops = fanout.get_walking_start_stops_for_timestamp( timestamp_string=file_timestamp ) if include_obj_ids is not None: use_obj_ids = include_obj_ids if exclude_obj_ids is not None: use_obj_ids = list(set(use_obj_ids).difference(exclude_obj_ids)) stim_xml = fanout.get_stimulus_for_timestamp( timestamp_string=file_timestamp ) if stim_xml.has_reconstructor(): stim_xml.verify_reconstructor(reconstructor) else: walking_start_stops = [] if dynamic_model is None: dynamic_model = extra.get("dynamic_model_name", None) if dynamic_model is None: if use_kalman_smoothing: warnings.warn( "no kalman smoothing will be performed because no " "dynamic model specified or found." ) use_kalman_smoothing = False else: print('detected file loaded with dynamic model "%s"' % dynamic_model) if use_kalman_smoothing: if dynamic_model.startswith("EKF "): dynamic_model = dynamic_model[4:] print(' for smoothing, will use dynamic model "%s"' % dynamic_model) subplots = subplot.keys() subplots.sort() # ensure consistency across runs dt = 1.0 / fps if obj_only is not None: use_obj_ids = [i for i in use_obj_ids if i in obj_only] subplot["xy"].set_aspect("equal") subplot["xz"].set_aspect("equal") subplot["xy"].set_xlabel("x (%s)" % units) subplot["xy"].set_ylabel("y (%s)" % units) subplot["xz"].set_xlabel("x (%s)" % units) subplot["xz"].set_ylabel("z (%s)" % units) if options.stim_xml: stim_xml.plot_stim( subplot["xy"], projection=xml_stimulus.SimpleOrthographicXYProjection() ) stim_xml.plot_stim( subplot["xz"], projection=xml_stimulus.SimpleOrthographicXZProjection() ) allX = {} frame0 = None results = collections.defaultdict(list) for obj_id in use_obj_ids: line = None ellipse_lines = [] MLE_line = None try: kalman_rows = ca.load_data( obj_id, data_file, use_kalman_smoothing=use_kalman_smoothing, dynamic_model_name=dynamic_model, frames_per_second=fps, up_dir=options.up_dir, ) except core_analysis.ObjectIDDataError: continue if options.show_observations: kobs_rows = ca.load_dynamics_free_MLE_position(obj_id, data_file) frame = kalman_rows["frame"] if options.show_observations: frame_obs = kobs_rows["frame"] if (start is not None) or (stop is not None): valid_cond = numpy.ones(frame.shape, dtype=numpy.bool) if options.show_observations: valid_obs_cond = np.ones(frame_obs.shape, dtype=numpy.bool) if start is not None: valid_cond = valid_cond & (frame >= start) if options.show_observations: valid_obs_cond = valid_obs_cond & (frame_obs >= start) if stop is not None: valid_cond = valid_cond & (frame <= stop) if options.show_observations: valid_obs_cond = valid_obs_cond & (frame_obs <= stop) kalman_rows = kalman_rows[valid_cond] if options.show_observations: kobs_rows = kobs_rows[valid_obs_cond] if not len(kalman_rows): continue frame = kalman_rows["frame"] Xx = kalman_rows["x"] Xy = kalman_rows["y"] Xz = kalman_rows["z"] if options.max_z is not None: cond = Xz <= options.max_z frame = numpy.ma.masked_where(~cond, frame) Xx = numpy.ma.masked_where(~cond, Xx) Xy = numpy.ma.masked_where(~cond, Xy) Xz = numpy.ma.masked_where(~cond, Xz) with keep_axes_dimensions_if(subplot["xz"], options.stim_xml): subplot["xz"].axhline(options.max_z) kws = {"markersize": options.markersize} if options.unicolor: kws["color"] = "k" landing_idxs = [] for walkstart, walkstop in walking_start_stops: if walkstart in frame: tmp_idx = numpy.nonzero(frame == walkstart)[0][0] landing_idxs.append(tmp_idx) with keep_axes_dimensions_if(subplot["xy"], options.stim_xml): (line,) = subplot["xy"].plot( Xx * scale, Xy * scale, ".", label="obj %d" % obj_id, **kws ) kws["color"] = line.get_color() if options.ellipsoids: for i in range(len(Xx)): rowi = kalman_rows[i] mu = [rowi["x"], rowi["y"], rowi["z"]] va = get_covariance(rowi) ellx, elly = densities.gauss_ell(mu, va, [0, 1], 30, 0.39) (ellipse_line,) = subplot["xy"].plot( ellx * scale, elly * scale, color=kws["color"] ) ellipse_lines.append(ellipse_line) if options.show_track_ends: subplot["xy"].plot( [Xx[0] * scale, Xx[-1] * scale], [Xy[0] * scale, Xy[-1] * scale], "cd", ms=6, label="track end", ) if options.show_obj_id: subplot["xy"].text(Xx[0] * scale, Xy[0] * scale, str(obj_id)) if options.show_landing: for landing_idx in landing_idxs: subplot["xy"].plot( [Xx[landing_idx] * scale], [Xy[landing_idx] * scale], "rD", ms=10, label="landing", ) if options.show_observations: mykw = {} mykw.update(kws) mykw["markersize"] *= 5 mykw["mew"] = obs_mew badcond = np.isnan(kobs_rows["x"]) Xox = np.ma.masked_where(badcond, kobs_rows["x"]) Xoy = np.ma.masked_where(badcond, kobs_rows["y"]) (MLE_line,) = subplot["xy"].plot( Xox * scale, Xoy * scale, "x", label="obj %d" % obj_id, **mykw ) with keep_axes_dimensions_if(subplot["xz"], options.stim_xml): (line,) = subplot["xz"].plot( Xx * scale, Xz * scale, ".", label="obj %d" % obj_id, **kws ) kws["color"] = line.get_color() if options.ellipsoids: for i in range(len(Xx)): rowi = kalman_rows[i] mu = [rowi["x"], rowi["y"], rowi["z"]] va = get_covariance(rowi) ellx, ellz = densities.gauss_ell(mu, va, [0, 2], 30, 0.39) (ellipse_line,) = subplot["xz"].plot( ellx * scale, ellz * scale, color=kws["color"] ) ellipse_lines.append(ellipse_line) if options.show_track_ends: subplot["xz"].plot( [Xx[0] * scale, Xx[-1] * scale], [Xz[0] * scale, Xz[-1] * scale], "cd", ms=6, label="track end", ) if options.show_obj_id: subplot["xz"].text(Xx[0] * scale, Xz[0] * scale, str(obj_id)) if options.show_landing: for landing_idx in landing_idxs: subplot["xz"].plot( [Xx[landing_idx] * scale], [Xz[landing_idx] * scale], "rD", ms=10, label="landing", ) if options.show_observations: mykw = {} mykw.update(kws) mykw["markersize"] *= 5 mykw["mew"] = obs_mew badcond = np.isnan(kobs_rows["x"]) Xox = np.ma.masked_where(badcond, kobs_rows["x"]) Xoz = np.ma.masked_where(badcond, kobs_rows["z"]) (MLE_line,) = subplot["xz"].plot( Xox * scale, Xoz * scale, "x", label="obj %d" % obj_id, **mykw ) results["lines"].append(line) results["ellipse_lines"].extend(ellipse_lines) results["MLE_line"].append(MLE_line) return results
def test_pickle(self): caldir = os.path.join(os.path.split(__file__)[0], "sample_calibration") x = reconstruct.Reconstructor(caldir) xpickle = pickle.dumps(x) x2 = pickle.loads(xpickle) assert x2 == x
def test_from_sample_directory(self): caldir = os.path.join(os.path.split(__file__)[0], "sample_calibration") reconstruct.Reconstructor(caldir)
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])
def doit( filename, obj_only=None, min_length=10, use_kalman_smoothing=True, data_fps=100.0, save_fps=25, vertical_scale=False, max_vel="auto", draw_stim_func_str=None, floor=True, animation_path_fname=None, output_dir=".", cam_only_move_duration=5.0, options=None, ): if not use_kalman_smoothing: if dynamic_model_name is not None: print( "ERROR: disabling Kalman smoothing (--disable-kalman-smoothing) is incompatable with setting dynamic model option (--dynamic-model)", file=sys.stderr, ) sys.exit(1) dynamic_model_name = options.dynamic_model if animation_path_fname is None: animation_path_fname = pkg_resources.resource_filename( __name__, "kdmovie_saver_default_path.kmp" ) camera_animation_path = AnimationPath(animation_path_fname) mat_data = None try: try: data_path, data_filename = os.path.split(filename) data_path = os.path.expanduser(data_path) sys.path.insert(0, data_path) mat_data = scipy.io.mio.loadmat(data_filename) finally: del sys.path[0] except IOError as err: print( "not a .mat file at %s, treating as .hdf5 file" % (os.path.join(data_path, data_filename)) ) ca = core_analysis.get_global_CachingAnalyzer() obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load(filename) if obj_only is not None: use_obj_ids = obj_only if options.stim_xml is not None: file_timestamp = data_file.filename[4:19] stim_xml = xml_stimulus.xml_stimulus_from_filename( options.stim_xml, timestamp_string=file_timestamp, ) try: fanout = xml_stimulus.xml_fanout_from_filename(options.stim_xml) except xml_stimulus.WrongXMLTypeError: pass else: include_obj_ids, exclude_obj_ids = fanout.get_obj_ids_for_timestamp( timestamp_string=file_timestamp ) if include_obj_ids is not None: use_obj_ids = include_obj_ids if exclude_obj_ids is not None: use_obj_ids = list(set(use_obj_ids).difference(exclude_obj_ids)) print("using object ids specified in fanout .xml file") if dynamic_model_name is None: if "dynamic_model_name" in extra: dynamic_model_name = extra["dynamic_model_name"] 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) else: print( "no dynamic model name specified, and it could not be determined from the file, either" ) filename_trimmed = os.path.split(os.path.splitext(filename)[0])[-1] assert use_obj_ids is not None ################# rw = tvtk.RenderWindow(size=(1024, 768)) ren = tvtk.Renderer(background=(1.0, 1.0, 1.0)) camera = ren.active_camera rw.add_renderer(ren) lut = tvtk.LookupTable(hue_range=(0.667, 0.0)) ################# if not os.path.exists(output_dir): os.mkdir(output_dir) if len(use_obj_ids) == 1: animate_path = True # allow path to grow during trajectory else: animate_path = False obj_verts = [] speeds = [] for obj_id in use_obj_ids: print("loading %d" % obj_id) results = ca.calculate_trajectory_metrics( obj_id, data_file, use_kalman_smoothing=use_kalman_smoothing, frames_per_second=data_fps, dynamic_model_name=dynamic_model_name, # method='position based', method_params={"downsample": 1,}, ) if len(use_obj_ids) == 1: obj_verts = results["X_kalmanized"] speeds = results["speed_kalmanized"] real_frames = results["frame"] else: obj_verts.append(results["X_kalmanized"]) speeds.append(results["speed_kalmanized"]) real_frames.append(results["frame"]) if options.start is not None: good_cond = real_frames >= options.start obj_verts = obj_verts[good_cond] speeds = speeds[good_cond] real_frames = real_frames[good_cond] if not len(use_obj_ids) == 1: obj_verts = numpy.concatenate(obj_verts, axis=0) speeds = numpy.concatenate(speeds, axis=0) real_frames = numpy.concatenate(real_frames, axis=0) ####################### start draw permanently on stuff ############################ if options.stim_xml is not None: if not is_mat_file: R = reconstruct.Reconstructor(data_file) stim_xml.verify_reconstructor(R) if not is_mat_file: assert data_file.filename.startswith("DATA") and ( data_file.filename.endswith(".h5") or data_file.filename.endswith(".kh5") ) file_timestamp = data_file.filename[4:19] actors = stim_xml.get_tvtk_actors() for actor in actors: ren.add_actor(actor) if 1: if 0: # Inspired by pyface.tvtk.decorated_scene marker = tvtk.OrientationMarkerWidget() axes = tvtk.AxesActor() axes.set( # normalized_tip_length=(0.04, 0.4, 0.4), # normalized_shaft_length=(0.6, 0.6, 0.6), shaft_type="cylinder", total_length=(0.15, 0.15, 0.15), ) if 1: axes.x_axis_label_text = "" axes.y_axis_label_text = "" axes.z_axis_label_text = "" else: p = axes.x_axis_caption_actor2d.caption_text_property axes.y_axis_caption_actor2d.caption_text_property = p axes.z_axis_caption_actor2d.caption_text_property = p p.color = 0.0, 0.0, 0.0 # black # axes.camera = camera # axes.attachment_point_coordinate = (0,0,0) axes.origin = (-0.5, 1, 0) if 0: rwi = rw.interactor print("rwi", rwi) marker.orientation_marker = axes # marker.interactive = False marker.interactor = rwi marker.enabled = True ren.add_actor(axes) ####################### if max_vel == "auto": max_vel = speeds.max() else: max_vel = float(max_vel) vel_mapper = tvtk.PolyDataMapper() vel_mapper.lookup_table = lut vel_mapper.scalar_range = 0.0, max_vel if 1: # Create a scalar bar if vertical_scale: scalar_bar = tvtk.ScalarBarActor( orientation="vertical", width=0.08, height=0.4 ) else: scalar_bar = tvtk.ScalarBarActor( orientation="horizontal", width=0.4, height=0.08 ) scalar_bar.title = "Speed (m/s)" scalar_bar.lookup_table = vel_mapper.lookup_table scalar_bar.property.color = 0.0, 0.0, 0.0 # black scalar_bar.title_text_property.color = 0.0, 0.0, 0.0 scalar_bar.title_text_property.shadow = False scalar_bar.label_text_property.color = 0.0, 0.0, 0.0 scalar_bar.label_text_property.shadow = False scalar_bar.position_coordinate.coordinate_system = "normalized_viewport" if vertical_scale: scalar_bar.position_coordinate.value = 0.01, 0.01, 0.0 else: scalar_bar.position_coordinate.value = 0.1, 0.01, 0.0 ren.add_actor(scalar_bar) imf = tvtk.WindowToImageFilter(input=rw, read_front_buffer="off") writer = tvtk.PNGWriter() ####################### end draw permanently on stuff ############################ save_dt = 1.0 / save_fps if animate_path: data_dt = 1.0 / data_fps n_frames = len(obj_verts) dur = n_frames * data_dt else: data_dt = 0.0 dur = 0.0 print("data_fps", data_fps) print("data_dt", data_dt) print("save_fps", save_fps) t_now = 0.0 frame_number = 0 while t_now <= dur: frame_number += 1 t_now += save_dt print("t_now", t_now) pos, ori = camera_animation_path.get_pos_ori(t_now) focal_point, view_up = pos_ori2fu(pos, ori) camera.position = tuple(pos) # camera.focal_point = (focal_point[0], focal_point[1], focal_point[2]) # camera.view_up = (view_up[0], view_up[1], view_up[2]) camera.focal_point = tuple(focal_point) camera.view_up = tuple(view_up) if data_dt != 0.0: draw_n_frames = int(round(t_now / data_dt)) else: draw_n_frames = len(obj_verts) print("frame_number, draw_n_frames", frame_number, draw_n_frames) ################# pd = tvtk.PolyData() pd.points = obj_verts[:draw_n_frames] real_frame_number = real_frames[:draw_n_frames][-1] pd.point_data.scalars = speeds if numpy.any(speeds > max_vel): print( "WARNING: maximum speed (%.3f m/s) exceeds color map max" % (speeds.max(),) ) g = tvtk.Glyph3D( scale_mode="data_scaling_off", vector_mode="use_vector", input=pd ) vel_mapper.input = g.output ss = tvtk.SphereSource(radius=options.radius) g.source = ss.output a = tvtk.Actor(mapper=vel_mapper) ################## ren.add_actor(a) if 1: imf.update() imf.modified() writer.input = imf.output # fname = 'movie_%s_%03d_frame%05d.png'%(filename_trimmed,obj_id,frame_number) fname = "movie_%s_%03d_frame%05d.png" % ( filename_trimmed, obj_id, real_frame_number, ) print("saving", fname) full_fname = os.path.join(output_dir, fname) writer.file_name = full_fname writer.write() ren.remove_actor(a) ren.add_actor(a) # restore actors removed dur = dur + cam_only_move_duration while t_now < dur: frame_number += 1 t_now += save_dt print("t_now", t_now) pos, ori = camera_animation_path.get_pos_ori(t_now) focal_point, view_up = pos_ori2fu(pos, ori) camera.position = tuple(pos) camera.focal_point = tuple(focal_point) camera.view_up = tuple(view_up) if 1: imf.update() imf.modified() writer.input = imf.output if len(use_obj_ids) == 1: fname = "movie_%s_%03d_frame%05d.png" % ( filename_trimmed, obj_id, frame_number, ) else: fname = "movie_%s_many_frame%05d.png" % (filename_trimmed, frame_number) full_fname = os.path.join(output_dir, fname) writer.file_name = full_fname writer.write() if not is_mat_file: data_file.close()
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()