class Demo:
def __init__(self, source):
self.cvim = None
# These variables are internal:
self.stereo_cam = source.cam()
print "Camera is", self.stereo_cam
self.connected = False
self.snail_trail = []
self.source = source
self.inlier_history = []
self.label = 0
self.skel_dist_thresh = 0.1; # in meters
# These variables can be tweaked:
self.fd = FeatureDetectorFast(300)
# self.fd = FeatureDetectorStar(500)
self.ds = DescriptorSchemeCalonder()
self.camera_preview = False
self.vo = VisualOdometer(self.stereo_cam,
scavenge = False,
position_keypoint_thresh = 0.1,
angle_keypoint_thresh = 3*pi/180,
sba=None,
num_ransac_iters=500,
inlier_error_threshold = 3.0)
self.skel = Skeleton(self.stereo_cam,
link_thresh = 100,
descriptor_scheme = self.ds,
optimize_after_addition = False)
self.skel.node_vdist = 0
self.running = -1 # run forever
if self.camera_preview:
cv.NamedWindow("Camera")
cv.MoveWindow("Camera", 0, 700)
def handleFrame(self):
r = False
(w,h,li,ri) = self.source.getImage()
self.f = SparseStereoFrame(li, ri,
disparity_range = 96,
feature_detector = self.fd,
descriptor_scheme = self.ds)
self.vo.handle_frame(self.f)
print self.vo.inl
self.inlier_history = self.inlier_history[-20:] + [self.vo.inl]
# If the VO inlier count falls below 20, we're not well-connected for this link
if self.vo.inl < 20:
self.connected = False
self.f.pose = Pose()
self.snail_trail = []
# Add a frame to graph if:
# Frame zero, or
# There have been N successful preceding frames, and either
# Not connected,
# or if connected, the distance from the youngest graph frame to this frame is > thresh
add_to_graph = None
if self.vo.keyframe.id == 0 and len(self.skel.nodes) == 0:
add_to_graph = "Zero keyframe"
elif min(self.inlier_history) > 20:
if not self.connected:
add_to_graph = "Unconnected - but now recovering good poses"
# This is a new graph, so generate a new label
self.label += 1
else:
relpose = ~self.last_added_pose * self.f.pose
if relpose.distance() > self.skel_dist_thresh:
add_to_graph = "Connected and distance thresh passed"
if add_to_graph:
self.skel.setlabel(self.label)
self.skel.add(self.vo.keyframe, self.connected)
print "ADD %d %s" % (self.f.id, add_to_graph)
self.connected = True
self.last_added_pose = self.f.pose
self.snail_trail = []
# self.ioptimize(10)
self.snail_trail.append(self.f.pose)
# Render the camera view using OpenCV, then load it into an OpenGL texture
if True:
if not self.cvim:
self.cvim = cv.CreateImage((w,h/2), cv.IPL_DEPTH_8U, 3)
im = cv.CreateImage((w,h), cv.IPL_DEPTH_8U, 1)
im2 = cv.CreateImage((w/2,h/2), cv.IPL_DEPTH_8U, 1)
for i,src in enumerate([li, ri]):
cv.SetData(im, str(src.tostring()), w)
cv.Resize(im, im2)
cv.SetImageROI(self.cvim, (i * w / 2, 0, w / 2, h / 2))
cv.CvtColor(im2, self.cvim, cv.CV_GRAY2BGR)
cv.ResetImageROI(self.cvim)
def annotate(cvim):
green = cv.RGB(0,255,0)
red = cv.RGB(0,0,255)
def half(p):
return (p[0] / 2, p[1] / 2)
for (x,y,d) in self.f.features():
cv.Circle(cvim, half((x, y)), 2, green)
d = int(d)
cv.Line(cvim, ((w+x-d)/2, y/2 - 2), ((w+x-d)/2, y/2 + 2), green)
a_features = self.vo.keyframe.features()
b_features = self.f.features()
inliers = set([ (b,a) for (a,b) in self.vo.pe.inl])
outliers = set(self.vo.pairs) - inliers
for (a,b) in inliers:
cv.Line(cvim, half(b_features[b]), half(a_features[a]), green)
for (a,b) in outliers:
cv.Line(cvim, half(b_features[b]), half(a_features[a]), red)
annotate(self.cvim)
glBindTexture(GL_TEXTURE_2D, demo.textures[2])
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, 640, 240, 0, GL_RGB, GL_UNSIGNED_BYTE, self.cvim.tostring())
if self.camera_preview:
cv.ShowImage("Camera", self.cvim)
if cv.WaitKey(10) == 27:
r = True
return r
def anchor(self):
""" Return the current pose of the most recently added skeleton node """
id = max(self.skel.nodes)
return self.skel.newpose(id)
def optimize(self):
self.skel.optimize(1000)
def ioptimize(self,iters = 30):
self.skel.ioptimize(iters)
def report(self):
print
print
print
print "-" * 80
print self.skel.edges
print "-" * 80
def pose(self):
""" Return the current camera pose in the skeleton frame """
if len(self.skel.nodes) == 0:
return self.vo.pose
else:
return self.anchor() * (~self.snail_trail[0] * self.snail_trail[-1])
def map_size(self):
xyz = [demo.skel.newpose(i).xform(0,0,0) for i in demo.skel.nodes]
xrange = max([x for (x,y,z) in xyz]) - min([x for (x,y,z) in xyz])
zrange = max([z for (x,y,z) in xyz]) - min([z for (x,y,z) in xyz])
return max(xrange, zrange)
xs -= 4.5 * 1e-3
f = -0.06
else:
f = 0.0
xp = xs * cos(f) - ys * sin(f)
yp = ys * cos(f) + xs * sin(f)
pylab.plot(xp, yp, c = colors[i], label = vos[i].name())
#pylab.quiver(xp, yp, vo_u[i], vo_v[i], color = colors[i]) #, label = '_nolegend_')
#xk = [ x for j,x in enumerate(vo_x[i]) if j in vos[i].log_keyframes ]
#yk = [ y for j,y in enumerate(vo_y[i]) if j in vos[i].log_keyframes ]
#pylab.scatter(xk, yk, c = colors[i], label = '_nolegend_')
pylab.plot(oe_x, oe_y, c = 'green', label = ground_truth_label)
pts = dict([ (f,skel.newpose(f.id).xform(0,0,0)) for f in skel.nodes ])
nodepts = pts.values()
pval = planar(numpy.array([x for (x,y,z) in nodepts]), numpy.array([y for (x,y,z) in nodepts]), numpy.array([z for (x,y,z) in nodepts]))
print "planarity of skeleton: ", pval
skel.optimize()
pts = dict([ (f,skel.newpose(f).xform(0,0,0)) for f in skel.nodes ])
nodepts = pts.values()
pval = planar(numpy.array([x for (x,y,z) in nodepts]), numpy.array([y for (x,y,z) in nodepts]), numpy.array([z for (x,y,z) in nodepts]))
print "planarity of skeleton: ", pval
skel.plot('blue')
print vos[0].log_keyframes
del nf.lf
del nf.rf
del L
del R
nf.lf = None
f.append(nf)
if skel:
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)
[q.x, q.y, q.z, q.w])[:3, :3], [p.x, p.y, p.z])
gtp = None
print "There are", len(vo.tracks), "tracks"
print "There are", len([t for t in vo.tracks if t.alive]), "live tracks"
print "There are", len(set([t.p[-1] for t in vo.tracks
if t.alive])), "unique endpoints on live tracks"
f = open('dead_reckon.poses', 'w')
for i, p in enumerate(log_dead_reckon):
print >> f, i, p.tolist()
f.close()
f = open('optimized.poses', 'w')
for i in sorted(skel.nodes):
print >> f, i, skel.newpose(i).tolist()
f.close()
fd.summarize_timers()
ds.summarize_timers()
for vo in vos:
print vo.name()
print "distance from start:", vo.pose.distance()
vo.summarize_timers()
print vo.log_keyframes
print
skel.summarize_timers()
skel.dump_timers('skel_timers.pickle')
#skel.trim()
