def workloop(self):
workframe = None
while not rospy.is_shutdown():
while self.frameq.empty() and not rospy.is_shutdown():
time.sleep(0.1)
while not self.frameq.empty() and not rospy.is_shutdown():
afmsg = self.frameq.get()
print "CORR: NEW WORK", "id", afmsg.id
p = Pose()
p.fromlist(afmsg.pose.v)
workframe = LibraryFrame(afmsg.id, p,
[(p.x, p.y, p.d)
for p in afmsg.keypoints],
[d.v for d in afmsg.descriptors])
assert len(workframe.descriptors) == len(workframe.kp)
for d in workframe.descriptors:
assert len(d) == 256
if workframe:
tosearch = list(self.library)
best = 100
best_frame = None
best_diff = None
for essay in tosearch:
prox, diffpose = self.vo.proximity(essay, workframe)
print "at", time.time(
), "essay.id", essay.id, "has prox", prox
if essay.id == workframe.id:
print len(essay.kp), len(workframe.kp)
if prox > best:
best = prox
best_frame = essay
best_diff = diffpose
if best_frame:
print "CORR: sending winner", best_frame.id, "at", time.time(
)
Top = workframe.pose
Tmk = best_frame.pose
Tkp = best_diff
Tmp = Tmk * Tkp
Tmo = Tmp * ~Top
for mat in ["Top", "Tmk", "Tkp", "Tmp", "Tmo", "Tmo*Top"]:
print mat
print eval(mat).M
print
print "corrector has pose Tmp:"
print(Tmo * Top).M
if 1:
a = Tmp.tolist()
b = (Tmo * Top).tolist()
for ai, bi in zip(a, b):
if abs(ai - bi) > 0.001:
print "FAIL"
for mat in ["Top", "Tmp", "Tmo", "Tmo*Top"]:
print mat
print eval(mat).M
assert 0
self.pub_tmo.publish(Pose44(Tmo.tolist()))
if 0:
Tpo = workframe.pose
Tpm = best_diff * best_frame.pose
Tp = ~Tpo
Tom = Top * Tpm
print "Tpo"
print Tpo.M
print "Tpm"
print Tpm.M
print "Computed Tom"
print Tom.M
print "Keyframe in map"
print(Tpo * Tom).M
self.pub_tom.publish(Pose44(Tom.tolist()))
#p = workframe.pose.invert().concatenate(best_frame.pose).concatenate(best_diff)
else:
print "CORR: did not find acceptable winner"
else:
time.sleep(0.1)
def handle_corrections(self, corrmsg):
print "GOT CORRECTION AT", time.time()
Tmo_pose = Pose()
Tmo_pose.fromlist(corrmsg.v)
self.Tmo = Tmo_pose
self.know_state = 'corrected'
def handle_corrections(self, corrmsg): print "GOT CORRECTION AT", time.time() Tmo_pose = Pose() Tmo_pose.fromlist(corrmsg.v) self.Tmo = Tmo_pose self.know_state = 'corrected'
def workloop(self):
workframe = None
while not rospy.is_shutdown():
while self.frameq.empty() and not rospy.is_shutdown():
time.sleep(0.1)
while not self.frameq.empty() and not rospy.is_shutdown():
afmsg = self.frameq.get()
print "CORR: NEW WORK", "id", afmsg.id
p = Pose()
p.fromlist(afmsg.pose.v)
workframe = LibraryFrame(afmsg.id, p, [ (p.x,p.y,p.d) for p in afmsg.keypoints], [d.v for d in afmsg.descriptors])
assert len(workframe.descriptors) == len(workframe.kp)
for d in workframe.descriptors:
assert len(d) == 256
if workframe:
tosearch = list(self.library)
best = 100
best_frame = None
best_diff = None
for essay in tosearch:
prox,diffpose = self.vo.proximity(essay, workframe)
print "at", time.time(), "essay.id", essay.id, "has prox", prox
if essay.id == workframe.id:
print len(essay.kp), len(workframe.kp)
if prox > best:
best = prox
best_frame = essay
best_diff = diffpose
if best_frame:
print "CORR: sending winner", best_frame.id, "at", time.time()
Top = workframe.pose
Tmk = best_frame.pose
Tkp = best_diff
Tmp = Tmk * Tkp
Tmo = Tmp * ~Top
for mat in [ "Top", "Tmk", "Tkp", "Tmp", "Tmo", "Tmo*Top" ]:
print mat
print eval(mat).M
print
print "corrector has pose Tmp:"
print (Tmo * Top).M
if 1:
a = Tmp.tolist()
b = (Tmo * Top).tolist()
for ai,bi in zip(a,b):
if abs(ai - bi) > 0.001:
print "FAIL"
for mat in [ "Top", "Tmp", "Tmo", "Tmo*Top" ]:
print mat
print eval(mat).M
assert 0
self.pub_tmo.publish(Pose44(Tmo.tolist()))
if 0:
Tpo = workframe.pose
Tpm = best_diff * best_frame.pose
Tp = ~Tpo
Tom = Top * Tpm
print "Tpo"
print Tpo.M
print "Tpm"
print Tpm.M
print "Computed Tom"
print Tom.M
print "Keyframe in map"
print (Tpo * Tom).M
self.pub_tom.publish(Pose44(Tom.tolist()))
#p = workframe.pose.invert().concatenate(best_frame.pose).concatenate(best_diff)
else:
print "CORR: did not find acceptable winner"
else:
time.sleep(0.1)
f = -0.06
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_')
if vos[0].sba:
CX = []
CY = []
for sbap in vos[0].posechain:
p = Pose()
p.fromlist(sbap.M)
x, y, z = p.xform(0, 0, 0)
CX.append(x)
CY.append(z)
xs = numpy.array(CX)
ys = numpy.array(CY)
xs -= 4.5 * 1e-3
f = -0.06
xp = xs * cos(f) - ys * sin(f)
yp = ys * cos(f) + xs * sin(f)
pylab.plot(xp, yp, c='magenta', label='with SBA')
pylab.plot(oe_x, oe_y, c='green', label='wheel + IMU odometry')
xlim = pylab.xlim()
ylim = pylab.ylim()
