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