def errorFunction(X, n_cameras, n_points, x2d_splits, x2ds_labels, x2ds): camera_params = X[:n_cameras * 11].reshape((n_cameras, 11)) x3ds = X[n_cameras * 11:].reshape((n_points, 3)) projected_x2ds = np.zeros_like(x2ds) for camVec, c0, c1 in zip(camera_params, x2d_splits[:-1], x2d_splits[1:]): P, distortion = vecToMat(camVec) x3d_labels = np.int32([x2ds_labels[i] for i in xrange(c0, c1)]) proj_x2ds, proj_splits, proj_labels = ISCV.project(np.float32(x3ds[x3d_labels]), x3d_labels, np.float32([P])) assert np.all(x3d_labels == proj_labels) ISCV.distort_points(proj_x2ds, float(camVec[9]), float(camVec[10]), float(distortion[0]), float(distortion[1]), proj_x2ds) projected_x2ds[c0:c1, :] = proj_x2ds return (projected_x2ds - x2ds).ravel()
def project(self, x3ds, aspect):
# set the buffer as the texture
K, RT, P, ks, T, wh = self.mat
#num_pts = len(x3ds)
#x2ds, splits, labels = ISCV.project(x3ds, np.arange(num_pts,dtype=np.int32), P[:3,:4].reshape(1,3,4))
#x2s = 1e10*np.ones((num_pts,2),dtype=np.float32)
#x2s[labels,:] = x2ds
# project the 3D vertices into the camera using the projection matrix
proj = np.dot(x3ds, P[:3, :3].T) + P[:3, 3]
ds = -proj[:, 2]
x2s = proj[:, :2] / ds.reshape(-1, 1)
# distort the projections using the camera lens
ISCV.distort_points(x2s, float(-K[0, 2]), float(-K[1, 2]),
float(ks[0]), float(ks[1]), x2s)
# convert to texture coordinates
x2s *= [0.5, -0.5 * aspect]
x2s += 0.5
self.x2ds = x2s
if 0: # wip
self.ds = ds
GL.glClearColor(0, 0, 0, 1)
GL.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT)
GL.glMatrixMode(GL.GL_PROJECTION)
GL.glLoadIdentity()
GL.glMultMatrixf(
np.array([[1, 0, 0, 0], [0, aspect, 0, 0], [0, 0, -1, -1],
[0, 0, cameraInterest * -0.02, 0]],
dtype=np.float32))
GL.glMatrixMode(GL.GL_MODELVIEW)
GL.glLoadIdentity()
GL.glMultMatrixd(P.T)
GL.glDisable(GL.GL_TEXTURE_2D)
GL.glEnable(GL.GL_DEPTH_TEST)
GL.glShadeModel(GL.GL_FLAT)
GL.glDisable(GL.GL_LIGHTING)
GL.glEnableClientState(GL.GL_VERTEX_ARRAY)
GL.glVertexPointerf(x3ds)
self.tris.bind()
GL.glDrawElementsui(GL.GL_TRIANGLES, self.tris)
self.tris.unbind()
def cook(self, location, interface, attrs):
if not self.useFrame(interface.frame(), attrs['frameRange']): return
if not attrs['calibration'] or not attrs['x3ds']: return
calibrationLocation = attrs['calibration']
Ps = interface.attr('Ps', atLocation=calibrationLocation)
mats = interface.attr('mats', atLocation=calibrationLocation)
if Ps is None:
if mats is None:
self.logger.warning('Could not find calibration data at: %s' %
calibrationLocation)
return
Ps = interface.getPsFromMats(mats)
if Ps is None: return
# Get the x3ds (and labels if available) from the cooked location
x3ds = interface.attr('x3ds', atLocation=attrs['x3ds'])
if x3ds is None:
self.logger.error('No 3D points found at: %s' % attrs['x3ds'])
return
which_labels = interface.attr('which_labels')
if which_labels is None:
which_labels = np.arange(len(x3ds))
x3ds = np.ascontiguousarray(x3ds, dtype=np.float32)
normals = interface.attr('normals', atLocation=attrs['x3ds'])
if 'x3dIndex' in attrs and attrs['x3dIndex'] >= 0:
idx = attrs['x3dIndex']
x3ds = x3ds[idx].reshape(1, -1)
which_labels = [idx]
# Check if we've got visibility lods
visibilityLod = None
if 'skeleton' in attrs and attrs['skeleton']:
skeletonLoc = attrs['skeleton']
skelDict = interface.attr('skelDict', atLocation=skeletonLoc)
visibilityLod = interface.getChild('visibilityLod',
parent=skeletonLoc)
if attrs['useVisibility'] and visibilityLod is None:
self.logger.error('No visibility LODs found at skeleton: %s' %
attrs['skeleton'])
return
mats = interface.attr('mats', atLocation=calibrationLocation)
cameraPositions = np.array([m[4] for m in mats], dtype=np.float32)
if self.visibility is None:
self.visibility = ISCV.ProjectVisibility.create()
# if attrs['useVisibility'] and normals is not None and visibilityLod is not None:
if attrs['useVisibility'] and visibilityLod is not None:
lodNames = visibilityLod['names']
lodTris = visibilityLod['tris']
lodVerts = visibilityLod['verts']
lodNormals = visibilityLod['faceNormals']
tris = lodVerts[lodTris]
if attrs['useNormals'] and normals is not None:
self.visibility.setNormalsAndLods(normals, tris,
cameraPositions,
np.concatenate((lodNormals)),
attrs['intersect_threshold'],
attrs['generateNormals'])
else:
self.visibility.setLods(tris, cameraPositions,
np.concatenate((lodNormals)),
attrs['intersect_threshold'],
attrs['generateNormals'])
x2ds, x2ds_splits, x2d_labels = ISCV.project_visibility(
x3ds, which_labels, Ps, self.visibility)
elif attrs['useNormals'] and normals is not None:
self.visibility.setNormals(normals)
x2ds, x2ds_splits, x2d_labels = ISCV.project_visibility(
x3ds, which_labels, Ps, self.visibility)
else:
x2ds, x2ds_splits, x2d_labels = ISCV.project(
x3ds, which_labels, Ps)
# Distort if needed
if 'distort' in attrs and attrs['distort']:
for ci, (s, e) in enumerate(zip(x2ds_splits[:-1],
x2ds_splits[1:])):
K, RT, P, ks, T, wh = mats[ci]
dets = x2ds[s:e]
ISCV.distort_points(dets, float(-K[0, 2]), float(-K[1, 2]),
float(ks[0]), float(ks[1]), dets)
x2ds[s:e] = dets
detsAttrs = {
'x2ds': x2ds,
'x2ds_splits': x2ds_splits,
'labels': x2d_labels,
'x2ds_colour': eval(attrs['colour']),
'x2ds_pointSize': attrs['pointSize']
}
if 'cameraOffset' in attrs and attrs['cameraOffset'] > 0:
x2ds_splits_render = np.insert(
x2ds_splits, np.zeros(attrs['cameraOffset'], dtype=np.int32),
0)
detsAttrs['x2ds_splits_render'] = x2ds_splits_render
interface.createChild(interface.name(),
'detections',
atLocation=interface.parentPath(),
attrs=detsAttrs)