#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/kk2/%06dL.png" % i), Image.open("../vslam/kk2/%06dR.png" % i)) for i in [670, 671] ]
(f0, f1) = [
ComputedDenseStereoFrame(Image.open("f%d-left.png" % i),
Image.open("f%d-right.png" % i)) for i in [0, 1]
]
d = "/u/jamesb/ros/ros-pkg/vision/vslam/trial"
#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("%s/%06dL.png" % (d,i)), Image.open("%s/%06dR.png" % (d,i))) for i in [276, 278] ]
#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/trial/%06dL.png" % i), Image.open("../vslam/trial/%06dR.png" % i)) for i in [277, 278] ]
#(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/trial/%06dL.png" % i), Image.open("../vslam/trial/%06dR.png" % i)) for i in [0, 1] ]
chipsize = (640, 480)
factor = 640 / chipsize[0]
vo = VisualOdometer(stereo_cam)
vo.process_frame(f0)
vo.process_frame(f1)
pairs = vo.temporal_match(f0, f1)
pairs = [(b, a) for (a, b) in pairs]
def xform(M, x, y, z):
nx = vop.mad(M[0], x, vop.mad(M[1], y, vop.mad(M[2], z, M[3])))
ny = vop.mad(M[4], x, vop.mad(M[5], y, vop.mad(M[6], z, M[7])))
nz = vop.mad(M[8], x, vop.mad(M[9], y, vop.mad(M[10], z, M[11])))
return (nx, ny, nz)
def img_err(f0, f1, RT):
# Each point in f1's point cloud, compute xyz
