def findCloseVerts(xs, threshold=80.0):
import ISCV
cloud = ISCV.HashCloud3D(xs, threshold)
scores, matches, matches_splits = cloud.score(xs)
good = (scores < (threshold**2))
D = [
matches[m0:m1][np.where(good[m0:m1])[0]]
for m0, m1 in zip(matches_splits[:-1], matches_splits[1:])
]
print 'avg verts', np.mean(map(len, D))
#for vi,Di in enumerate(D): Di.discard(vi); D[vi] = np.array(list(Di),dtype=np.int32)
return D
def getMapping(hi_geo, triangles, lo_geo, threshold=20.0):
'''given a hi-res geometry and topology, and a lo-res geometry, find the triangles and barycentric weights that
when applied to the hi-res geometry, best fit the lo-res geometry.
The mapping is returned as a list of weight triples and a list of index triples, per vertex.
The output vertex is the weighted sum of the extracted indicated source vertices.'''
is3D = (hi_geo.shape[1] == 3)
lunterp = lunterp3D if is3D else lunterp2D
numVertsHi = hi_geo.shape[0]
numVertsLo = lo_geo.shape[0]
weights = np.zeros((numVertsLo, 3), dtype=np.float32)
indices = -np.ones((numVertsLo, 3), dtype=np.int32)
import ISCV
cloud = ISCV.HashCloud3D(hi_geo, threshold) if is3D else ISCV.HashCloud2D(
hi_geo, threshold)
scores, matches, matches_splits = cloud.score(lo_geo.copy())
# the indices of the closest 3 hi verts to each lo vert
D = [
matches[m0 + np.argsort(scores[m0:m1])[:3]] if m0 != m1 else []
for m0, m1 in zip(matches_splits[:-1], matches_splits[1:])
]
# for speed-up, compute all the triangles involving each hi vertex.
T = [[] for x in xrange(numVertsHi)]
for ti, tri in enumerate(triangles):
for tj in tri:
T[tj].append(tri)
bads = []
for vi, (lo_x, nearIndices, ws,
xis) in enumerate(zip(lo_geo, D, weights, indices)):
best = -10
for ni in nearIndices:
for tri in T[ni]:
xws = lunterp(hi_geo[tri], lo_x)
sc = np.min(xws)
if sc > best: # pick the best triangle (the one that it's closest to being inside)
best = sc
xis[:] = tri
ws[:] = xws
if best >= 0: break
if best >= 0: break
# the vertex *might* not be inside any of these triangles
if best < -0.1:
bads.append(vi)
ws[:] = 0.0 # ensure there's no weight
xis[:] = -1 # and no label
if len(bads):
print 'vertices outside', len(bads)
print bads[:10], '...'
which = np.where(indices[:, 0] != -1)[0]
print len(which), 'vertices inside'
return which, weights[which], indices[which]
def intersect_rays(x2ds,
splits,
Ps,
mats,
seed_x3ds=None,
tilt_threshold=0.0002,
x2d_threshold=0.01,
x3d_threshold=30.0,
min_rays=3,
numPolishIts=3,
forceRayAgreement=False,
visibility=None):
"""
Given 2D detections, we would like to find bundles of rays from different cameras that have a common solution.
For each pair of rays, we can solve for a 3D point. Each such solve has a residual: we want to find low residual pairs.
Closer together camera pairs and cameras with more unlabelled markers should have more matches.
Visit the camera pairs by order of distance-per-unlabelled-marker score (lower is better).
For a given camera pair, each ray can be given an order which is the tilt (angle between the ray from the camera to
that ray and a line perpendicular to a reference plain containing both camera centres).
tilt = asin(norm(raydir^(c2-c1)).ocdir))
TODO: compare atan2(raydir^(c2-c1).ocdir,|raydir^(c2-c1)^ocdir|)
Precisely the rays with the same tilt (within tolerance) intersect.
This fails only if the first camera is looking directly at the second.
For each pair of cameras, sort the unassigned rays by tilt and read off the matches.
(DON'T match if there are two candidates with the same tilt on the same camera.)
For each match, solve the 3D point.
Naively, this costs ~NumDetections^2.
However, if we project the point in all the cameras and assign rays then we can soak up all the rays in the other cameras.
The maximum number of matches should be ~NumPoints.
So the dominant cost becomes project assign (NumPoints * NumCameras using hash).
Polish all the 3D points.
Check for any 3D merges (DON'T merge if there are two rays from the same camera).
Project all the points in all the cameras and reassign.
Cull any points with fewer than 2 rays.
Potentially repeat for the remaining unassigned rays.
Args:
x2ds (float[][2]): 2D Detections.
splits (int): Indices of ranges of 2Ds per camera.
Ps (?): Projection matrices of the cameras?
mats (GcameraMat[]): Camera Matrices.
seed_x3ds (float[][3]): existing 3D data? Default = None.
tilt_threshold (float): Slack factor for tilt pairing = 0.0002
x2d_threshold (float): What's this? Default = 0.01
x3d_threshold (float): What's this? = 30.0
min_rays (int): Min number of rays to reconstruct. Default = 3.
Returns:
float[][3]: (x3ds_ret) List of 3D points produced as a result of intersecting the 2Ds
int[]: (labels) List of labels corresponding to the x3ds.
Requires:
ISCV.compute_E
ISCV.HashCloud2DList
ISCV.HashCloud3D
clouds.project_assign
"""
Ks = np.array(zip(*mats)[0], dtype=np.float32)
RTs = np.array(zip(*mats)[1], dtype=np.float32)
Ts = np.array(zip(*mats)[4], dtype=np.float32)
if visibility is not None:
ret2 = ISCV.intersect_rays_base(x2ds, splits, Ps, Ks, RTs, Ts,
seed_x3ds, tilt_threshold,
x2d_threshold, x3d_threshold, min_rays,
numPolishIts, forceRayAgreement,
visibility)
else:
ret2 = ISCV.intersect_rays2(x2ds, splits, Ps, Ks, RTs, Ts, seed_x3ds,
tilt_threshold, x2d_threshold,
x3d_threshold, min_rays, numPolishIts,
forceRayAgreement)
return ret2
import itertools
numCameras = len(splits) - 1
numDets = splits[-1]
labels = -np.ones(numDets, dtype=np.int32)
E = ISCV.compute_E(x2ds, splits, Ps)
rays = dets_to_rays(x2ds, splits, mats)
Ts = np.array([m[4] for m in mats], dtype=np.float32)
def norm(a):
return a / (np.sum(a**2)**0.5)
tilt_axes = np.array([
norm(np.dot([-m[0][0, 2], -m[0][1, 2], m[0][0, 0]], m[1][:3, :3]))
for m in mats
],
dtype=np.float32)
corder = np.array(list(itertools.combinations(range(numCameras), 2)),
dtype=np.int32) # all combinations ci < cj
#corder = np.array(np.concatenate([zip(range(ci),[ci]*ci) for ci in xrange(1,numCameras)]),dtype=np.int32)
clouds = ISCV.HashCloud2DList(x2ds, splits, x2d_threshold)
x3ds_ret = []
if seed_x3ds is not None:
x3ds_ret = list(seed_x3ds)
# initialise labels from seed_x3ds
_, labels, _ = clouds.project_assign_visibility(
seed_x3ds, np.arange(len(x3ds_ret), dtype=np.int32), Ps,
x2d_threshold, visibility)
# visit the camera pairs by distance-per-unlabelledmarker
#camDists = np.array([np.sum((Ts - Ti)**2, axis=1) for Ti in Ts],dtype=np.float32)
#for oit in range(10):
#if len(corder) == 0: break
#urcs = np.array([1.0/(np.sum(labels[splits[ci]:splits[ci+1]]==-1)+1e-10) for ci in xrange(numCameras)],dtype=np.float32)
#scmat = camDists*np.array([np.maximum(urcs,uci) for uci in urcs],dtype=np.float32)
#scores = scmat[corder[:,0],corder[:,1]]
#so = np.argsort(scores)
#corder = corder[so]
#for it in range(10):
#if len(corder) == 0: break
#ci,cj = corder[0]
#corder = corder[1:]
for ci in xrange(numCameras):
for cj in xrange(ci + 1, numCameras):
ui, uj = np.where(
labels[splits[ci]:splits[ci + 1]] == -1)[0], np.where(
labels[splits[cj]:splits[cj + 1]] == -1)[0]
if len(ui) == 0 or len(uj) == 0: continue
ui += splits[ci]
uj += splits[cj]
axis = Ts[cj] - Ts[ci]
tilt_i = np.dot(map(norm, np.cross(rays[ui], axis)), tilt_axes[ci])
tilt_j = np.dot(map(norm, np.cross(rays[uj], axis)),
tilt_axes[ci]) # NB tilt_axes[ci] not a bug
io = np.argsort(tilt_i)
jo = np.argsort(tilt_j)
ii, ji = 0, 0
data = []
while ii < len(io) and ji < len(jo):
d0, d1 = tilt_i[io[ii]], tilt_j[jo[ji]]
diff = d0 - d1
if abs(diff) < tilt_threshold:
# test for colliding pairs
# if ii+1 < len(io) and tilt_i[io[ii+1]]-d0 < tilt_threshold: ii+=2; continue
# if ji+1 < len(jo) and tilt_j[jo[ji+1]]-d1 < tilt_threshold: ji+=2; continue
# test for colliding triples
# if ii > 0 and d0-tilt_i[io[ii-1]] < tilt_threshold: ii+=1; continue
# if ji > 0 and d1-tilt_j[jo[ji-1]] < tilt_threshold: ji+=1; continue
d = [ui[io[ii]], uj[jo[ji]]]
data.append(d)
ii += 1
ji += 1
elif diff < 0:
ii += 1
else:
ji += 1
if len(data) != 0:
# intersect rays
for d in data:
E0, e0 = E[d, :, :3].reshape(-1, 3), E[d, :, 3].reshape(-1)
x3d = np.linalg.solve(
np.dot(E0.T, E0) + np.eye(3) * 1e-7, -np.dot(E0.T, e0))
sc, labels_out, _ = clouds.project_assign_visibility(
np.array([x3d], dtype=np.float32),
np.array([0], dtype=np.int32), Ps, x2d_threshold,
visibility)
tmp = np.where(labels_out == 0)[0]
if len(tmp) >= min_rays:
tls_empty = np.where(labels[tmp] == -1)[0]
if len(tls_empty) >= min_rays:
labels[tmp[tls_empty]] = len(x3ds_ret)
x3ds_ret.append(x3d)
# TODO: polish, merge, reassign, cull, repeat
# merge
if False:
x3ds_ret = np.array(x3ds_ret, dtype=np.float32).reshape(-1, 3)
cloud = ISCV.HashCloud3D(x3ds_ret, x3d_threshold)
scores, matches, matches_splits = cloud.score(x3ds_ret)
mergers = np.where(matches_splits[1:] - matches_splits[:-1] > 1)[0]
for li in mergers:
i0, i1 = matches_splits[li:li + 2]
collisions = np.where(scores[i0:i1] < x3d_threshold**2)[0]
if len(collisions) > 1:
collisions += i0
#print 'merger',li,i0,i1,scores[i0:i1] # TODO merge these (frame 7854)
# now cull the seed_x3ds, because they could confuse matters
if seed_x3ds is not None:
labels[np.where(labels < len(seed_x3ds))] = -1
minNumRays1 = np.min(
[len(np.where(labels == l)[0]) for l in np.unique(labels)])
maxNumRays1 = np.max(
[len(np.where(labels == l)[0]) for l in np.unique(labels) if l != -1])
# final polish
x3ds_ret, x3ds_labels, E_x2ds_single, x2ds_single_labels = solve_x3ds(
x2ds, splits, labels, Ps)
# throw away the single rays and their 3d points by renumbering the generated 3d points
# _,labels,_ = clouds.project_assign_visibility(x3ds_ret, None, Ps, x2d_threshold, visibility)
minNumRays3 = np.min(
[len(np.where(labels == l)[0]) for l in np.unique(labels)])
maxNumRays3 = np.max(
[len(np.where(labels == l)[0]) for l in np.unique(labels) if l != -1])
_, labels, _ = clouds.project_assign(x3ds_ret, None, Ps, x2d_threshold)
minNumRays2 = np.min(
[len(np.where(labels == l)[0]) for l in np.unique(labels)])
maxNumRays2 = np.max(
[len(np.where(labels == l)[0]) for l in np.unique(labels) if l != -1])
x3ds_ret, x3ds_labels, E_x2ds_single, x2ds_single_labels = solve_x3ds(
x2ds, splits, labels, Ps)
ret = x3ds_ret, labels
return ret
def getClusterData(self, x3ds):
if self.cloud is None:
cloud = ISCV.HashCloud3D(x3ds, self.x3d_threshold)
self.scores, self.matches, self.matches_splits = cloud.score(x3ds)
return self.scores, self.matches, self.matches_splits