import cv2
import numpy as np


def main():
    lhs_camera = cv2.imread('stereo_images/frame_00113_0.bmp')
    rhs_camera = cv2.imread('stereo_images/frame_00113_1.bmp')

    sift = cv2.xfeatures2d.SIFT_create()

    lhs_camera = cv2.cvtColor(lhs_camera, cv2.COLOR_BGR2GRAY)
    rhs_camera = cv2.cvtColor(rhs_camera, cv2.COLOR_BGR2GRAY)

    key_points_of_lhs_image, descriptors_of_lhs_image = sift.detectAndCompute(lhs_camera, None)
    key_points_of_rhs_image, descriptors_of_rhs_image = sift.detectAndCompute(rhs_camera, None)

    bf = cv2.BFMatcher_create(cv2.NORM_L2, crossCheck=True)

    matches = bf.match(descriptors_of_lhs_image, descriptors_of_rhs_image)

    matches = sorted(matches, key=lambda x: x.distance)

    good_key_points_query_index = []
    good_key_point_train_index = []
    for i, (m) in enumerate(matches):
        if m.distance < 20:
            good_key_points_query_index.append(key_points_of_rhs_image[m.queryIdx].pt)
            good_key_point_train_index.append(key_points_of_lhs_image[m.trainIdx].pt)

    good_key_points_query_index = np.asarray(good_key_points_query_index)
    good_key_point_train_index = np.asarray(good_key_point_train_index)

    fundamental_mat, _ = cv2.findFundamentalMat(good_key_points_query_index, good_key_point_train_index, cv2.FM_RANSAC)
    print(fundamental_mat)


if __name__ == '__main__':
    main()