fundamentalmatrix.py

from __future__ import division
import numpy as np
from skimage import io, color
import matplotlib.pyplot as plt
import vlfeat as vlf
import os
import math

# Reduced matches
l1,d1 = vlf.read_features_from_file('bust_matches.txt')

i = 0
reduced_matches = np.zeros((l1.shape[0], 4))
for k in xrange(l1.shape[0]):
    if (l1[k, 0] - l1[k, 2])**2 + (l1[k, 1] - l1[k, 3])**2 < 200**2:
        reduced_matches[i, :] = l1[k, :]
        i += 1 
        
reduced_matches = reduced_matches[0:i, :]

# Normalization
mean_x1 = np.mean(reduced_matches[:, 0])
mean_y1 = np.mean(reduced_matches[:, 1])
d1 = np.std(reduced_matches[:, 0:2])
T1 = np.array([[math.sqrt(2)/d1, 0, -mean_x1*math.sqrt(2)/d1], [0, math.sqrt(2)/d1, -mean_y1*math.sqrt(2)/d1], [0, 0, 1]])

mean_x2 = np.mean(reduced_matches[:, 2])
mean_y2 = np.mean(reduced_matches[:, 3])
d2 = np.std(reduced_matches[:, 2:4])
T2 = np.array([[math.sqrt(2)/d2, 0, -mean_x2*math.sqrt(2)/d2], [0, math.sqrt(2)/d2, -mean_y2*math.sqrt(2)/d2], [0, 0, 1]])

temp1 = np.zeros((3, len(reduced_matches)))
temp2 = np.zeros((3, len(reduced_matches)))

temp1[0, :] = reduced_matches[:, 0]
temp1[1, :] = reduced_matches[:, 1]
temp1[2, :] = np.ones((1, len(reduced_matches)))

temp2[0, :] = reduced_matches[:, 2]
temp2[1, :] = reduced_matches[:, 3]
temp2[2, :] = np.ones((1, len(reduced_matches)))

new_x = np.zeros((3, len(reduced_matches)))
for s in xrange(len(reduced_matches)):
    new_x[:, s] = np.dot(T1, temp1[:, s])

new_xx = np.zeros((3, len(reduced_matches)))
for r in xrange(len(reduced_matches)):
    new_xx[:, r] = np.dot(T2, temp2[:, r])

final = np.zeros((new_x.shape[1], 4))
final[:, 0] = new_x[0, :].T
final[:, 1] = new_x[1, :].T
final[:, 2] = new_xx[0, :].T
final[:, 3] = new_xx[1, :].T

