def xtest_image_pan(self): cam = camera.Camera((1.0, 1.0, 89.23, 320., 320., 240.0)) vo = VisualOdometer(cam) prev_af = None pose = None im = Image.open("img1.pgm") for x in [0,5]: # range(0,100,10) + list(reversed(range(0, 100, 10))): lf = im.crop((x, 0, x + 640, 480)) rf = im.crop((x, 0, x + 640, 480)) af = SparseStereoFrame(lf, rf) vo.find_keypoints(af) vo.find_disparities(af) vo.collect_descriptors(af) if prev_af: pairs = vo.temporal_match(prev_af, af) pose = vo.solve(prev_af.kp, af.kp, pairs) for i in range(10): old = prev_af.kp[pairs[i][0]] new = af.kp[pairs[i][1]] print old, new, new[0] - old[0] prev_af = af print "frame", x, "has", len(af.kp), "keypoints", pose
def xtest_image_pan(self): cam = camera.Camera((1.0, 1.0, 89.23, 320., 320., 240.0)) vo = VisualOdometer(cam) prev_af = None pose = None im = Image.open("img1.pgm") for x in [0, 5]: # range(0,100,10) + list(reversed(range(0, 100, 10))): lf = im.crop((x, 0, x + 640, 480)) rf = im.crop((x, 0, x + 640, 480)) af = SparseStereoFrame(lf, rf) vo.find_keypoints(af) vo.find_disparities(af) vo.collect_descriptors(af) if prev_af: pairs = vo.temporal_match(prev_af, af) pose = vo.solve(prev_af.kp, af.kp, pairs) for i in range(10): old = prev_af.kp[pairs[i][0]] new = af.kp[pairs[i][1]] print old, new, new[0] - old[0] prev_af = af print "frame", x, "has", len(af.kp), "keypoints", pose
def test_sad(self): cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95)) vo = VisualOdometer(cam) class adapter: def __init__(self, im): self.rawdata = im.tostring() self.size = im.size im = adapter(Image.open("img1.pgm")) vo.feature_detector.thresh *= 15 vo.find_keypoints(im) im.kp = im.kp2d vo.collect_descriptors(im) print len(im.kp) matches = vo.temporal_match(im, im) for (a,b) in matches: self.assert_(a == b)
def test_sad(self): cam = camera.Camera((389.0, 389.0, 89.23, 323.42, 323.42, 274.95)) vo = VisualOdometer(cam) class adapter: def __init__(self, im): self.rawdata = im.tostring() self.size = im.size im = adapter(Image.open("img1.pgm")) vo.feature_detector.thresh *= 15 vo.find_keypoints(im) im.kp = im.kp2d vo.collect_descriptors(im) print len(im.kp) matches = vo.temporal_match(im, im) for (a, b) in matches: self.assert_(a == b)
if cam and topic.endswith("videre/images"): imgR = imgAdapted(msg.images[0]) imgL = imgAdapted(msg.images[1]) assert msg.images[0].label == "right_rectified" assert msg.images[1].label == "left_rectified" frame = SparseStereoFrame(imgL, imgR) vo.find_keypoints(frame) vo.find_disparities(frame) #frame.kp = [ (x,y,d) for (x,y,d) in frame.kp if d > 8] all_ds += [d for (x, y, d) in frame.kp] vo.collect_descriptors(frame) if prev_frame: pairs = vo.temporal_match(prev_frame, frame) solution = vo.solve(prev_frame.kp, frame.kp, pairs, True) (inl, rot, shift) = solution sos += numpy.array(shift) print sos prev_frame = frame framecounter += 1 ds[filename] = all_ds for i, (filename, ds) in enumerate(ds.items()): pylab.figure(i + 1) pylab.title(filename) pylab.hist(ds, 50, normed=1, facecolor='green', alpha=0.75) pylab.show()
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()
if cam and topic.endswith("videre/images"): imgR = imgAdapted(msg.images[0]) imgL = imgAdapted(msg.images[1]) assert msg.images[0].label == "right_rectified" assert msg.images[1].label == "left_rectified" frame = SparseStereoFrame(imgL, imgR) vo.find_keypoints(frame) vo.find_disparities(frame) #frame.kp = [ (x,y,d) for (x,y,d) in frame.kp if d > 8] all_ds += [ d for (x,y,d) in frame.kp ] vo.collect_descriptors(frame) if prev_frame: pairs = vo.temporal_match(prev_frame, frame) solution = vo.solve(prev_frame.kp, frame.kp, pairs, True) (inl, rot, shift) = solution sos += numpy.array(shift) print sos prev_frame = frame framecounter += 1 ds[filename] = all_ds for i,(filename, ds) in enumerate(ds.items()): pylab.figure(i+1) pylab.title(filename) pylab.hist(ds, 50, normed=1, facecolor='green', alpha=0.75) pylab.show()
cam = camera.Camera((432.0, 432.0, 0.088981018518518529, 313.78210000000001, 313.78210000000001, 220.40700000000001)) vo = VisualOdometer(cam, scavenge=False, feature_detector=FeatureDetectorFast(), descriptor_scheme=DescriptorSchemeCalonder(), inlier_error_threshold=3.0, sba=None, inlier_thresh=99999, position_keypoint_thresh=0.2, angle_keypoint_thresh=0.15) dir = "dump" f = [] for i in [int(a) for a in sys.argv[1:]]: L = Image.open("%s/%06dL.png" % (dir, i)) R = Image.open("%s/%06dR.png" % (dir, i)) nf = SparseStereoFrame(L, R) vo.setup_frame(nf) nf.id = len(f) f.append(nf) inl, pose = vo.proximity(f[0], f[1]) print "inliers=", inl, "pose", pose.xform(0, 0, 0) pairs = vo.temporal_match(f[0], f[1]) vo.show_pairs(pairs, f[0], f[1])