def cook(self, location, interface, attrs): if not self.useFrame(interface.frame(), attrs['frameRange']): return x3ds = interface.attr('x3ds') if x3ds is None: self.logger.error('No x3ds found at: %s' % location) return x3ds_labels = interface.attr('x3ds_labels') if x3ds_labels is None: self.logger.error('No 3D labels found at: %s' % location) return x2dsLocation = attrs['x2ds'] x2ds, splits = interface.attr('x2ds', atLocation=x2dsLocation), interface.attr( 'x2ds_splits', atLocation=x2dsLocation) if x2ds is None or splits is None: self.logger.error('No detections found at: %s' % x2dsLocation) return calLocation = attrs['calibration'] Ps = interface.attr('Ps', atLocation=calLocation) if Ps is None: self.logger.error('No calibration data found at: %s' % calLocation) return import ISCV x2d_threshold, pred_2d_threshold = 6. / 2000., 100. / 2000 clouds = ISCV.HashCloud2DList(x2ds, splits, max(pred_2d_threshold, x2d_threshold)) sc, labels, _ = clouds.project_assign( np.ascontiguousarray(x3ds, dtype=np.float32), x3ds_labels, Ps, x2d_threshold) mats = interface.attr('mats', atLocation=calLocation) camPositions = np.array([m[4] for m in mats], dtype=np.float32) normals = np.zeros_like(x3ds) for xi, (x3d, label3d) in enumerate(zip(x3ds, x3ds_labels)): camIds = [ interface.findCameraIdFromRayId(rayId, splits) for rayId in np.where(labels == label3d)[0] ] if not camIds: continue # camPos = Ps[camIds][:, :3, 3] camPos = camPositions[camIds] rays = camPos - [x3d] * len(camPos) rays = np.float32([ray / np.linalg.norm(ray) for ray in rays]) raysDps = np.dot(rays, rays.T) bestRay = np.sum(raysDps > 0, axis=0).argmax() # goodRays = np.where(raysDps[bestRay] > 0.05)[0] normals[xi] = rays[bestRay] interface.setAttr('normals', normals)
def test_2D(frames, x3ds, detections, mats, x2d_threshold=0.025): '''Test the labelling of a 2d point sequence by propagating the labels to the next frame.''' import IO print 'loading 2d' print 'num frames', len(frames) prev_x2ds, prev_splits = detections[frames[0]] prev_vels = np.zeros_like(prev_x2ds) clouds = ISCV.HashCloud2DList(prev_x2ds, prev_splits, 6. / 2000.) x3ds_labels = np.arange(len(x3ds), dtype=np.int32) Ps = np.array([m[2] / (m[0][0, 0]) for m in mats], dtype=np.float32) sc, prev_labels, _ = Label.project_assign(clouds, x3ds, x3ds_labels, Ps, 6. / 2000.) ret = [] for fi in frames: x2ds, splits = detections[fi] clouds = ISCV.HashCloud2DList(x2ds, splits, x2d_threshold) sc, labels, vels = clouds.assign_with_vel(prev_x2ds, prev_vels, prev_splits, prev_labels, x2d_threshold) prev_x2ds, prev_splits, prev_labels, prev_vels = x2ds, splits, labels, vels ret.append(labels) return ret
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 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 cook(self, location, interface, attrs): # Get x3ds and 3D labels from the cooked location x3ds = interface.attr('x3ds') if x3ds is None: self.logger.error('Could not find attribute: x3ds') return x3ds_labels = interface.attr('x3ds_labels') if x3ds_labels is None: self.logger.error('Could not find attribute: x3ds_labels') return normals = interface.attr('normals') x2d_threshold = attrs['x2d_threshold'] # Set the detections and calibration locations as the cook location if not defined x2ds_location = attrs['x2ds'] if not x2ds_location: x2ds_location = location calibrationLocation = attrs['calibration'] if not calibrationLocation: calibrationLocation = interface.root() # Fetch 2D and calibration data x2ds = interface.attr('x2ds', atLocation=x2ds_location) x2ds_splits = interface.attr('x2ds_splits', atLocation=x2ds_location) Ps = interface.attr('Ps', atLocation=interface.root() + '/cameras') if x2ds is None or x2ds_splits is None: self.logger.error('2D detection data at %s is not valid' % x2ds_location) return if Ps is None: mats = interface.attr('mats', atLocation=interface.root()) if mats: Ps = np.array([m[2] / (np.sum(m[2][0, :3] ** 2) ** 0.5) for m in mats], dtype=np.float32) else: self.logger.error('Attribute mats not found at %s' % calibrationLocation) self.logger.error('Attribute Ps not found at %s' % calibrationLocation) return # Check if we've got visibility lods if 'skeleton' in attrs and attrs['skeleton']: skeletonLoc = attrs['skeleton'] skelDict = interface.attr('skelDict', atLocation=skeletonLoc) visibilityLod = interface.getChild('visibilityLod', parent=skeletonLoc) if visibilityLod is None: self.logger.error('No visibility LODs found at skeleton: %s' % attrs['skeleton']) return lodNames = visibilityLod['names'] lodTris = visibilityLod['tris'] lodVerts = visibilityLod['verts'] lodNormals = visibilityLod['faceNormals'] tris = lodVerts[lodTris] mats = interface.attr('mats', atLocation=attrs['calibration']) cameraPositions = np.array([m[4] for m in mats], dtype=np.float32) clouds = ISCV.HashCloud2DList(x2ds, x2ds_splits, x2d_threshold) if self.visibility is None: self.visibility = ISCV.ProjectVisibility.create() proj_x2ds = None if attrs['useVisibility'] and normals is not None: self.visibility.setNormalsAndLods(normals, tris, cameraPositions, np.concatenate((lodNormals)), attrs['intersect_threshold'], attrs['generateNormals']) # proj_x2ds, proj_splits, proj_labels = ISCV.project_visibility(x3ds, x3ds_labels, Ps, self.visibility) # score, x2d_labels, residuals = clouds.assign(proj_x2ds, proj_splits, proj_labels, x2d_threshold) score, x2d_labels, residuals = clouds.project_assign_visibility(x3ds, x3ds_labels, Ps, x2d_threshold, self.visibility) elif attrs['useNormals'] and normals is not None: self.visibility.setNormals(normals) proj_x2ds, proj_splits, proj_labels = ISCV.project_visibility(x3ds, x3ds_labels, Ps, self.visibility) score, x2d_labels, residuals = clouds.assign(proj_x2ds, proj_splits, proj_labels, x2d_threshold) else: proj_x2ds, proj_splits, proj_labels = ISCV.project(x3ds, x3ds_labels, Ps) score, x2d_labels, vels = clouds.assign(proj_x2ds, proj_splits, proj_labels, x2d_threshold) if proj_x2ds is not None: projectedLocsAttrs = { 'x2ds': proj_x2ds, 'x2ds_splits': proj_splits, 'labels': proj_labels, 'x2ds_colour': (1.0, 0.0, 0.0, 0.7), 'x2ds_pointSize': 10, 'score': score } if 'showProjected' in attrs and attrs['showProjected']: interface.createChild('projected', 'points2d', attrs=projectedLocsAttrs) else: interface.createChild('projected', 'group', attrs=projectedLocsAttrs) if attrs['newLocation']: locAttrs = { 'x2ds': x2ds, 'x2ds_splits': x2ds_splits, 'labels': x2d_labels, 'x2ds_colour': eval(attrs['colour']), 'x2ds_pointSize': attrs['pointSize'], 'score': score } labelColours = interface.getLabelColours(x2d_labels, eval(attrs['colour'])) if labelColours.any(): locAttrs['x2ds_colours'] = labelColours interface.createChild('assigned', 'points2d', attrs=locAttrs) else: interface.setAttr('labels', x2d_labels, atLocation=x2ds_location) interface.setAttr('score', score) labelColours = interface.getLabelColours(x2d_labels, interface.attr('x2ds_colour', atLocation=x2ds_location)) if labelColours.any(): interface.setAttr('x2ds_colours', labelColours, atLocation=x2ds_location)
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] x3s[tiara_xis] = tiara_true singles = np.where([x in list(x2d_labels) for x in x2s_labels])[0] x2s_labels[singles] = -1
def intersectRaysCB(fi): global x2d_frames, mats, Ps, c3d_frames, view, primitives, primitives2D, track3d, prev_frame, track_orn, orn_graph, boot, g_all_skels, md, orn_mapper, mar_mapper skipping = prev_frame is None or np.abs(fi - prev_frame) > 10 prev_frame = fi view = QApp.view() points, altpoints = primitives g2d = primitives2D[0] frame = x2d_frames[fi] x2ds_data, x2ds_splits = ViconReader.frameCentroidsToDets(frame, mats) g2d.setData(x2ds_data, x2ds_splits) if skipping: x3ds, x3ds_labels = track3d.boot(x2ds_data, x2ds_splits) #trackGraph = Label.TrackGraph() boot = -10 else: x3ds, x3ds_labels = track3d.push(x2ds_data, x2ds_splits) if False: boot = boot + 1 if boot == 0: x2d_threshold_hash = 0.01 penalty = 10.0 # the penalty for unlabelled points. this number should be about 10. to force more complete labellings, set it higher. maxHyps = 500 # the number of hypotheses to maintain. print "booting:" numLabels = len(orn_graph[0]) l2x = -np.ones(numLabels, dtype=np.int32) label_score = ISCV.label_from_graph(x3ds, orn_graph[0], orn_graph[1], orn_graph[2], orn_graph[3], maxHyps, penalty, l2x) clouds = ISCV.HashCloud2DList(x2ds_data, x2ds_splits, x2d_threshold_hash) which = np.array(np.where(l2x != -1)[0], dtype=np.int32) pras_score, x2d_labels, vels = Label.project_assign( clouds, x3ds[l2x[which]], which, Ps, x2d_threshold=x2d_threshold_hash) print fi, label_score, pras_score labelled_x3ds = x3ds[l2x[which]] print track_orn.bootPose(x2ds_data, x2ds_splits, x2d_labels) if boot > 0: track_orn.push(x2ds_data, x2ds_splits, its=4) #x3ds,x2ds_labels = Recon.intersect_rays(x2ds_data, x2ds_splits, Ps, mats, seed_x3ds = None) points.setData(x3ds) if c3d_frames != None: c3ds = c3d_frames[(fi - 832) / 2] true_labels = np.array(np.where(c3ds[:, 3] == 0)[0], dtype=np.int32) x3ds_true = c3ds[true_labels, :3] altpoints.setData(x3ds_true) ci = view.cameraIndex() - 1 if True: #ci == -1: MovieReader.readFrame(md, seekFrame=max((fi - 14) / 4, 0)) QApp.app.refreshImageData() (orn_skel_dict, orn_t) = g_all_skels['orn'] orn_mesh_dict, orn_skel_mesh, orn_geom_mesh = orn_t orn_anim_dict = orn_skel_dict['anim_dict'] orn_skel_dict['chanValues'][:] = orn_anim_dict['dofData'][fi] Character.updatePoseAndMeshes(orn_skel_dict, orn_skel_mesh, orn_geom_mesh) (mar_skel_dict, mar_t) = g_all_skels['mar'] mar_anim_dict = mar_skel_dict['anim_dict'] mar_mesh_dict, mar_skel_mesh, mar_geom_mesh = mar_t Character.updatePoseAndMeshes(mar_skel_dict, mar_skel_mesh, mar_geom_mesh, mar_anim_dict['dofData'][fi]) from PIL import Image #orn_geom_mesh.setImage((md['vbuffer'],(md['vheight'],md['vwidth'],3))) #orn_geom_mesh.refreshImage() w, h = 1024, 1024 cam = view.cameras[0] cam.refreshImageData(view) aspect = float(max(1, cam.bindImage.width())) / float( cam.bindImage.height()) if cam.bindImage is not None else 1.0 orn_mapper.project(orn_skel_dict['geom_Vs'], aspect) data = Opengl.renderGL(w, h, orn_mapper.render, cam.bindId) orn_geom_mesh.setImage(data) mar_mapper.project(mar_skel_dict['geom_Vs'], aspect) data = Opengl.renderGL(w, h, mar_mapper.render, cam.bindId) mar_geom_mesh.setImage(data) #image = Image.fromstring(mode='RGB', size=(w, h), data=data) #image = image.transpose(Image.FLIP_TOP_BOTTOM) #image.save('screenshot.png') if 0: global g_screen image = Opengl.renderGL(1920, 1080, Opengl.quad_render, (cam.bindId, g_screen)) import pylab as pl pl.imshow(image) pl.show() view.updateGL()