application_vnd.jupyter.stderr_output_1.png
import cv2

import matplotlib.pyplot as plt

import numpy as np # linear algebra

import os

import pandas as pd # data processing, CSV file I/O (e.g. pd.read_csv)

import scipy
import math
import numpy as np
import pandas as pd
import cv2
import matplotlib.pyplot as plt
import os
eps = 1e-15
train_calibration_csvs = []
train_pair_covisibility_csvs = []
test_calibration_csvs = []
test_pair_covisibility_csvs = []
for dirname, _, filenames in os.walk('/kaggle/input'):
    for filename in filenames:
        if 'train' in dirname and 'pair_covisibility.csv' in filename:
            train_pair_covisibility_csvs.append(os.path.join(dirname, filename))
        if 'train' in dirname and 'calibration.csv' in filename:
            train_calibration_csvs.append(os.path.join(dirname, filename))
        if 'test' in dirname and 'pair_covisibility.csv' in filename:
            test_pair_covisibility_csvs.append(os.path.join(dirname, filename))
        if 'test' in dirname and 'calibration.csv' in filename:
            test_calibration_csvs.append(os.path.join(dirname, filename))
test_csv = '/kaggle/input/image-matching-challenge-2022/test.csv'
train_csv = '/kaggle/input/image-matching-challenge-2022/train/scaling_factors.csv'

def getImagesFromCSV(train_csvf, calibration_csvs, pair_covisibility_csvs, category_index, pair_index):
    train_csv = pd.read_csv(train_csvf)
    category_csv = pd.read_csv(calibration_csvs[category_index])
    pair_covisibility = pd.read_csv(pair_covisibility_csvs[category_index])
    pair_covisibility = pair_covisibility[pair_covisibility['covisibility'] >= 0.1]
    imgs = pair_covisibility['pair'][pair_index].split('-')
    F = pair_covisibility['fundamental_matrix'][pair_index]
    p_new = []
    R = []
    T = []
    K = []
    for img in imgs:
        image_loc = '/kaggle/input/image-matching-challenge-2022/train/' + str(train_csv['scene'][category_index]) + '/images/' + str(img) + '.jpg'
        p_new.append(cv2.imread(image_loc))
        R.append(np.array(np.array(category_csv[category_csv['image_id'] == img]['rotation_matrix'])[0].split(' '), dtype=np.float32).reshape((3, 3)))
        T.append(np.array(np.array(category_csv[category_csv['image_id'] == img]['translation_vector'])[0].split(' '), dtype=np.float32).reshape((3,)))
        K.append(np.array(np.array(category_csv[category_csv['image_id'] == img]['camera_intrinsics'])[0].split(' '), dtype=np.float32).reshape((3, 3)))
    df = pd.DataFrame({'imgs': p_new, 'R': R, 'T': T, 'K': K, 'F': F})
    return df
df = getImagesFromCSV(train_csv, train_calibration_csvs, train_pair_covisibility_csvs, 0, 0)

def extract_sift(img, step_size=1):
    """
    Extract SIFT features for a given grayscale image. Instead of detecting 
    keypoints, we will set the keypoints to be uniformly distanced pixels.
    Feel free to use OpenCV functions.
    Note: Check sift.compute and cv2.KeyPoint
    Args:
        img: Grayscale image of shape (H, W)
        step_size: Size of the step between keypoints.
    Return:
        descriptors: numpy array of shape (int(img.shape[0]/step_size) * int(img.shape[1]/step_size), 128)
                     contains sift feature.
    """
    sift = cv2.SIFT_create()
    descriptors = np.zeros((int(img.shape[0] / step_size) * int(img.shape[1] / step_size), 128))
    keypoints = [cv2.KeyPoint(x, y, step_size) for y in range(0, img.shape[0], step_size) for x in range(0, img.shape[1], step_size)]
    _, descriptors = sift.compute(img, keypoints)
    return descriptors
def extract_sift_for_dataset(data, step_size=1):
    all_features = []
    for i in range(len(data)):
        img = data[i]
        img = cv2.cvtColor(np.uint8(img), cv2.COLOR_BGR2GRAY)
        descriptors = extract_sift(img, step_size)
        all_features.append(descriptors)

