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():
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():
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)