skel.optimize() skel.plot('blue', True) #pylab.plot([x for (x,y,z) in gt], [z for (x,y,z) in gt], c = 'g') pylab.show() node_ids = [f.id for f in skel.nodes] for i in sorted(node_ids): f = open("trial/%06d.pose.pickle" % i, "w") pickle.dump(skel.newpose(i), f) f.close() sys.exit(1) gtc = [p.xform(0, 0, 0) for p in gt] # Write confusion matrix to file - for Patrick if 0: confusion = [[vo.proximity(a, b, True)[0] for a in f] for b in f] pylab.pcolor(numpy.array(confusion)) pylab.show() txt = open("confusion-inliers.txt", "w") for l in confusion: print >> txt, l txt.close() sys.exit(0) if 0: for i in range(146): print i, i + 1, vo.proximity(f[i], f[i + 1], True) sys.exit(0) all_ids = set(range(len(f))) focus = 0
cam = camera.Camera((432.0, 432.0, 0.088981018518518529, 313.78210000000001, 313.78210000000001, 220.40700000000001)) vo = VisualOdometer(cam, scavenge=False, feature_detector=FeatureDetectorFast(), descriptor_scheme=DescriptorSchemeCalonder(), inlier_error_threshold=3.0, sba=None, inlier_thresh=99999, position_keypoint_thresh=0.2, angle_keypoint_thresh=0.15) dir = "dump" f = [] for i in [int(a) for a in sys.argv[1:]]: L = Image.open("%s/%06dL.png" % (dir, i)) R = Image.open("%s/%06dR.png" % (dir, i)) nf = SparseStereoFrame(L, R) vo.setup_frame(nf) nf.id = len(f) f.append(nf) inl, pose = vo.proximity(f[0], f[1]) print "inliers=", inl, "pose", pose.xform(0, 0, 0) pairs = vo.temporal_match(f[0], f[1]) vo.show_pairs(pairs, f[0], f[1])
afs = {} for (id, pose, kp, desc) in db: lf = LibraryFrame(pose, kp, desc) afs[id] = lf vo = VisualOdometer(cam, feature_detector=FeatureDetectorFast(), descriptor_scheme=DescriptorSchemeSAD()) print "%d frames in library" % len(afs) most_overlap = 99999 while most_overlap > 180: ov = [] for i in afs.keys(): for j in [k for k in afs.keys() if k > i]: (overlap, _) = vo.proximity(afs[i], afs[j]) ov.append((overlap, i)) print max(ov) most_overlap = max(ov)[0] del afs[max(ov)[1]] print "%d frames in library" % len(afs) f = open("pruned.pickle", "w") pickle.dump(cam, f) db = [(i, afs[i].pose, afs[i].kp, afs[i].descriptors) for i in afs.keys()] pickle.dump(db, f) f.close() vos = [vo]
self.descriptors = desc afs = {} for (id, pose, kp, desc) in db: lf = LibraryFrame(pose, kp, desc) afs[id] = lf vo = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD()) print "%d frames in library" % len(afs) most_overlap = 99999 while most_overlap > 180: ov = [] for i in afs.keys(): for j in [k for k in afs.keys() if k > i]: (overlap, _) = vo.proximity(afs[i], afs[j]) ov.append((overlap, i)) print max(ov) most_overlap = max(ov)[0] del afs[max(ov)[1]] print "%d frames in library" % len(afs) f = open("pruned.pickle", "w") pickle.dump(cam, f) db = [(i, afs[i].pose, afs[i].kp, afs[i].descriptors) for i in afs.keys()] pickle.dump(db, f) f.close() vos = [ vo ] def metric(af0, af1):