def main(): parser = argparse.ArgumentParser(description='Initial rectangle localization.') parser.add_argument('input', type=argparse.FileType('r')) parser.add_argument('img_a', type=int) parser.add_argument('img_b', type=int) parser.add_argument('px', type=int) parser.add_argument('py', type=int) args = parser.parse_args() ## Initial position guess px, py = args.px, args.py ## Object that provides an interface to the images, and contains ## information such as the camera parameters ds = rectrckr.DataSource(args.input) img = ds.get_image(args.img_a) ## Extract edgels edges = img.extract_edgels(px,py) lx = (edges[1] - edges[0])/2 ly = (edges[3] - edges[2])/2 print px, py, lx, ly kalman = KalmanFilter() kalman.state = array([px,py,lx,ly,0,0,0,0]) kalman.Mobser = array([ [1,0,-1, 0,0,0,0,0], [1,0, 1, 0,0,0,0,0], [0,1, 0,-1,0,0,0,0], [0,1, 0, 1,0,0,0,0], ]) new_data = array([ 50,150,100,200.0 ]) dt = 1.0 ## import cProfile pr = cProfile.Profile() pr.enable() ## fig = figure(1) subplot(1,1,1) fig.tight_layout() log_zh = zeros((args.img_b-args.img_a,4)) log_z = zeros((args.img_b-args.img_a,4)) for k in range(args.img_b-args.img_a): print '--['+'%03d'%(k + args.img_a)+']' + 70 * '-' kalman.predict_state(dt) print 'pred st: ', kalman.state kalman.predict_observations() print 'pred obs:', kalman.z_hat img = ds.get_image(k + args.img_a) px,py = kalman.state[:2] zhat = copy(kalman.z_hat) new_data = img.extract_edgels(px,py) print 'measured:', new_data merr = norm(kalman.z_hat - new_data) print 'measurement error:', merr kalman.update_from_observations(new_data) print 'updt st:', kalman.state # ## Log predicted state and outputs log_zh[k] = zhat log_z[k] = new_data ## ## Plot image and the extracted edges cla() imshow(copy(img.img), cmap=cm.gray) plot(new_data[:2], [py,py], 'k--') plot([px,px], new_data[2:4], 'k--') plot(zhat[:2], [py,py], 'bo') plot([px,px], zhat[2:4], 'bo') plot(px,py, 'bs') plot(new_data[:2], [py,py], 'ro') plot([px,px], new_data[2:4], 'ro') plot(px,py, 'rs') axis([0,img.img.shape[1],img.img.shape[0],0]) savefig(ds.get_output_path(k)) ## pr.dump_stats('mycoutstats.profi') ## savetxt('zh.dat', log_zh) savetxt('z.dat', log_z) code.interact(local=locals())
def main(): parser = argparse.ArgumentParser(description='Initial rectangle localization.') parser.add_argument('input', type=argparse.FileType('r')) parser.add_argument('img_a', type=int) parser.add_argument('img_b', type=int) parser.add_argument('px', type=int) parser.add_argument('py', type=int) args = parser.parse_args() ## Initial position guess px, py = args.px, args.py ## Object that provides an interface to the images, and contains ## information such as the camera parameters ds = rectrckr.DataSource(args.input) imgana = ds.get_image(args.img_a) img = imgana.img ## Extract edgels edges = imgana.extract_edgels(px,py) lx = (edges[1] - edges[0])/2 ly = (edges[3] - edges[2])/2 print px, py, lx, ly kalman = KalmanFilter() kalman.state = array([px,py,lx,ly,0,0,0,0]) kalman.Mobser = array([ [1,0,-1, 0,0,0,0,0], [1,0, 1, 0,0,0,0,0], [0,1, 0,-1,0,0,0,0], [0,1, 0, 1,0,0,0,0], ]) new_data = array([ 50, 150, 100, 200.0 ]) dt = 1.0 ## import cProfile pr = cProfile.Profile() pr.enable() ## fig = figure(1) subplot(1,1,1) fig.tight_layout() vv = array([-1.0,1.0])*20.0 cc = array([.0,1.0])*20.0 log_zh = zeros((args.img_b-args.img_a,4)) log_z = zeros((args.img_b-args.img_a,4)) log_edgels = zeros(((args.img_b-args.img_a)*12,4)) qopt = Quat(1.0,0,0,0) for k in range(args.img_b-args.img_a): print '--['+'%03d'%(k + args.img_a)+']' + 70 * '-' kalman.predict_state(dt) print 'pred st: ', kalman.state kalman.predict_observations() print 'pred obs:', kalman.z_hat imgana = ds.get_image(k + args.img_a) img = imgana.img px,py = kalman.state[:2] lx,ly = kalman.state[2:4] zhat = copy(kalman.z_hat) new_edgels = imgana.extract_moar_edgels(px,py, gs=int(min(lx,ly)/3)) new_data = array([new_edgels[0,0], new_edgels[1,0], new_edgels[2,1], new_edgels[3,1]]) print 'measured:', new_data merr = norm(kalman.z_hat - new_data[:4]) print 'measurement error:', merr kalman.update_from_observations(new_data) print 'updt st:', kalman.state ## Log predicted state and outputs log_zh[k] = zhat log_z[k] = new_data dirs = array([imgana.estimate_direction_at_point(int(round(new_edgels[ed,0])), int(round(new_edgels[ed,1]))) for ed in range(12)]) dirs = array([dd/norm(dd) for dd in dirs]) edgels = ascontiguousarray(array(c_[new_edgels, dirs], dtype=float32)) log_edgels[k*12:(k+1)*12] = edgels corisco = corisco_module.Corisco(edgels) qini = qopt#Quat(1.0,0,0,0) qopt = corisco.estimate_orientation(qini) ## ## Plot image and the extracted edges cla() imshow(copy(img), cmap=cm.gray) plot(new_data[:2], [py,py], 'k--') plot([px,px], new_data[2:4], 'k--') plot(zhat[:2], [py,py], 'bx') plot([px,px], zhat[2:4], 'bx') plot(px,py, 'bs') corisco.plot_edgels(gca()) corisco.plot_vps(gca(), qopt) plot(px,py, 'rs') axis([0,img.shape[1],img.shape[0],0]) savefig(ds.get_output_path(k)) ## pr.dump_stats('mycoutstats.profi') ## savetxt('zh.dat', log_zh) savetxt('z.dat', log_z) savetxt('edgels.dat', log_edgels) code.interact(local=locals())