def clean_depth_buffer(coord, mat, depths, kernel_size=2):
'''Given camera calibration data, clean the geometry using a median filter.'''
# step 1: depth per pixel
K, RT, P, ks, T, wh = mat
h, w = coord.shape[:2]
coord = coord.copy()
Calibrate.undistort_points_mat(coord.reshape(-1, 2), mat,
coord.reshape(_1, 2))
shape = [h, w, 3]
which = np.where(depths.reshape(-1) == 0)[0]
depths.reshape(-1)[which] = 0
# step 2: median filter
filtered_depths = 1e20 * np.ones([h, w, 1], dtype=np.float32)
filtered_depths.reshape(-1)[:] = depths.reshape(-1)
filtered_depths.reshape(-1)[which] = 1e20
if 0:
for y in range(kernel_size, h - kernel_size):
for x in range(kernel_size, w - kernel_size):
d = depths[y - kernel_size:y + kernel_size + 1,
x - kernel_size:x + kernel_size + 1].reshape(-1)
which = np.where(d != 0)[0]
if len(which): filtered_depths[y, x] = np.median(d[which])
#filtered_depths[:,:,0] = depths # HACK, now the results should look the same
# step 3: ray per pixel
rays = np.dot(coord, RT[:2, :3]) # ray directions (unnormalized)
rays -= np.dot([-K[0, 2], -K[1, 2], K[0, 0]], RT[:3, :3])
rays /= (np.sum(rays**2,
axis=-1)**0.5).reshape(h, w,
1) # normalized ray directions
# step 4: compose
return (rays * filtered_depths).reshape(depths.shape[0], depths.shape[1],
3) + T
def testP(self):
K, RT = Calibrate.decomposeKRT(self.P())
print ('K', K, self.K())
print ('RT', RT, self.RT())
print (self.cameraFovX,self.cameraKox,self.cameraKoy,self.cameraKsquare,self.cameraKskew)
print ((self.cameraPan, self.cameraTilt, self.cameraRoll), self.cameraT, self.cameraInterest)
print ('cf')
print (Calibrate.decomposeK(K))
print (Calibrate.decomposeRT(RT, self.cameraInterest, False))
def cook(self, location, interface, attrs):
if not self.useFrame(interface.frame(), attrs['frameRange']): return
from GCore import Label, Calibrate
x2ds = interface.attr('x2ds')
x2ds_splits = interface.attr('x2ds_splits')
if x2ds is None or x2ds_splits is None:
self.logger.error('No detections found at: %s' % location)
return
mats = interface.attr('mats', atLocation=attrs['calibration'])
if mats is None:
self.logger.error('No calibration found at: %s' % attrs['calibration'])
return
x3ds, x3ds_labels, x2ds_labels = Calibrate.detect_wand(x2ds, x2ds_splits, mats)
if x3ds is None or x2ds_labels is None: return
wandAttrs = {
'x3ds': x3ds,
'x3ds_labels': x3ds_labels,
'x3ds_pointSize': attrs['pointSize'],
'x3ds_colours': self.colours
}
interface.createChild('wand3d', 'points3d', attrs=wandAttrs)
def GiantViewer(raw_filename, cal_filename):
'''Generate a 3D view of an x2d file, using the calibration.'''
raw_frames = {100: None}
print 'loading calibration'
camera_info = GiantReader.readCal(cal_filename)
camera_ids = None
print "Camera IDs:\n{}".format(camera_ids)
print 'loading 2d'
raw_dict = GiantReader.readAsciiRaw(raw_filename)
raw_frames = raw_dict['frames']
print 'num frames', raw_dict['numFrames']
mats = [
Calibrate.makeMat(camera['MAT'], camera['DISTORTION'], (512, 440))
for camera in camera_info['Cameras']
]
track3d = Label.Track3D(mats)
primitives = QGLViewer.makePrimitives(vertices=[], altVertices=[])
primitives2D = QGLViewer.makePrimitives2D(([], [0]))
cb = functools.partial(intersectRaysCB,
raw_frames=raw_frames,
mats=mats,
primitives=primitives,
primitives2D=primitives2D,
track3d=track3d)
QGLViewer.makeViewer(primitives=primitives,
primitives2D=primitives2D,
timeRange=(1, max(raw_frames.keys()), 1, 100.0),
callback=cb,
mats=mats,
camera_ids=camera_ids) # , callback=intersectRaysCB
def cook(self, location, interface, attrs):
imgs = interface.attr('jpegimgs')
if imgs is None: return
img_count = len(imgs)
vwidth, vheight = attrs['vwidth'], attrs['vheight']
self.mats = interface.attr('mats', atLocation=attrs['cameraLocations'])
updateMats = interface.attr('updateMats',
atLocation=attrs['cameraLocations'],
default=False)
if self.mats is None or updateMats:
self.mats = []
for i in xrange(img_count):
self.mats.append(
Calibrate.makeUninitialisedMat(i, (vheight, vwidth)))
interface.setAttr('updateMats',
False,
atLocation=attrs['cameraLocations'])
self.camAttrs = {
'vheight': [vheight] * img_count,
'vwidth': [vwidth] * img_count,
'camera_ids': range(img_count),
'mats': self.mats,
'updateImage': True,
'jpegimgs': imgs,
'updateMats': updateMats
}
for k, v in self.camAttrs.iteritems():
interface.setAttr(k, v)
def setP(self, P, distortion = None, setInterest = True, store = False): '''Set the view to match the given camera projection matrix. P can be 4x4 or 3x4.''' K, RT = Calibrate.decomposeKRT(P) self.setK(K) self.setRT(RT, setInterest) self.cameraDistortion = distortion if distortion is not None: self.cameraDistortion = (float(distortion[0]),float(distortion[1])) # make it hashable if store: self.setResetData()
def frameCentroidsToDets(frame, mats=None):
'''Extract the centroids for given cameras and undistorts them. Returns a list of x2ds and splits per camera.'''
detRawData, splits = SolveIK.list_of_lists_to_splits(frame,
dtype=np.float32)
detRawData = (detRawData - np.float32([256, 256])) / np.float32(
[256, -256])
if mats is None: return detRawData[:, :2].copy(), splits
return Calibrate.undistort_dets(detRawData, splits, mats)
def matToVec(P, distortion): outVec = np.zeros(11, dtype=np.float32) K, RT = Calibrate.decomposeKRT(P) outVec[:3] = cv2.Rodrigues(RT[:3, :3])[0].ravel() outVec[3:6] = RT[:3, 3] outVec[6] = K[0, 0] # Focal Length outVec[7:9] = distortion outVec[9:] = K[:2, 2] # Optical Centre return outVec
def cook(self, location, interface, attrs):
if not self.client.IsConnected():
print "Not connected"
if not self.client.Connect(attrs['ipAddress'],
int(attrs['port'])):
print "Connection Failed"
return False
print "New Connection!"
currentFrame = self.client.GetCurrentFrame()
RT = currentFrame.CameraTracking.Transform
fovX, fovY = currentFrame.OpticalParameters.FOV
if np.abs(fovY) < 1e-10:
return False
cx, cy = currentFrame.OpticalParameters.ProjectionCenter # TODO: Investigate format
ox, oy = 2 * (cx - 0.5), 2 * (cy - 0.5)
K = Calibrate.composeK(fovX,
ox=ox,
oy=oy,
square=(fovY / fovX),
skew=0)[:3, :3]
width_height = currentFrame.OpticalParameters.Resolution
P = np.dot(K, RT)
mat = Calibrate.makeMat(P, (0.0, 0.0),
width_height) # TODO Distortion Param
self.setupAttrs = {
'Ps': [P],
'camera_ids': ["1"],
'camera_names': ["NCAM_Camera"],
'mats': [mat],
'updateMats': True
}
interface.createChild('cameras',
'cameras',
atLocation=location,
attrs=self.setupAttrs)
return True
def cook(self, location, interface, attrs):
if not self.listeners: return
import StringIO
imgs, vwidths, vheights, camera_ids, camera_names, mats = [], [], [], [], [], []
ci = 0
for r, listener in self.listeners.iteritems():
data = listener.poll()
if data is None:
self.logger.error('No data on %d' % r)
continue
print data
timeCode, imgStr = data
sio = StringIO.StringIO(imgStr)
img = PIL.Image.open(sio)
if not self.initialised:
vwidth, vheight = attrs['vwidth'], attrs['vheight']
mat = Calibrate.makeUninitialisedMat(0, (vheight, vwidth))
vwidths.append(vwidth)
vheights.append(vheight)
camera_ids.append('Camera %d' % ci)
camera_names.append(str(r))
mats.append(mat)
imgs.append(img.tobytes())
ci += 1
if not self.initialised:
self.camAttrs['vheight'] = vheights
self.camAttrs['vwidth'] = vwidths
# self.camAttrs['camera_ids'] = camera_ids
# self.camAttrs['camera_names'] = camera_names
self.camAttrs['camera_ids'] = interface.attr('camera_ids')
self.camAttrs['camera_names'] = camera_names
self.camAttrs['mats'] = interface.attr('mats')
self.initialised = True
if imgs:
self.camAttrs['imgs'] = imgs
# self.camAttrs['updateImage'] = True
interface.createChild(interface.name(),
'cameras',
atLocation=interface.parentPath(),
attrs=self.camAttrs)
tcAttrs = {
'x3ds': np.array([[0, 0, 0]], dtype=np.float32),
'x3ds_labels': [timeCode]
}
interface.createChild(interface.name() + '/tc',
'points',
atLocation=interface.parentPath(),
attrs=tcAttrs)
def cook(self, location, interface, attrs):
detections = attrs['detections']
matsLocation = attrs['matsLocation']
if not detections or not matsLocation: return
wand_frames = interface.attr('x2ds', atLocation=detections)
print wand_frames[1:2]
vicon_mats = interface.attr('mats', atLocation=matsLocation)
vicon_solved = [not (m[1][1,3] == 0.0 and m[1][2,3] == 0.0 and m[1][0,3] != 0.0) for m in vicon_mats]
x2s_cameras, x3s_cameras, frames_cameras, num_kept_frames = Calibrate.generate_wand_correspondences(wand_frames, vicon_mats, vicon_solved)
def vecToMat(vec): f, k1, k2, ox, oy = vec[6:] rot = vec[:3] trans = vec[3:6] K = np.eye(3) K[[0,1],[0,1]] = f K[:2, 2] = [ox, oy] R = cv2.Rodrigues(rot)[0] RT = np.zeros((3, 4), dtype=np.float32) RT[:3, :3] = R RT[:3, 3] = trans P = Calibrate.composeKRT(K, RT)[:3,:] return np.float32(P), (k1, k2)
def cook(self, location, interface, attrs):
vwidth, vheight = attrs['vwidth'], attrs['vheight']
if self.cam is None:
self.cam = CamVideoStream(src=0).start()
self.mats = [Calibrate.makeUninitialisedMat(0, (vheight, vwidth))]
self.camAttrs = {
'vheight': [vheight],
'vwidth': [vwidth],
'camera_ids': [0],
'mats': self.mats,
'updateImage': True
}
md = {'frame': self.cam.read()}
self.camAttrs['imgs'] = [md['frame']]
# self.attrs['imgs'] = np.array(frame, dtype=np.uint8)
interface.createChild(interface.name(),
'cameras',
atLocation=interface.parentPath(),
attrs=self.camAttrs)
def get_labels(frames, x3ds_seq, detections_seq, mats, x2d_threshold=0.01):
'''Project all the 3d points in all the views and label the detections.'''
num_cameras = len(mats)
ret = {}
Ps = np.array([m[2] / (m[0][0, 0]) for m in mats], dtype=np.float32)
for fi in frames:
print fi, '\r',
x3ds, x3ds_labels = x3ds_seq[fi]
x2ds_raw_data, splits = detections_seq[fi][0]
assert (num_cameras + 1 == len(splits))
x2ds_labels = -np.ones(len(x2ds_raw_data), dtype=np.int32)
x2ds_data, _ = Calibrate.undistort_dets(x2ds_raw_data, splits, mats)
if len(x2ds_data):
clouds = ISCV.HashCloud2DList(x2ds_data, splits, x2d_threshold)
sc, x2ds_labels, x2ds_vels = Label.project_assign(
clouds, x3ds, x3ds_labels, Ps, x2d_threshold)
zeros = np.where(x2ds_labels == -1)[0]
# these lines remove all the data for the unlabelled points
x2ds_data[zeros] = -1
x2ds_raw_data[zeros] = -1
ret[fi] = x2ds_raw_data, splits, x2ds_labels
return ret
def setK(self, K): '''Set the view to match the given intrinsic matrix.''' self.cameraFovX,self.cameraKox,self.cameraKoy,self.cameraKsquare,self.cameraKskew = Calibrate.decomposeK(K)
def main(x2d_filename, xcp_filename, c3d_filename=None):
'''Generate a 3D view of an x2d file, using the calibration.'''
global x2d_frames, mats, Ps, c3d_frames, primitives, primitives2D, track3d, prev_frame, track_orn, orn_graph, boot, orn_mapper, mar_mapper
prev_frame = None
c3d_frames = None
if c3d_filename != None:
c3d_dict = C3D.read(c3d_filename)
c3d_frames, c3d_fps, c3d_labels = c3d_dict['frames'], c3d_dict[
'fps'], c3d_dict['labels']
mats, xcp_data = ViconReader.loadXCP(xcp_filename)
camera_ids = [int(x['DEVICEID']) for x in xcp_data]
print 'loading 2d'
x2d_dict = ViconReader.loadX2D(x2d_filename)
x2d_frames = x2d_dict['frames']
cameras_info = ViconReader.extractCameraInfo(x2d_dict)
print 'num frames', len(x2d_frames)
Ps = [m[2] / (m[0][0, 0]) for m in mats]
track3d = Label.Track3D(mats)
primitives = QGLViewer.makePrimitives(vertices=[], altVertices=[])
primitives2D = QGLViewer.makePrimitives2D(([], [0]))
global g_all_skels, md
directory = os.path.join(os.environ['GRIP_DATA'], '151110')
_, orn_skel_dict = IO.load(os.path.join(directory, 'orn.skel'))
movie_fn = os.path.join(directory, '50_Grip_RoomCont_AA_02.v2.mov')
md = MovieReader.open_file(movie_fn,
audio=True,
frame_offset=0,
volume_ups=10)
asf_filename = os.path.join(directory, 'Martha.asf')
amc_filename = os.path.join(directory, 'Martha.amc')
asf_dict = ASFReader.read_ASF(asf_filename)
mar_skel_dict = ASFReader.asfDict_to_skelDict(asf_dict)
mar_skel_dict['anim_dict'] = ASFReader.read_AMC(amc_filename, asf_dict)
for k in ('geom_Vs', 'geom_vsplits', 'geom_Gs'):
mar_skel_dict[k] = orn_skel_dict[k].copy()
mar_skel_dict['shape_weights'] = orn_skel_dict['shape_weights']
mar_skel_dict['geom_dict'] = orn_skel_dict['geom_dict']
orn_vss = ViconReader.loadVSS(os.path.join(directory, 'Orn.vss'))
orn_vss_chan_mapping = [
orn_vss['chanNames'].index(n) for n in orn_skel_dict['chanNames']
]
orn_anim_dict = orn_skel_dict['anim_dict']
orn_vss_anim = np.zeros(
(orn_anim_dict['dofData'].shape[0], orn_vss['numChans']),
dtype=np.float32)
orn_vss_anim[:, orn_vss_chan_mapping] = orn_anim_dict['dofData']
orn_anim_dict['dofData'] = orn_vss_anim
orn_vss['anim_dict'] = orn_anim_dict
for x in [
'geom_dict', 'geom_Vs', 'geom_vsplits', 'geom_Gs', 'shape_weights'
]:
orn_vss[x] = orn_skel_dict[x]
orn_skel_dict = orn_vss
g_all_skels = {}
orn_mesh_dict, orn_skel_mesh, orn_geom_mesh = orn_t = Character.make_geos(
orn_skel_dict)
g_all_skels['orn'] = (orn_skel_dict, orn_t)
orn_skel_dict['chanValues'][:] = 0
Character.updatePoseAndMeshes(orn_skel_dict, orn_skel_mesh, orn_geom_mesh)
mar_mesh_dict, mar_skel_mesh, mar_geom_mesh = mar_t = Character.make_geos(
mar_skel_dict)
g_all_skels['mar'] = (mar_skel_dict, mar_t)
#ted_mesh_dict, ted_skel_mesh, ted_geom_mesh = ted_t = Character.make_geos(ted_skel_dict)
#g_all_skels['ted'] = (ted_skel_dict, ted_t)
#ted_skel_dict['chanValues'][0] += 1000
#Character.updatePoseAndMeshes(ted_skel_dict, ted_skel_mesh, ted_geom_mesh)
mnu = orn_skel_dict['markerNamesUnq']
mns = orn_skel_dict['markerNames']
effectorLabels = np.array([mnu.index(n) for n in mns], dtype=np.int32)
orn_graph = Label.graph_from_skel(orn_skel_dict, mnu)
boot = -10
track_orn = Label.TrackModel(orn_skel_dict, effectorLabels, mats)
#ted = GLSkel(ted_skel_dict['Bs'], ted_skel_dict['Gs']) #, mvs=ted_skel_dict['markerOffsets'], mvis=ted_skel_dict['markerParents'])
#ted = GLSkeleton(ted_skel_dict['jointNames'],ted_skel_dict['jointParents'], ted_skel_dict['Gs'][:,:,3])
#ted.setName('ted')
#ted.color = (1,1,0)
#orn = GLSkeleton(orn_skel_dict['jointNames'],orn_skel_dict['jointParents'], orn_skel_dict['Gs'][:,:,3])
#orn.setName('orn')
#orn.color = (0,1,1)
#square = GLMeshes(names=['square'],verts=[[[0,0,0],[1000,0,0],[1000,1000,0],[0,1000,0]]],vts=[[[0,0],[1,0],[1,1],[0,1]]],faces=[[[0,1,2,3]]],fts=[[[0,1,2,3]]])
#square.setImageData(np.array([[[0,0,0],[255,255,255]],[[255,255,255],[0,0,0]]],dtype=np.uint8))
#orn_geom_mesh.setImageData(np.array([[[0,0,0],[255,255,255]],[[255,255,255],[0,0,0]]],dtype=np.uint8))
P = Calibrate.composeP_fromData((60.8, ), (-51.4, 14.7, 3.2),
(6880, 2860, 5000),
0) # roughed in camera for 151110
ks = (0.06, 0.0)
mat = Calibrate.makeMat(P, ks, (1080, 1920))
orn_mapper = Opengl.ProjectionMapper(mat)
orn_mapper.setGLMeshes(orn_geom_mesh)
orn_geom_mesh.setImage((md['vbuffer'], (md['vheight'], md['vwidth'], 3)))
mar_mapper = Opengl.ProjectionMapper(mat)
mar_mapper.setGLMeshes(mar_geom_mesh)
mar_geom_mesh.setImage((md['vbuffer'], (md['vheight'], md['vwidth'], 3)))
global g_screen
g_screen = Opengl.make_quad_distortion_mesh()
QGLViewer.makeViewer(mat=mat,md=md,layers = {\
#'ted':ted, 'orn':orn,
#'ted_skel':ted_skel_mesh,'ted_geom':ted_geom_mesh,\
#'square':square,
'orn_skel':orn_skel_mesh,'orn_geom':orn_geom_mesh,\
'mar_skel':mar_skel_mesh,'mar_geom':mar_geom_mesh,\
},
primitives=primitives, primitives2D=primitives2D, timeRange=(0, len(x2d_frames) - 1, 4, 25.0), callback=intersectRaysCB, mats=mats,camera_ids=camera_ids)
def P(self, upright=True): '''Yields the camera projection matrix P = K RT.''' return Calibrate.composeKRT(self.K(),self.RT(upright = upright))
def K(self): '''Yields the intrinsic matrix for the view, K.''' return Calibrate.composeK(self.cameraFovX,self.cameraKox,self.cameraKoy,self.cameraKsquare,self.cameraKskew)
def RT(self, upright=False): '''Yields the extrinsic matrix for the view [R T].''' return Calibrate.composeRT(self.R(upright),self.cameraT,self.cameraInterest)
def R(self, upright=False): '''Yields the orientation matrix for the view, R.''' return Calibrate.composeR([self.cameraPan,self.cameraTilt,0 if (self.lockedUpright and upright) else self.cameraRoll])
def frameCentroidsToDets(frame, mats=None): '''Extract the centroids for given cameras and undistorts them. Returns a list of x2ds and splits per camera.''' x2ds_raw_data, x2ds_splits = frame[0],frame[1] if mats is None: return x2ds_raw_data[:,:2].copy(), x2ds_splits return Calibrate.undistort_dets(x2ds_raw_data, x2ds_splits, mats)
dx, dy = 10, 10
img[int(r.sy - dy):int(r.sy + dy),
int(r.sx - dx):int(r.sx + dx), 0] = 128
else:
pts0 = pts1 = []
return (pts0, pts1)
def tighten_calibration(
(x3s, x3s_labels), (x2s, x2s_splits, x2s_labels), mats):
x3s_original = x3s.copy()
x2s_labels_original = x2s_labels.copy()
for it in range(10):
x2d_threshold = 0.08 # - it * 0.04/50.
Ps = np.array([m[2] / (m[0][0, 0]) for m in mats], dtype=np.float32)
u2s, _ = Calibrate.undistort_dets(x2s, x2s_splits, mats)
x3s, x3s_labels, E, x2d_labels = Recon.solve_x3ds(
u2s, x2s_splits, x2s_labels_original, Ps, True)
clouds = ISCV.HashCloud2DList(u2s, x2s_splits, x2d_threshold)
sc, x2s_labels, _ = Label.project_assign(clouds, x3s, x3s_labels, Ps,
x2d_threshold)
print 'it', it, sc
tiara_xis = np.where(x3s_labels < len(VICON_tiara_x3ds))[0]
tiara_lis = x3s_labels[tiara_xis]
tiara_true = VICON_tiara_x3ds[tiara_lis] + [0, 1000, 0]
tiara_xs = x3s[tiara_xis]
# now solve the tiara into place by finding a rigid transform
RT, inliers = Calibrate.rigid_align_points_inliers(tiara_xs,
tiara_true,
scale=True)
x3s = np.dot(x3s, RT[:3, :3].T) + RT[:, 3]
def cook(self, location, interface, attrs):
if not self.useFrame(interface.frame(), attrs['frameRange']):
interface.setAttr('updateMats', False)
return
# We need 2D data e.g. wand detections from a wand op
# We need 3D wand data from e.g. c3d or a 3D wand detector
dets_location = attrs['detections']
x3ds_location = attrs['x3ds']
if not dets_location or not x3ds_location: return
# Get the 2D and 3D data
x2ds = interface.attr('rx2ds', atLocation=dets_location)
x2d_splits = interface.attr('x2ds_splits', atLocation=dets_location)
x3ds = interface.attr('x3ds', atLocation=x3ds_location)
if x2ds is None or x2d_splits is None or x3ds is None: return
numCameras = len(x2d_splits) - 1
error_threshold = attrs['error_threshold']
# Get the data we've collected already so we can add to it
frame = interface.frame()
dets_colours = interface.attr('x2ds_colours', atLocation=dets_location)
collectedDets = interface.attr('collect_rx2ds')
collectedX3ds = interface.attr('collect_x3ds')
lastFrame = interface.attr('lastFrame', [frame] * numCameras)
emptyFrame3d = np.array([[]], dtype=np.float32).reshape(-1, 3)
# This is potentially used by other ops so we only set it when we have some confidence
# (and we might reset or tweak the values to indicate confidence levels at some point)
cameraErrors = interface.attr('cameraErrors', [-1] * numCameras)
# This is never modified to allow checking the camera rms values regardless of what we make of them
rmsValues = interface.attr('rms', [-1] * numCameras)
# Get the width and height for the videos
vwidth = interface.attr('vwidth', [1920] * numCameras)
vheight = interface.attr('vheight', [1080] * numCameras)
# Get the frame mapping for x3ds
x3ds_frames = interface.attr('x3ds_frames', {})
x2ds_frames = interface.attr('x2ds_frames', [[] for i in xrange(numCameras)])
# Get the camera matrices. We initialise them with default settings if we don't find any
mats = interface.attr('mats', atLocation=location)
if mats is None:
mats = []
for ci in range(numCameras):
mats.append(Calibrate.makeUninitialisedMat(ci, (vheight[ci], vwidth[ci])))
# Allow overriding the error threshold using an attribute (on the cooked location)
error_threshold_attr = interface.attr('error_threshold')
if error_threshold_attr is not None:
error_threshold = error_threshold_attr
Ps = interface.attr('Ps')
if Ps is None: Ps = [np.array([], dtype=np.float32) for n in range(numCameras)]
# Get the minimum number of samples we need to start solving distortion etc. as specified by the user
minSamples = attrs['min_samples']
# Prepare the collected data for further processing (or initialise if nothing has been collected)
if collectedDets is not None:
c_x2ds, c_splits = collectedDets
cams_collected = [c_x2ds[c0:c1] for ci, (c0, c1) in enumerate(zip(c_splits[:-1], c_splits[1:]))]
else:
cams_collected = [[] for ci, (c0, c1) in enumerate(zip(x2d_splits[:-1], x2d_splits[1:]))]
collectedX3ds = []
for ci, (c0, c1) in enumerate(zip(x2d_splits[:-1], x2d_splits[1:])):
collectedX3ds.append(emptyFrame3d)
# Process each camera by looking for a wand and attempt a calibration. If we're happy with the results we'll
# add it to our collection
for ci, (c0, c1) in enumerate(zip(x2d_splits[:-1], x2d_splits[1:])):
elapsed = frame - lastFrame[ci]
if 0 < elapsed < attrs['jumpFrames']: continue
# Get the 2Ds and 3Ds for the wand in this camera (if any)
cameraDetections = x2ds[c0:c1]
cameraX3ds = x3ds
if not cameraDetections.any() or not cameraX3ds.any(): continue
# Add the new detection to the existing collection as a candidate for a new collection
if cams_collected[ci] is None or len(cams_collected[ci]) == 0:
proposalDets, proposalX3ds = cameraDetections, cameraX3ds
else:
proposalDets = np.concatenate((cams_collected[ci], cameraDetections))
proposalX3ds = np.concatenate((collectedX3ds[ci], cameraX3ds))
# Check if we want to solve for distortion and focal length by looking at the number of samples
# we've got already compared to our minimum number of samples required
numSamples = len(proposalDets) / 5
# if numSamples == minSamples: self.logger.info('Camera %d reached min samples of %d' % (ci, minSamples))
solveTrigger = True if numSamples > minSamples else False
solve_focal_length = attrs['solve_focal_length'] if solveTrigger else False
solve_distortion = attrs['solve_distortion'] if solveTrigger else False
# The wand is assumed to have 5 points so we make sure we've got at least one wand before attempting
# to calibrate
if len(proposalDets) >= 5 and len(proposalX3ds) >= 5:
P, ks, rms = Calibrate.cv2_solve_camera_from_3d(proposalX3ds, proposalDets,
solve_focal_length=solve_focal_length,
solve_distortion=solve_distortion)
if ks[0] < -3. or ks[0] > 3.: ks[0] = 0.
if ks[1] < -3. or ks[1] > 3.: ks[1] = 0.
# This shouldn't' happen but if we lose confidence in the camera we can visualise it
# by resetting the camera error (this will change the colour in the UI)
if rms > error_threshold:
cameraErrors[ci] = -1
continue
# See how the rms for the calibration compares to the last recorded value for this camera
prevRms = rms if rmsValues[ci] == -1 else rmsValues[ci]
rmsDelta = rms - prevRms
# If the rms is lower than the last recorded error for this camera then
# we want to keep this data
if rmsDelta <= 0 or not solveTrigger:
cams_collected[ci] = proposalDets
collectedX3ds[ci] = proposalX3ds
if frame not in x3ds_frames:
x3ds_frames[frame] = proposalX3ds[-5:]
x2ds_frames[ci] += ([frame] * 5)
else:
continue
# Record the rms value for the camera
rmsValues[ci] = rms
# Once we've solved for distortion etc. we are more confident with the accuracy of our
# error so we start reporting it, where the value can be used for visualiation etc.
if solveTrigger: cameraErrors[ci] = rms
lastFrame[ci] = frame
# Everything has gone well so far so we create and add the new camera matrix
mat = Calibrate.makeMat(P, ks, (vheight[ci], vwidth[ci]))
mats[ci] = mat
Ps[ci] = P
# Concatenate the results from all the cameras
cams = [np.concatenate((cc)) for cc in cams_collected if len(cc)]
if not cams:
# We haven't found a wand in any camera so we just keep calm and return
return
# Build our collections and write to the interface
collectedDets = np.array(np.concatenate(cams), dtype=np.float32).reshape(-1, 2), \
Interface.makeSplitBoundaries(map(len, cams_collected))
interface.setAttr('collect_rx2ds', collectedDets)
interface.setAttr('collect_x3ds', collectedX3ds)
interface.setAttr('x2ds_frames', x2ds_frames)
interface.setAttr('x3ds_frames', x3ds_frames)
interface.setAttr('lastFrame', lastFrame)
# Write the calibration data to the interface and request an update at render time
interface.setAttr('mats', mats)
interface.setAttr('Ps', Ps)
interface.setAttr('rms', rmsValues)
interface.setAttr('cameraErrors', cameraErrors)
interface.setAttr('updateMats', True)
# Optionally display all the collected wand detections
if 'showDetections' in attrs and attrs['showDetections']:
colours = np.tile(dets_colours, (len(collectedDets[0]) / 5, 1))
allAttrs = {'x2ds': collectedDets[0], 'x2ds_splits': collectedDets[1],
'x2ds_colours': colours}
interface.createChild('collected', 'detections', attrs=allAttrs)
def main():
global State, mats, movieFilenames, primitives
global movies, primitives2D, deinterlacing, detectingWands
import IO
import sys, os
deinterlacing = False
detectingWands = False
detectingTiara = False
dot_detections = None
detections_filename = None
frame_offsets = None
firstFrame, lastFrame = 0, 5000
drawDotSize = 4.0
fovX, (ox,
oy), pan_tilt_roll, tx_ty_tz, distortion = 50., (0,
0), (0, 0,
0), (0, 1250,
0), (0,
0)
mats = []
grip_directory = os.environ['GRIP_DATA']
if 0:
fovX, (ox, oy), pan_tilt_roll, tx_ty_tz, distortion = 37.9, (0, 0), (
-66.0, 3.5, -0.2), (4850, 1330, 3280), (0, 0) # roughed in
K, RT = Calibrate.composeK(fovX, ox, oy), Calibrate.composeRT(
Calibrate.composeR(pan_tilt_roll), tx_ty_tz, 0)
mat0 = [
K[:3, :3], RT[:3, :4],
np.dot(K, RT)[:3, :], distortion, -np.dot(RT[:3, :3].T, RT[:3, 3]),
[1920, 1080]
]
fovX, (ox, oy), pan_tilt_roll, tx_ty_tz, distortion = 55.8, (0, 0), (
-103.6, 3.5, -0.3), (2980, 1380, -2180), (0, 0) # roughed in
K, RT = Calibrate.composeK(fovX, ox, oy), Calibrate.composeRT(
Calibrate.composeR(pan_tilt_roll), tx_ty_tz, 0)
mat1 = [
K[:3, :3], RT[:3, :4],
np.dot(K, RT)[:3, :], distortion, -np.dot(RT[:3, :3].T, RT[:3, 3]),
[1920, 1080]
]
fovX, (ox, oy), pan_tilt_roll, tx_ty_tz, distortion = 49.3, (0, 0), (
27.9, 4.0, -0.2), (-5340, 1150, 5030), (0, 0) # roughed in
K, RT = Calibrate.composeK(fovX, ox, oy), Calibrate.composeRT(
Calibrate.composeR(pan_tilt_roll), tx_ty_tz, 0)
mat2 = [
K[:3, :3], RT[:3, :4],
np.dot(K, RT)[:3, :], distortion, -np.dot(RT[:3, :3].T, RT[:3, 3]),
[1920, 1080]
]
fovX, (ox, oy), pan_tilt_roll, tx_ty_tz, distortion = 50.6, (0, 0), (
-156.6, 4.9, 0.2), (-105, 1400, -4430), (0, 0) # roughed in
K, RT = Calibrate.composeK(fovX, ox, oy), Calibrate.composeRT(
Calibrate.composeR(pan_tilt_roll), tx_ty_tz, 0)
mat3 = [
K[:3, :3], RT[:3, :4],
np.dot(K, RT)[:3, :], distortion, -np.dot(RT[:3, :3].T, RT[:3, 3]),
[1920, 1080]
]
mats = [mat0, mat1, mat2, mat3]
xcp_filename = '154535_Cal168_Floor_Final.xcp'
directory = os.path.join(grip_directory, 'REFRAME')
movieFilenames = [
'001E0827_01.MP4', '001F0813_01.MP4', '001G0922_01.MP4',
'001H0191_01.MP4'
]
#mats,movieFilenames = mats[:1],movieFilenames[:1] # restrict to single-view
frame_offsets = [119 + 160, 260, 339, 161]
small_blur, large_blur = 1, 25
min_dot_size = 1.0
max_dot_size = 20.0
circularity_threshold = 3.0
threshold_bright, threshold_dark_inv = 250, 250 #135,135
elif 0:
xcp_filename = '201401211653-4Pico-32_Quad_Dialogue_01_Col_wip_01.xcp'
detections_filename = 'detections.dat'
detectingTiara = True
pan_tilt_roll = (0, 0, 90)
distortion = (0.291979, 0.228389)
directory = os.path.join(os.environ['GRIP_DATA'], 'ted')
movieFilenames = [
'201401211653-4Pico-32_Quad_Dialogue_01_%d.mpg' % xi
for xi in range(1)
]
firstFrame = 511
small_blur, large_blur = 1, 20
min_dot_size = 1.0
max_dot_size = 16.0
circularity_threshold = 3.0
threshold_bright, threshold_dark_inv = 0, 170
elif 1:
xcp_filename = '50_Grip_RoomCont_AA_02.xcp'
detections_filename = 'detections.dat'
pan_tilt_roll = (0, 0, 0)
distortion = (0.291979, 0.228389)
directory = os.path.join(os.environ['GRIP_DATA'], '151110')
movieFilenames = ['50_Grip_RoomCont_AA_02.v2.mov']
firstFrame = 0
small_blur, large_blur = 1, 20
min_dot_size = 1.0
max_dot_size = 16.0
circularity_threshold = 3.0
threshold_bright, threshold_dark_inv = 170, 170
attrs = dict([(v, eval(v)) for v in [
'small_blur', 'large_blur', 'threshold_bright', 'threshold_dark_inv',
'circularity_threshold', 'min_dot_size', 'max_dot_size'
]])
primitives2D = QGLViewer.makePrimitives2D(([], []), ([], []))
primitives = []
if len(movieFilenames) is 1:
# TODO: time_base, timecode
K, RT = Calibrate.composeK(fovX, ox, oy), Calibrate.composeRT(
Calibrate.composeR(pan_tilt_roll), tx_ty_tz, 0)
mats = [[
K[:3, :3], RT[:3, :4],
np.dot(K, RT)[:3, :], distortion, -np.dot(RT[:3, :3].T, RT[:3, 3]),
[1920, 1080]
]]
camera_ids = ['video']
movies = [
MovieReader.open_file(os.path.join(directory, movieFilenames[0]),
audio=False)
]
else: # hard coded cameras
if xcp_filename.endswith('.xcp'):
if detectingTiara: # gruffalo
c3d_filename = os.path.join(
directory,
'201401211653-4Pico-32_Quad_Dialogue_01_Col_wip_02.c3d')
from IO import C3D
c3d_dict = C3D.read(c3d_filename)
global c3d_frames
c3d_frames, c3d_fps, c3d_labels = c3d_dict['frames'], c3d_dict[
'fps'], c3d_dict['labels']
c3d_subject = '' #'TedFace'
which = np.where(
[s.startswith(c3d_subject) for s in c3d_labels])[0]
c3d_frames = c3d_frames[:, which, :]
c3d_labels = [c3d_labels[i] for i in which]
print len(c3d_frames)
xcp, xcp_data = ViconReader.loadXCP(
os.path.join(directory, xcp_filename))
mats.extend(xcp)
elif xcp_filename.endswith('.cal'):
from IO import OptitrackReader
xcp, xcp_data = OptitrackReader.load_CAL(
os.path.join(directory, xcp_filename))
mats = xcp
print 'mats', len(mats), len(movieFilenames)
assert (len(mats) == len(movieFilenames))
camera_ids = []
movies = []
for ci, mf in enumerate(movieFilenames):
fo = 0 if frame_offsets is None else frame_offsets[ci]
movies.append(
MovieReader.open_file(os.path.join(directory, mf),
audio=False,
frame_offset=fo))
camera_ids = ['cam_%d' % ci for ci in xrange(len(mats))]
print len(mats), len(movies), len(camera_ids)
primitives.append(GLPoints3D([]))
primitives.append(GLPoints3D([]))
primitives.append(GLPoints3D([]))
primitives[0].colour = (0, 1, 1, 0.5) # back-projected "cyan" points
primitives[1].colour = (0, 0, 1, 0.5)
primitives[1].pointSize = 5
primitives[2].colour = (1, 0, 0, 0.99)
if len(movieFilenames) != 1 and detections_filename != None:
try:
dot_detections = IO.load(detections_filename)[1]
except:
numFrames = len(c3d_frames) # TODO HACK HACK
dot_detections = movies_to_detections(movies, range(numFrames),
deinterlacing, attrs)
IO.save(detections_filename, dot_detections)
if detectingTiara:
x3ds_seq = {}
for fi in dot_detections.keys():
frame = c3d_frames[(fi - 55) % len(c3d_frames)]
which = np.array(np.where(frame[:, 3] == 0)[0], dtype=np.int32)
x3ds_seq[fi] = np.concatenate((VICON_tiara_x3ds + np.array([150,-100,0],dtype=np.float32),frame[which,:3])), \
np.concatenate((np.arange(len(VICON_tiara_x3ds),dtype=np.int32),which+len(VICON_tiara_x3ds)))
dot_labels = get_labels(dot_detections.keys(),
x3ds_seq,
dot_detections,
mats,
x2d_threshold=0.05)
calibration_fi = 546 - 2 - 6
RT = tighten_calibration(x3ds_seq[calibration_fi],
dot_labels[calibration_fi], mats)
for v in c3d_frames:
v[:, :3] = np.dot(v[:, :3], RT[:3, :3].T) + RT[:, 3]
if True:
dot_detections = IO.load(detections_filename)[1]
x3ds_seq = {}
for fi in dot_detections.keys():
frame = c3d_frames[(fi - 55) % len(c3d_frames)]
which = np.array(np.where(frame[:, 3] == 0)[0],
dtype=np.int32)
x3ds_seq[fi] = np.concatenate((VICON_tiara_x3ds + np.array([0,1000,0],dtype=np.float32),frame[which,:3])), \
np.concatenate((np.arange(len(VICON_tiara_x3ds),dtype=np.int32),which+len(VICON_tiara_x3ds)))
#dot_labels = get_labels(dot_detections.keys(), x3ds_seq, dot_detections, mats, x2d_threshold = 0.05)
if detectingTiara:
primitives.append(GLPoints3D(VICON_tiara_x3ds + [0, 1000, 0]))
primitives[-1].pointSize = 5
global track3d, prev_frame, booting, trackGraph
track3d = Label.Track3D(mats[:len(movies)],
x2d_threshold=0.03,
x3d_threshold=5.0,
min_rays=3,
boot_interval=2) #tilt_threshold = 0.01, gruffalo
trackGraph = Label.TrackGraph()
prev_frame = 0
booting = 1
from UI import QApp
from PySide import QtGui
from GCore import State
# Modified the options parameter for fields to be the range of acceptable values for the box
# Previously would crash if small_blur got too low
QApp.fields = {
'image filter': [
('small_blur', 'Small blur radius',
'This is part of the image filter which controls the size of smallest detected features.',
'int', small_blur, {
"min": 0,
"max": None
}),
('large_blur', 'Large blur radius',
'This is part of the image filter which controls the size of largest detected features.',
'int', large_blur, {
"min": 0,
"max": None
}),
('threshold_bright', 'threshold_bright',
'This is part of the image filter which controls the size of smallest detected features.',
'int', threshold_bright, {
"min": 0,
"max": 255
}),
('threshold_dark_inv', 'threshold_dark_inv',
'This is part of the image filter which controls the size of largest detected features.',
'int', threshold_dark_inv, {
"min": 0,
"max": 255
}),
('circularity_threshold', 'circularity_threshold',
'How circular?.', 'float', circularity_threshold, {
"min": 0,
"max": 100
}),
('min_dot_size', 'min_dot_size',
'min_dot_size smallest detected features.', 'float', min_dot_size,
{
"min": 0,
"max": 100
}),
('max_dot_size', 'max_dot_size',
'max_dot_size largest detected features.', 'float', max_dot_size,
{
"min": 0,
"max": 100
}),
]
}
State.addKey('dotParams', {'type': 'image filter', 'attrs': attrs})
State.setSel('dotParams')
appIn = QtGui.QApplication(sys.argv)
appIn.setStyle('plastique')
win = QApp.QApp()
win.setWindowTitle('Imaginarium Dots Viewer')
QGLViewer.makeViewer(primitives=primitives,
primitives2D=primitives2D,
timeRange=(firstFrame, lastFrame),
callback=setFrame,
mats=mats,
camera_ids=camera_ids,
movies=movies,
pickCallback=picked,
appIn=appIn,
win=win)
def cb(frame):
global g_record, g_frame
g_frame = frame
global g_camera_rays, g_camera_mat
#print 'in cb'
img = freenect.sync_get_video()[0]
geom_mesh = QApp.app.getLayer('geom_mesh')
geom_mesh.setImage(img)
if 1:
depths = freenect.sync_get_depth(format=freenect.DEPTH_REGISTERED)[0]
#print 'depths',np.median(depths)
if 0: # recording
if frame not in g_record: return
img, depths = g_record[frame]['video'], g_record[frame]['depths']
g_record[frame] = {'video': img.copy(), 'depths': depths.copy()}
if frame == 99: IO.save('dump', g_record)
depths_sum = np.array(depths != 0, dtype=np.int32)
lookup = np.array([0, 1, 0.5, 1.0 / 3, 0.25], dtype=np.float32)
if 1: # average
depths_lo = np.array(depths[::2, ::2] + depths[1::2, ::2] +
depths[::2, 1::2] + depths[1::2, 1::2],
dtype=np.float32)
depths_lo = depths_lo * lookup[
(depths_sum[::2, ::2] + depths_sum[1::2, ::2] +
depths_sum[::2, 1::2] +
depths_sum[1::2, 1::2]).reshape(-1)].reshape(depths_lo.shape)
else: # fullsize
depths_lo = depths * lookup[depths_sum.reshape(-1)].reshape(
depths_lo.shape)
K, RT, P, ks, T, wh = g_camera_mat
vs = depths_to_points(g_camera_rays, T, depths_lo)
geom_mesh.setVs(vs.reshape(-1, 3))
#QApp.view().setImage(img, img.shape[0], img.shape[1], img.shape[2])
#camera = QApp.view().camera
#geom_mesh.image = camera.image
#geom_mesh.bindImage = camera.bindImage
#geom_mesh.bindId = camera.bindId
global g_predictor, reference_3d, geo_vs, geo_vts
h, w, _3 = img.shape
global g_prev_vs
try:
g_prev_vs
except:
g_prev_vs = None
use_prev_vs = True
if g_prev_vs is None:
reference_3d[:, :2] = g_predictor['ref_shape'] * [100, 100]
tmp = Face.detect_face(img,
g_predictor) if g_prev_vs is None else g_prev_vs
tmp = Face.track_face(img, g_predictor, tmp)
if use_prev_vs: g_prev_vs = tmp
if frame == 0 or Face.test_reboot(img, g_prev_vs): g_prev_vs = None
geo_vts[:len(tmp)] = tmp
geo_vts[:, 1] = img.shape[0] - geo_vts[:, 1]
current_shape = geo_vts[:len(tmp)].copy()
if 1:
ds = extract_depths(vs, current_shape * 0.5)
M, inliers = Calibrate.rigid_align_points_inliers(ds,
reference_3d,
scale=True,
threshold_ratio=5.0)
ds = np.dot(ds, M[:3, :3].T) + M[:, 3]
which = np.where(np.sum((reference_3d - ds)**2, axis=1) < 100 * 100)[0]
reference_3d[which] = reference_3d[which] * 0.99 + ds[which] * 0.01
reference_3d[
inliers] = reference_3d[inliers] * 0.95 + ds[inliers] * 0.05
ds[:] = reference_3d[:]
M[1, 3] += 1000
M[0, 3] -= 300
else:
M = np.eye(3, 4, dtype=np.float32)
M[1, 3] += 1000
geom_mesh.setPose(M.reshape(1, 3, 4))
ref_pinv = g_predictor['ref_pinv']
xform = np.dot(ref_pinv, current_shape)
ut, s, v = np.linalg.svd(xform)
s = (s[0] * s[1])**-0.5
xform_inv = np.dot(v.T, ut.T) * s
current_shape = np.dot(current_shape - np.mean(current_shape, axis=0),
xform_inv) * 100.
geo_vs[:] = 0
geo_vs[:len(current_shape), :2] = current_shape
geo_vs[:70] = reference_3d
#geo_vs[:68,:] += [0,100,5500]
#print geo_vts[:4],w,h
geo_mesh = QApp.app.getLayer('geo_mesh')
geo_mesh.setVs(geo_vs,
vts=geo_vts *
np.array([1.0 / w, 1.0 / h], dtype=np.float32))
geo_mesh.setImage(img)
#geo_mesh.transforms[0][:,:3] = [[1,0,0],[0,1,0],[0,0,1],[0,1000,0.1]]
if 1:
global g_model
w, h = 160, 160
shp = geo_vs[:68, :2]
shape_u, tex_u, A_inv, mn = Face.fit_aam(g_model, tmp, img)
Face.render_aam(g_model, A_inv * 0.5, mn * 0.5, shape_u, tex_u, img)
img_mesh = QApp.app.getLayer('img_mesh')
img_mesh.setImage(img)
QApp.view().updateGL()
def main():
grip_dir = os.environ['GRIP_DATA']
global g_model
g_model = IO.load(os.path.join(grip_dir, 'aam.new.io'))[1]
global g_predictor, reference_3d, geo_vs, geo_vts, rect
rect = None
pred_fn = os.path.join(grip_dir, 'pred.new.io')
g_predictor = Face.load_predictor(pred_fn) #, cutOff=15)
reference_shape = g_predictor['ref_shape']
size = reference_shape.shape[0]
geo_vs = np.zeros((size, 3), dtype=np.float32)
geo_vs[:size, :2] = reference_shape
geo_vts = np.zeros((size, 2), dtype=np.float32)
geo_vts[:size] = reference_shape + 0.5
geo_ts = np.array([[1, 0, 0, 0], [0, 1, 0, 1000], [0, 0, 1, 0]],
dtype=np.float32)
geo_fs = Face.triangulate_2D(reference_shape)
geo_bs = []
for p0, p1, p2 in geo_fs:
geo_bs.append((p0, p1))
geo_bs.append((p1, p2))
geo_bs.append((p2, p0))
reference_3d = np.zeros((reference_shape.shape[0], 3), dtype=np.float32)
reference_3d[:, :2] = reference_shape * [100, 100]
img_vs = np.array([[0, 0, 0], [640, 0, 0], [640, 480, 0], [0, 480, 0]],
dtype=np.float32)
img_vts = np.array([[0, 1], [1, 1], [1, 0], [0, 0]], dtype=np.float32)
img_fs = np.array([[0, 1, 2, 3]], dtype=np.int32)
img_ts = np.array([[1, 0, 0, 0], [0, 1, 0, 1000], [0, 0, 1, 0]],
dtype=np.float32)
geo_mesh = GLMeshes(names=['geo_mesh'],
verts=[geo_vs],
faces=[geo_fs],
transforms=[geo_ts],
bones=[geo_bs],
vts=[geo_vts])
img_mesh = GLMeshes(names=['img_mesh'],
verts=[img_vs],
faces=[img_fs],
transforms=[img_ts],
bones=[None],
vts=[img_vts])
kinect = freenect.init()
tilt, roll = 0, 0
if 1:
kdev = freenect.open_device(kinect, 0)
freenect.set_led(kdev, 0) # turn off LED
freenect.set_tilt_degs(kdev, 25)
kstate = freenect.get_tilt_state(kdev)
freenect.update_tilt_state(kdev)
tilt_angle, tilt_status = kstate.tilt_angle, kstate.tilt_status
ax, ay, az = kstate.accelerometer_x, kstate.accelerometer_y, kstate.accelerometer_z
#bottom facing down: (85, 743, 369, 52, 0)
#right side down: (916, 71, 96, 112, 0)
#front side down: (52, 63, -863, -128, 0)
freenect.close_device(kdev)
y_axis = np.array((ax, ay, az), dtype=np.float32)
y_axis = y_axis / np.linalg.norm(y_axis)
roll = np.degrees(np.arctan2(ax, ay))
tilt = -np.degrees(np.arctan2(az, (ax**2 + ay**2)**0.5))
fovX = 62.0
pan_tilt_roll = (0, tilt, roll)
tx_ty_tz = (0, 1000, 6000)
P = Calibrate.composeP_fromData((fovX, ), (pan_tilt_roll), (tx_ty_tz), 0)
global g_camera_rays, g_camera_mat
h, w = 480 // 2, 640 // 2
coord, pix_coord = make_coords(h, w)
#P = np.eye(3,4,dtype=np.float32)
#P[0,0] = P[1,1] = 2.0
k1, k2 = 0, 0
g_camera_mat = Calibrate.makeMat(P, (k1, k2), [w, h])
K, RT, P, ks, T, wh = g_camera_mat
coord_undist = coord.copy()
Calibrate.undistort_points_mat(coord.reshape(-1, 2), g_camera_mat,
coord_undist.reshape(-1, 2))
g_camera_rays = np.dot(coord_undist,
RT[:2, :3]) # ray directions (unnormalized)
g_camera_rays -= np.dot([-K[0, 2], -K[1, 2], K[0, 0]], RT[:3, :3])
g_camera_rays /= (np.sum(g_camera_rays**2, axis=-1)**0.5).reshape(
h, w, 1) # normalized ray directions
names = ['kinect']
vs = [np.zeros((h * w, 3), dtype=np.float32)]
ts = [np.eye(3, 4, dtype=np.float32)]
vts = [pix_coord * (1.0 / w, 1.0 / h)]
faces = [make_faces(h, w)]
mats = None
geom_mesh = GLMeshes(names=names,
verts=vs,
faces=faces,
transforms=ts,
vts=vts)
layers = {
'geom_mesh': geom_mesh,
'geo_mesh': geo_mesh,
'img_mesh': img_mesh
}
QGLViewer.makeViewer(layers=layers,
mats=mats,
callback=cb,
timeRange=(0, 10000))
def setRT(self, RT, setInterest = True): '''Set the view to match the given extrinsic matrix.''' (self.cameraPan, self.cameraTilt, self.cameraRoll), self.cameraT, self.cameraInterest = Calibrate.decomposeRT(RT, self.cameraInterest, setInterest) if self.cameraInterest == 0: self.cameraInterest = 1e-6
wavFilename = os.path.join(ted_dir, '32T01.WAV')
md = MovieReader.open_file(wavFilename)
c3d_filename = os.path.join(
ted_dir, '201401211653-4Pico-32_Quad_Dialogue_01_Col_wip_02.c3d')
c3d_dict = C3D.read(c3d_filename)
c3d_frames, c3d_fps, c3d_labels = c3d_dict['frames'], c3d_dict[
'fps'], c3d_dict['labels']
if False: # only for cleaned-up data
c3d_subject = 'TedFace'
which = np.where([s.startswith(c3d_subject) for s in c3d_labels])[0]
c3d_frames = c3d_frames[:, which, :]
c3d_labels = [c3d_labels[i] for i in which]
print c3d_labels
if False: # this is for the cleaned-up data (don't apply the other offset...)
offset = Calibrate.composeRT(Calibrate.composeR((0.0, 0.0, 0)),
(0, 0, -8), 0) # 0.902
c3d_frames[:, :, :3] = np.dot(c3d_frames[:, :, :3] - offset[:3, 3],
offset[:3, :3])[:, :, :3]
offset = Calibrate.composeRT(Calibrate.composeR((3.9, -38.7, 0)),
(-159.6, 188.8, 123 - 12), 0) # 0.902
c3d_frames[:, :, :3] = np.dot(c3d_frames[:, :, :3] - offset[:3, 3],
offset[:3, :3])[:, :, :3]
geos = []
dat_directory = os.path.join(os.environ['GRIP_DATA'], 'dat')
if False: # experiments involving deformation transfer
geos_filename = 'geos'
if not os.path.exists(geos_filename):
ted_dir = os.environ['GRIP_DATA']
ted_obj = readFlatObjFlipMouth(os.path.join(ted_dir, 'ted.obj'))
def generate_skeleton_lods(skelDict, Gs=None):
# TODO: First iteration: Improve code and optimise
# TODO: Contains hard coded values (generalise.. actually probably better to use a callback.. lodgenerator visitor)
from GCore import Calibrate
vs, tris, orientation, names = [], [], [], []
if 'jointWidth' not in skelDict: return
jointWidth = skelDict['jointWidth']
jointHeightMultiplier = 1.3
if Gs is None: Gs = skelDict['Gs']
Bs = skelDict['Bs']
lodVerts = skelDict['verts']
lodTris = skelDict['tris']
for jointIdx, jointName in enumerate(skelDict['jointNames']):
if 'Free' in jointName: continue
jointGs = Gs[jointIdx]
jointBs = Bs[jointIdx]
whichAxis = np.where(jointBs == 0)[0]
R, T, _ = Calibrate.decomposeRT(jointGs, 1, False)
jointMeshScale = jointBs.copy()
if jointName == 'root': jointMeshScale = jointMeshScale * 1.4
elif 'Spine' in jointName: jointMeshScale = jointMeshScale * 1.2
jointMeshScale[whichAxis] = jointWidth[jointName]
if jointName == 'VSS_Chest':
jointMeshScale[0] = jointWidth[jointName][0]
jointMeshScale[1] = 120.
jointMeshScale[2] = jointWidth[jointName][1]
axisToggle = np.array([1, 1, 1], dtype=np.float32)
axisToggle[whichAxis] = 0.0
translations = jointMeshScale / 2
if jointName == 'VSS_Chest': translations[0:1] = 0
offset = translations * axisToggle
boneVerts = lodVerts.copy()
for vi, v in enumerate(boneVerts):
v = v * jointMeshScale
if jointName in ['root']:
v = v - offset
v = np.dot(
Calibrate.composeR(
np.array([0, 0, 90], dtype=np.float32)), v.T)
else:
v = v + offset
v = np.dot(jointGs, np.hstack((v, 1)).T)
boneVerts[vi] = v[:3]
tris.append(lodTris + len(vs) * 8)
vs.append(boneVerts)
boneLength = jointBs[np.where(jointBs != 0)[0]]
orientation.append(
0 if boneLength.any() and boneLength[0] < 0 else 1)
names.append(jointName)
v = np.concatenate((vs))
t = np.concatenate((tris)).tolist()
lodAttrs = {
'triangles': v[t],
'verts': v,
'tris': t,
'faces': tris,
'names': names
}
skelDict['visibilityLod'] = lodAttrs
return v, t, vs, tris, orientation, names
def load_xcp_and_x2d(xcp_filename, x2d_filename, raw=False):
'''Load an x2d, xcp pair and make a valid data structure.
The returned cameras are in the order of the x2d file -- as it happens, this is in order of deviceid.
If any particular camera is not in the xcp then it's initialised on the positive x-axis.
If a camera is only in the xcp then it is discarded.'''
from GCore import Calibrate
print ('loading xcp')
vicon_mats,xcp_data = loadXCP(xcp_filename)
xcp_camera_ids = np.array([int(x['DEVICEID']) for x in xcp_data],dtype=np.int32)
camera_names = ['%s:%s'%(x['LABEL'],x['DEVICEID']) for x in xcp_data]
camera_vicon_errors = np.array([x['IMAGE_ERROR'] for x in xcp_data],dtype=np.float32)
#print (camera_names)
#print ('vicon_errors',camera_vicon_errors,np.min(camera_vicon_errors),np.max(camera_vicon_errors),np.mean(camera_vicon_errors))
print ('loading x2d',x2d_filename)
x2d_dict = loadX2D(x2d_filename)
cameras_info = extractCameraInfo(x2d_dict) # W,H,ID per camera
x2d_cids = cameras_info[:,2]
x2ds = [(x[0][:,:2].copy(),x[1]) if raw else frameCentroidsToDets(x, vicon_mats) for x in x2d_dict['frames']]
if not(np.all(xcp_camera_ids == x2d_cids)):
print ('WARNING not all the cameras from the x2d were in the xcp file?') # TODO, we should report this
print (xcp_camera_ids, x2d_cids)
vicon_mats = [vicon_mats[list(xcp_camera_ids).index(ci)] if ci in list(xcp_camera_ids) else Calibrate.makeUninitialisedMat(ci,(w,h)) for w,h,ci in cameras_info]
camera_names = ['CAM_%s'%x for x in x2d_cids]
xcp_camera_ids = [f for f in x2d_cids]
Ps = np.array([m[2]/(np.sum(m[2][0,:3]**2)**0.5) for m in vicon_mats],dtype=np.float32)
headerMetadata = readX2DMetadata(x2d_dict)
return Ps, vicon_mats, x2d_cids, camera_names, x2ds, x2d_dict['header']