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 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 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 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 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(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 plot_timeseries(subplot=None, options=None):
kalman_filename = options.kalman_filename
if not hasattr(options, 'frames'):
options.frames = False
if not hasattr(options, 'show_landing'):
options.show_landing = False
if not hasattr(options, 'unicolor'):
options.unicolor = False
if not hasattr(options, 'show_obj_id'):
options.show_obj_id = True
if not hasattr(options, 'show_track_ends'):
options.show_track_ends = False
start = options.start
stop = options.stop
obj_only = options.obj_only
fps = options.fps
dynamic_model = options.dynamic_model
use_kalman_smoothing = options.use_kalman_smoothing
if not use_kalman_smoothing:
if (dynamic_model is not None):
print >> sys.stderr, (
'WARNING: disabling Kalman smoothing '
'(--disable-kalman-smoothing) is incompatable '
'with setting dynamic model options (--dynamic-model)')
ca = core_analysis.get_global_CachingAnalyzer()
if kalman_filename is None:
raise ValueError('No kalman_filename given. Nothing to do.')
m = hashlib.md5()
m.update(open(kalman_filename, mode='rb').read())
actual_md5 = m.hexdigest()
(obj_ids, use_obj_ids, is_mat_file, data_file,
extra) = ca.initial_file_load(kalman_filename)
print 'opened kalman file %s %s, %d obj_ids' % (
kalman_filename, actual_md5, len(use_obj_ids))
if 'frames' in extra:
if (start is not None) or (stop is not None):
valid_frames = np.ones((len(extra['frames']), ), dtype=np.bool)
if start is not None:
valid_frames &= extra['frames'] >= start
if stop is not None:
valid_frames &= extra['frames'] <= stop
this_use_obj_ids = np.unique(obj_ids[valid_frames])
use_obj_ids = list(set(use_obj_ids).intersection(this_use_obj_ids))
include_obj_ids = None
exclude_obj_ids = None
do_fuse = False
if options.stim_xml:
file_timestamp = data_file.filename[4:19]
fanout = xml_stimulus.xml_fanout_from_filename(options.stim_xml)
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))
if options.fuse:
do_fuse = True
else:
walking_start_stops = []
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
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
import warnings
warnings.warn('Setting fps to default value of %f' % fps)
tz = result_utils.get_tz(data_file)
dt = 1.0 / fps
all_vels = []
if obj_only is not None:
use_obj_ids = [i for i in use_obj_ids if i in obj_only]
allX = {}
frame0 = None
line2obj_id = {}
Xz_all = []
fuse_did_once = False
if not hasattr(options, 'timestamp_file'):
options.timestamp_file = None
if not hasattr(options, 'ori_qual'):
options.ori_qual = None
if options.timestamp_file is not None:
h5 = tables.open_file(options.timestamp_file, mode='r')
print 'reading timestamps and frames'
table_data2d_frames = h5.root.data2d_distorted.read(field='frame')
table_data2d_timestamps = h5.root.data2d_distorted.read(
field='timestamp')
print 'done'
h5.close()
table_data2d_frames_find = utils.FastFinder(table_data2d_frames)
if len(use_obj_ids) == 0:
print 'No obj_ids to plot, quitting'
sys.exit(0)
time0 = 0.0 # set default value
for obj_id in use_obj_ids:
if not do_fuse:
try:
kalman_rows = ca.load_data(
obj_id,
data_file,
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,
)
except core_analysis.ObjectIDDataError:
continue
#kobs_rows = ca.load_dynamics_free_MLE_position( obj_id, data_file )
else:
if options.show_3d_orientations:
raise NotImplementedError('orientation data is not supported '
'when fusing obj_ids')
if fuse_did_once:
break
fuse_did_once = True
kalman_rows = flydra_analysis.a2.flypos.fuse_obj_ids(
use_obj_ids,
data_file,
dynamic_model_name=dynamic_model,
frames_per_second=fps)
frame = kalman_rows['frame']
if (start is not None) or (stop is not None):
valid_cond = numpy.ones(frame.shape, dtype=numpy.bool)
if start is not None:
valid_cond = valid_cond & (frame >= start)
if stop is not None:
valid_cond = valid_cond & (frame <= stop)
kalman_rows = kalman_rows[valid_cond]
if not len(kalman_rows):
continue
walking_and_flying_kalman_rows = kalman_rows # preserve original data
for flystate in ['flying', 'walking']:
frame = walking_and_flying_kalman_rows['frame'] # restore
if flystate == 'flying':
# assume flying unless we're told it's walking
state_cond = numpy.ones(frame.shape, dtype=numpy.bool)
else:
state_cond = numpy.zeros(frame.shape, dtype=numpy.bool)
if len(walking_start_stops):
for walkstart, walkstop in walking_start_stops:
frame = walking_and_flying_kalman_rows['frame'] # restore
# handle each bout of walking
walking_bout = numpy.ones(frame.shape, dtype=numpy.bool)
if walkstart is not None:
walking_bout &= (frame >= walkstart)
if walkstop is not None:
walking_bout &= (frame <= walkstop)
if flystate == 'flying':
state_cond &= ~walking_bout
else:
state_cond |= walking_bout
kalman_rows = np.take(walking_and_flying_kalman_rows,
np.nonzero(state_cond)[0])
assert len(kalman_rows) == np.sum(state_cond)
frame = kalman_rows['frame']
if frame0 is None:
frame0 = int(frame[0])
time0 = 0.0
if options.timestamp_file is not None:
frame_idxs = table_data2d_frames_find.get_idxs_of_equal(frame0)
if len(frame_idxs):
time0 = table_data2d_timestamps[frame_idxs[0]]
else:
raise ValueError(
'could not fine frame %d in timestamp file' % frame0)
Xx = kalman_rows['x']
Xy = kalman_rows['y']
Xz = kalman_rows['z']
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 not options.frames:
f2t = Frames2Time(frame0, fps, time0)
else:
def identity(x):
return x
f2t = identity
kws = {
'linewidth': 2,
'picker': 5,
}
if options.unicolor:
kws['color'] = 'k'
line = None
if 'frame' in subplot:
subplot['frame'].plot(f2t(frame), frame)
if 'P55' in subplot:
subplot['P55'].plot(f2t(frame), kalman_rows['P55'])
if 'x' in subplot:
line, = subplot['x'].plot(f2t(frame),
Xx,
label='obj %d (%s)' %
(obj_id, flystate),
**kws)
line2obj_id[line] = obj_id
kws['color'] = line.get_color()
if 'y' in subplot:
line, = subplot['y'].plot(f2t(frame),
Xy,
label='obj %d (%s)' %
(obj_id, flystate),
**kws)
line2obj_id[line] = obj_id
kws['color'] = line.get_color()
if 'z' in subplot:
frame_data = numpy.ma.getdata(
frame) # works if frame is masked or not
# plot landing time
if options.show_landing:
if flystate == 'flying': # only do this once
for walkstart, walkstop in walking_start_stops:
if walkstart in frame_data:
landing_dix = numpy.nonzero(
frame_data == walkstart)[0][0]
subplot['z'].plot([f2t(walkstart)],
[Xz.data[landing_dix]],
'rD',
ms=10,
label='landing')
if options.show_track_ends:
if flystate == 'flying': # only do this once
subplot['z'].plot(f2t([frame_data[0], frame_data[-1]]),
[
numpy.ma.getdata(Xz)[0],
numpy.ma.getdata(Xz)[-1]
],
'cd',
ms=6,
label='track end')
line, = subplot['z'].plot(f2t(frame),
Xz,
label='obj %d (%s)' %
(obj_id, flystate),
**kws)
kws['color'] = line.get_color()
line2obj_id[line] = obj_id
if flystate == 'flying':
# only do this once
if options.show_obj_id:
subplot['z'].text(f2t(frame_data[0]),
numpy.ma.getdata(Xz)[0],
'%d' % (obj_id, ))
line2obj_id[line] = obj_id
if flystate == 'flying':
Xz_all.append(np.ma.array(Xz).compressed())
#bins = np.linspace(0,.8,30)
#print 'Xz.shape',Xz.shape
#pylab.hist(Xz, bins=bins)
for (dir_var, Dd) in [('dx', Dx), ('dy', Dy), ('dz', Dz)]:
if dir_var in subplot:
line, = subplot[dir_var].plot(f2t(frame),
Dd,
label='obj %d (%s)' %
(obj_id, flystate),
**kws)
line2obj_id[line] = obj_id
kws['color'] = line.get_color()
if numpy.__version__ >= '1.2.0':
X = numpy.ma.array((Xx, Xy, Xz))
else:
# See http://scipy.org/scipy/numpy/ticket/820
X = numpy.ma.vstack(
(Xx[numpy.newaxis, :], Xy[numpy.newaxis, :],
Xz[numpy.newaxis, :]))
dist_central_diff = (X[:, 2:] - X[:, :-2])
vel_central_diff = dist_central_diff / (2 * dt)
vel2mag = numpy.ma.sqrt(numpy.ma.sum(vel_central_diff**2, axis=0))
xy_vel2mag = numpy.ma.sqrt(
numpy.ma.sum(vel_central_diff[:2, :]**2, axis=0))
frames2 = frame[1:-1]
accel4mag = (vel2mag[2:] - vel2mag[:-2]) / (2 * dt)
frames4 = frames2[1:-1]
if 'vel' in subplot:
line, = subplot['vel'].plot(f2t(frames2),
vel2mag,
label='obj %d (%s)' %
(obj_id, flystate),
**kws)
line2obj_id[line] = obj_id
kws['color'] = line.get_color()
if 'xy_vel' in subplot:
line, = subplot['xy_vel'].plot(f2t(frames2),
xy_vel2mag,
label='obj %d (%s)' %
(obj_id, flystate),
**kws)
line2obj_id[line] = obj_id
kws['color'] = line.get_color()
if len(accel4mag.compressed()) and 'accel' in subplot:
line, = subplot['accel'].plot(f2t(frames4),
accel4mag,
label='obj %d (%s)' %
(obj_id, flystate),
**kws)
line2obj_id[line] = obj_id
kws['color'] = line.get_color()
if flystate == 'flying':
valid_vel2mag = vel2mag.compressed()
all_vels.append(valid_vel2mag)
if len(all_vels):
all_vels = numpy.hstack(all_vels)
else:
all_vels = numpy.array([], dtype=float)
if 1:
cond = all_vels < 2.0
if numpy.ma.sum(cond) != len(all_vels):
all_vels = all_vels[cond]
import warnings
warnings.warn('clipping all velocities > 2.0 m/s')
if not options.frames:
xlabel = 'time (s)'
else:
xlabel = 'frame'
for ax in subplot.itervalues():
ax.xaxis.set_major_formatter(ticker.FormatStrFormatter("%d"))
ax.yaxis.set_major_formatter(ticker.FormatStrFormatter("%s"))
fixup_ax = FixupAxesWithTimeZone(tz).fixup_ax
if 'frame' in subplot:
if time0 != 0.0:
fixup_ax(subplot['frame'])
else:
subplot['frame'].set_xlabel(xlabel)
if 'x' in subplot:
subplot['x'].set_ylim([-1, 1])
subplot['x'].set_ylabel(r'x (m)')
if time0 != 0.0:
fixup_ax(subplot['x'])
else:
subplot['x'].set_xlabel(xlabel)
if 'y' in subplot:
subplot['y'].set_ylim([-0.5, 1.5])
subplot['y'].set_ylabel(r'y (m)')
if time0 != 0.0:
fixup_ax(subplot['y'])
else:
subplot['y'].set_xlabel(xlabel)
max_z = None
if options.stim_xml:
file_timestamp = options.kalman_filename[4:19]
stim_xml = xml_stimulus.xml_stimulus_from_filename(
options.stim_xml, timestamp_string=file_timestamp)
post_max_zs = []
for post_num, post in enumerate(stim_xml.iterate_posts()):
post_max_zs.append(max(post['verts'][0][2],
post['verts'][1][2])) # max post height
if len(post_max_zs):
max_z = min(post_max_zs) # take shortest of posts
if 'z' in subplot:
subplot['z'].set_ylim([0, 1])
subplot['z'].set_ylabel(r'z (m)')
if max_z is not None:
subplot['z'].axhline(max_z, color='m')
if time0 != 0.0:
fixup_ax(subplot['z'])
else:
subplot['z'].set_xlabel(xlabel)
for dir_var in ['dx', 'dy', 'dz']:
if dir_var in subplot:
subplot[dir_var].set_ylabel(dir_var)
if time0 != 0.0:
fixup_ax(subplot[dir_var])
else:
subplot[dir_var].set_xlabel(xlabel)
if 'z_hist' in subplot: # and flystate=='flying':
Xz_all = np.hstack(Xz_all)
bins = np.linspace(0, .8, 30)
ax = subplot['z_hist']
ax.hist(Xz_all, bins=bins, orientation='horizontal')
ax.set_xticks([])
ax.set_yticks([])
xlim = tuple(ax.get_xlim()) # matplotlib 0.98.3 returned np.array view
ax.set_xlim((xlim[1], xlim[0]))
ax.axhline(max_z, color='m')
if 'vel' in subplot:
subplot['vel'].set_ylim([0, 2])
subplot['vel'].set_ylabel(r'vel (m/s)')
subplot['vel'].set_xlabel(xlabel)
if time0 != 0.0:
fixup_ax(subplot['vel'])
else:
subplot['vel'].set_xlabel(xlabel)
if 'xy_vel' in subplot:
#subplot['xy_vel'].set_ylim([0,2])
subplot['xy_vel'].set_ylabel(r'horiz vel (m/s)')
subplot['xy_vel'].set_xlabel(xlabel)
if time0 != 0.0:
fixup_ax(subplot['xy_vel'])
else:
subplot['xy_vel'].set_xlabel(xlabel)
if 'accel' in subplot:
subplot['accel'].set_ylabel(r'acceleration (m/(s^2))')
subplot['accel'].set_xlabel(xlabel)
if time0 != 0.0:
fixup_ax(subplot['accel'])
else:
subplot['accel'].set_xlabel(xlabel)
if 'vel_hist' in subplot:
ax = subplot['vel_hist']
bins = numpy.linspace(0, 2, 50)
ax.set_title('excluding walking')
pdf, bins, patches = ax.hist(all_vels, bins=bins, normed=True)
ax.set_xlim(0, 2)
ax.set_ylabel('probability density')
ax.set_xlabel('velocity (m/s)')
return line2obj_id
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()
ML_estimates_2d_idxs = data_file.root.kalman_observations_2d_idxs[:]
except tables.exceptions.NoSuchNodeError, 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)