Two View Geometry

import os
import sys
import cv2
import _pickle as pickle
#import cPickle as pickle
import math
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

from utils import *

MIN_MATCH_COUNT = 10

src_image_path = "./imgs/0001.png"
dst_image_path = "./imgs/0002.png"
src_image = cv2.imread(src_image_path, cv2.IMREAD_COLOR)
dst_image = cv2.imread(dst_image_path, cv2.IMREAD_COLOR)

src_gray = cv2.cvtColor(src_image , cv2.COLOR_BGR2GRAY)
dst_gray = cv2.cvtColor(dst_image , cv2.COLOR_BGR2GRAY)

kp1, desc1 = computeDescriptions(src_gray, "feats/{}_feats.kp".format(os.path.basename(src_image_path).split(".")[0]))
kp2, desc2 = computeDescriptions(dst_gray, "feats/{}_feats.kp".format(os.path.basename(dst_image_path).split(".")[0]))

matcher = genMatcher()
knn_matches = matcher.knnMatch(desc1, desc2, 2)
#-- Filter matches using the Lowe's ratio test
ratio_thresh = 0.8
good = []
pts1 = []
pts2 = []
for m,n in knn_matches:
    if m.distance < ratio_thresh * n.distance:
        good.append(m)
        pts2.append(kp2[m.trainIdx].pt)
        pts1.append(kp1[m.queryIdx].pt) 

if len(good) <= MIN_MATCH_COUNT:
    print( "Not enough matches are found - {}/{}".format(len(good), MIN_MATCH_COUNT) )
    matchesMask = None
    
    

h_,w_, _ = src_image.shape

pts1 = np.int32(pts1)
pts2 = np.int32(pts2)

F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC )  #FM_LMEDS

# We select only inlier points
pts1 = pts1[mask.ravel()==1]
pts2 = pts2[mask.ravel()==1]

F

#https://docs.opencv.org/master/da/de9/tutorial_py_epipolar_geometry.html

def drawlines(img1,img2,lines,pts1,pts2):
    ''' img1 - image on which we draw the epilines for the points in img2
        lines - corresponding epilines '''
    r,c = img1.shape
    img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)
    img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)
    for r,pt1,pt2 in zip(lines,pts1,pts2):
        color = tuple(np.random.randint(0,255,3).tolist())
        x0,y0 = map(int, [0, -r[2]/r[1] ])
        x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ])
        img1 = cv2.line(img1, (x0,y0), (x1,y1), color,1)
        img1 = cv2.circle(img1,tuple(pt1),5,color,-1)
        img2 = cv2.circle(img2,tuple(pt2),5,color,-1)
    return img1,img2

# Find epilines corresponding to points in right image (second image) and
# drawing its lines on left image
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F)
lines1 = lines1.reshape(-1,3)
img5,img6 = drawlines(src_gray,dst_gray,lines1,pts1,pts2)

# Find epilines corresponding to points in left image (first image) and
# drawing its lines on right image
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F)
lines2 = lines2.reshape(-1,3)
img3,img4 = drawlines(dst_gray, src_gray, lines2,pts2,pts1)

ff, axs = plt.subplots(2,1,figsize=(15,15))
plt.subplot(121),plt.imshow(img5)
plt.subplot(122),plt.imshow(img3)
plt.show()

def fundamental_matrix_transform(x1, x2):
    n = x1.shape[1]
    if x2.shape[1] != n:
        print("Dimensions mismatch")
        return

    # build matrix for equations
    A = np.zeros((n,9))
    for i in range(n):
        A[i] = [x1[0,i]*x2[0,i], x1[0,i]*x2[1,i], x1[0,i]*x2[2,i],
                x1[1,i]*x2[0,i], x1[1,i]*x2[1,i], x1[1,i]*x2[2,i],
                x1[2,i]*x2[0,i], x1[2,i]*x2[1,i], x1[2,i]*x2[2,i] ]

    # compute linear least square solution
    U,S,V = np.linalg.svd(A)
    F = V[-1].reshape(3,3)

    # constrain F
    # make rank 2 by zeroing out last singular value
    U,S,V = np.linalg.svd(F)
    S[2] = 0
    F = np.dot(U,np.dot(np.diag(S),V))
    return F/F[2,2]

F2 = compute_fundamental().estimate(pts1, pts2 )
F2

# Find epilines corresponding to points in right image (second image) and
# drawing its lines on left image
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F2)
lines1 = lines1.reshape(-1,3)
img5,img6 = drawlines(src_gray,dst_gray,lines1,pts1,pts2)

# Find epilines corresponding to points in left image (first image) and
# drawing its lines on right image
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F2)
lines2 = lines2.reshape(-1,3)
img3,img4 = drawlines(dst_gray, src_gray, lines2,pts2,pts1)

ff, axs = plt.subplots(2,1,figsize=(15,15))
plt.subplot(121),plt.imshow(img5)
plt.subplot(122),plt.imshow(img3)
plt.show()

from skimage.transform import FundamentalMatrixTransform
from skimage.measure import ransac

model, inliers = ransac((pts1,pts2),
                        FundamentalMatrixTransform, min_samples=8,
                        residual_threshold=1, max_trials=5000)

F3 = model.params

model.params

# Find epilines corresponding to points in right image (second image) and
# drawing its lines on left image
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F3)
lines1 = lines1.reshape(-1,3)
img5,img6 = drawlines(src_gray,dst_gray,lines1,pts1,pts2)

# Find epilines corresponding to points in left image (first image) and
# drawing its lines on right image
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F3)
lines2 = lines2.reshape(-1,3)
img3,img4 = drawlines(dst_gray, src_gray, lines2,pts2,pts1)

ff, axs = plt.subplots(2,1,figsize=(15,15))
plt.subplot(121),plt.imshow(img5)
plt.subplot(122),plt.imshow(img3)
plt.show()

K = np.loadtxt("./calib.txt")

def unitDeterminantCheck(M):
    return abs(np.linalg.det(M)) - 1.0 < 1e-7

def decompEssentialMatrix(E):
    # svd::modify_A
    _, u, vt = cv2.SVDecomp(E, 1)
    if np.linalg.det(vt) < 0:
        vt *= -1
    if np.linalg.det(u) < 0:
        u *= -1
    """R = svd.u * Mat(W) * svd.vt;"""
    W = np.array([[0,-1,0],[1,0,0],[0,0,1]]);
    R1 = np.matmul( u, np.matmul( W, vt ) )
    R2 = np.matmul( u, np.matmul( W.T, vt ) )
    t = u[:,-1]
    if not unitDeterminantCheck(R1) and not unitDeterminantCheck(R2):
        print("Not rotation matrices")
    return R1, R2, t

Kinv = np.linalg.inv(K)
"""Pose for 1st view"""
P0 = np.eye(3,4)

""""E = K.T * F * K"""
E = np.matmul( K.T , np.matmul( F, K ) )
R1, R2, t = decompEssentialMatrix(E)


"""Poses for 2nd view. One possilbe solutions based on chirality"""

P1 = np.column_stack( [ R1, t ] )

P1