# Distribution of keypoint responses
def distributionKeypointResponses(keypoints):
    responses = []
    for keypoint in keypoints:
        responses.append(keypoint.response)

    n, bins, patches = plt.hist(responses, 100, 
                                density = 1, 
                                color ='green',
                                alpha = 0.7)
    plt.xlabel('Keypoint Responses')
    plt.ylabel('Count')

    plt.title('Distribution of Keypoint Response Intensity',
              fontweight ="bold")

    plt.show() 
    
    
def decodeFundamental(f_matrix):
    F = np.zeros((3,3))
    for index, value in enumerate(f_matrix.split(" ")):
        F[int(np.floor(index/3))][index%3] = float(value)
    return F

def encodeFundamental(f_matrix):
    F = np.zeros((9,))
    for index, value in enumerate(f_matrix.ravel()):
        F[index] = value
    return F

def NormalizeKeypoints(keypoints, K):
    C_x = K[0, 2]
    C_y = K[1, 2]
    f_x = K[0, 0]
    f_y = K[1, 1]
    keypoints = (keypoints - np.array([[C_x, C_y]])) / np.array([[f_x, f_y]])
    return keypoints

def ComputeEssentialMatrix(F, K1, K2, kp1, kp2):
    '''Compute the Essential matrix from the Fundamental matrix, given the calibration matrices. Note that we ask participants to estimate F, i.e., without relying on known intrinsics.'''
    
    # Warning! Old versions of OpenCV's RANSAC could return multiple F matrices, encoded as a single matrix size 6x3 or 9x3, rather than 3x3.
    # We do not account for this here, as the modern RANSACs do not do this:
    # https://opencv.org/evaluating-opencvs-new-ransacs
    assert F.shape[0] == 3, 'Malformed F?'

    # Use OpenCV's recoverPose to solve the cheirality check:
    # https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#gadb7d2dfcc184c1d2f496d8639f4371c0
    E = np.matmul(np.matmul(K2.T, F), K1).astype(np.float64)
    
    kp1n = NormalizeKeypoints(kp1, K1)
    kp2n = NormalizeKeypoints(kp2, K2)
    num_inliers, R, T, mask = cv2.recoverPose(E, kp1n, kp2n)

    return E, R, T

