def handle_array(self, iar):
if self.vo:
t0 = time.time()
self.intervals.append(t0)
if (self.modulo % self.decimate) == 0:
imgR = imgAdapted(iar.images[0])
imgL = imgAdapted(iar.images[1])
af = SparseStereoFrame(imgL, imgR)
if 1:
pose = self.vo.handle_frame(af)
else:
pose = Pose()
#print self.vo.num_frames, pose.xform(0,0,0), pose.quaternion()
p = VOPose()
p.inliers = self.vo.inl
# XXX - remove after camera sets frame_id
p.header = rostools.msg.Header(0, iar.header.stamp, "stereo_link")
p.pose = stdmsg.Pose(stdmsg.Point(*pose.xform(0,0,0)), stdmsg.Quaternion(*pose.quaternion()))
self.pub_vo.publish(p)
self.modulo += 1
self.took.append(time.time() - t0)
if (len(self.took) % 100) == 0:
print len(self.took)
imL, imR = imR, imL
imL = imL.transpose(Image.FLIP_LEFT_RIGHT)
imR = imR.transpose(Image.FLIP_LEFT_RIGHT)
if 1:
for j, vo in enumerate(vos):
f = SparseStereoFrame(imgStereo(imL), imgStereo(imR))
#f = MagicStereoFrame(imgStereo(imL), imgStereo(imR), imD, stereo_cam)
#f = PhonyFrame(~desired_pose, stereo_cam)
#vo.lock_handle_frame(f)
vo.handle_frame(f)
#visualize.viz(vo, f)
(x, y, z) = vo.pose.xform(0, 0, 0)
if 0:
print vo.name(), (x, y, z)
(ex, ey, ez) = desired_pose.xform(0, 0, 0)
Ys[j].append(z - ez)
else:
trajectory[j].append((x, y, z))
def planar(x, y, z):
from scipy import optimize
def silly(sol, args):
a, b, c = sol
x, y, z = args
return sum((y - (a * x + b * z + c))**2)
sol = [1.0, 1.0, 1.0]
sol = optimize.fmin(silly, sol, args=((x, y, z), ))
disparities = []
trajectory = [[] for i in vos]
ground_truth = []
for i in range(8):
print i
if 0:
desired_pose = Pose(rotation(0, 0, 1, 0), (0, 0, 0.01 * i))
else:
theta = (i / 400.0) * math.pi * 2
wobble = 0.25 * (sin(theta * 9.127) + cos(theta * 3.947))
r = 4.0 + wobble
desired_pose = Pose(
rotation(theta, 0, 1,
0), (4.0 - r * math.cos(theta), 0, r * math.sin(theta)))
ground_truth.append(desired_pose.xform(0, 0, 0))
cam = ray_camera(desired_pose, stereo_cam)
if 0: # set to 1 for the first time to generate cache
imL = Image.new("RGB", (640, 480))
imR = Image.new("RGB", (640, 480))
imD = [
Image.new("F", (640, 480)),
Image.new("F", (640, 480)),
Image.new("F", (640, 480))
]
def is_visible(p, pt):
camx, camy, camz = (~p).xform(pt.x, pt.y, pt.z)
if camz <= 0:
return False
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()
xrange = xlim[1] - xlim[0]
