from stereo import ComputedDenseStereoFrame, SparseStereoFrame
from visualodometer import VisualOdometer, Pose, DescriptorSchemeCalonder, DescriptorSchemeSAD, FeatureDetectorFast, FeatureDetector4x4, FeatureDetectorStar, FeatureDetectorHarris, from_xyz_euler
stereo_cam = camera.Camera(
(389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
vo = VisualOdometer(stereo_cam,
feature_detector=FeatureDetectorStar(),
descriptor_scheme=DescriptorSchemeCalonder())
(f0, f1) = [
SparseStereoFrame(Image.open("f%d-left.png" % i),
Image.open("f%d-right.png" % i)) for i in [0, 1]
]
vo.setup_frame(f0)
vo.setup_frame(f1)
pairs = vo.temporal_match(f0, f1)
for (a, b) in pairs:
pylab.plot([f0.kp[a][0], f1.kp[b][0]], [f0.kp[a][1], f1.kp[b][1]])
pylab.imshow(numpy.fromstring(f0.lf.tostring(), numpy.uint8).reshape(480, 640),
cmap=pylab.cm.gray)
pylab.scatter([x for (x, y, d) in f0.kp], [y for (x, y, d) in f0.kp],
label='f0 kp',
c='red')
pylab.scatter([x for (x, y, d) in f1.kp], [y for (x, y, d) in f1.kp],
label='f1 kp',
c='green')
pylab.legend()
pylab.show()
from skeleton import Skeleton
if 0:
skel = Skeleton(cam)
skel.node_vdist = 0
skel.adaptive = False
else:
skel = None
gt = []
f = []
for i in range(2100): # range(1,146):
dir = "/u/jamesb/ros/ros-pkg/vision/vslam/trial"
L = Image.open("%s/%06dL.png" % (dir, i))
R = Image.open("%s/%06dR.png" % (dir, i))
nf = SparseStereoFrame(L, R)
vo.setup_frame(nf)
print i, "kp=", len(nf.kp)
nf.id = len(f)
if vt:
vt.add(nf.lf, nf.descriptors)
if skel:
vo.handle_frame_0(nf)
skel.add(vo.keyframe)
vo.correct(skel.correct_frame_pose, nf)
print len(gt), vo.inl
gt.append(vo.pose.xform(0, 0, 0))
else:
pf = open("trial/%06d.pose.pickle" % i, "r")
gt.append(pickle.load(pf))
pf.close()
if not wheel_pose:
has_moved = True # be conservative until wheel odom starts up
elif not prev_wheel_pose or \
((not flag_15Hz or framecounter%2 == 0) and \
prev_wheel_pose.further_than(wheel_pose, dist_thresh, angle_thresh)):
prev_wheel_pose = wheel_pose
prev_wheel_o = wheel_o
has_moved = True
if has_moved and start <= framecounter:
af = SparseStereoFrame(dcamImage(msg.left_image),
dcamImage(msg.right_image))
# save frames, hope there's enough storage
if framecounter > f1start and framecounter < f1end:
frames1.append(af)
vo.setup_frame(af)
if framecounter > f2start and framecounter < f2end:
frames2.append(af)
vo.setup_frame(af)
for fr in frames1:
vo.check_inliers(af, fr)
print "Inliers: ", vo.inl
# vo.handle_frame(af)
x, y, z = vo.pose.xform(0, 0, 0)
trajectory.append((x, y, z))
vo_x.append(x)
vo_y.append(z)
x1, y1, z1 = vo.pose.xform(0, 0, 1)
vo_u.append(x1 - x)
vo_v.append(z1 - z)