def QuaternionFromMatrix(matrix):
    '''Transform a rotation matrix into a quaternion.'''

    M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4]
    m00 = M[0, 0]
    m01 = M[0, 1]
    m02 = M[0, 2]
    m10 = M[1, 0]
    m11 = M[1, 1]
    m12 = M[1, 2]
    m20 = M[2, 0]
    m21 = M[2, 1]
    m22 = M[2, 2]

    K = np.array([[m00 - m11 - m22, 0.0, 0.0, 0.0],
              [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
              [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
              [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]])
    K /= 3.0

    # The quaternion is the eigenvector of K that corresponds to the largest eigenvalue.
    w, V = np.linalg.eigh(K)
    q = V[[3, 0, 1, 2], np.argmax(w)]

    if q[0] < 0:
        np.negative(q, q)

    return q

def ComputeErrorForOneExample(q_gt, T_gt, q, T, scale):
    '''Compute the error metric for a single example.
    The function returns two errors, over rotation and translation. 
    These are combined at different thresholds by ComputeMaa in order to compute the mean Average Accuracy.'''
    
    q_gt_norm = q_gt / (np.linalg.norm(q_gt) + eps)
    q_norm = q / (np.linalg.norm(q) + eps)

    loss_q = np.maximum(eps, (1.0 - np.sum(q_norm * q_gt_norm)**2))
    err_q = np.arccos(1 - 2 * loss_q)

    # Apply the scaling factor for this scene.
    T_gt_scaled = T_gt * scale
    T_scaled = T * np.linalg.norm(T_gt) * scale / (np.linalg.norm(T) + eps)

    err_t = min(np.linalg.norm(T_gt_scaled - T_scaled), np.linalg.norm(T_gt_scaled + T_scaled))

    return err_q * 180 / np.pi, err_t

def BuildCompositeImage(im1, im2, axis=1, margin=0, background=1):
    '''Convenience function to stack two images with different sizes.'''
    
    if background != 0 and background != 1:
        background = 1
    if axis != 0 and axis != 1:
        raise RuntimeError('Axis must be 0 (vertical) or 1 (horizontal')

    h1, w1, _ = im1.shape
    h2, w2, _ = im2.shape

    if axis == 1:
        composite = np.zeros((max(h1, h2), w1 + w2 + margin, 3), dtype=np.uint8) + 255 * background
        if h1 > h2:
            voff1, voff2 = 0, (h1 - h2) // 2
        else:
            voff1, voff2 = (h2 - h1) // 2, 0
        hoff1, hoff2 = 0, w1 + margin
    else:
        composite = np.zeros((h1 + h2 + margin, max(w1, w2), 3), dtype=np.uint8) + 255 * background
        if w1 > w2:
            hoff1, hoff2 = 0, (w1 - w2) // 2
        else:
            hoff1, hoff2 = (w2 - w1) // 2, 0
        voff1, voff2 = 0, h1 + margin
    composite[voff1:voff1 + h1, hoff1:hoff1 + w1, :] = im1
    composite[voff2:voff2 + h2, hoff2:hoff2 + w2, :] = im2

    return (composite, (voff1, voff2), (hoff1, hoff2))

def DrawMatches(im1, im2, kp1, kp2, matches, axis=1, margin=0, background=0, linewidth=2):
    '''Draw keypoints and matches.'''
    
    composite, v_offset, h_offset = BuildCompositeImage(im1, im2, axis, margin, background)

    # Draw all keypoints.
    for coord_a, coord_b in zip(kp1, kp2):
        composite = cv2.drawMarker(composite, (int(coord_a[0] + h_offset[0]), int(coord_a[1] + v_offset[0])), color=(255, 0, 0), markerType=cv2.MARKER_CROSS, markerSize=5, thickness=1)
        composite = cv2.drawMarker(composite, (int(coord_b[0] + h_offset[1]), int(coord_b[1] + v_offset[1])), color=(255, 0, 0), markerType=cv2.MARKER_CROSS, markerSize=5, thickness=1)
    
    # Draw matches, and highlight keypoints used in matches.
    for idx_a, idx_b in matches:
        composite = cv2.drawMarker(composite, (int(kp1[idx_a, 0] + h_offset[0]), int(kp1[idx_a, 1] + v_offset[0])), color=(0, 0, 255), markerType=cv2.MARKER_CROSS, markerSize=12, thickness=1)
        composite = cv2.drawMarker(composite, (int(kp2[idx_b, 0] + h_offset[1]), int(kp2[idx_b, 1] + v_offset[1])), color=(0, 0, 255), markerType=cv2.MARKER_CROSS, markerSize=12, thickness=1)
        composite = cv2.line(composite,
                             tuple([int(kp1[idx_a][0] + h_offset[0]),
                                   int(kp1[idx_a][1] + v_offset[0])]),
                             tuple([int(kp2[idx_b][0] + h_offset[1]),
                                   int(kp2[idx_b][1] + v_offset[1])]), color=(0, 0, 255), thickness=1)
    return composite

def ArrayFromCvKps(kps):
    '''Convenience function to convert OpenCV keypoints into a simple numpy array.'''
    return np.array([kp.pt for kp in kps])

k = np.min(decodeFundamental(df['F'][0]))
print(encodeFundamental(F * k).reshape(9))
print(decodeFundamental(df['F'][0]).reshape((9,)))
print('Absolute Diff')
print(np.sum(np.absolute(k * encodeFundamental(F).reshape((3, 3)) - decodeFundamental(df['F'][0]))))
plt.clf()
plt.bar(np.linspace(1, 9, 9), encodeFundamental(F * k) - decodeFundamental(df['F'][0]).reshape((9,)))
plt.show()