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):
