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)
Exemple #3
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
Exemple #4
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 + '/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)
Exemple #6
0
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)
Exemple #7
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)

    #
    # 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)