def main():
import sys
from glob import iglob
from itertools import groupby
np.set_printoptions(precision=4, suppress=True)
folder = sys.argv[1]
saved_files = iglob(folder + '/*.lh0')
hmodels = [ HomographyModel.load_from_file(f) for f in saved_files ]
print '%d hmodels\n' % len(hmodels)
itag_getter = lambda e: e.itag
hmodels.sort(key=itag_getter)
def random_combinations(n, m):
n_choose_m = [ x for x in xrange(2**n) if bin(x).count('1') == m ]
return np.random.choice(n_choose_m, 200, replace=False)
def take_by_bitindex(list_, bits_as_int):
indices = xrange(len(list_)-1, -1, -1)
bool_bits = ( bool(bits_as_int & 2**i) for i in indices )
return [ entry for entry, bit in zip(list_, bool_bits) if bit ]
for itag, group in groupby(hmodels, key=itag_getter):
group = list(group)
homographies = [ hm.homography_at_center() for hm in group ]
# K = robust_estimate_intrinsics_noskew(homographies)
# print np.array(matrix_to_intrinsics(K))
K = estimate_intrinsics_noskew(homographies)
print np.array(matrix_to_intrinsics(K))
bootstrap_estimates = []
for combination in random_combinations(len(homographies), 3):
subset = take_by_bitindex(homographies, combination)
try:
K = estimate_intrinsics_noskew(subset)
bootstrap_estimates.append(matrix_to_intrinsics(K))
except:
pass
bootstrap_estimates = np.vstack(bootstrap_estimates)
from scipy.spatial.distance import pdist, squareform
distances = squareform(pdist(bootstrap_estimates))
medoid_ix = np.argmin(distances.mean(axis=0))
print bootstrap_estimates[medoid_ix]
print np.median(bootstrap_estimates, axis=0)
def main():
import sys
from glob import iglob
from itertools import groupby
np.set_printoptions(precision=4, suppress=True)
folder = sys.argv[1]
saved_files = iglob(folder + '/*.lh0')
hmodels = [HomographyModel.load_from_file(f) for f in saved_files]
print '%d hmodels\n' % len(hmodels)
itag_getter = lambda e: e.itag
hmodels.sort(key=itag_getter)
def random_combinations(n, m):
n_choose_m = [x for x in xrange(2**n) if bin(x).count('1') == m]
return np.random.choice(n_choose_m, 200, replace=False)
def take_by_bitindex(list_, bits_as_int):
indices = xrange(len(list_) - 1, -1, -1)
bool_bits = (bool(bits_as_int & 2**i) for i in indices)
return [entry for entry, bit in zip(list_, bool_bits) if bit]
for itag, group in groupby(hmodels, key=itag_getter):
group = list(group)
homographies = [hm.homography_at_center() for hm in group]
# K = robust_estimate_intrinsics_noskew(homographies)
# print np.array(matrix_to_intrinsics(K))
K = estimate_intrinsics_noskew(homographies)
print np.array(matrix_to_intrinsics(K))
bootstrap_estimates = []
for combination in random_combinations(len(homographies), 3):
subset = take_by_bitindex(homographies, combination)
try:
K = estimate_intrinsics_noskew(subset)
bootstrap_estimates.append(matrix_to_intrinsics(K))
except:
pass
bootstrap_estimates = np.vstack(bootstrap_estimates)
from scipy.spatial.distance import pdist, squareform
distances = squareform(pdist(bootstrap_estimates))
medoid_ix = np.argmin(distances.mean(axis=0))
print bootstrap_estimates[medoid_ix]
print np.median(bootstrap_estimates, axis=0)
def refine_homography_subset(args):
""" Load homographies pickled in `homography_files` and
choose a subset of these homographies according to `bit_index`.
Refine this subset of homographies """
homography_files, bit_index = args
def take_by_bitindex(list_, bits_as_int):
indices = xrange(len(list_)-1, -1, -1)
bool_bits = ( bool(bits_as_int & 2**i) for i in indices )
return [ entry for entry, bit in zip(list_, bool_bits) if bit ]
homography_files = take_by_bitindex(homography_files, bit_index)
hmodels = [ HomographyModel.load_from_file(f) for f in homography_files ]
#
# Deconstruct information in the HomographyModels into
# a graph of nodes and constraints
#
graph = ConstraintGraph()
itag_getter = lambda e: e.itag
hmodels.sort(key=itag_getter)
for itag, group in groupby(hmodels, key=itag_getter):
group = list(group)
homographies = [ hm.homography_at_center() for hm in group ]
K = estimate_intrinsics_noskew(homographies)
inode = IntrinsicsNode(*matrix_to_intrinsics(K), tag=itag)
# For each HomographyModel in `group` construct an ExtrinsicsNode
enode_tags = [ '%s/%s' % (itag, hm.etag) for hm in group ]
E_matrices = ( get_extrinsics_from_homography(H, K) for H in homographies )
enodes = [ ExtrinsicsNode(*matrix_to_xyzrph(E), tag=tag) for E, tag in zip(E_matrices, enode_tags) ]
# Instantiate constraints between each ExtrinsicsNode and the single IntrinsicsNode
constraints = [ HomographyConstraint(hm, inode, enode) for hm, enode in zip(group, enodes) ]
# Add nodes and constraints to graph
graph.inodes[itag] = inode
graph.enodes.update( (e.tag, e) for e in enodes )
graph.constraints.extend( constraints )
#
# Optimize graph to reduce error in constraints
#
def objective(x):
graph.state = x
return graph.constraint_errors()
x0 = graph.state
result = root(objective, x0, method='lm', options={'factor': 0.1, 'col_deriv': 1})
graph.state = result.x
return graph.istate
def load_from_file(class_, filename):
# parse the filename to get intrinsic/extrinsic tags
filestem = os.path.splitext(filename)[0]
itag, etag = filestem.split('/')[-2:]
with open(filestem + '.lh0+') as f:
K, E = pickle.load(f)
with open(filestem + '.uv') as f:
undistortion = pickle.load(f)
# create and populate instance
instance = class_()
instance.etag = etag
instance.itag = itag
instance.intrinsics = matrix_to_intrinsics(K)
instance.extrinsics = matrix_to_xyzrph(E)
instance.undistortion = undistortion
return instance
def load_from_file(class_, filename):
# parse the filename to get intrinsic/extrinsic tags
filestem = os.path.splitext(filename)[0]
itag, etag = filestem.split('/')[-2:]
with open(filestem + '.lh0+') as f:
K, E = pickle.load(f)
with open(filestem + '.uv') as f:
undistortion = pickle.load(f)
# create and populate instance
instance = class_()
instance.etag = etag
instance.itag = itag
instance.intrinsics = matrix_to_intrinsics(K)
instance.extrinsics = matrix_to_xyzrph(E)
instance.undistortion = undistortion
return instance
def main():
import sys
from glob import iglob
from itertools import groupby
np.set_printoptions(precision=4, suppress=True)
folder = sys.argv[1]
saved_files = iglob(folder + '/pose?/*.lh0')
hmodels = [HomographyModel.load_from_file(f) for f in saved_files]
#
# Deconstruct information in the HomographyModels into
# a graph of nodes and constraints
#
graph = ConstraintGraph()
#
# Construct intrinsic nodes
#
itag_getter = lambda e: e.itag
hmodels.sort(key=itag_getter)
for itag, group in groupby(hmodels, key=itag_getter):
homographies = [hm.homography_at_center() for hm in group]
K = estimate_intrinsics_noskew(homographies)
graph.inodes[itag] = IntrinsicsNode(*matrix_to_intrinsics(K), tag=itag)
#
# Construct extrinsic nodes
#
etag_getter = lambda e: e.etag
hmodels.sort(key=etag_getter)
for etag, group in groupby(hmodels, key=etag_getter):
estimates = []
for hm in group:
K = graph.inodes.get(hm.itag).to_matrix()[:, :3]
E = get_extrinsics_from_homography(hm.homography_at_center(), K)
estimates.append(matrix_to_xyzrph(E))
graph.enodes[etag] = ExtrinsicsNode(*np.mean(estimates, axis=0),
tag=etag)
print 'Graph'
print '-----'
print ' %d intrinsic nodes' % len(graph.inodes)
print ' %d extrinsic nodes' % len(graph.enodes)
print ''
#
# Connect nodes by constraints
#
for hm in hmodels:
inode = graph.inodes[hm.itag]
enode = graph.enodes[hm.etag]
constraint = HomographyConstraint(hm, inode, enode)
graph.constraints.append(constraint)
rmse = np.sqrt(constraint.sq_unweighted_reprojection_errors().mean())
print ' %s %s rmse: %.2f' % (hm.etag, hm.itag, rmse)
#
# Optimize graph to reduce error in constraints
#
def print_graph_summary(title):
print '\n' + title
print '-' * len(title)
print ' rmse: %.4f' % np.sqrt(graph.sq_pixel_errors().mean())
print ' rmaxse: %.4f' % np.sqrt(graph.sq_pixel_errors().max())
print ''
for itag, inode in graph.inodes.iteritems():
print ' intrinsics@ ' + itag + " =", np.array(inode.to_tuple())
print ' extrinsics@ pose0 =', np.array(
graph.enodes['pose0'].to_tuple())
def objective(x):
graph.state = x
return graph.constraint_errors()
def optimize_graph():
x0 = graph.state
print_graph_summary('Initial:')
print '\nOptimizing graph ...'
result = root(objective,
x0,
method='lm',
options={
'factor': 0.1,
'col_deriv': 1
})
print ' Success: ' + str(result.success)
print ' %s' % result.message
graph.state = result.x
print_graph_summary('Final:')
print '\n'
print '====================='
print ' Optimization 1'
print '====================='
print ' Optimizing all intrinsics and extrinisics'
optimize_graph()
#
# Now optimize with just the constraints of
# the poses required for estimating distortion
#
candidate_poses = set(sys.argv[2:])
candidate_constraints = (c for c in graph.constraints
if isinstance(c, HomographyConstraint))
candidate_constraints = [
c for c in candidate_constraints if c.enode.tag in candidate_poses
]
graph.constraints = candidate_constraints
print '\n'
print '====================='
print ' Optimization 2'
print '====================='
print ' Optimizing candidate intrinsics (%d constraints)' % len(
graph.constraints)
optimize_graph()
#
# Write out the refined intrinsics and extrinsics
#
homography_constraints = (c for c in graph.constraints
if isinstance(c, HomographyConstraint))
for constraint in homography_constraints:
etag, itag = constraint.enode.tag, constraint.inode.tag
K = constraint.inode.to_matrix()
E = constraint.enode.to_matrix()
H = K.dot(E)
filename = '%s/%s/%s.lh0+' % (folder, etag, itag)
with open(filename, 'w') as f:
pickle.dump((K, E), f)
def main():
import sys
from glob import iglob
from itertools import groupby
np.set_printoptions(precision=4, suppress=True)
folder = sys.argv[1]
saved_files = iglob(folder + '/*/*.lh0')
hmodels = [ HomographyModel.load_from_file(f) for f in saved_files ]
print '%d hmodels\n' % len(hmodels)
#
# Deconstruct information in the HomographyModels into
# a graph of nodes and constraints
#
graph = ConstraintGraph()
itag_getter = lambda e: e.itag
hmodels.sort(key=itag_getter)
for itag, group in groupby(hmodels, key=itag_getter):
group = list(group)
homographies = [ hm.homography_at_center() for hm in group ]
K = estimate_intrinsics_noskew(homographies)
inode = IntrinsicsNode(*matrix_to_intrinsics(K), tag=itag)
# For each HomographyModel in `group` construct an ExtrinsicsNode
enode_tags = [ '%s/%s' % (itag, hm.etag) for hm in group ]
E_matrices = ( get_extrinsics_from_homography(H, K) for H in homographies )
enodes = [ ExtrinsicsNode(*matrix_to_xyzrph(E), tag=tag) for E, tag in zip(E_matrices, enode_tags) ]
# Instantiate constraints between each ExtrinsicsNode and the single IntrinsicsNode
constraints = [ HomographyConstraint(hm, inode, enode) for hm, enode in zip(group, enodes) ]
# Leave out the worst two constraints
if False:
rmse = [ c.sq_unweighted_reprojection_errors().mean() for c in constraints ]
inliner_idx = np.argsort(rmse)[:-2]
enodes = [ enodes[i] for i in inliner_idx ]
constraints = [ constraints[i] for i in inliner_idx ]
# Add nodes and constraints to graph
graph.inodes[itag] = inode
graph.enodes.update( (e.tag, e) for e in enodes )
graph.constraints.extend( constraints )
print 'Graph'
print '-----'
print ' %d intrinsic nodes' % len(graph.inodes)
print ' %d extrinsic nodes' % len(graph.enodes)
print ' %d constraints' % len(graph.constraints)
print ''
for constraint in (c for c in graph.constraints if isinstance(c, HomographyConstraint)):
rmse = np.sqrt(constraint.sq_unweighted_reprojection_errors().mean())
print ' %s rmse: %.2f' % (constraint.enode.tag, rmse)
#
# Optimize graph to reduce error in constraints
#
def print_graph_summary(title):
print '\n' + title
print '-'*len(title)
print ' rmse: %.4f' % np.sqrt(graph.sq_pixel_errors().mean())
print ' rmaxse: %.4f' % np.sqrt(graph.sq_pixel_errors().max())
print ''
for itag, inode in graph.inodes.iteritems():
print ' intrinsics@ ' + itag + " =", np.array(inode.to_tuple())
def objective(x):
graph.state = x
return graph.constraint_errors()
def optimize_graph():
x0 = graph.state
print_graph_summary('Initial:')
print '\nOptimizing graph ...'
result = root(objective, x0, method='lm', options={'factor': 0.1, 'col_deriv': 1})
print ' Success: ' + str(result.success)
print ' %s' % result.message
graph.state = result.x
print_graph_summary('Final:')
print '\n'
print '====================='
print ' Optimization'
print '====================='
optimize_graph()
print ' '
for constraint in (c for c in graph.constraints if isinstance(c, HomographyConstraint)):
if constraint.enode.tag.endswith("/pose0"):
rmse = np.sqrt(constraint.sq_unweighted_reprojection_errors().mean())
print ' %s rmse: %.2f' % (constraint.enode.tag, rmse)
#
# Write out the refined intrinsics and extrinsics
#
homography_constraints = ( c for c in graph.constraints if isinstance(c, HomographyConstraint) )
for constraint in homography_constraints:
etag, itag = constraint.enode.tag, constraint.inode.tag
K = constraint.inode.to_matrix()
E = constraint.enode.to_matrix()
filename = '%s/%s.lh0+' % (folder, etag)
with open(filename, 'w') as f:
pickle.dump((K, E), f)
def main():
matplotlib.rcParams.update({'font.size': 10})
with open(sys.argv[1] + '/intrinsics.model') as f:
data, focus_model, cx_model, cy_model = pickle.load(f)
zoom, fx, fy, cx, cy = data.T
calibration_data = []
for filename in glob.iglob(sys.argv[1] + '/*/pose0.lh0+'):
filestem = os.path.splitext(filename)[0]
itag, etag = filestem.split('/')[-2:]
with open(filename) as f:
K, E = pickle.load(f)
k = matrix_to_intrinsics(K)
calibration_data.append([float(itag), k[0], k[1], k[2], k[3] ])
calibration_data = np.vstack(calibration_data)
plt.figure(figsize=(6,8))
plt.subplot(211)
plt.title('focal lengths')
plt.grid(b=True, which='major', color='#ededed', linestyle='-')
plt.plot(zoom, fy, '.', color='#DA7C30', markersize=3)
plt.plot(zoom, fx, '.', color='#CC2529', markersize=3)
plt.plot(calibration_data[:,0], calibration_data[:,1], 'k+')
r = np.arange(18, 70, 0.1)
p = np.polyval(focus_model, r)
plt.plot(r, p, '-', color='#396AB1', linewidth=2)
plt.ylabel('pixels')
plt.xticks([18, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
plt.xlim( (15, 75) )
plt.ylim( (0, 10000) )
plt.setp( plt.gca().get_xticklabels(), visible=False )
plt.gca().set_axisbelow(True)
# plt.subplot(312)
# plt.title('fy')
# plt.grid(b=True, which='major', color='#ededed', linestyle='-')
# plt.plot(zoom, fy, '.', color='#DA7C30', markersize=3)
# plt.plot(calibration_data[:,0], calibration_data[:,2], 'k+')
# r = np.arange(18, 70, 0.1)
# p = np.polyval(focus_model, r)
# plt.plot(r, p, '-', color='#396AB1', linewidth=2)
# plt.xticks([18, 20, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
# plt.xlim( (10, 80) )
# plt.ylim( (0, 10000) )
# plt.setp( plt.gca().get_yticklabels(), visible=False )
# plt.setp( plt.gca().get_xticklabels(), visible=False )
plt.subplot(212)
plt.title('principal point')
plt.grid(b=True, which='major', color='#ededed', linestyle='-')
plt.plot(zoom, cx, '.', color='#CC2529', markersize=3)
plt.plot(calibration_data[:,0], calibration_data[:,3], 'k+')
r = np.arange(18, 70, 0.1)
p = np.polyval(cx_model, r)
plt.plot(r, p, '-', color='#396AB1', linewidth=2)
plt.text(18, 1350, 'cx', fontdict={'size': 10})
plt.xticks([18, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
plt.xlim( (15, 75) )
plt.ylim( (800, 1700) )
plt.xlabel('lens zoom setting')
plt.ylabel('pixels')
plt.subplot(212)
plt.grid(b=True, which='major', color='#ededed', linestyle='-')
plt.plot(zoom, cy, '.', color='#DA7C30', markersize=3)
plt.plot(calibration_data[:,0], calibration_data[:,4], 'k+')
r = np.arange(18, 70, 0.1)
p = np.polyval(cy_model, r)
plt.plot(r, p, '-', color='#396AB1', linewidth=2)
plt.text(18, 1125, 'cy', fontdict={'size': 10})
plt.xticks([18, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
plt.xlim( (15, 75) )
plt.ylim( (800, 1700) )
plt.xlabel('lens zoom setting')
plt.gca().set_axisbelow(True)
#plt.setp( plt.gca().get_yticklabels(), visible=False )
plt.tight_layout()
plt.show()
def main():
np.set_printoptions(precision=4, suppress=True)
import sys
homography_info = [ get_homography_estimate(f) for f in sys.argv[1:] ]
#
# Estimate intrinsics
#
from camera_math import estimate_intrinsics_noskew
from camera_math import get_extrinsics_from_homography
from camera_math import matrix_to_xyzrph, matrix_to_intrinsics
K = estimate_intrinsics_noskew([ hinf.H for hinf in homography_info])
#
# Setup factor graph
#
graph = ConstraintGraph()
inode = IntrinsicsNode('in', *matrix_to_intrinsics(K))
graph.inodes['in'] = inode
for i, hinf in enumerate(homography_info):
E = get_extrinsics_from_homography(hinf.H, K)
enode = ExtrinsicsNode('pose%d'%i, *matrix_to_xyzrph(E))
c = HomographyConstraint(hinf.corrs, hinf.imshape, inode, enode)
graph.enodes['pose%d'%i] = enode
graph.constraints.append(c)
print 'Graph'
print '-----'
print ' %d intrinsic nodes' % len(graph.inodes)
print ' %d extrinsic nodes' % len(graph.enodes)
print ' %d constraints' % len(graph.constraints)
print ''
for constraint in (c for c in graph.constraints if isinstance(c, HomographyConstraint)):
rmse = np.sqrt(constraint.sq_errors().mean())
print ' %s rmse: %.2f' % (constraint.enode.tag, rmse)
#
# Optimize graph to reduce error in constraints
#
def print_graph_summary(title):
print '\n' + title
print '-'*len(title)
print ' rmse: %.4f' % np.sqrt(graph.sq_pixel_errors().mean())
print ' rmaxse: %.4f' % np.sqrt(graph.sq_pixel_errors().max())
print ''
for itag, inode in graph.inodes.iteritems():
print ' intrinsics@ ' + itag + " =", np.array(inode.to_tuple())
def objective(x):
graph.state = x
return graph.constraint_errors()
def optimize_graph():
x0 = graph.state
print_graph_summary('Initial:')
print '\nOptimizing graph ...'
from scipy.optimize import root
result = root(objective, x0, method='lm', options={'factor': 100, 'col_deriv': 1})
print ' Success: ' + str(result.success)
print ' %s' % result.message
graph.state = result.x
print_graph_summary('Final:')
print '\n'
print '====================='
print ' Optimization'
print '====================='
optimize_graph()
import os.path
folder = os.path.dirname(sys.argv[1])
import cPickle as pickle
warp = ClassicLensWarp(inode, homography_info[0].imshape)
with open(folder + '/classic.poly', 'w') as f:
pickle.dump(warp, f)
def main():
matplotlib.rcParams.update({'font.size': 10})
with open(sys.argv[1] + '/intrinsics.model') as f:
data, focus_model, cx_model, cy_model = pickle.load(f)
zoom, fx, fy, cx, cy = data.T
calibration_data = []
for filename in glob.iglob(sys.argv[1] + '/*/pose0.lh0+'):
filestem = os.path.splitext(filename)[0]
itag, etag = filestem.split('/')[-2:]
with open(filename) as f:
K, E = pickle.load(f)
k = matrix_to_intrinsics(K)
calibration_data.append([float(itag), k[0], k[1], k[2], k[3]])
calibration_data = np.vstack(calibration_data)
plt.figure(figsize=(6, 8))
plt.subplot(211)
plt.title('focal lengths')
plt.grid(b=True, which='major', color='#ededed', linestyle='-')
plt.plot(zoom, fy, '.', color='#DA7C30', markersize=3)
plt.plot(zoom, fx, '.', color='#CC2529', markersize=3)
plt.plot(calibration_data[:, 0], calibration_data[:, 1], 'k+')
r = np.arange(18, 70, 0.1)
p = np.polyval(focus_model, r)
plt.plot(r, p, '-', color='#396AB1', linewidth=2)
plt.ylabel('pixels')
plt.xticks([18, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
plt.xlim((15, 75))
plt.ylim((0, 10000))
plt.setp(plt.gca().get_xticklabels(), visible=False)
plt.gca().set_axisbelow(True)
# plt.subplot(312)
# plt.title('fy')
# plt.grid(b=True, which='major', color='#ededed', linestyle='-')
# plt.plot(zoom, fy, '.', color='#DA7C30', markersize=3)
# plt.plot(calibration_data[:,0], calibration_data[:,2], 'k+')
# r = np.arange(18, 70, 0.1)
# p = np.polyval(focus_model, r)
# plt.plot(r, p, '-', color='#396AB1', linewidth=2)
# plt.xticks([18, 20, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
# plt.xlim( (10, 80) )
# plt.ylim( (0, 10000) )
# plt.setp( plt.gca().get_yticklabels(), visible=False )
# plt.setp( plt.gca().get_xticklabels(), visible=False )
plt.subplot(212)
plt.title('principal point')
plt.grid(b=True, which='major', color='#ededed', linestyle='-')
plt.plot(zoom, cx, '.', color='#CC2529', markersize=3)
plt.plot(calibration_data[:, 0], calibration_data[:, 3], 'k+')
r = np.arange(18, 70, 0.1)
p = np.polyval(cx_model, r)
plt.plot(r, p, '-', color='#396AB1', linewidth=2)
plt.text(18, 1350, 'cx', fontdict={'size': 10})
plt.xticks([18, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
plt.xlim((15, 75))
plt.ylim((800, 1700))
plt.xlabel('lens zoom setting')
plt.ylabel('pixels')
plt.subplot(212)
plt.grid(b=True, which='major', color='#ededed', linestyle='-')
plt.plot(zoom, cy, '.', color='#DA7C30', markersize=3)
plt.plot(calibration_data[:, 0], calibration_data[:, 4], 'k+')
r = np.arange(18, 70, 0.1)
p = np.polyval(cy_model, r)
plt.plot(r, p, '-', color='#396AB1', linewidth=2)
plt.text(18, 1125, 'cy', fontdict={'size': 10})
plt.xticks([18, 24, 28, 31, 35, 40, 44, 50, 55, 60, 65, 70])
plt.xlim((15, 75))
plt.ylim((800, 1700))
plt.xlabel('lens zoom setting')
plt.gca().set_axisbelow(True)
#plt.setp( plt.gca().get_yticklabels(), visible=False )
plt.tight_layout()
plt.show()
def main():
import sys
from glob import iglob
from itertools import groupby
np.set_printoptions(precision=4, suppress=True)
folder = sys.argv[1]
saved_files = iglob(folder + '/*/*.lh0')
hmodels = [HomographyModel.load_from_file(f) for f in saved_files]
print '%d hmodels\n' % len(hmodels)
#
# Deconstruct information in the HomographyModels into
# a graph of nodes and constraints
#
graph = ConstraintGraph()
itag_getter = lambda e: e.itag
hmodels.sort(key=itag_getter)
for itag, group in groupby(hmodels, key=itag_getter):
group = list(group)
homographies = [hm.homography_at_center() for hm in group]
K = estimate_intrinsics_noskew(homographies)
inode = IntrinsicsNode(*matrix_to_intrinsics(K), tag=itag)
# For each HomographyModel in `group` construct an ExtrinsicsNode
enode_tags = ['%s/%s' % (itag, hm.etag) for hm in group]
E_matrices = (get_extrinsics_from_homography(H, K)
for H in homographies)
enodes = [
ExtrinsicsNode(*matrix_to_xyzrph(E), tag=tag)
for E, tag in zip(E_matrices, enode_tags)
]
# Instantiate constraints between each ExtrinsicsNode and the single IntrinsicsNode
constraints = [
HomographyConstraint(hm, inode, enode)
for hm, enode in zip(group, enodes)
]
# Leave out the worst two constraints
if False:
rmse = [
c.sq_unweighted_reprojection_errors().mean()
for c in constraints
]
inliner_idx = np.argsort(rmse)[:-2]
enodes = [enodes[i] for i in inliner_idx]
constraints = [constraints[i] for i in inliner_idx]
# Add nodes and constraints to graph
graph.inodes[itag] = inode
graph.enodes.update((e.tag, e) for e in enodes)
graph.constraints.extend(constraints)
print 'Graph'
print '-----'
print ' %d intrinsic nodes' % len(graph.inodes)
print ' %d extrinsic nodes' % len(graph.enodes)
print ' %d constraints' % len(graph.constraints)
print ''
for constraint in (c for c in graph.constraints
if isinstance(c, HomographyConstraint)):
rmse = np.sqrt(constraint.sq_unweighted_reprojection_errors().mean())
print ' %s rmse: %.2f' % (constraint.enode.tag, rmse)
#
# Optimize graph to reduce error in constraints
#
def print_graph_summary(title):
print '\n' + title
print '-' * len(title)
print ' rmse: %.4f' % np.sqrt(graph.sq_pixel_errors().mean())
print ' rmaxse: %.4f' % np.sqrt(graph.sq_pixel_errors().max())
print ''
for itag, inode in graph.inodes.iteritems():
print ' intrinsics@ ' + itag + " =", np.array(inode.to_tuple())
def objective(x):
graph.state = x
return graph.constraint_errors()
def optimize_graph():
x0 = graph.state
print_graph_summary('Initial:')
print '\nOptimizing graph ...'
result = root(objective,
x0,
method='lm',
options={
'factor': 0.1,
'col_deriv': 1
})
print ' Success: ' + str(result.success)
print ' %s' % result.message
graph.state = result.x
print_graph_summary('Final:')
print '\n'
print '====================='
print ' Optimization'
print '====================='
optimize_graph()
print ' '
for constraint in (c for c in graph.constraints
if isinstance(c, HomographyConstraint)):
if constraint.enode.tag.endswith("/pose0"):
rmse = np.sqrt(
constraint.sq_unweighted_reprojection_errors().mean())
print ' %s rmse: %.2f' % (constraint.enode.tag, rmse)
#
# Write out the refined intrinsics and extrinsics
#
homography_constraints = (c for c in graph.constraints
if isinstance(c, HomographyConstraint))
for constraint in homography_constraints:
etag, itag = constraint.enode.tag, constraint.inode.tag
K = constraint.inode.to_matrix()
E = constraint.enode.to_matrix()
filename = '%s/%s.lh0+' % (folder, etag)
with open(filename, 'w') as f:
pickle.dump((K, E), f)
def main():
import sys
from glob import iglob
from itertools import groupby
np.set_printoptions(precision=4, suppress=True)
folder = sys.argv[1]
saved_files = iglob(folder + '/pose?/*.lh0')
hmodels = [ HomographyModel.load_from_file(f) for f in saved_files ]
#
# Deconstruct information in the HomographyModels into
# a graph of nodes and constraints
#
graph = ConstraintGraph()
#
# Construct intrinsic nodes
#
itag_getter = lambda e: e.itag
hmodels.sort(key=itag_getter)
for itag, group in groupby(hmodels, key=itag_getter):
homographies = [ hm.homography_at_center() for hm in group ]
K = estimate_intrinsics_noskew(homographies)
graph.inodes[itag] = IntrinsicsNode(*matrix_to_intrinsics(K), tag=itag)
#
# Construct extrinsic nodes
#
etag_getter = lambda e: e.etag
hmodels.sort(key=etag_getter)
for etag, group in groupby(hmodels, key=etag_getter):
estimates = []
for hm in group:
K = graph.inodes.get(hm.itag).to_matrix()[:,:3]
E = get_extrinsics_from_homography(hm.homography_at_center(), K)
estimates.append(matrix_to_xyzrph(E))
graph.enodes[etag] = ExtrinsicsNode(*np.mean(estimates, axis=0), tag=etag)
print 'Graph'
print '-----'
print ' %d intrinsic nodes' % len(graph.inodes)
print ' %d extrinsic nodes' % len(graph.enodes)
print ''
#
# Connect nodes by constraints
#
for hm in hmodels:
inode = graph.inodes[hm.itag]
enode = graph.enodes[hm.etag]
constraint = HomographyConstraint(hm, inode, enode)
graph.constraints.append(constraint)
rmse = np.sqrt(constraint.sq_unweighted_reprojection_errors().mean())
print ' %s %s rmse: %.2f' % (hm.etag, hm.itag, rmse)
#
# Optimize graph to reduce error in constraints
#
def print_graph_summary(title):
print '\n' + title
print '-'*len(title)
print ' rmse: %.4f' % np.sqrt(graph.sq_pixel_errors().mean())
print ' rmaxse: %.4f' % np.sqrt(graph.sq_pixel_errors().max())
print ''
for itag, inode in graph.inodes.iteritems():
print ' intrinsics@ ' + itag + " =", np.array(inode.to_tuple())
print ' extrinsics@ pose0 =', np.array(graph.enodes['pose0'].to_tuple())
def objective(x):
graph.state = x
return graph.constraint_errors()
def optimize_graph():
x0 = graph.state
print_graph_summary('Initial:')
print '\nOptimizing graph ...'
result = root(objective, x0, method='lm', options={'factor': 0.1, 'col_deriv': 1})
print ' Success: ' + str(result.success)
print ' %s' % result.message
graph.state = result.x
print_graph_summary('Final:')
print '\n'
print '====================='
print ' Optimization 1'
print '====================='
print ' Optimizing all intrinsics and extrinisics'
optimize_graph()
#
# Now optimize with just the constraints of
# the poses required for estimating distortion
#
candidate_poses = set(sys.argv[2:])
candidate_constraints = ( c for c in graph.constraints if isinstance(c, HomographyConstraint) )
candidate_constraints = [ c for c in candidate_constraints if c.enode.tag in candidate_poses ]
graph.constraints = candidate_constraints
print '\n'
print '====================='
print ' Optimization 2'
print '====================='
print ' Optimizing candidate intrinsics (%d constraints)' % len(graph.constraints)
optimize_graph()
#
# Write out the refined intrinsics and extrinsics
#
homography_constraints = ( c for c in graph.constraints if isinstance(c, HomographyConstraint) )
for constraint in homography_constraints:
etag, itag = constraint.enode.tag, constraint.inode.tag
K = constraint.inode.to_matrix()
E = constraint.enode.to_matrix()
H = K.dot(E)
filename = '%s/%s/%s.lh0+' % (folder, etag, itag)
with open(filename, 'w') as f:
pickle.dump((K, E), f)