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 K(self): '''Yields the intrinsic matrix for the view, K.''' return Calibrate.composeK(self.cameraFovX,self.cameraKox,self.cameraKoy,self.cameraKsquare,self.cameraKskew)
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)
(219.5 - 2., -180. - 2., 202. - 1.), (0.291979, 0.228389)),
(307, 76.41, (0.036673, 0.081622),
(-20.4 - 1.2 + 2. - 2.1 - 0.6, 37.0 + 1.1 + 1.3 + 0.1 - 0.3 - 0.2,
101.5 + 0.3 - 0.3 + 1.0 + 0.7),
(215. - 1 - 5 + 8 - 1, -126. + 1 + 4 + 3 - 3,
182. - 2.5 - 3), (0.292402446, 0.214726448)),
(215, 76.4, (0.019527, 0.037984), (29.3 - 0.2, 35.0 - 0.2,
-104.6 + 0.1),
(63 + 2., -119 - 1, 170 + 1), (0.29172, 0.22112)),
(261, 76.7, (-0.050844, 0.078558),
(18.1 - 0.3, 24.8 + 1.2, -101.7), (81. + 2, -158 + 4,
199), (0.29115, 0.228548)),
]
for ci, (c, x) in enumerate(zip(cams, xcp)):
interest, fovX, (ox, oy), pan_tilt_roll, tx_ty_tz, distortion = c
K, RT = Calibrate.composeK(fovX, ox=ox,
oy=oy), Calibrate.composeRT(
Calibrate.composeR(pan_tilt_roll),
tx_ty_tz, 0)
K, RT = Calibrate.decomposeKRT(
xcp[ci][2]) # !!!!use the original values!!!!
P = np.dot(K, RT)[:3, :]
P2 = x[2]
print 'diff', np.dot(P2[:3, :3], np.linalg.pinv(
P[:3, :3])), 'dt', P[:, 3] - P2[:, 3]
RT = np.dot(RT, offset)
xcp[ci] = [
K[:3, :3], RT[:3, :4],
np.dot(K, RT)[:3, :], distortion,
-np.dot(RT[:3, :3].T, RT[:3, 3])
]