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)