RANSAC for homography estimation

ransac_homography.py

@@ -0,0 +1,135 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+def ransac_homography_(feature_set, threshold, max_iter=100, success_prob=0.95):
+    # INPUT
+    # feature_set  : 4 by N matrix consist of matched feature set
+    # which type of first row is (x, y, x', y'). N is number of matched features
+    # threshold    : 2-norm < threshold than feature become inlier.
+    # max_iter     : maximum iteration constraint.
+    # success_prob : number of iterations = log(1-success_prob) / log (1-inlier_ratio^4)
+
+    # OUTPUT
+    # est_homo_matrix     : estimated homography matrix by RANSAC method
+    # inlier_ratio        : inlier ratio
+
+    # Author: Hayoung Kim
+    # Lab   : Machine Monitoring and Control Lab.
+    # Date  : May 3, 2015 - May 4, 2015
+
+    # print "start!"
+    if len(feature_set) < 4:
+        raise ValueError, "For estimate Homography matrix, at least 4 matched features are needed"
+
+    N = len(feature_set)  # number of data set
+    x1 = feature_set[:, 0]
+    y1 = feature_set[:, 1]
+    x2 = feature_set[:, 2]
+    y2 = feature_set[:, 3]  # (x,y) coordiate for two images
+
+    inlier = []  # allocate a room for inlier
+    count_iter = 0
+    max_num_inlier = 0
+
+    while count_iter < max_iter:
+        # print "first iter"
+        count_iter = count_iter + 1
+        # rank_of_A = 0
+        A = np.matrix(np.zeros(shape=(8, 8)))
+
+        while np.linalg.matrix_rank(A) < 8:  # check sigularity of matrix A (Ax = b)
+
+            # print "rank"
+            A = np.zeros(shape=(8, 8))
+            b = np.zeros(shape=(8, 1))
+
+            index_rnd = np.random.randint(1, N, size=4)  # random index for RANSAC
+            # print index_rnd
+            c = 0
+
+            for i in index_rnd:
+                b[c] = x2[i] - x1[i]
+                A[c] = [x1[i], y1[i], 1, 0, 0, 0, -x2[i] * x1[i], -x2[i] * y1[i]]
+                c += 1
+
+                b[c] = y2[i] - y1[i]
+                A[c] = [0, 0, 0, x1[i], y1[i], 1, -y2[i] * x1[i], -y2[i] * y1[i]]
+                c += 1
+            # print A
+            # rank_of_A = np.linalg.matrix_rank(A)  # calculate rank
+
+    homography_param = np.dot(np.linalg.inv(A), b)  # calcualte homography transform parameter hxx
+    homography_param = np.append(homography_param, 0)  # append 0 for reshape to homography matrix
+
+    homography_matrix = np.reshape(homography_param, (3, 3))  # reshape array to 3 by 3 matrix
+    homography_matrix = homography_matrix + np.eye(3)  # Making complete homography transform matrix
+
+    inlier = []
+
+    for i in range(N):
+        f_x_homo = np.dot(homography_matrix, [x1[i], y1[i], 1])
+        f_x = [f_x_homo[0] / f_x_homo[2], f_x_homo[1] / f_x_homo[2]]  # homogeneous coordinate to normal coordinate
+
+        residual = np.sqrt((f_x[0] - x2[i]) ** 2 + (f_x[1] - y2[i]) ** 2)
+
+        if residual < threshold:
+            inlier.append(i)
+
+    num_inlier = len(inlier)
+
+    if num_inlier > max_num_inlier:
+        est_homo_matrix = homography_matrix
+        inlier_ratio = float(num_inlier) / N
+        max_num_inlier = num_inlier
+
+
+
+
+
+    return est_homo_matrix, inlier_ratio
+
+
+# Demo
+
+homography_matrix = np.random.random((3,3)) * 3
+homography_matrix[2][2] = 1
+
+# print "###### homography matrix (True): ######"
+# print homography_matrix
+# print "--------------------------------"
+N = 50        # number of data set
+
+feature1 = np.ones((N,3))
+feature1[:,:-1] = np.random.randint(1,640, (N, 2))
+
+feature2_homo = np.dot(homography_matrix, feature1.T).T
+feature2_mat = np.matrix(feature2_homo)                            # type: array to matrix
+
+# print feature2_mat
+
+feature2_xy_temp = np.matrix(np.zeros(shape=(N,3)))
+feature2_xy = np.matrix(np.zeros(shape=(N,3)))
+feature_set = np.matrix(np.zeros(shape=(N,4)))
+
+sig = 4
+
+for i in range(N):
+    feature2_xy[i] = feature2_mat[i] / feature2_mat[i,2]
+
+for i in range(N):
+    # feature_set[i, 0] = feature1[i,0] + sig * np.random.randn()
+    # feature_set[i, 1] = feature1[i,1] + sig * np.random.randn()
+    feature_set[i, 0] = feature1[i,0]
+    feature_set[i, 1] = feature1[i,1]
+    feature_set[i, 2] = feature2_xy[i,0]
+    feature_set[i, 3] = feature2_xy[i,1]
+
+est_homo_mat, inlier_ratio = ransac_homography_(feature_set, 2 * sig, 1)
+print "-----------------------------------------------"
+print "##### true homography matrix #####"
+print homography_matrix
+print "##### estimated homography matrix #####"
+print est_homo_mat
+print "## inlier ratio ##"
+print inlier_ratio