# Find a fundamental matrix (RANSAC)
def ransac_homography(l1, max_iter, inlier_threshold):
    best_noi = 0
    
    for i in range(0, max_iter):
        rand = l1.shape[0]*np.random.random_sample((8,))
        for p in range(0,8):
            rand[p] = int(rand[p])
        A1 = np.array([[l1[rand[0], 0], l1[rand[0], 1], 1, 0, 0, 0, -l1[rand[0], 2]*l1[rand[0],0],       
        -l1[rand[0], 2]*l1[rand[0], 1], -l1[rand[0], 2]],
        [0, 0, 0, l1[rand[0], 0], l1[rand[0], 1], 1, -l1[rand[0], 3]*l1[rand[0], 0], 
        -l1[rand[0], 3]*l1[rand[0], 1], -l1[rand[0], 1]],
        [l1[rand[1], 0], l1[rand[1], 1], 1, 0, 0, 0, -l1[rand[1], 2]*l1[rand[1], 0],
        -l1[rand[1], 2]*l1[rand[1], 1], -l1[rand[1], 2]], 
        [0, 0, 0, l1[rand[1], 0], l1[rand[1], 1], 1, -l1[rand[1], 3]*l1[rand[1], 0], 
        -l1[rand[1], 3]*l1[rand[1], 1], -l1[rand[1], 3]],
        [l1[rand[2], 0], l1[rand[2], 1], 1, 0, 0, 0, -l1[rand[2], 2]*l1[rand[2], 0], 
        -l1[rand[2], 2]*l1[rand[2], 1], -l1[rand[2], 2]], 
        [0, 0, 0, l1[rand[2], 0], l1[rand[2], 1], 1, -l1[rand[2], 3]*l1[rand[2], 0],
        -l1[rand[2], 3]*l1[rand[2], 1], -l1[rand[2], 3]], 
        [l1[rand[3], 0], l1[rand[3], 1], 1, 0, 0, 0, -l1[rand[3], 2]*l1[rand[3], 0],
        -l1[rand[3], 2]*l1[rand[3], 1], -l1[rand[3], 2]], 
        [0, 0, 0, l1[rand[3], 0], l1[rand[3], 1], 1, -l1[rand[3], 3]*l1[rand[3], 0],
        -l1[rand[3], 3]*l1[rand[3], 1], -l1[rand[3], 3]]])

        U, S, V = np.linalg.svd(A1)
        f = V.T[:, -1]
        F = np.reshape(f, (3,3))

        U1, S1, V1 = np.linalg.svd(F)
        Snew = np.zeros((U.shape[1], V.shape[0]))
        Snew[0, 0] = S1[0]
        Snew[1, 1] = S1[1] 
        F = np.dot(U1, np.dot(Snew, V1))
        print F.shape
        
        # Sampson distance
        inliers_count = []
        indices = []
        for k in range(l1.shape[0]):
            prod1 = np.dot(np.append(l1[k, 2:4], 1), np.dot(F, np.reshape(np.append(l1[k, 0:2], 1), (3, 1)))
            prod2 = np.dot(F, np.reshape(np.append(l1[k, 0:2], 1), (3, 1)))[0, 0]
            prod3 = np.dot(F, np.reshape(np.append(l1[k, 0:2], 1), (3, 1)))[1, 0]          
            prod4 = np.dot(F.T, np.reshape(np.append(l1[k, 2:4], 1), (3, 1)))[0, 0]          
            prod5 = np.dot(F.T, np.reshape(np.append(l1[k, 2:4], 1), (3, 1)))[1, 0]
            if prod1**2/(prod2**2 + prod3**2 + prod4**2 + prod5**2) < inlier_threshold**2:
                inliers_count.append(1)
                indices.append(k)


        inliers = np.zeros((sum(inliers_count), 4))
        v = 0
        for w in indices:
            inliers[v, :] = l1[w, :]
            v += 1
        
        current_noi = sum(inliers_count)
        if current_noi > best_noi:
            best_noi = current_noi
            best_inliers = inliers

    inliers = best_inliers

    A2 = np.zeros((inliers.shape[0]*2, 9))
    for s in range(0, len(inliers), 2):
        A2[s, :] = np.array([inliers[s, 0], inliers[s, 1], 1, 0, 0, 0, -inliers[s,2]*inliers[s,0], -inliers[s,2]*inliers[s,1], -inliers[s,2]]) 
        A2[s+1, :] = np.array([0, 0, 0, inliers[s,0], inliers[s,1], 1, -inliers[s, 3]*inliers[s,0], -inliers[s, 3]*inliers[s,1], -inliers[s,3]])

    U1, S1, V1 = np.linalg.svd(A2)
    f = V1.T[:, -1]
    F = np.reshape(F, (3,3))

    return inliers, F

l11 = final
max_iter = 1000
inlier_threshold = 10e-05
F, inliers = ransac_homography(l11, max_iter, inlier_threshold)

# Force rank 2
U2, S2, V2 = np.linalg.svd(F)

Snew = np.zeros((3, 3))
Snew[0, 0] = S2[0]
Snew[1, 1] = S2[1] 
F2 = np.dot(U2, np.dot(Snew, V2))

# Denormalization
F = np.dot(T2.T, np.dot(F2, T1))

print F