def get_lines(self):
x1 = (0.61809734343982814, -0.013406307631868021, -0.0035917469609576813)
x2 = (1.198826951196984, -0.012485555989418704, 0.0025602971809217849)
line_mark = geom.line_from_points(geom.ThreeTuple(x1), geom.ThreeTuple(x2))
#line_mark = geom.line_from_HZline( (0.02439605071482559, 0.051420313894467287, -0.99564385219908413, -0.00097158319016768459, 0.015447764310623923, -0.072211676414489459))
#(-0.061248834007043639, -0.25331384274265706, 0.92406568281060819, 0.0036304349314720606, -0.053126804348089779, 0.27449527110946625))
#(0.0097110036703961945, 0.071801811107512326, -0.98489513490720026, 0.00048467528993275467, 0.027587746154489213, -0.15482393522159865))
#(-0.072175447568079532, -0.31684664370365356, 0.86817678663756903, 0.0048959408494628444, -0.070490367875258558, 0.36834102959859433))
#(0.026432522291979302, 0.066719950163430089, -0.98930078556948686, -0.0017052044659353833, 0.024116927663042458, -0.12469639836376363))
#(0.036051887242803483, 0.1225934342601759, -0.97241823143838102, -0.0025815311891907609, 0.035919256583691377, -0.19177340058510137))
line_clean = geom.line_from_HZline( (-0.27936512712372191, 0.04624199569732089, -0.9586296181568541, -0.0080073601565274575, 0.0023435304010169695, 0.027864831277130742))
x1=(0.49949904826195857, -0.012373271634116311, 0.32286252608243299)
x2=(0.38901860101731867, -0.01294145950243236, 0.32642509461777808)
top_mark_line = geom.line_from_points(geom.ThreeTuple(x1), geom.ThreeTuple(x2))
#top_mark_line= geom.line_from_HZline((0.18050156903291037, 0.31868846699973336, -0.73306806601973318, -0.065965871714262242, 0.15803989259450685, -0.54693688052443501))
#(0.17074976404913067, 0.32855038182801033, -0.67404658417915053, -0.072283972587872641, 0.16975362231152435, -0.61197965820220468))
#(0.17544381933023984, 0.49024035729704918, -0.56597775796347805, -0.05688098204074131, 0.1552881996760179, -0.61741652451985551))
top_clean_line = geom.line_from_HZline((-0.26575764351965903, -0.29763801029854864, -0.91692940763466335, -0.0018355385669680962, 0.0018496262007779841, 0.0042615527956441791))
verts = []
lines = []
count = 0
for line in [line_mark, line_clean, top_mark_line, top_clean_line]:
this_verts = []
this_lines = []
d = line.u
direction = numpy.array([d[0],d[1],d[2]])
norm = direction / numpy.sqrt( numpy.sum(direction**2) )
d = line.closest()
pt0 = numpy.array([d[0],d[1],d[2]])
for d in numpy.linspace(-2,2,100):
this_verts.append( pt0 + d*norm )
for i in range(len(this_verts)-1):
this_lines.append( [count+i, count+i+1] )
count += len(this_verts)
## print 'this_verts[0]',this_verts[0]
## print 'this_verts[-1]',this_verts[-1]
## print
verts.extend( this_verts )
lines.extend( this_lines )
# make homogeneous
verts = [ numpy.array((v[0], v[1], v[2], 1.0)) for v in verts ]
## print 'verts\n',
## for v in verts:
## print v
## print 'lines\n',
## for l in lines:
## print l
return verts, lines
def tst_line_closest1(self, geom):
if geom is fastgeom:
return # not implemented
xaxis = geom.line_from_points(geom.ThreeTuple((0, 0, 0)),
geom.ThreeTuple((1, 0, 0)))
zline = geom.line_from_points(geom.ThreeTuple((.5, 0, 0)),
geom.ThreeTuple((.5, 0, 1)))
result = xaxis.get_my_point_closest_to_line(zline)
eps = 1e-10
print 'result', result
assert result.dist_from(geom.ThreeTuple((0.5, 0, 0))) < eps
def tstXX(self, geom, x1, y1, z1, x2):
pt_a = [x1, y1, z1, 1]
pt_b = [x2, 5, 6, 1]
hz_p = reconstruct.pluecker_from_verts(pt_a, pt_b)
a = geom.ThreeTuple(pt_a[:3])
b = geom.ThreeTuple(pt_b[:3])
L = geom.line_from_points(a, b)
hzL = geom.line_from_HZline(hz_p)
strL = str(L)
strhzL = str(hzL)
assert strL == strhzL
if 0:
print("hz_p", hz_p)
print("correct", L)
print("test ", hzL)
print()
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 tst_line_from_points(self, geom):
line = geom.line_from_points(
geom.ThreeTuple((2, 1, 0)), geom.ThreeTuple((2, 0, 0))
)
line.closest()
line.dist2()
def state_to_hzline(x, A):
Ax, Ay, Az = A[0], A[1], A[2]
Ux, Uy, Uz = state_to_ori(x)
line = geom.line_from_points(geom.ThreeTuple((Ax, Ay, Az)),
geom.ThreeTuple((Ax + Ux, Ay + Uy, Az + Uz)))
return line.to_hz()
