Example #1
0
def two_view_reconstruction(p1, p2, f1, f2, threshold):
    assert len(p1) == len(p2)
    assert len(p1) >= 5
    npoints = len(p1)

    # Run 5-point algorithm.
    R, t, inliers = csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)

    K1 = np.array([[f1, 0, 0], [0, f1, 0], [0, 0, 1.]])
    K2 = np.array([[f2, 0, 0], [0, f2, 0], [0, 0, 1.]])

    old_inliers = np.zeros(npoints)
    for it in range(10):
        # Triangulate Features.
        P1 = P_from_KRt(K1, np.eye(3), np.zeros(3))
        P2 = P_from_KRt(K2, R, t)
        Ps = [P1, P2]
        Xs = []
        inliers = []
        for x1, x2 in zip(p1, p2):
            X = triangulate(Ps, [x1, x2])
            Xs.append(X)
            P1X = P1.dot(homogeneous(X))
            P2X = P2.dot(homogeneous(X))
            e1 = np.linalg.norm(x1 - euclidean(P1X))
            e2 = np.linalg.norm(x2 - euclidean(P2X))
            inliers.append(e1 < threshold and e2 < threshold and P1X[2] > 0 and P2X[2] > 0)
        inliers = np.array(inliers)
        Xs = np.array(Xs)

        inlier_changes = np.count_nonzero(inliers - old_inliers)
        old_inliers = inliers.copy()
        if inlier_changes < npoints * 0.05:
            break

        # Refine R, t
        ba = csfm.BundleAdjuster()
        ba.add_camera('c1', f1, 0, 0, f1, True)
        ba.add_camera('c2', f2, 0, 0, f2, True)
        ba.add_shot('s1', 'c1', 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, True)
        r = cv2.Rodrigues(R)[0].ravel()
        ba.add_shot('s2', 'c2', r[0], r[1], r[2], t[0], t[1], t[2], 0, 0, 0, 1, False)
        for i in xrange(npoints):
            if inliers[i]:
                X = Xs[i]
                ba.add_point(str(i), X[0], X[1], X[2], False)
                ba.add_observation('s1', str(i), p1[i][0], p1[i][1])
                ba.add_observation('s2', str(i), p2[i][0], p2[i][1])

        ba.set_loss_function('TruncatedLoss', threshold);
        ba.run()
        s = ba.get_shot('s2')
        R = cv2.Rodrigues((s.rx, s.ry, s.rz))[0]
        t = np.array((s.tx, s.ty, s.tz))
        t /= np.linalg.norm(t)

    return R, t, Xs, np.nonzero(inliers)[0]
Example #2
0
def bootstrap_reconstruction(data, graph, im1, im2):
    '''Starts a reconstruction using two shots.
    '''
    print 'Initial reconstruction with', im1, 'and', im2
    d1 = data.load_exif(im1)
    d2 = data.load_exif(im2)
    cameras = data.load_camera_models()

    tracks, p1, p2 = dataset.common_tracks(graph, im1, im2)
    print 'Number of common tracks', len(tracks)

    f1 = d1['focal_prior']
    f2 = d2['focal_prior']
    threshold = data.config.get('five_point_algo_threshold', 0.006)
    ret = csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)
    if ret is not None:
        R, t, cov, inliers = ret
    else:
        return None
    if len(inliers):
        print 'Number of inliers', len(inliers)
        reconstruction = {
            "cameras": cameras,

            "shots" : {
                im1: {
                    "camera": str(d1['camera']),
                    "rotation": [0.0, 0.0, 0.0],
                    "translation": [0.0, 0.0, 0.0],
                },
                im2: {
                    "camera": str(d2['camera']),
                    "rotation": list(R),
                    "translation": list(t),
                },
            },

            "points" : {
            },
        }
        add_gps_position(data, reconstruction['shots'][im1], im1)
        add_gps_position(data, reconstruction['shots'][im2], im2)
        triangulate_shot_features(
                    graph, reconstruction, im1,
                    data.config.get('triangulation_threshold', 0.004),
                    data.config.get('triangulation_min_ray_angle', 2.0))
        print 'Number of reconstructed 3D points :{}'.format(len(reconstruction['points']))
        if len(reconstruction['points']) > data.config.get('five_point_algo_min_inliers', 50):
            print 'Found initialize good pair', im1 , 'and', im2
            return reconstruction

    print 'Pair', im1, ':', im2, 'fails'
    return None
Example #3
0
def two_view_reconstruction_run_csfm(p1, p2, f1, f2, threshold):
    return csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)
Example #4
0
def two_view_reconstruction_run_csfm(p1, p2, f1, f2, threshold):
    return csfm.two_view_reconstruction(p1, p2, f1, f2, threshold)