def test_pymvg():
R1 = reconstruct.Reconstructor(sample_cal)
R2 = R1.convert_to_pymvg()
pts_3d = [ (0.0, 0.0, 0.0),
(0.1, 0.0, 0.0),
(0.0, 0.1, 0.0),
(0.0, 0.0, 0.1),
(0.1, 0.1, 0.0),
(0.1, 0.0, 0.1),
(0.0, 0.1, 0.1),
]
pts_3d = np.array(pts_3d)
# make homogeneous coords
pts_3d_h = np.ones( (len(pts_3d),4) )
pts_3d_h[:, :3] = pts_3d
# check 3D->2D projection
for cam_id in R1.get_cam_ids():
fpix = R1.find2d( cam_id, pts_3d_h )
cpix = R2.find2d( cam_id, pts_3d )
assert np.allclose( fpix, cpix)
# check 2D->3D triangulation
for X in pts_3d:
inputs = []
for cam_id in R1.get_cam_ids():
inputs.append( (cam_id, R1.find2d(cam_id,X)) )
tri1 = R1.find3d( inputs, return_line_coords=False )
tri2 = R2.find3d( inputs )
assert np.allclose(tri1, tri2)
def doit(calsource, options=None):
r = reconstruct.Reconstructor(calsource)
if options.scaled:
r = r.get_scaled()
root = ET.Element("root")
r.add_element(root)
child = root[0]
result = reconstruct.pretty_dump(child, ind=' ')
if options.dest:
with open(options.dest, 'wb') as the_file:
the_file.write(result)
print 'saved calibration to %s' % options.dest
else:
print result
def test_find_line():
R = reconstruct.Reconstructor(sample_cal)
# define a line by pair of 3D points
pts_3d = np.array([
(0.0, 0.0, 0.0),
(0.1, 0.0, 0.0),
])
ptA, ptB = pts_3d
pluecker_expected = reconstruct.pluecker_from_verts(ptA, ptB)
# build 3D->2D projection of line
inputs = []
for cam_id in R.get_cam_ids():
hlper = R.get_reconstruct_helper_dict()[cam_id]
pmat_inv = R.get_pmat_inv(cam_id)
cc = R.get_camera_center(cam_id)[:, 0]
cc = np.array([cc[0], cc[1], cc[2], 1.0])
a = R.find2d(cam_id, ptA)
b = R.find2d(cam_id, ptB)
x0_abs, y0_abs = a
x0u, y0u = hlper.undistort(x0_abs, y0_abs)
x, y = a
x1, y1 = b
rise = y1 - y
run = x1 - x
slope = rise / run
area = 1.0
eccentricity = 4.0
if 1:
# ugly. :(
(p1, p2, p3, p4, ray0, ray1, ray2, ray3, ray4,
ray5) = reconstruct.do_3d_operations_on_2d_point(
hlper, x0u, y0u, pmat_inv, cc, x0_abs, y0_abs, rise, run)
value_tuple = x, y, area, slope, eccentricity, p1, p2, p3, p4
inputs.append((cam_id, value_tuple))
X, Lcoords = R.find3d(inputs,
return_X_coords=True,
return_line_coords=True)
assert np.allclose(X, ptA)
norm = reconstruct.norm_vec
assert np.allclose(norm(pluecker_expected), norm(Lcoords))
def check_homography(distorted=True):
"""check that realignment of calibration doesn't shift 2D projections
"""
srcR = reconstruct.Reconstructor(sample_cal)
cam_ids = srcR.cam_ids
s = 0.180548337471
R = [[-0.90583342, -0.41785822, 0.06971599],
[0.42066911, -0.90666599, 0.03153224],
[0.05003311, 0.05789032, 0.9970684]]
t = [0.0393599, -0.01713819, 0.71691032]
M = align.build_xform(s, R, t)
alignedR = srcR.get_aligned_copy(M)
srcX = [[1, 2, 3], [4, 5, 6], [7, 8, 9], [10, 11, 12]]
for pt in srcX:
all_src_pts = []
for cam_id in cam_ids:
uv = srcR.find2d(cam_id, pt, distorted=distorted)
all_src_pts.append((cam_id, uv))
pth = np.ones((4, 1))
pth[:3, 0] = pt
expected_pth = np.dot(M, pth)
expected_pt = expected_pth[:3, 0] / expected_pth[3]
aligned_pt = alignedR.find3d(all_src_pts,
undistort=not distorted,
return_line_coords=False)
assert np.allclose(expected_pt, aligned_pt)
all_reproj_pts = []
for cam_id in cam_ids:
uv = alignedR.find2d(cam_id, aligned_pt, distorted=distorted)
all_reproj_pts.append((cam_id, uv))
for i in range(len(all_src_pts)):
orig2d = all_src_pts[i][1]
new2d = all_reproj_pts[i][1]
assert np.allclose(orig2d, new2d)
def test_xml(self):
caldir = os.path.join(os.path.split(__file__)[0], "sample_calibration")
x = reconstruct.Reconstructor(caldir)
root = ET.Element("xml")
x.add_element(root)
tree = ET.ElementTree(root)
fd = StringIO.StringIO()
tree.write(fd)
if 0:
tree.write("test.xml")
fd.seek(0)
root = ET.parse(fd).getroot()
assert root.tag == 'xml'
assert len(root) == 1
y = reconstruct.Reconstructor_from_xml(root[0])
assert x == y
def main():
parser = argparse.ArgumentParser()
parser.add_argument('filename', nargs='+',
help='name of flydra .hdf5 file',
)
parser.add_argument("--stim-xml",
type=str,
default=None,
help="name of XML file with stimulus info",
required=True,
)
parser.add_argument("--align-json",
type=str,
default=None,
help="previously exported json file containing s,R,T",
)
parser.add_argument("--radius", type=float,
help="radius of line (in meters)",
default=0.002,
metavar="RADIUS")
parser.add_argument("--obj-only", type=str)
parser.add_argument("--obj-filelist", type=str,
help="use object ids from list in text file",
)
parser.add_argument(
"-r", "--reconstructor", dest="reconstructor_path",
type=str,
help=("calibration/reconstructor path (if not specified, "
"defaults to FILE)"))
args = parser.parse_args()
options = args # optparse OptionParser backwards compatibility
reconstructor_path = args.reconstructor_path
fps = None
ca = core_analysis.get_global_CachingAnalyzer()
by_file = {}
for h5_filename in args.filename:
assert(tables.is_hdf5_file(h5_filename))
obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load(
h5_filename)
this_fps = result_utils.get_fps( data_file, fail_on_error=False )
if fps is None:
if this_fps is not None:
fps = this_fps
if reconstructor_path is None:
reconstructor_path = data_file
by_file[h5_filename] = (use_obj_ids, data_file)
del h5_filename
del obj_ids, use_obj_ids, is_mat_file, data_file, extra
if options.obj_only is not None:
obj_only = core_analysis.parse_seq(options.obj_only)
else:
obj_only = None
if reconstructor_path is None:
raise RuntimeError('must specify reconstructor from CLI if not using .h5 files')
R = reconstruct.Reconstructor(reconstructor_path)
if fps is None:
fps = 100.0
warnings.warn('Setting fps to default value of %f'%fps)
else:
fps = 1.0
if options.stim_xml is None:
raise ValueError(
'stim_xml must be specified (how else will you align the data?')
if 1:
stim_xml = xml_stimulus.xml_stimulus_from_filename(
options.stim_xml,
)
try:
fanout = xml_stimulus.xml_fanout_from_filename( options.stim_xml )
except xml_stimulus.WrongXMLTypeError:
pass
else:
include_obj_ids, exclude_obj_ids = fanout.get_obj_ids_for_timestamp(
timestamp_string=file_timestamp )
if include_obj_ids is not None:
use_obj_ids = include_obj_ids
if exclude_obj_ids is not None:
use_obj_ids = list( set(use_obj_ids).difference(
exclude_obj_ids ) )
print('using object ids specified in fanout .xml file')
if stim_xml.has_reconstructor():
stim_xml.verify_reconstructor(R)
x = []
y = []
z = []
speed = []
if options.obj_filelist is not None:
obj_filelist=options.obj_filelist
else:
obj_filelist=None
if obj_filelist is not None:
obj_only = 1
if obj_only is not None:
if len(by_file) != 1:
raise RuntimeError("specifying obj_only can only be done for a single file")
if obj_filelist is not None:
data = np.loadtxt(obj_filelist,delimiter=',')
obj_only = np.array(data[:,0], dtype='int')
print(obj_only)
use_obj_ids = numpy.array(obj_only)
h5_filename = by_file.keys()[0]
(prev_use_ob_ids, data_file) = by_file[h5_filename]
by_file[h5_filename] = (use_obj_ids, data_file)
for h5_filename in by_file:
(use_obj_ids, data_file) = by_file[h5_filename]
for obj_id_enum,obj_id in enumerate(use_obj_ids):
rows = ca.load_data( obj_id, data_file,
use_kalman_smoothing=False,
#dynamic_model_name = dynamic_model_name,
#frames_per_second=fps,
#up_dir=up_dir,
)
verts = numpy.array( [rows['x'], rows['y'], rows['z']] ).T
if len(verts)>=3:
verts_central_diff = verts[2:,:] - verts[:-2,:]
dt = 1.0/fps
vels = verts_central_diff/(2*dt)
speeds = numpy.sqrt(numpy.sum(vels**2,axis=1))
# pad end points
speeds = numpy.array([speeds[0]] + list(speeds) + [speeds[-1]])
else:
speeds = numpy.zeros( (verts.shape[0],) )
if verts.shape[0] != len(speeds):
raise ValueError('mismatch length of x data and speeds')
x.append( verts[:,0] )
y.append( verts[:,1] )
z.append( verts[:,2] )
speed.append(speeds)
data_file.close()
del h5_filename, use_obj_ids, data_file
if 0:
# debug
if stim_xml is not None:
v = None
for child in stim_xml.root:
if child.tag == 'cubic_arena':
info = stim_xml._get_info_for_cubic_arena(child)
v=info['verts4x4']
if v is not None:
for vi in v:
print('adding',vi)
x.append( [vi[0]] )
y.append( [vi[1]] )
z.append( [vi[2]] )
speed.append( [100.0] )
x = np.concatenate(x)
y = np.concatenate(y)
z = np.concatenate(z)
w = np.ones_like(x)
speed = np.concatenate(speed)
# homogeneous coords
verts = np.array([x,y,z,w])
#######################################################
# Create the MayaVi engine and start it.
e = Engine()
# start does nothing much but useful if someone is listening to
# your engine.
e.start()
# Create a new scene.
from tvtk.tools import ivtk
#viewer = ivtk.IVTK(size=(600,600))
viewer = IVTKWithCalGUI(size=(800,600))
viewer.open()
e.new_scene(viewer)
viewer.cal_align.set_data(verts,speed,R,args.align_json)
if 0:
# Do this if you need to see the MayaVi tree view UI.
ev = EngineView(engine=e)
ui = ev.edit_traits()
# view aligned data
e.add_source(viewer.cal_align.source)
v = Vectors()
v.glyph.scale_mode = 'data_scaling_off'
v.glyph.color_mode = 'color_by_scalar'
v.glyph.glyph_source.glyph_position='center'
v.glyph.glyph_source.glyph_source = tvtk.SphereSource(
radius=options.radius,
)
e.add_module(v)
if stim_xml is not None:
if 0:
stim_xml.draw_in_mayavi_scene(e)
else:
actors = stim_xml.get_tvtk_actors()
viewer.scene.add_actors(actors)
gui = GUI()
gui.start_event_loop()
def make_montage(h5_filename,
cfg_filename=None,
ufmf_dir=None,
dest_dir=None,
save_ogv_movie=False,
no_remove=False,
max_n_frames=None,
start=None,
stop=None,
movie_fnames=None,
movie_cam_ids=None,
caminfo_h5_filename=None,
colormap=None,
kalman_filename=None,
candidate_index=0,
nth_frame=1,
verbose=False,
reconstructor=None,
**kwargs):
config = get_config_defaults()
if cfg_filename is not None:
loaded_cfg = cherrypy.lib.reprconf.as_dict(cfg_filename)
for section in loaded_cfg:
config[section].update(loaded_cfg.get(section, {}))
else:
warnings.warn('no configuration file specified -- using defaults')
orientation_3d_line_length = 0.1
if (config['what to show']['show_3d_smoothed_position']
or config['what to show']['show_3d_MLE_position']
or config['what to show']['show_3d_raw_orientation']
or config['what to show']['show_3d_raw_chosen_orientation']
or config['what to show']['show_3d_smoothed_orientation']
or config['what to show']['show_3d_obj_position_text']):
if kalman_filename is None:
raise ValueError('need kalman filename to show requested 3D data')
if config['what to show']['obj_labels']:
if kalman_filename is None:
raise ValueError('need kalman filename to show object labels')
if kalman_filename is not None:
if (config['what to show']['show_3d_smoothed_orientation']
or config['what to show']['show_3d_raw_orientation']
or config['what to show']['show_3d_raw_chosen_orientation']):
need_quality_data = True
else:
need_quality_data = False
if need_quality_data:
# need data about quality of tracking
data3d, dataqual_3d = load_3d_data(kalman_filename,
start=start,
stop=stop,
require_qual=True,
**kwargs)
else:
data3d = load_3d_data(kalman_filename,
start=start,
stop=stop,
require_qual=False,
**kwargs)
dataqual_3d = None
if (config['what to show']['show_3d_MLE_position']
or config['what to show']['show_3d_raw_orientation']):
if need_quality_data:
data_raw_3d, dataqual_raw_3d = load_3d_raw_data(
kalman_filename, **kwargs)
else:
data_raw_3d = load_3d_raw_data(kalman_filename,
require_qual=False,
**kwargs)
dataqual_raw_3d = None
else:
data_raw_3d, dataqual_raw_3d = None, None
if reconstructor is None:
R = reconstruct.Reconstructor(kalman_filename)
else:
R = reconstruct.Reconstructor(reconstructor)
else:
data3d = R = data_raw_3d = None
dataqual_raw_3d = None
dataqual_3d = None
min_ori_qual = config['what to show'][
'minimum_display_orientation_quality']
if movie_fnames is None:
# This works based on UUIDs
movie_fnames = auto_discover_movies.find_movies(
h5_filename,
ufmf_dir=ufmf_dir,
candidate_index=candidate_index,
verbose=verbose)
if verbose:
print 'autodiscovery: found movie_fnames: %r' % (movie_fnames, )
else:
if verbose:
print 'autodiscovery: movie_fnames specified, not finding movies'
if len(movie_fnames) == 0:
if verbose:
print 'autodiscovery: no FMF files found, looking for ufmfs'
movie_fnames = auto_discover_ufmfs.find_ufmfs(
h5_filename,
ufmf_dir=ufmf_dir,
careful=True,
verbose=verbose,
)
else:
if verbose:
print 'autodiscovery: prefixing directory'
if ufmf_dir is not None:
if verbose:
print 'autodiscovery: prefixing movie names with directory %r' % (
ufmf_dir, )
movie_fnames = [os.path.join(ufmf_dir, f) for f in movie_fnames]
if len(movie_fnames) == 0:
raise ValueError('no input movies -- nothing to do')
elif verbose:
print 'movie_fnames:', movie_fnames
if dest_dir is None:
dest_dir = os.curdir
else:
if not os.path.exists(dest_dir):
os.makedirs(dest_dir)
# get name of data
datetime_str = os.path.splitext(os.path.split(h5_filename)[-1])[0]
if datetime_str.startswith('DATA'):
datetime_str = datetime_str[4:19]
workaround_ffmpeg2theora_bug = True
if caminfo_h5_filename is None:
caminfo_h5_filename = h5_filename
if caminfo_h5_filename is not None:
with open_file_safe(caminfo_h5_filename, mode='r') as h5:
camn2cam_id, tmp = result_utils.get_caminfo_dicts(h5)
del tmp
else:
camn2cam_id = None
blank_images = {}
all_frame_montages = []
for frame_enum, (frame_dict, frame) in enumerate(
ufmf_tools.iterate_frames(
h5_filename,
movie_fnames,
movie_cam_ids=movie_cam_ids,
white_background=config['what to show']['white_background'],
max_n_frames=max_n_frames,
start=start,
stop=stop,
rgb8_if_color=True,
camn2cam_id=camn2cam_id,
)):
if frame_enum % nth_frame != 0:
continue
tracker_data = frame_dict['tracker_data']
global_data = frame_dict['global_data']
if data3d is not None:
this_frame_3d_data = data3d[data3d['frame'] == frame]
if dataqual_3d is None:
this_frame_dataqual = None
else:
this_frame_dataqual = dataqual_3d[data3d['frame'] == frame]
else:
this_frame_3d_data = None
this_frame_dataqual = None
if data_raw_3d is not None:
this_frame_raw_3d_data = data_raw_3d[data_raw_3d['frame'] == frame]
if dataqual_raw_3d is None:
this_frame_raw_dataqual = None
else:
this_frame_raw_dataqual = dataqual_raw_3d[data_raw_3d['frame']
== frame]
else:
this_frame_raw_3d_data = None
this_frame_raw_dataqual = None
if config['what to show']['zoom_obj']:
zoom_cond_3d = this_frame_3d_data['obj_id'] == config[
'what to show']['zoom_obj']
if np.sum(zoom_cond_3d) == 0:
# object not in this frame
this_frame_this_obj_3d_data = None
else:
this_frame_this_obj_3d_data = this_frame_3d_data[zoom_cond_3d]
if (frame_enum % 100) == 0:
print '%s: frame %d' % (datetime_str, frame)
saved_fnames = []
for movie_idx, ufmf_fname in enumerate(movie_fnames):
try:
frame_data = frame_dict[ufmf_fname]
except KeyError:
# no data saved (frame skip on Prosilica camera?)
if movie_cam_ids is not None:
cam_id = movie_cam_ids[movie_idx]
else:
cam_id = ufmf_tools.get_cam_id_from_ufmf_fname(ufmf_fname)
camn = None
if cam_id not in blank_images:
im_w, im_h = global_data['width_heights'][cam_id]
image = np.empty((im_h, im_w), dtype=np.uint8)
image.fill(255)
blank_images[cam_id] = image
image = blank_images[cam_id]
mean_image = None
else:
cam_id = frame_data['cam_id']
camn = frame_data['camn']
image = frame_data['image']
if config['what to show']['image_manipulation'] == 'absdiff':
mean_image = frame_data['mean']
del frame_data
save_fname = 'tmp_frame%07d_%s.png' % (frame, cam_id)
save_fname_path = os.path.join(dest_dir, save_fname)
pixel_aspect = config[cam_id].get('pixel_aspect', 1)
transform = config[cam_id].get('transform', 'orig')
border_pixels = config['what to show']['border_pixels']
if config['what to show']['max_resolution'] is not None:
b2 = border_pixels * 2
fix_w, fix_h = config['what to show']['max_resolution']
fix_aspect = (fix_w - b2) / float(fix_h - b2)
desire_aspect = image.shape[1] / float(
image.shape[0] * pixel_aspect)
if desire_aspect >= fix_aspect:
# image is wider than resolution given
device_w = fix_w - b2
device_h = (fix_w - b2) / desire_aspect
device_x = border_pixels
device_y = (fix_h - device_h + border_pixels) / 2.0
else:
# image is taller than resolution given
device_h = fix_h - b2
device_w = (fix_h - b2) * desire_aspect
device_y = border_pixels
device_x = (fix_w - device_w + border_pixels) / 2.0
user_rect = (0, 0, image.shape[1], image.shape[0])
elif config['what to show']['zoom_obj']:
if border_pixels != 0:
raise NotImplementedError()
device_x = 0
device_y = 0
device_w = config['what to show']['zoom_orig_pixels'] * config[
'what to show']['zoom_factor']
device_h = device_w
fix_w = device_w
fix_h = device_h
if this_frame_this_obj_3d_data is not None:
X = np.array([
this_frame_this_obj_3d_data['x'],
this_frame_this_obj_3d_data['y'],
this_frame_this_obj_3d_data['z'],
np.ones_like(this_frame_this_obj_3d_data['x'])
]).T
xarr, yarr = R.find2d(cam_id, X, distorted=True)
assert len(xarr) == 1
x = xarr[0]
y = yarr[0]
r = config['what to show']['zoom_orig_pixels'] * 0.5
user_rect = (x - r, y - r, r * 2, r * 2)
else:
# we're not tracking object -- don't draw anything
user_rect = (-1000, -1000, 10, 10)
else:
device_x = border_pixels
device_y = border_pixels
device_w = image.shape[1]
device_h = int(image.shape[0] *
pixel_aspect) # compensate for pixel_aspect
fix_w = device_w + 2 * border_pixels
fix_h = device_h + 2 * border_pixels
user_rect = (0, 0, image.shape[1], image.shape[0])
canv = benu.Canvas(save_fname_path, fix_w, fix_h)
device_rect = (device_x, device_y, device_w, device_h)
with canv.set_user_coords(device_rect,
user_rect,
transform=transform):
if config['what to show']['image_manipulation'] == 'raw':
canv.imshow(image, 0, 0, cmap=colormap)
if config['what to show']['image_manipulation'] == 'absdiff':
if mean_image is not None:
adsdiff_image = abs(
image.astype(np.int16) -
mean_image.astype(np.int16))
scaled_show = np.clip((5 * adsdiff_image) + 127, 0,
255).astype(np.uint8)
canv.imshow(scaled_show, 0, 0, cmap=colormap)
if config['what to show'][
'show_2d_position'] and camn is not None:
cond = tracker_data['camn'] == camn
this_cam_data = tracker_data[cond]
xarr = np.atleast_1d(this_cam_data['x'])
yarr = np.atleast_1d(this_cam_data['y'])
canv.scatter(
xarr,
yarr,
color_rgba=(0, 0, 0, 1),
radius=10,
markeredgewidth=config['what to show']['linewidth'],
)
# draw shadow
canv.scatter(
xarr + config['what to show']['linewidth'],
yarr + config['what to show']['linewidth'],
color_rgba=(1, 1, 1, 1),
radius=10,
markeredgewidth=config['what to show']['linewidth'],
)
if config['what to show'][
'show_2d_orientation'] and camn is not None:
cond = tracker_data['camn'] == camn
this_cam_data = tracker_data[cond]
xarr = np.atleast_1d(this_cam_data['x'])
yarr = np.atleast_1d(this_cam_data['y'])
slope = np.atleast_1d(this_cam_data['slope'])
thetaarr = np.arctan(slope)
line_len = 30.0
xinc = np.cos(thetaarr) * line_len
yinc = np.sin(thetaarr) * line_len / float(pixel_aspect)
for x, y, xi, yi in zip(xarr, yarr, xinc, yinc):
xarr = np.array([x - xi, x + xi])
yarr = np.array([y - yi, y + yi])
if np.any(np.isnan(xarr)) or np.any(np.isnan(yarr)):
continue
canv.plot(
xarr,
yarr,
color_rgba=(0, 1, 0, 0.4),
linewidth=config['what to show']['linewidth'],
)
if config['what to show'][
'show_3d_smoothed_position'] and camn is not None:
if len(this_frame_3d_data):
X = np.array([
this_frame_3d_data['x'], this_frame_3d_data['y'],
this_frame_3d_data['z'],
np.ones_like(this_frame_3d_data['x'])
]).T
xarr, yarr = R.find2d(cam_id, X, distorted=True)
canv.scatter(
xarr,
yarr,
color_rgba=(0, 1, 1, 1),
radius=10,
markeredgewidth=config['what to show']
['linewidth'],
)
if config['what to show'][
'show_3d_MLE_position'] and camn is not None:
if len(this_frame_raw_3d_data):
X = np.array([
this_frame_raw_3d_data['x'],
this_frame_raw_3d_data['y'],
this_frame_raw_3d_data['z'],
np.ones_like(this_frame_raw_3d_data['x'])
]).T
xarr, yarr = R.find2d(cam_id, X, distorted=True)
canv.scatter(
xarr,
yarr,
color_rgba=(0.2, 0.2, 0.5, 1),
radius=8,
markeredgewidth=config['what to show']
['linewidth'],
)
# draw shadow
canv.scatter(
xarr + config['what to show']['linewidth'],
yarr + config['what to show']['linewidth'],
color_rgba=(0.7, 0.7, 1, 1), # blue
radius=8,
markeredgewidth=config['what to show']
['linewidth'],
)
if config['what to show'][
'show_3d_raw_orientation'] and camn is not None:
if len(this_frame_raw_3d_data):
hzs = np.array([
this_frame_raw_3d_data['hz_line0'],
this_frame_raw_3d_data['hz_line1'],
this_frame_raw_3d_data['hz_line2'],
this_frame_raw_3d_data['hz_line3'],
this_frame_raw_3d_data['hz_line4'],
this_frame_raw_3d_data['hz_line5']
]).T
Xs = np.array([
this_frame_raw_3d_data['x'],
this_frame_raw_3d_data['y'],
this_frame_raw_3d_data['z']
]).T
cam_center = R.get_camera_center(cam_id)[:, 0]
for (X, hz,
this_dataqual) in zip(Xs, hzs,
this_frame_raw_dataqual):
if this_dataqual < min_ori_qual:
continue
cam_ray = geom.line_from_points(
geom.ThreeTuple(cam_center),
geom.ThreeTuple(X))
raw_ori_line = geom.line_from_HZline(hz)
X_ = raw_ori_line.get_my_point_closest_to_line(
cam_ray)
ld = raw_ori_line.direction()
dmag = abs(ld)
du = ld * (1. / dmag
) # unit length direction (normalize)
length = 0.5 # arbitrary, 0.5 meters
N = 100 # n segments (to deal with distortion)
X0 = X_.vals + du.vals * -length / 2.0
X = X0[:, np.newaxis] + np.linspace(0, length, N)[
np.newaxis, :] * du.vals[:, np.newaxis]
Xh = np.vstack(
(X, np.ones_like(X[0, np.newaxis, :]))).T
xarr, yarr = R.find2d(cam_id, Xh, distorted=True)
canv.plot(
xarr,
yarr,
color_rgba=(0, 0, 1, 1), # blue
linewidth=config['what to show']['linewidth'],
)
if config['what to show'][
'show_3d_smoothed_orientation'] and camn is not None:
if len(this_frame_3d_data):
for (row, ori_qual) in zip(this_frame_3d_data,
this_frame_dataqual):
if ori_qual < min_ori_qual:
continue
X0 = np.array([
row['x'], row['y'], row['z'],
np.ones_like(row['x'])
]).T
dx = np.array([
row['dir_x'], row['dir_y'], row['dir_z'],
np.zeros_like(row['x'])
]).T
X1 = X0 + dx * orientation_3d_line_length
if np.any(np.isnan(X1)):
continue
pts = np.vstack([X0, X1])
xarr, yarr = R.find2d(cam_id, pts, distorted=True)
canv.plot(
xarr,
yarr,
color_rgba=(1, 0, 0, 1), # red
linewidth=config['what to show']['linewidth'],
)
if config['what to show'][
'show_3d_raw_chosen_orientation'] and camn is not None:
if len(this_frame_3d_data):
for (row, ori_qual) in zip(this_frame_3d_data,
this_frame_dataqual):
if ori_qual < min_ori_qual:
continue
X0 = np.array([
row['x'], row['y'], row['z'],
np.ones_like(row['x'])
]).T
dx = np.array([
row['rawdir_x'], row['rawdir_y'],
row['rawdir_z'],
np.zeros_like(row['x'])
]).T
X1 = X0 + dx * orientation_3d_line_length
if np.any(np.isnan(X1)):
continue
pts = np.vstack([X0, X1])
xarr, yarr = R.find2d(cam_id, pts, distorted=True)
canv.plot(
xarr,
yarr,
color_rgba=(1, 159. / 255, 0, 1), # orange
linewidth=config['what to show']['linewidth'],
)
if config['what to show']['obj_labels'] and camn is not None:
if len(this_frame_3d_data):
X = np.array([
this_frame_3d_data['x'], this_frame_3d_data['y'],
this_frame_3d_data['z'],
np.ones_like(this_frame_3d_data['x'])
]).T
xarr, yarr = R.find2d(cam_id, X, distorted=True)
for i in range(len(xarr)):
obj_id = this_frame_3d_data['obj_id'][i]
canv.text('%d' % obj_id,
xarr[i],
yarr[i],
font_size=14,
color_rgba=(1, 0, 0, 1))
if config['what to show'][
'show_3d_obj_position_text'] and camn is not None:
if len(this_frame_3d_data):
X = np.array([
this_frame_3d_data['x'], this_frame_3d_data['y'],
this_frame_3d_data['z'],
np.ones_like(this_frame_3d_data['x'])
]).T
xarr, yarr = R.find2d(cam_id, X, distorted=True)
for i in range(len(xarr)):
canv.text('(%.1f, %.1f, %.1f) mm' %
(X[i, 0] * 1000.0, X[i, 1] * 1000.0,
X[i, 2] * 1000.0),
xarr[i] + 10,
yarr[i],
font_size=14,
color_rgba=(0, 1, 1, 1))
if config['what to show']['show_cam_id']:
canv.text('%s' % cam_id,
0,
20,
font_size=14,
color_rgba=(1, 0, 0, 1))
if workaround_ffmpeg2theora_bug:
# first frame should get a colored pixel so that
# ffmpeg doesn't interpret the whole move as grayscale
canv.plot(
[0, 1],
[0, 1],
color_rgba=(1, 0, 0, 0.1),
)
workaround_ffmpeg2theora_bug = False # Now we already did it.
canv.save()
saved_fnames.append(save_fname_path)
target = os.path.join(
dest_dir, 'movie%s_frame%07d.png' % (datetime_str, frame_enum + 1))
# All cameras saved for this frame, make montage
title = '%s frame %d' % (datetime_str, frame)
montage(saved_fnames, title, target)
all_frame_montages.append(target)
if not no_remove:
for fname in saved_fnames:
os.unlink(fname)
print '%s: %d frames montaged' % (
datetime_str,
len(all_frame_montages),
)
if save_ogv_movie:
orig_dir = os.path.abspath(os.curdir)
os.chdir(dest_dir)
try:
CMD = 'ffmpeg2theora -v 10 movie%s_frame%%07d.png -o movie%s.ogv' % (
datetime_str, datetime_str)
subprocess.check_call(CMD, shell=True)
finally:
os.chdir(orig_dir)
if not no_remove:
for fname in all_frame_montages:
os.unlink(fname)
def generate_calibration(
n_cameras=5,
return_full_info=False,
radial_distortion=False,
):
pi = numpy.pi
sccs = []
# 1. extrinsic parameters:
if 1:
# method 1:
# arrange cameras in circle around common point
common_point = numpy.array((0, 0, 0), dtype=numpy.float64)
r = 10.0
theta = numpy.linspace(0, 2 * pi, n_cameras, endpoint=False)
x = numpy.cos(theta)
y = numpy.sin(theta)
z = numpy.zeros(y.shape)
cc = numpy.c_[x, y, z]
#cam_up = numpy.array((0,0,1))
#cam_ups = numpy.resize(cam_up,cc.shape)
#cam_forwads = -cc
cam_centers = r * cc + common_point
# Convert up/forward into rotation matrix.
if 1:
Rs = []
for i, th in enumerate(theta):
pos = cam_centers[i]
target = common_point
up = (0, 0, 1)
if 0:
print 'pos', pos
print 'target', target
print 'up', up
R = cgtypes.mat4().lookAt(pos, target, up)
#print 'R4',R
R = R.getMat3()
#print 'R3',R
R = numpy.asarray(R).T
#print 'R',R
#print
Rs.append(R)
else:
# (Camera coords: looking forward -z, up +y, right +x)
R = cgtypes.mat3().identity()
if 1:
# (looking forward -z, up +x, right -y)
R = R.rotation(-pi / 2, (0, 0, 1))
# (looking forward +x, up +z, right -y)
R = R.rotation(-pi / 2, (0, 1, 0))
# rotate to point -theta (with up +z)
Rs = [R.rotation(float(th) + pi, (0, 0, 1)) for th in theta]
#Rs = [ R for th in theta ]
else:
Rs = [R.rotation(pi / 2.0, (1, 0, 0)) for th in theta]
#Rs = [ R for th in theta ]
Rs = [numpy.asarray(R).T for R in Rs]
print 'Rs', Rs
# 2. intrinsic parameters
resolutions = {}
for cam_no in range(n_cameras):
cam_id = 'fake_%d' % (cam_no + 1)
# resolution of image
res = (1600, 1200)
resolutions[cam_id] = res
# principal point
cc1 = res[0] / 2.0
cc2 = res[1] / 2.0
# focal length
fc1 = 1.0
fc2 = 1.0
alpha_c = 0.0
# R = numpy.asarray(Rs[cam_no]).T # conversion between cgkit and numpy
R = Rs[cam_no]
C = cam_centers[cam_no][:, numpy.newaxis]
K = numpy.array(((fc1, alpha_c * fc1, cc1), (0, fc2, cc2), (0, 0, 1)))
t = numpy.dot(-R, C)
Rt = numpy.concatenate((R, t), axis=1)
P = numpy.dot(K, Rt)
if 0:
print 'cam_id', cam_id
print 'P'
print P
print 'K'
print K
print 'Rt'
print Rt
print
KR = numpy.dot(K, R)
print 'KR', KR
K3, R3 = reconstruct.my_rq(KR)
print 'K3'
print K3
print 'R3'
print R3
K3R3 = numpy.dot(K3, R3)
print 'K3R3', K3R3
print '*' * 60
if radial_distortion:
f = 1000.0
r1 = 0.8
r2 = -0.2
helper = reconstruct_utils.ReconstructHelper(
f,
f, # focal length
cc1,
cc2, # image center
r1,
r2, # radial distortion
0,
0) # tangential distortion
scc = reconstruct.SingleCameraCalibration_from_basic_pmat(
P,
cam_id=cam_id,
res=res,
)
sccs.append(scc)
if 1:
# XXX test
K2, R2 = scc.get_KR()
if 0:
print 'C', C
print 't', t
print 'K', K
print 'K2', K2
print 'R', R
print 'R2', R2
print 'P', P
print 'KR|t', numpy.dot(K, Rt)
t2 = scc.get_t()
print 't2', t2
Rt2 = numpy.concatenate((R2, t2), axis=1)
print 'KR2|t', numpy.dot(K2, Rt2)
print
KR2 = numpy.dot(K2, R2)
KR = numpy.dot(K, R)
if not numpy.allclose(KR2, KR):
if not numpy.allclose(KR2, -KR):
raise ValueError('expected KR2 and KR to be identical')
else:
print 'WARNING: weird sign error in calibration math FIXME!'
recon = reconstruct.Reconstructor(sccs)
full_info = {
'reconstructor': recon,
'center': common_point, # where all the cameras are looking
'camera_dist_from_center': r,
'resolutions': resolutions,
}
if return_full_info:
return full_info
return recon
good_cond = (real_frames >= options.start)
obj_verts = obj_verts[good_cond]
speeds = speeds[good_cond]
real_frames = real_frames[good_cond]
if not len(use_obj_ids) == 1:
obj_verts = numpy.concatenate(obj_verts, axis=0)
speeds = numpy.concatenate(speeds, axis=0)
real_frames = numpy.concatenate(real_frames, axis=0)
####################### start draw permanently on stuff ############################
if options.stim_xml is not None:
if not is_mat_file:
R = reconstruct.Reconstructor(data_file)
stim_xml.verify_reconstructor(R)
if not is_mat_file:
assert (data_file.filename.startswith('DATA')
and (data_file.filename.endswith('.h5')
or data_file.filename.endswith('.kh5')))
file_timestamp = data_file.filename[4:19]
actors = stim_xml.get_tvtk_actors()
for actor in actors:
ren.add_actor(actor)
if 1:
if 0:
# Inspired by pyface.tvtk.decorated_scene
marker = tvtk.OrientationMarkerWidget()
def plot_top_and_side_views(
subplot=None, options=None, obs_mew=None, scale=1.0, units="m",
):
"""
inputs
------
subplot - a dictionary of matplotlib axes instances with keys 'xy' and/or 'xz'
fps - the framerate of the data
"""
assert subplot is not None
assert options is not None
if not hasattr(options, "show_track_ends"):
options.show_track_ends = False
if not hasattr(options, "unicolor"):
options.unicolor = False
if not hasattr(options, "show_landing"):
options.show_landing = False
kalman_filename = options.kalman_filename
fps = options.fps
dynamic_model = options.dynamic_model
use_kalman_smoothing = options.use_kalman_smoothing
if not hasattr(options, "ellipsoids"):
options.ellipsoids = False
if not hasattr(options, "show_observations"):
options.show_observations = False
if not hasattr(options, "markersize"):
options.markersize = 0.5
if options.ellipsoids and use_kalman_smoothing:
warnings.warn(
"plotting ellipsoids while using Kalman smoothing does not reveal original error estimates"
)
assert kalman_filename is not None
start = options.start
stop = options.stop
obj_only = options.obj_only
if not use_kalman_smoothing:
if dynamic_model is not None:
print(
"ERROR: disabling Kalman smoothing (--disable-kalman-smoothing) is incompatable with setting dynamic model options (--dynamic-model)",
file=sys.stderr,
)
sys.exit(1)
ca = core_analysis.get_global_CachingAnalyzer()
if kalman_filename is not None:
obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load(
kalman_filename
)
if not is_mat_file:
mat_data = None
if fps is None:
fps = result_utils.get_fps(data_file, fail_on_error=False)
if fps is None:
fps = 100.0
warnings.warn("Setting fps to default value of %f" % fps)
reconstructor = reconstruct.Reconstructor(data_file)
else:
reconstructor = None
if options.stim_xml:
file_timestamp = data_file.filename[4:19]
stim_xml = xml_stimulus.xml_stimulus_from_filename(
options.stim_xml, timestamp_string=file_timestamp,
)
try:
fanout = xml_stimulus.xml_fanout_from_filename(options.stim_xml)
except xml_stimulus.WrongXMLTypeError:
walking_start_stops = []
else:
include_obj_ids, exclude_obj_ids = fanout.get_obj_ids_for_timestamp(
timestamp_string=file_timestamp
)
walking_start_stops = fanout.get_walking_start_stops_for_timestamp(
timestamp_string=file_timestamp
)
if include_obj_ids is not None:
use_obj_ids = include_obj_ids
if exclude_obj_ids is not None:
use_obj_ids = list(set(use_obj_ids).difference(exclude_obj_ids))
stim_xml = fanout.get_stimulus_for_timestamp(
timestamp_string=file_timestamp
)
if stim_xml.has_reconstructor():
stim_xml.verify_reconstructor(reconstructor)
else:
walking_start_stops = []
if dynamic_model is None:
dynamic_model = extra.get("dynamic_model_name", None)
if dynamic_model is None:
if use_kalman_smoothing:
warnings.warn(
"no kalman smoothing will be performed because no "
"dynamic model specified or found."
)
use_kalman_smoothing = False
else:
print('detected file loaded with dynamic model "%s"' % dynamic_model)
if use_kalman_smoothing:
if dynamic_model.startswith("EKF "):
dynamic_model = dynamic_model[4:]
print(' for smoothing, will use dynamic model "%s"' % dynamic_model)
subplots = subplot.keys()
subplots.sort() # ensure consistency across runs
dt = 1.0 / fps
if obj_only is not None:
use_obj_ids = [i for i in use_obj_ids if i in obj_only]
subplot["xy"].set_aspect("equal")
subplot["xz"].set_aspect("equal")
subplot["xy"].set_xlabel("x (%s)" % units)
subplot["xy"].set_ylabel("y (%s)" % units)
subplot["xz"].set_xlabel("x (%s)" % units)
subplot["xz"].set_ylabel("z (%s)" % units)
if options.stim_xml:
stim_xml.plot_stim(
subplot["xy"], projection=xml_stimulus.SimpleOrthographicXYProjection()
)
stim_xml.plot_stim(
subplot["xz"], projection=xml_stimulus.SimpleOrthographicXZProjection()
)
allX = {}
frame0 = None
results = collections.defaultdict(list)
for obj_id in use_obj_ids:
line = None
ellipse_lines = []
MLE_line = None
try:
kalman_rows = ca.load_data(
obj_id,
data_file,
use_kalman_smoothing=use_kalman_smoothing,
dynamic_model_name=dynamic_model,
frames_per_second=fps,
up_dir=options.up_dir,
)
except core_analysis.ObjectIDDataError:
continue
if options.show_observations:
kobs_rows = ca.load_dynamics_free_MLE_position(obj_id, data_file)
frame = kalman_rows["frame"]
if options.show_observations:
frame_obs = kobs_rows["frame"]
if (start is not None) or (stop is not None):
valid_cond = numpy.ones(frame.shape, dtype=numpy.bool)
if options.show_observations:
valid_obs_cond = np.ones(frame_obs.shape, dtype=numpy.bool)
if start is not None:
valid_cond = valid_cond & (frame >= start)
if options.show_observations:
valid_obs_cond = valid_obs_cond & (frame_obs >= start)
if stop is not None:
valid_cond = valid_cond & (frame <= stop)
if options.show_observations:
valid_obs_cond = valid_obs_cond & (frame_obs <= stop)
kalman_rows = kalman_rows[valid_cond]
if options.show_observations:
kobs_rows = kobs_rows[valid_obs_cond]
if not len(kalman_rows):
continue
frame = kalman_rows["frame"]
Xx = kalman_rows["x"]
Xy = kalman_rows["y"]
Xz = kalman_rows["z"]
if options.max_z is not None:
cond = Xz <= options.max_z
frame = numpy.ma.masked_where(~cond, frame)
Xx = numpy.ma.masked_where(~cond, Xx)
Xy = numpy.ma.masked_where(~cond, Xy)
Xz = numpy.ma.masked_where(~cond, Xz)
with keep_axes_dimensions_if(subplot["xz"], options.stim_xml):
subplot["xz"].axhline(options.max_z)
kws = {"markersize": options.markersize}
if options.unicolor:
kws["color"] = "k"
landing_idxs = []
for walkstart, walkstop in walking_start_stops:
if walkstart in frame:
tmp_idx = numpy.nonzero(frame == walkstart)[0][0]
landing_idxs.append(tmp_idx)
with keep_axes_dimensions_if(subplot["xy"], options.stim_xml):
(line,) = subplot["xy"].plot(
Xx * scale, Xy * scale, ".", label="obj %d" % obj_id, **kws
)
kws["color"] = line.get_color()
if options.ellipsoids:
for i in range(len(Xx)):
rowi = kalman_rows[i]
mu = [rowi["x"], rowi["y"], rowi["z"]]
va = get_covariance(rowi)
ellx, elly = densities.gauss_ell(mu, va, [0, 1], 30, 0.39)
(ellipse_line,) = subplot["xy"].plot(
ellx * scale, elly * scale, color=kws["color"]
)
ellipse_lines.append(ellipse_line)
if options.show_track_ends:
subplot["xy"].plot(
[Xx[0] * scale, Xx[-1] * scale],
[Xy[0] * scale, Xy[-1] * scale],
"cd",
ms=6,
label="track end",
)
if options.show_obj_id:
subplot["xy"].text(Xx[0] * scale, Xy[0] * scale, str(obj_id))
if options.show_landing:
for landing_idx in landing_idxs:
subplot["xy"].plot(
[Xx[landing_idx] * scale],
[Xy[landing_idx] * scale],
"rD",
ms=10,
label="landing",
)
if options.show_observations:
mykw = {}
mykw.update(kws)
mykw["markersize"] *= 5
mykw["mew"] = obs_mew
badcond = np.isnan(kobs_rows["x"])
Xox = np.ma.masked_where(badcond, kobs_rows["x"])
Xoy = np.ma.masked_where(badcond, kobs_rows["y"])
(MLE_line,) = subplot["xy"].plot(
Xox * scale, Xoy * scale, "x", label="obj %d" % obj_id, **mykw
)
with keep_axes_dimensions_if(subplot["xz"], options.stim_xml):
(line,) = subplot["xz"].plot(
Xx * scale, Xz * scale, ".", label="obj %d" % obj_id, **kws
)
kws["color"] = line.get_color()
if options.ellipsoids:
for i in range(len(Xx)):
rowi = kalman_rows[i]
mu = [rowi["x"], rowi["y"], rowi["z"]]
va = get_covariance(rowi)
ellx, ellz = densities.gauss_ell(mu, va, [0, 2], 30, 0.39)
(ellipse_line,) = subplot["xz"].plot(
ellx * scale, ellz * scale, color=kws["color"]
)
ellipse_lines.append(ellipse_line)
if options.show_track_ends:
subplot["xz"].plot(
[Xx[0] * scale, Xx[-1] * scale],
[Xz[0] * scale, Xz[-1] * scale],
"cd",
ms=6,
label="track end",
)
if options.show_obj_id:
subplot["xz"].text(Xx[0] * scale, Xz[0] * scale, str(obj_id))
if options.show_landing:
for landing_idx in landing_idxs:
subplot["xz"].plot(
[Xx[landing_idx] * scale],
[Xz[landing_idx] * scale],
"rD",
ms=10,
label="landing",
)
if options.show_observations:
mykw = {}
mykw.update(kws)
mykw["markersize"] *= 5
mykw["mew"] = obs_mew
badcond = np.isnan(kobs_rows["x"])
Xox = np.ma.masked_where(badcond, kobs_rows["x"])
Xoz = np.ma.masked_where(badcond, kobs_rows["z"])
(MLE_line,) = subplot["xz"].plot(
Xox * scale, Xoz * scale, "x", label="obj %d" % obj_id, **mykw
)
results["lines"].append(line)
results["ellipse_lines"].extend(ellipse_lines)
results["MLE_line"].append(MLE_line)
return results
def test_pickle(self):
caldir = os.path.join(os.path.split(__file__)[0], "sample_calibration")
x = reconstruct.Reconstructor(caldir)
xpickle = pickle.dumps(x)
x2 = pickle.loads(xpickle)
assert x2 == x
def test_from_sample_directory(self):
caldir = os.path.join(os.path.split(__file__)[0], "sample_calibration")
reconstruct.Reconstructor(caldir)
def doit(
filenames=None,
start=None,
stop=None,
kalman_filename=None,
fps=None,
use_kalman_smoothing=True,
dynamic_model=None,
up_dir=None,
options=None,
):
if options.save_fig is not None:
matplotlib.use('Agg')
import pylab
if not use_kalman_smoothing:
if (fps is not None) or (dynamic_model is not None):
print(
'WARNING: disabling Kalman smoothing '
'(--disable-kalman-smoothing) is '
'incompatable with setting fps and '
'dynamic model options (--fps and '
'--dynamic-model)',
file=sys.stderr)
ax = None
ax_by_cam = {}
fig = pylab.figure()
assert len(filenames) >= 1, 'must give at least one filename!'
n_files = 0
for filename in filenames:
if options.show_source_name:
figtitle = filename
if kalman_filename is not None:
figtitle += ' ' + kalman_filename
else:
figtitle = ''
if options.obj_only is not None:
figtitle += ' only showing objects: ' + ' '.join(
map(str, options.obj_only))
if figtitle != '':
pylab.figtext(0.01, 0.01, figtitle, verticalalignment='bottom')
with PT.open_file(filename, mode='r') as h5:
if options.spreadh5 is not None:
h5spread = PT.open_file(options.spreadh5, mode='r')
else:
h5spread = None
if fps is None:
fps = result_utils.get_fps(h5)
camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5)
cam_ids = cam_id2camns.keys()
cam_ids.sort()
if start is not None or stop is not None:
frames = h5.root.data2d_distorted.read(field='frame')
valid_cond = numpy.ones(frames.shape, dtype=numpy.bool)
if start is not None:
valid_cond = valid_cond & (frames >= start)
if stop is not None:
valid_cond = valid_cond & (frames <= stop)
read_idxs = np.nonzero(valid_cond)[0]
all_data = []
for start_stop in utils.iter_contig_chunk_idxs(read_idxs):
(read_idx_start_idx, read_idx_stop_idx) = start_stop
start_idx = read_idxs[read_idx_start_idx]
stop_idx = read_idxs[read_idx_stop_idx - 1]
these_rows = h5.root.data2d_distorted.read(start=start_idx,
stop=stop_idx +
1)
all_data.append(these_rows)
if len(all_data) == 0:
print('file %s has no frames in range %s - %s' %
(filename, start, stop))
continue
all_data = np.concatenate(all_data)
del valid_cond, frames, start_idx, stop_idx, these_rows, read_idxs
else:
all_data = h5.root.data2d_distorted[:]
tmp_frames = all_data['frame']
if len(tmp_frames) == 0:
print('file %s has no frames, skipping.' % filename)
continue
n_files += 1
start_frame = tmp_frames.min()
stop_frame = tmp_frames.max()
del tmp_frames
for cam_id_enum, cam_id in enumerate(cam_ids):
if cam_id in ax_by_cam:
ax = ax_by_cam[cam_id]
else:
n_subplots = len(cam_ids)
if kalman_filename is not None:
n_subplots += 1
if h5spread is not None:
n_subplots += 1
ax = pylab.subplot(n_subplots,
1,
cam_id_enum + 1,
sharex=ax)
ax_by_cam[cam_id] = ax
ax.fmt_xdata = str
ax.fmt_ydata = str
camns = cam_id2camns[cam_id]
cam_id_n_valid = 0
for camn in camns:
this_idx = numpy.nonzero(all_data['camn'] == camn)[0]
data = all_data[this_idx]
xdata = data['x']
valid = ~numpy.isnan(xdata)
data = data[valid]
del valid
if options.area_threshold > 0.0:
area = data['area']
valid2 = area >= options.area_threshold
data = data[valid2]
del valid2
if options.likely_only:
pt_area = data['area']
cur_val = data['cur_val']
mean_val = data['mean_val']
sumsqf_val = data['sumsqf_val']
p_y_x = some_rough_negative_log_likelihood(
pt_area, cur_val, mean_val, sumsqf_val)
valid3 = np.isfinite(p_y_x)
data = data[valid3]
n_valid = len(data)
cam_id_n_valid += n_valid
if options.timestamps:
xdata = data['timestamp']
else:
xdata = data['frame']
if n_valid >= 1:
ax.plot(xdata, data['x'], 'ro', ms=2, mew=0)
ax.plot(xdata, data['y'], 'go', ms=2, mew=0)
ax.text(
0.1,
0,
'%s %s: %d pts' %
(cam_id, cam_id2camns[cam_id], cam_id_n_valid),
horizontalalignment='left',
verticalalignment='bottom',
transform=ax.transAxes,
)
ax.set_ylabel('pixels')
if not options.timestamps:
ax.set_xlim((start_frame, stop_frame))
ax.set_xlabel('frame')
if options.timestamps:
timezone = result_utils.get_tz(h5)
df = DateFormatter(timezone)
ax.xaxis.set_major_formatter(
ticker.FuncFormatter(df.format_date))
else:
ax.xaxis.set_major_formatter(ticker.FormatStrFormatter("%d"))
ax.yaxis.set_major_formatter(ticker.FormatStrFormatter("%d"))
if h5spread is not None:
if options.timestamps:
raise NotImplementedError(
'--timestamps is currently incompatible with --spreadh5'
)
ax_by_cam['h5spread'] = ax
if kalman_filename is not None:
# this is 2nd to last
ax = pylab.subplot(n_subplots,
1,
n_subplots - 1,
sharex=ax)
else:
# this is last
ax = pylab.subplot(n_subplots, 1, n_subplots, sharex=ax)
frames = h5spread.root.framenumber[:]
spread = h5spread.root.spread[:]
valid_cond = numpy.ones(frames.shape, dtype=numpy.bool)
if start is not None:
valid_cond = valid_cond & (frames >= start)
if stop is not None:
valid_cond = valid_cond & (frames <= stop)
spread_msec = spread[valid_cond] * 1000.0
ax.plot(frames[valid_cond], spread_msec, 'o', ms=2, mew=0)
if spread_msec.max() < 1.0:
ax.set_ylim((0, 1))
ax.set_yticks([0, 1])
ax.set_xlabel('frame')
ax.set_ylabel('timestamp spread (msec)')
ax.xaxis.set_major_formatter(ticker.FormatStrFormatter("%d"))
ax.yaxis.set_major_formatter(ticker.FormatStrFormatter("%d"))
h5spread.close()
del frames
del spread
if options.timestamps:
fig.autofmt_xdate()
if kalman_filename is not None:
if 1:
ax = pylab.subplot(n_subplots, 1, n_subplots, sharex=ax)
ax_by_cam['kalman pmean'] = ax
ax.fmt_xdata = str
ax.set_ylabel('3d error\nmeters')
frame_start = start
frame_stop = stop
# copied from save_movies_overlay.py
ca = core_analysis.get_global_CachingAnalyzer()
(obj_ids, use_obj_ids, is_mat_file, data_file,
extra) = ca.initial_file_load(kalman_filename)
if options.timestamps:
time_model = result_utils.get_time_model_from_data(data_file)
if 'frames' in extra:
frames = extra['frames']
valid_cond = np.ones((len(frames, )), dtype=np.bool)
if start is not None:
valid_cond &= frames >= start
if stop is not None:
valid_cond &= frames <= stop
obj_ids = obj_ids[valid_cond]
use_obj_ids = np.unique(obj_ids)
print('quick found use_obj_ids', use_obj_ids)
if is_mat_file:
raise ValueError('cannot use .mat file for kalman_filename '
'because it is missing the reconstructor '
'and ability to get framenumbers')
R = reconstruct.Reconstructor(data_file)
if options.obj_only is not None:
use_obj_ids = options.obj_only
if dynamic_model is None and use_kalman_smoothing:
dynamic_model = extra['dynamic_model_name']
print('detected file loaded with dynamic model "%s"' %
dynamic_model)
if dynamic_model.startswith('EKF '):
dynamic_model = dynamic_model[4:]
print(' for smoothing, will use dynamic model "%s"' %
dynamic_model)
if options.reproj_error:
reproj_error = collections.defaultdict(list)
max_reproj_error = {}
kalman_rows = []
for obj_id in use_obj_ids:
kalman_rows.append(ca.load_observations(obj_id, data_file))
kalman_rows = numpy.concatenate(kalman_rows)
kalman_3d_frame = kalman_rows['frame']
if start is not None or stop is not None:
if start is None:
start = -numpy.inf
if stop is None:
stop = numpy.inf
valid_cond = ((kalman_3d_frame >= start) &
(kalman_3d_frame <= stop))
kalman_rows = kalman_rows[valid_cond]
kalman_3d_frame = kalman_3d_frame[valid_cond]
# modified from save_movies_overlay
for this_3d_row_enum, this_3d_row in enumerate(kalman_rows):
if this_3d_row_enum % 100 == 0:
print('doing reprojection error for MLE 3d estimate for '
'row %d of %d' %
(this_3d_row_enum, len(kalman_rows)))
vert = numpy.array(
[this_3d_row['x'], this_3d_row['y'], this_3d_row['z']])
obj_id = this_3d_row['obj_id']
if numpy.isnan(vert[0]):
# no observation this frame
continue
obs_2d_idx = this_3d_row['obs_2d_idx']
try:
kobs_2d_data = data_file.root.ML_estimates_2d_idxs[int(
obs_2d_idx)]
except tables.exceptions.NoSuchNodeError, err:
# backwards compatibility
kobs_2d_data = data_file.root.kalman_observations_2d_idxs[
int(obs_2d_idx)]
# parse VLArray
this_camns = kobs_2d_data[0::2]
this_camn_idxs = kobs_2d_data[1::2]
# find original 2d data
# narrow down search
obs2d = all_data[all_data['frame'] == this_3d_row['frame']]
for camn, this_camn_idx in zip(this_camns, this_camn_idxs):
cam_id = camn2cam_id[camn]
# do projection to camera image plane
vert_image = R.find2d(cam_id, vert, distorted=True)
new_cond = ((obs2d['camn'] == camn) &
(obs2d['frame_pt_idx'] == this_camn_idx))
assert numpy.sum(new_cond) == 1
x = obs2d[new_cond]['x'][0]
y = obs2d[new_cond]['y'][0]
this_reproj_error = numpy.sqrt((vert_image[0] - x)**2 +
(vert_image[1] - y)**2)
if this_reproj_error > 100:
print(' reprojection error > 100 (%.1f) at frame %d '
'for camera %s, obj_id %d' %
(this_reproj_error, this_3d_row['frame'], cam_id,
obj_id))
if numpy.isnan(this_reproj_error):
print('error:')
print(this_camns, this_camn_idxs)
print(cam_id)
print(vert_image)
print(vert)
raise ValueError('nan at frame %d' %
this_3d_row['frame'])
reproj_error[cam_id].append(this_reproj_error)
if cam_id in max_reproj_error:
(cur_max_frame, cur_max_reproj_error,
cur_obj_id) = max_reproj_error[cam_id]
if this_reproj_error > cur_max_reproj_error:
max_reproj_error[cam_id] = (this_3d_row['frame'],
this_reproj_error,
obj_id)
else:
max_reproj_error[cam_id] = (this_3d_row['frame'],
this_reproj_error, obj_id)
del kalman_rows, kalman_3d_frame, obj_ids
print('mean reprojection errors:')
cam_ids = reproj_error.keys()
cam_ids.sort()
for cam_id in cam_ids:
errors = reproj_error[cam_id]
mean_error = numpy.mean(errors)
worst_frame, worst_error, worst_obj_id = max_reproj_error[
cam_id]
print(' %s: %.1f (worst: frame %d, obj_id %d, error %.1f)' %
(cam_id, mean_error, worst_frame, worst_obj_id,
worst_error))
print()
for kalman_smoothing in [True, False]:
if use_kalman_smoothing == False and kalman_smoothing == True:
continue
print('loading frame numbers for kalman objects (estimates)')
kalman_rows = []
for obj_id in use_obj_ids:
try:
my_rows = ca.load_data(
obj_id,
data_file,
use_kalman_smoothing=kalman_smoothing,
dynamic_model_name=dynamic_model,
frames_per_second=fps,
up_dir=up_dir,
)
except core_analysis.NotEnoughDataToSmoothError, err:
# OK, we don't have data from this obj_id
continue
else:
kalman_rows.append(my_rows)
if not len(kalman_rows):
# no data
continue
kalman_rows = numpy.concatenate(kalman_rows)
kalman_3d_frame = kalman_rows['frame']
if start is not None or stop is not None:
if start is None:
start = -numpy.inf
if stop is None:
stop = numpy.inf
valid_cond = ((kalman_3d_frame >= start) &
(kalman_3d_frame <= stop))
kalman_rows = kalman_rows[valid_cond]
kalman_3d_frame = kalman_3d_frame[valid_cond]
obj_ids = kalman_rows['obj_id']
use_obj_ids = numpy.unique(obj_ids)
non_nan_rows = ~np.isnan(kalman_rows['x'])
print('plotting %d Kalman objects' % (len(use_obj_ids), ))
for obj_id in use_obj_ids:
cond = obj_ids == obj_id
cond &= non_nan_rows
x = kalman_rows['x'][cond]
y = kalman_rows['y'][cond]
z = kalman_rows['z'][cond]
w = numpy.ones(x.shape)
X = numpy.vstack((x, y, z, w)).T
frame = kalman_rows['frame'][cond]
#print '%d %d %d'%(frame[0],obj_id, len(frame))
if options.timestamps:
time_est = time_model.framestamp2timestamp(frame)
if kalman_smoothing:
kwprops = dict(lw=0.5)
else:
kwprops = dict(lw=1)
for cam_id in cam_ids:
if cam_id not in R.get_cam_ids():
print(
'no calibration for %s: not showing 3D projections'
% (cam_id, ))
continue
ax = ax_by_cam[cam_id]
x2d = R.find2d(cam_id, X, distorted=True)
## print '%d %d %s (%f,%f)'%(
## obj_id,frame[0],cam_id,x2d[0,0],x2d[1,0])
if options.timestamps:
xdata = time_est
else:
xdata = frame
ax.text(xdata[0], x2d[0, 0], '%d' % obj_id)
thisline, = ax.plot(xdata,
x2d[0, :],
'b-',
picker=5,
**kwprops) #5pt tolerance
all_kalman_lines[thisline] = obj_id
thisline, = ax.plot(xdata,
x2d[1, :],
'y-',
picker=5,
**kwprops) #5pt tolerance
all_kalman_lines[thisline] = obj_id
ax.set_ylim([-100, 800])
if options.timestamps:
## ax.set_xlim( *time_model.framestamp2timestamp(
## (start_frame, stop_frame) ))
pass
else:
ax.set_xlim((start_frame, stop_frame))
if 1:
ax = ax_by_cam['kalman pmean']
P00 = kalman_rows['P00'][cond]
P11 = kalman_rows['P11'][cond]
P22 = kalman_rows['P22'][cond]
Pmean = numpy.sqrt(P00**2 + P11**2 + P22**2) # variance
std = numpy.sqrt(Pmean) # standard deviation (in meters)
if options.timestamps:
xdata = time_est
else:
xdata = frame
ax.plot(xdata, std, 'k-', **kwprops)
if options.timestamps:
ax.set_xlabel('time (sec)')
timezone = result_utils.get_tz(h5)
df = DateFormatter(timezone)
ax.xaxis.set_major_formatter(
ticker.FuncFormatter(df.format_date))
for label in ax.get_xticklabels():
label.set_rotation(30)
else:
ax.set_xlabel('frame')
ax.xaxis.set_major_formatter(
ticker.FormatStrFormatter("%d"))
ax.yaxis.set_major_formatter(
ticker.FormatStrFormatter("%s"))
if not kalman_smoothing:
# plot 2D data contributing to 3D object
# this is forked from flydra_analysis_plot_kalman_2d.py
kresults = ca.get_pytables_file_by_filename(kalman_filename)
try:
kobs = kresults.root.ML_estimates
except tables.exceptions.NoSuchNodeError:
# backward compatibility
kobs = kresults.root.kalman_observations
kframes = kobs.read(field='frame')
if frame_start is not None:
k_after_start = numpy.nonzero(kframes >= frame_start)[0]
else:
k_after_start = None
if frame_stop is not None:
k_before_stop = numpy.nonzero(kframes <= frame_stop)[0]
else:
k_before_stop = None
if k_after_start is not None and k_before_stop is not None:
k_use_idxs = numpy.intersect1d(k_after_start,
k_before_stop)
elif k_after_start is not None:
k_use_idxs = k_after_start
elif k_before_stop is not None:
k_use_idxs = k_before_stop
else:
k_use_idxs = numpy.arange(kobs.nrows)
obs_2d_idxs = kobs.read(field='obs_2d_idx')[k_use_idxs]
kframes = kframes[k_use_idxs]
try:
kobs_2d = kresults.root.ML_estimates_2d_idxs
except tables.exceptions.NoSuchNodeError:
# backwards compatibility
kobs_2d = kresults.root.kalman_observations_2d_idxs
# this will be slooow...
used_cam_ids = collections.defaultdict(list)
for obs_2d_idx, kframe in zip(obs_2d_idxs, kframes):
obs_2d_row = kobs_2d[int(obs_2d_idx)]
#print kframe,obs_2d_row
for camn in obs_2d_row[::2]:
try:
cam_id = camn2cam_id[camn]
except KeyError:
cam_id = None
if cam_id is not None:
used_cam_ids[cam_id].append(kframe)
for cam_id, kframes_used in used_cam_ids.iteritems():
kframes_used = numpy.array(kframes_used)
yval = -99 * numpy.ones_like(kframes_used)
ax = ax_by_cam[cam_id]
if options.timestamps:
ax.plot(time_model.framestamp2timestamp(kframes_used),
yval, 'kx')
else:
ax.plot(kframes_used, yval, 'kx')
ax.set_xlim((start_frame, stop_frame))
ax.set_ylim([-100, 800])
def doit(
filename,
obj_only=None,
min_length=10,
use_kalman_smoothing=True,
data_fps=100.0,
save_fps=25,
vertical_scale=False,
max_vel="auto",
draw_stim_func_str=None,
floor=True,
animation_path_fname=None,
output_dir=".",
cam_only_move_duration=5.0,
options=None,
):
if not use_kalman_smoothing:
if dynamic_model_name is not None:
print(
"ERROR: disabling Kalman smoothing (--disable-kalman-smoothing) is incompatable with setting dynamic model option (--dynamic-model)",
file=sys.stderr,
)
sys.exit(1)
dynamic_model_name = options.dynamic_model
if animation_path_fname is None:
animation_path_fname = pkg_resources.resource_filename(
__name__, "kdmovie_saver_default_path.kmp"
)
camera_animation_path = AnimationPath(animation_path_fname)
mat_data = None
try:
try:
data_path, data_filename = os.path.split(filename)
data_path = os.path.expanduser(data_path)
sys.path.insert(0, data_path)
mat_data = scipy.io.mio.loadmat(data_filename)
finally:
del sys.path[0]
except IOError as err:
print(
"not a .mat file at %s, treating as .hdf5 file"
% (os.path.join(data_path, data_filename))
)
ca = core_analysis.get_global_CachingAnalyzer()
obj_ids, use_obj_ids, is_mat_file, data_file, extra = ca.initial_file_load(filename)
if obj_only is not None:
use_obj_ids = obj_only
if options.stim_xml is not None:
file_timestamp = data_file.filename[4:19]
stim_xml = xml_stimulus.xml_stimulus_from_filename(
options.stim_xml, timestamp_string=file_timestamp,
)
try:
fanout = xml_stimulus.xml_fanout_from_filename(options.stim_xml)
except xml_stimulus.WrongXMLTypeError:
pass
else:
include_obj_ids, exclude_obj_ids = fanout.get_obj_ids_for_timestamp(
timestamp_string=file_timestamp
)
if include_obj_ids is not None:
use_obj_ids = include_obj_ids
if exclude_obj_ids is not None:
use_obj_ids = list(set(use_obj_ids).difference(exclude_obj_ids))
print("using object ids specified in fanout .xml file")
if dynamic_model_name is None:
if "dynamic_model_name" in extra:
dynamic_model_name = extra["dynamic_model_name"]
print('detected file loaded with dynamic model "%s"' % dynamic_model_name)
if dynamic_model_name.startswith("EKF "):
dynamic_model_name = dynamic_model_name[4:]
print(' for smoothing, will use dynamic model "%s"' % dynamic_model_name)
else:
print(
"no dynamic model name specified, and it could not be determined from the file, either"
)
filename_trimmed = os.path.split(os.path.splitext(filename)[0])[-1]
assert use_obj_ids is not None
#################
rw = tvtk.RenderWindow(size=(1024, 768))
ren = tvtk.Renderer(background=(1.0, 1.0, 1.0))
camera = ren.active_camera
rw.add_renderer(ren)
lut = tvtk.LookupTable(hue_range=(0.667, 0.0))
#################
if not os.path.exists(output_dir):
os.mkdir(output_dir)
if len(use_obj_ids) == 1:
animate_path = True
# allow path to grow during trajectory
else:
animate_path = False
obj_verts = []
speeds = []
for obj_id in use_obj_ids:
print("loading %d" % obj_id)
results = ca.calculate_trajectory_metrics(
obj_id,
data_file,
use_kalman_smoothing=use_kalman_smoothing,
frames_per_second=data_fps,
dynamic_model_name=dynamic_model_name,
# method='position based',
method_params={"downsample": 1,},
)
if len(use_obj_ids) == 1:
obj_verts = results["X_kalmanized"]
speeds = results["speed_kalmanized"]
real_frames = results["frame"]
else:
obj_verts.append(results["X_kalmanized"])
speeds.append(results["speed_kalmanized"])
real_frames.append(results["frame"])
if options.start is not None:
good_cond = real_frames >= options.start
obj_verts = obj_verts[good_cond]
speeds = speeds[good_cond]
real_frames = real_frames[good_cond]
if not len(use_obj_ids) == 1:
obj_verts = numpy.concatenate(obj_verts, axis=0)
speeds = numpy.concatenate(speeds, axis=0)
real_frames = numpy.concatenate(real_frames, axis=0)
####################### start draw permanently on stuff ############################
if options.stim_xml is not None:
if not is_mat_file:
R = reconstruct.Reconstructor(data_file)
stim_xml.verify_reconstructor(R)
if not is_mat_file:
assert data_file.filename.startswith("DATA") and (
data_file.filename.endswith(".h5")
or data_file.filename.endswith(".kh5")
)
file_timestamp = data_file.filename[4:19]
actors = stim_xml.get_tvtk_actors()
for actor in actors:
ren.add_actor(actor)
if 1:
if 0:
# Inspired by pyface.tvtk.decorated_scene
marker = tvtk.OrientationMarkerWidget()
axes = tvtk.AxesActor()
axes.set(
# normalized_tip_length=(0.04, 0.4, 0.4),
# normalized_shaft_length=(0.6, 0.6, 0.6),
shaft_type="cylinder",
total_length=(0.15, 0.15, 0.15),
)
if 1:
axes.x_axis_label_text = ""
axes.y_axis_label_text = ""
axes.z_axis_label_text = ""
else:
p = axes.x_axis_caption_actor2d.caption_text_property
axes.y_axis_caption_actor2d.caption_text_property = p
axes.z_axis_caption_actor2d.caption_text_property = p
p.color = 0.0, 0.0, 0.0 # black
# axes.camera = camera
# axes.attachment_point_coordinate = (0,0,0)
axes.origin = (-0.5, 1, 0)
if 0:
rwi = rw.interactor
print("rwi", rwi)
marker.orientation_marker = axes
# marker.interactive = False
marker.interactor = rwi
marker.enabled = True
ren.add_actor(axes)
#######################
if max_vel == "auto":
max_vel = speeds.max()
else:
max_vel = float(max_vel)
vel_mapper = tvtk.PolyDataMapper()
vel_mapper.lookup_table = lut
vel_mapper.scalar_range = 0.0, max_vel
if 1:
# Create a scalar bar
if vertical_scale:
scalar_bar = tvtk.ScalarBarActor(
orientation="vertical", width=0.08, height=0.4
)
else:
scalar_bar = tvtk.ScalarBarActor(
orientation="horizontal", width=0.4, height=0.08
)
scalar_bar.title = "Speed (m/s)"
scalar_bar.lookup_table = vel_mapper.lookup_table
scalar_bar.property.color = 0.0, 0.0, 0.0 # black
scalar_bar.title_text_property.color = 0.0, 0.0, 0.0
scalar_bar.title_text_property.shadow = False
scalar_bar.label_text_property.color = 0.0, 0.0, 0.0
scalar_bar.label_text_property.shadow = False
scalar_bar.position_coordinate.coordinate_system = "normalized_viewport"
if vertical_scale:
scalar_bar.position_coordinate.value = 0.01, 0.01, 0.0
else:
scalar_bar.position_coordinate.value = 0.1, 0.01, 0.0
ren.add_actor(scalar_bar)
imf = tvtk.WindowToImageFilter(input=rw, read_front_buffer="off")
writer = tvtk.PNGWriter()
####################### end draw permanently on stuff ############################
save_dt = 1.0 / save_fps
if animate_path:
data_dt = 1.0 / data_fps
n_frames = len(obj_verts)
dur = n_frames * data_dt
else:
data_dt = 0.0
dur = 0.0
print("data_fps", data_fps)
print("data_dt", data_dt)
print("save_fps", save_fps)
t_now = 0.0
frame_number = 0
while t_now <= dur:
frame_number += 1
t_now += save_dt
print("t_now", t_now)
pos, ori = camera_animation_path.get_pos_ori(t_now)
focal_point, view_up = pos_ori2fu(pos, ori)
camera.position = tuple(pos)
# camera.focal_point = (focal_point[0], focal_point[1], focal_point[2])
# camera.view_up = (view_up[0], view_up[1], view_up[2])
camera.focal_point = tuple(focal_point)
camera.view_up = tuple(view_up)
if data_dt != 0.0:
draw_n_frames = int(round(t_now / data_dt))
else:
draw_n_frames = len(obj_verts)
print("frame_number, draw_n_frames", frame_number, draw_n_frames)
#################
pd = tvtk.PolyData()
pd.points = obj_verts[:draw_n_frames]
real_frame_number = real_frames[:draw_n_frames][-1]
pd.point_data.scalars = speeds
if numpy.any(speeds > max_vel):
print(
"WARNING: maximum speed (%.3f m/s) exceeds color map max"
% (speeds.max(),)
)
g = tvtk.Glyph3D(
scale_mode="data_scaling_off", vector_mode="use_vector", input=pd
)
vel_mapper.input = g.output
ss = tvtk.SphereSource(radius=options.radius)
g.source = ss.output
a = tvtk.Actor(mapper=vel_mapper)
##################
ren.add_actor(a)
if 1:
imf.update()
imf.modified()
writer.input = imf.output
# fname = 'movie_%s_%03d_frame%05d.png'%(filename_trimmed,obj_id,frame_number)
fname = "movie_%s_%03d_frame%05d.png" % (
filename_trimmed,
obj_id,
real_frame_number,
)
print("saving", fname)
full_fname = os.path.join(output_dir, fname)
writer.file_name = full_fname
writer.write()
ren.remove_actor(a)
ren.add_actor(a) # restore actors removed
dur = dur + cam_only_move_duration
while t_now < dur:
frame_number += 1
t_now += save_dt
print("t_now", t_now)
pos, ori = camera_animation_path.get_pos_ori(t_now)
focal_point, view_up = pos_ori2fu(pos, ori)
camera.position = tuple(pos)
camera.focal_point = tuple(focal_point)
camera.view_up = tuple(view_up)
if 1:
imf.update()
imf.modified()
writer.input = imf.output
if len(use_obj_ids) == 1:
fname = "movie_%s_%03d_frame%05d.png" % (
filename_trimmed,
obj_id,
frame_number,
)
else:
fname = "movie_%s_many_frame%05d.png" % (filename_trimmed, frame_number)
full_fname = os.path.join(output_dir, fname)
writer.file_name = full_fname
writer.write()
if not is_mat_file:
data_file.close()
def doit(output_h5_filename=None,
kalman_filename=None,
data2d_filename=None,
start=None,
stop=None,
gate_angle_threshold_degrees=40.0,
area_threshold_for_orientation=0.0,
obj_only=None,
options=None):
gate_angle_threshold_radians = gate_angle_threshold_degrees * D2R
if options.show:
import matplotlib.pyplot as plt
import matplotlib.ticker as mticker
M = SymobolicModels()
x = sympy.DeferredVector('x')
G_symbolic = M.get_observation_model(x)
dx_symbolic = M.get_process_model(x)
if 0:
print 'G_symbolic'
sympy.pprint(G_symbolic)
print
G_linearized = [G_symbolic.diff(x[i]) for i in range(7)]
if 0:
print 'G_linearized'
for i in range(len(G_linearized)):
sympy.pprint(G_linearized[i])
print
arg_tuple_x = (M.P00, M.P01, M.P02, M.P03, M.P10, M.P11, M.P12, M.P13,
M.P20, M.P21, M.P22, M.P23, M.Ax, M.Ay, M.Az, x)
xm = sympy.DeferredVector('xm')
arg_tuple_x_xm = (M.P00, M.P01, M.P02, M.P03, M.P10, M.P11, M.P12, M.P13,
M.P20, M.P21, M.P22, M.P23, M.Ax, M.Ay, M.Az, x, xm)
eval_G = lambdify(arg_tuple_x, G_symbolic, 'numpy')
eval_linG = lambdify(arg_tuple_x, G_linearized, 'numpy')
# coord shift of observation model
phi_symbolic = M.get_observation_model(xm)
# H = G - phi
H_symbolic = G_symbolic - phi_symbolic
# We still take derivative wrt x (not xm).
H_linearized = [H_symbolic.diff(x[i]) for i in range(7)]
eval_phi = lambdify(arg_tuple_x_xm, phi_symbolic, 'numpy')
eval_H = lambdify(arg_tuple_x_xm, H_symbolic, 'numpy')
eval_linH = lambdify(arg_tuple_x_xm, H_linearized, 'numpy')
if 0:
print 'dx_symbolic'
sympy.pprint(dx_symbolic)
print
eval_dAdt = drop_dims(lambdify(x, dx_symbolic, 'numpy'))
debug_level = 0
if debug_level:
np.set_printoptions(linewidth=130, suppress=True)
if os.path.exists(output_h5_filename):
raise RuntimeError("will not overwrite old file '%s'" %
output_h5_filename)
ca = core_analysis.get_global_CachingAnalyzer()
with open_file_safe(output_h5_filename, mode='w') as output_h5:
with open_file_safe(kalman_filename, mode='r') as kh5:
with open_file_safe(data2d_filename, mode='r') as h5:
for input_node in kh5.root._f_iter_nodes():
# copy everything from source to dest
input_node._f_copy(output_h5.root, recursive=True)
try:
dest_table = output_h5.root.ML_estimates
except tables.exceptions.NoSuchNodeError, err1:
# backwards compatibility
try:
dest_table = output_h5.root.kalman_observations
except tables.exceptions.NoSuchNodeError, err2:
raise err1
for colname in ['hz_line%d' % i for i in range(6)]:
clear_col(dest_table, colname)
dest_table.flush()
if options.show:
fig1 = plt.figure()
ax1 = fig1.add_subplot(511)
ax2 = fig1.add_subplot(512, sharex=ax1)
ax3 = fig1.add_subplot(513, sharex=ax1)
ax4 = fig1.add_subplot(514, sharex=ax1)
ax5 = fig1.add_subplot(515, sharex=ax1)
ax1.xaxis.set_major_formatter(
mticker.FormatStrFormatter("%d"))
min_frame_range = np.inf
max_frame_range = -np.inf
reconst = reconstruct.Reconstructor(kh5)
camn2cam_id, cam_id2camns = result_utils.get_caminfo_dicts(h5)
fps = result_utils.get_fps(h5)
dt = 1.0 / fps
used_camn_dict = {}
# associate framenumbers with timestamps using 2d .h5 file
data2d = h5.root.data2d_distorted[:] # load to RAM
if start is not None:
data2d = data2d[data2d['frame'] >= start]
if stop is not None:
data2d = data2d[data2d['frame'] <= stop]
data2d_idxs = np.arange(len(data2d))
h5_framenumbers = data2d['frame']
h5_frame_qfi = result_utils.QuickFrameIndexer(h5_framenumbers)
ML_estimates_2d_idxs = (kh5.root.ML_estimates_2d_idxs[:])
all_kobs_obj_ids = dest_table.read(field='obj_id')
all_kobs_frames = dest_table.read(field='frame')
use_obj_ids = np.unique(all_kobs_obj_ids)
if obj_only is not None:
use_obj_ids = obj_only
if hasattr(kh5.root.kalman_estimates.attrs,
'dynamic_model_name'):
dynamic_model = kh5.root.kalman_estimates.attrs.dynamic_model_name
if dynamic_model.startswith('EKF '):
dynamic_model = dynamic_model[4:]
else:
dynamic_model = 'mamarama, units: mm'
warnings.warn(
'could not determine dynamic model name, using "%s"' %
dynamic_model)
for obj_id_enum, obj_id in enumerate(use_obj_ids):
# Use data association step from kalmanization to load potentially
# relevant 2D orientations, but discard previous 3D orientation.
if obj_id_enum % 100 == 0:
print 'obj_id %d (%d of %d)' % (obj_id, obj_id_enum,
len(use_obj_ids))
if options.show:
all_xhats = []
all_ori = []
output_row_obj_id_cond = all_kobs_obj_ids == obj_id
obj_3d_rows = ca.load_dynamics_free_MLE_position(
obj_id, kh5)
if start is not None:
obj_3d_rows = obj_3d_rows[
obj_3d_rows['frame'] >= start]
if stop is not None:
obj_3d_rows = obj_3d_rows[obj_3d_rows['frame'] <= stop]
try:
smoothed_3d_rows = ca.load_data(
obj_id,
kh5,
use_kalman_smoothing=True,
frames_per_second=fps,
dynamic_model_name=dynamic_model)
except core_analysis.NotEnoughDataToSmoothError:
continue
smoothed_frame_qfi = result_utils.QuickFrameIndexer(
smoothed_3d_rows['frame'])
slopes_by_camn_by_frame = collections.defaultdict(dict)
x0d_by_camn_by_frame = collections.defaultdict(dict)
y0d_by_camn_by_frame = collections.defaultdict(dict)
pt_idx_by_camn_by_frame = collections.defaultdict(dict)
min_frame = np.inf
max_frame = -np.inf
start_idx = None
for this_idx, this_3d_row in enumerate(obj_3d_rows):
# iterate over each sample in the current camera
framenumber = this_3d_row['frame']
if not np.isnan(this_3d_row['hz_line0']):
# We have a valid initial 3d orientation guess.
if framenumber < min_frame:
min_frame = framenumber
assert start_idx is None, "frames out of order?"
start_idx = this_idx
max_frame = max(max_frame, framenumber)
h5_2d_row_idxs = h5_frame_qfi.get_frame_idxs(
framenumber)
frame2d = data2d[h5_2d_row_idxs]
frame2d_idxs = data2d_idxs[h5_2d_row_idxs]
obs_2d_idx = this_3d_row['obs_2d_idx']
kobs_2d_data = ML_estimates_2d_idxs[int(obs_2d_idx)]
# Parse VLArray.
this_camns = kobs_2d_data[0::2]
this_camn_idxs = kobs_2d_data[1::2]
# Now, for each camera viewing this object at this
# frame, extract images.
for camn, camn_pt_no in zip(this_camns,
this_camn_idxs):
# find 2D point corresponding to object
cam_id = camn2cam_id[camn]
cond = ((frame2d['camn'] == camn) &
(frame2d['frame_pt_idx'] == camn_pt_no))
idxs = np.nonzero(cond)[0]
if len(idxs) == 0:
continue
assert len(idxs) == 1
## if len(idxs)!=1:
## raise ValueError('expected one (and only one) frame, got %d'%len(idxs))
idx = idxs[0]
orig_data2d_rownum = frame2d_idxs[idx]
frame_timestamp = frame2d[idx]['timestamp']
row = frame2d[idx]
assert framenumber == row['frame']
if ((row['eccentricity'] <
reconst.minimum_eccentricity)
or (row['area'] <
area_threshold_for_orientation)):
slopes_by_camn_by_frame[camn][
framenumber] = np.nan
x0d_by_camn_by_frame[camn][
framenumber] = np.nan
y0d_by_camn_by_frame[camn][
framenumber] = np.nan
pt_idx_by_camn_by_frame[camn][
framenumber] = camn_pt_no
else:
slopes_by_camn_by_frame[camn][
framenumber] = row['slope']
x0d_by_camn_by_frame[camn][framenumber] = row[
'x']
y0d_by_camn_by_frame[camn][framenumber] = row[
'y']
pt_idx_by_camn_by_frame[camn][
framenumber] = camn_pt_no
if start_idx is None:
warnings.warn("skipping obj_id %d: "
"could not find valid start frame" %
obj_id)
continue
obj_3d_rows = obj_3d_rows[start_idx:]
# now collect in a numpy array for all cam
assert int(min_frame) == min_frame
assert int(max_frame + 1) == max_frame + 1
frame_range = np.arange(int(min_frame), int(max_frame + 1))
if debug_level >= 1:
print 'frame range %d-%d' % (frame_range[0],
frame_range[-1])
camn_list = slopes_by_camn_by_frame.keys()
camn_list.sort()
cam_id_list = [camn2cam_id[camn] for camn in camn_list]
n_cams = len(camn_list)
n_frames = len(frame_range)
save_cols = {}
save_cols['frame'] = []
for camn in camn_list:
save_cols['dist%d' % camn] = []
save_cols['used%d' % camn] = []
save_cols['theta%d' % camn] = []
# NxM array with rows being frames and cols being cameras
slopes = np.ones((n_frames, n_cams), dtype=np.float)
x0ds = np.ones((n_frames, n_cams), dtype=np.float)
y0ds = np.ones((n_frames, n_cams), dtype=np.float)
for j, camn in enumerate(camn_list):
slopes_by_frame = slopes_by_camn_by_frame[camn]
x0d_by_frame = x0d_by_camn_by_frame[camn]
y0d_by_frame = y0d_by_camn_by_frame[camn]
for frame_idx, absolute_frame_number in enumerate(
frame_range):
slopes[frame_idx, j] = slopes_by_frame.get(
absolute_frame_number, np.nan)
x0ds[frame_idx,
j] = x0d_by_frame.get(absolute_frame_number,
np.nan)
y0ds[frame_idx,
j] = y0d_by_frame.get(absolute_frame_number,
np.nan)
if options.show:
frf = np.array(frame_range, dtype=np.float)
min_frame_range = min(np.min(frf), min_frame_range)
max_frame_range = max(np.max(frf), max_frame_range)
ax1.plot(frame_range,
slope2modpi(slopes[:, j]),
'.',
label=camn2cam_id[camn])
if options.show:
ax1.legend()
if 1:
# estimate orientation of initial frame
row0 = obj_3d_rows[:
1] # take only first row but keep as 1d array
hzlines = np.array([
row0['hz_line0'], row0['hz_line1'],
row0['hz_line2'], row0['hz_line3'],
row0['hz_line4'], row0['hz_line5']
]).T
directions = reconstruct.line_direction(hzlines)
q0 = PQmath.orientation_to_quat(directions[0])
assert not np.isnan(
q0.x), "cannot start with missing orientation"
w0 = 0, 0, 0 # no angular rate
init_x = np.array(
[w0[0], w0[1], w0[2], q0.x, q0.y, q0.z, q0.w])
Pminus = np.zeros((7, 7))
# angular rate part of state variance is .5
for i in range(0, 3):
Pminus[i, i] = .5
# quaternion part of state variance is 1
for i in range(3, 7):
Pminus[i, i] = 1
if 1:
# setup of noise estimates
Q = np.zeros((7, 7))
# angular rate part of state variance
for i in range(0, 3):
Q[i, i] = Q_scalar_rate
# quaternion part of state variance
for i in range(3, 7):
Q[i, i] = Q_scalar_quat
preA = np.eye(7)
ekf = kalman_ekf.EKF(init_x, Pminus)
previous_posterior_x = init_x
if options.show:
_save_plot_rows = []
_save_plot_rows_used = []
for frame_idx, absolute_frame_number in enumerate(
frame_range):
# Evaluate the Jacobian of the process update
# using previous frame's posterior estimate. (This
# is not quite the same as this frame's prior
# estimate. The difference this frame's prior
# estimate is _after_ the process update
# model. Which we need to get doing this.)
if options.show:
_save_plot_rows.append(np.nan * np.ones(
(n_cams, )))
_save_plot_rows_used.append(np.nan * np.ones(
(n_cams, )))
this_dx = eval_dAdt(previous_posterior_x)
A = preA + this_dx * dt
if debug_level >= 1:
print
print 'frame', absolute_frame_number, '-' * 40
print 'previous posterior', previous_posterior_x
if debug_level > 6:
print 'A'
print A
xhatminus, Pminus = ekf.step1__calculate_a_priori(A, Q)
if debug_level >= 1:
print 'new prior', xhatminus
# 1. Gate per-camera orientations.
this_frame_slopes = slopes[frame_idx, :]
this_frame_theta_measured = slope2modpi(
this_frame_slopes)
this_frame_x0d = x0ds[frame_idx, :]
this_frame_y0d = y0ds[frame_idx, :]
if debug_level >= 5:
print 'this_frame_slopes', this_frame_slopes
save_cols['frame'].append(absolute_frame_number)
for j, camn in enumerate(camn_list):
# default to no detection, change below
save_cols['dist%d' % camn].append(np.nan)
save_cols['used%d' % camn].append(0)
save_cols['theta%d' % camn].append(
this_frame_theta_measured[j])
all_data_this_frame_missing = False
gate_vector = None
y = [] # observation (per camera)
hx = [] # expected observation (per camera)
C = [] # linearized observation model (per camera)
N_obs_this_frame = 0
cams_without_data = np.isnan(this_frame_slopes)
if np.all(cams_without_data):
all_data_this_frame_missing = True
smoothed_pos_idxs = smoothed_frame_qfi.get_frame_idxs(
absolute_frame_number)
if len(smoothed_pos_idxs) == 0:
all_data_this_frame_missing = True
smoothed_pos_idx = None
smooth_row = None
center_position = None
else:
try:
assert len(smoothed_pos_idxs) == 1
except:
print 'obj_id', obj_id
print 'absolute_frame_number', absolute_frame_number
if len(frame_range):
print 'frame_range[0],frame_rang[-1]', frame_range[
0], frame_range[-1]
else:
print 'no frame range'
print 'len(smoothed_pos_idxs)', len(
smoothed_pos_idxs)
raise
smoothed_pos_idx = smoothed_pos_idxs[0]
smooth_row = smoothed_3d_rows[smoothed_pos_idx]
assert smooth_row['frame'] == absolute_frame_number
center_position = np.array(
(smooth_row['x'], smooth_row['y'],
smooth_row['z']))
if debug_level >= 2:
print 'center_position', center_position
if not all_data_this_frame_missing:
if expected_orientation_method == 'trust_prior':
state_for_phi = xhatminus # use a priori
elif expected_orientation_method == 'SVD_line_fits':
# construct matrix of planes
P = []
for camn_idx in range(n_cams):
this_x0d = this_frame_x0d[camn_idx]
this_y0d = this_frame_y0d[camn_idx]
slope = this_frame_slopes[camn_idx]
plane, ray = reconst.get_3D_plane_and_ray(
cam_id, this_x0d, this_y0d, slope)
if np.isnan(plane[0]):
continue
P.append(plane)
if len(P) < 2:
# not enough data to do SVD... fallback to prior
state_for_phi = xhatminus # use a priori
else:
Lco = reconstruct.intersect_planes_to_find_line(
P)
q = PQmath.pluecker_to_quat(Lco)
state_for_phi = cgtypes_quat2statespace(q)
cams_with_data = ~cams_without_data
possible_cam_idxs = np.nonzero(cams_with_data)[0]
if debug_level >= 6:
print 'possible_cam_idxs', possible_cam_idxs
gate_vector = np.zeros((n_cams, ), dtype=np.bool)
## flip_vector = np.zeros( (n_cams,), dtype=np.bool)
for camn_idx in possible_cam_idxs:
cam_id = cam_id_list[camn_idx]
camn = camn_list[camn_idx]
# This ignores distortion. To incorporate
# distortion, this would require
# appropriate scaling of orientation
# vector, which would require knowing
# target's size. In which case we should
# track head and tail separately and not
# use this whole quaternion mess.
## theta_measured=slope2modpi(
## this_frame_slopes[camn_idx])
theta_measured = this_frame_theta_measured[
camn_idx]
if debug_level >= 6:
print 'cam_id %s, camn %d' % (cam_id, camn)
if debug_level >= 3:
a = reconst.find2d(cam_id, center_position)
other_position = get_point_on_line(
xhatminus, center_position)
b = reconst.find2d(cam_id, other_position)
theta_expected = find_theta_mod_pi_between_points(
a, b)
print(' theta_expected,theta_measured',
theta_expected * R2D,
theta_measured * R2D)
P = reconst.get_pmat(cam_id)
if 0:
args_x = (P[0, 0], P[0, 1], P[0, 2],
P[0, 3], P[1, 0], P[1, 1], P[1,
2],
P[1, 3], P[2, 0], P[2, 1], P[2,
2],
P[2, 3], center_position[0],
center_position[1],
center_position[2], xhatminus)
this_y = theta_measured
this_hx = eval_G(*args_x)
this_C = eval_linG(*args_x)
else:
args_x_xm = (P[0, 0], P[0, 1], P[0, 2],
P[0, 3], P[1, 0], P[1,
1], P[1,
2],
P[1, 3], P[2, 0], P[2,
1], P[2,
2],
P[2, 3], center_position[0],
center_position[1],
center_position[2], xhatminus,
state_for_phi)
this_phi = eval_phi(*args_x_xm)
this_y = angle_diff(theta_measured,
this_phi,
mod_pi=True)
this_hx = eval_H(*args_x_xm)
this_C = eval_linH(*args_x_xm)
if debug_level >= 3:
print(' this_phi,this_y',
this_phi * R2D, this_y * R2D)
save_cols['dist%d' % camn][-1] = this_y # save
# gate
if abs(this_y) < gate_angle_threshold_radians:
save_cols['used%d' % camn][-1] = 1
gate_vector[camn_idx] = 1
if debug_level >= 3:
print ' good'
if options.show:
_save_plot_rows_used[-1][
camn_idx] = this_y
y.append(this_y)
hx.append(this_hx)
C.append(this_C)
N_obs_this_frame += 1
# Save which camn and camn_pt_no was used.
if absolute_frame_number not in used_camn_dict:
used_camn_dict[
absolute_frame_number] = []
camn_pt_no = (pt_idx_by_camn_by_frame[camn]
[absolute_frame_number])
used_camn_dict[
absolute_frame_number].append(
(camn, camn_pt_no))
else:
if options.show:
_save_plot_rows[-1][camn_idx] = this_y
if debug_level >= 6:
print ' bad'
if debug_level >= 1:
print 'gate_vector', gate_vector
#print 'flip_vector',flip_vector
all_data_this_frame_missing = not bool(
np.sum(gate_vector))
# 3. Construct observations model using all
# gated-in camera orientations.
if all_data_this_frame_missing:
C = None
R = None
hx = None
else:
C = np.array(C)
R = R_scalar * np.eye(N_obs_this_frame)
hx = np.array(hx)
if 0:
# crazy observation error scaling
for i in range(N_obs_this_frame):
beyond = abs(y[i]) - 10 * D2R
beyond = max(0, beyond) # clip at zero
R[i:i] = R_scalar * (1 + 10 * beyond)
if debug_level >= 6:
print 'full values'
print 'C', C
print 'hx', hx
print 'y', y
print 'R', R
if debug_level >= 1:
print 'all_data_this_frame_missing', all_data_this_frame_missing
xhat, P = ekf.step2__calculate_a_posteriori(
xhatminus,
Pminus,
y=y,
hx=hx,
C=C,
R=R,
missing_data=all_data_this_frame_missing)
if debug_level >= 1:
print 'xhat', xhat
previous_posterior_x = xhat
if center_position is not None:
# save
output_row_frame_cond = all_kobs_frames == absolute_frame_number
output_row_cond = output_row_frame_cond & output_row_obj_id_cond
output_idxs = np.nonzero(output_row_cond)[0]
if len(output_idxs) == 0:
pass
else:
assert len(output_idxs) == 1
idx = output_idxs[0]
hz = state_to_hzline(xhat, center_position)
for row in dest_table.iterrows(start=idx,
stop=(idx + 1)):
for i in range(6):
row['hz_line%d' % i] = hz[i]
row.update()
## xhat_results[ obj_id ][absolute_frame_number ] = (
## xhat,center_position)
if options.show:
all_xhats.append(xhat)
all_ori.append(state_to_ori(xhat))
# save to H5 file
names = [colname for colname in save_cols]
names.sort()
arrays = []
for name in names:
if name == 'frame':
dtype = np.int64
elif name.startswith('dist'):
dtype = np.float32
elif name.startswith('used'):
dtype = np.bool
elif name.startswith('theta'):
dtype = np.float32
else:
raise NameError('unknown name %s' % name)
arr = np.array(save_cols[name], dtype=dtype)
arrays.append(arr)
save_recarray = np.rec.fromarrays(arrays, names=names)
h5group = core_analysis.get_group_for_obj(obj_id,
output_h5,
writeable=True)
output_h5.create_table(h5group,
'obj%d' % obj_id,
save_recarray,
filters=tables.Filters(
1, complib='lzo'))
if options.show:
all_xhats = np.array(all_xhats)
all_ori = np.array(all_ori)
_save_plot_rows = np.array(_save_plot_rows)
_save_plot_rows_used = np.array(_save_plot_rows_used)
ax2.plot(frame_range, all_xhats[:, 0], '.', label='p')
ax2.plot(frame_range, all_xhats[:, 1], '.', label='q')
ax2.plot(frame_range, all_xhats[:, 2], '.', label='r')
ax2.legend()
ax3.plot(frame_range, all_xhats[:, 3], '.', label='a')
ax3.plot(frame_range, all_xhats[:, 4], '.', label='b')
ax3.plot(frame_range, all_xhats[:, 5], '.', label='c')
ax3.plot(frame_range, all_xhats[:, 6], '.', label='d')
ax3.legend()
ax4.plot(frame_range, all_ori[:, 0], '.', label='x')
ax4.plot(frame_range, all_ori[:, 1], '.', label='y')
ax4.plot(frame_range, all_ori[:, 2], '.', label='z')
ax4.legend()
colors = []
for i in range(n_cams):
line, = ax5.plot(frame_range,
_save_plot_rows_used[:, i] * R2D,
'o',
label=cam_id_list[i])
colors.append(line.get_color())
for i in range(n_cams):
# loop again to get normal MPL color cycling
ax5.plot(frame_range,
_save_plot_rows[:, i] * R2D,
'o',
mec=colors[i],
ms=1.0)
ax5.set_ylabel('observation (deg)')
ax5.legend()
