homography_operations.py

import numpy as np
import cv2
import util
from scipy.optimize import curve_fit


def create_homography(image_points, world_points):
    util.info("Creating homography matrix...")
    a_list = []
    # Get similarity transformations
    t = util.get_transformation_matrix(image_points, 0)
    t_prime = util.get_transformation_matrix(world_points, 1)
    for i in range(len(image_points)):
        # In Zhang's calibration method, the world coordinate system is placed on the checkerboard plane,
        # and the checkerboard plane is set to the plane with z = 0.
        # First, convert each points into homogeneous coordinates
        point1 = util.to_homogeneous(image_points[i], 0)
        point2 = util.to_homogeneous(world_points[i], 1)
        # Get normalized points with the matrix calculated before
        normalized_point1 = np.dot(point1, t.T)
        normalized_point2 = np.dot(point2, t_prime.T)
        image_x, image_y = normalized_point1.item(0), normalized_point1.item(1)
        model_x, model_y = normalized_point2.item(0), normalized_point2.item(1)

        # Form the 2n x 12 matrix A by stacking the equations generated by each correspondence.
        a_row1 = [-model_x, -model_y, -1., 0, 0, 0, image_x * model_x, image_x * model_y, image_x]
        a_row2 = [0, 0, 0, -model_x, -model_y, -1., image_y * model_x, image_y * model_y, image_y]

        # Assemble the 2n x 9 matrices Ai into a single matrix A.
        a_list.append(a_row1)
        a_list.append(a_row2)

    a_matrix = np.matrix(a_list)

    # A solution of Ah = 0 is obtained from the right singular vector of V associated with the smallest singular value.
    U, S, V_t = np.linalg.svd(a_matrix)

    # The parameters are in the min line of Vh
    L = V_t[-1]
    H = L.reshape(3, 3)

    # Denormalization
    H = np.dot(np.dot(np.linalg.inv(t), H), t_prime)

    util.info("DONE.\n")
    return H

def cost_function(coordinates, *params):
    h11, h12, h13, h21, h22, h23, h31, h32, h33 = params

    N = coordinates.shape[0] // 2

    X = coordinates[:N]
    Y = coordinates[N:]

    x = (h11 * X + h12 * Y + h13) / (h31 * X + h32 * Y + h33)
    y = (h21 * X + h22 * Y + h23) / (h31 * X + h32 * Y + h33)

    result = np.zeros_like(coordinates)
    result[:N] = x
    result[N:] = y

    return result


def jacobian_function(coordinates, *params):
    # The  partial  derivatives  of  the  first  2  elements  of u and v  given  as  follows.
    # These  values  are  used  for computing the Jacobian used for LM optimization
    h11, h12, h13, h21, h22, h23, h31, h32, h33 = params

    N = coordinates.shape[0] // 2

    X = coordinates[:N]
    Y = coordinates[N:]

    J = np.zeros((N * 2, 9))
    J_x = J[:N]
    J_y = J[N:]

    s_x = h11 * X + h12 * Y + h13
    s_y = h21 * X + h22 * Y + h23
    w = h31 * X + h32 * Y + h33
    w_sq = w ** 2

    J_x[:, 0] = X / w
    J_x[:, 1] = Y / w
    J_x[:, 2] = 1. / w
    J_x[:, 6] = (-s_x * X) / w_sq
    J_x[:, 7] = (-s_x * Y) / w_sq
    J_x[:, 8] = -s_x / w_sq

    J_y[:, 3] = X / w
    J_y[:, 4] = Y / w
    J_y[:, 5] = 1. / w
    J_y[:, 6] = (-s_y * X) / w_sq
    J_y[:, 7] = (-s_y * Y) / w_sq
    J_y[:, 8] = -s_y / w_sq

    J[:N] = J_x
    J[N:] = J_y

    return J


def refine_homography(homography, image_coordinates, world_coordinates):
    util.info("Refining homography...")
    x, y, X, Y = image_coordinates[:, 0][:, 0], image_coordinates[:,
                                                                  0][:, 1], world_coordinates[:, 0], world_coordinates[:, 1]

    N = X.shape[0]

    h0 = homography.ravel()

    x_points = np.zeros(N * 2)
    x_points[:N] = X
    x_points[N:] = Y

    y_points = np.zeros(N * 2)
    y_points[:N] = x
    y_points[N:] = y

    # Use Levenberg-Marquardt to refine the linear homography estimate
    popt, pcov = curve_fit(cost_function, x_points,
                           y_points, p0=h0, jac=jacobian_function, maxfev=5000)
    h_refined = popt

    # Normalize and reconstitute homography
    h_refined /= h_refined[-1]
    H_refined = h_refined.reshape((3, 3))

    util.info("DONE.")
    return H_refined