Multi View Geometry II

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

from two_view_geometry import two_view_geometry,TriangulatePoints, unitDeterminantCheck,genMatcher, computeDescriptions, plot3D, write_ply

#https://documents.epfl.ch/groups/c/cv/cvlab-unit/www/data/multiview/modelFountain.html
ff, axs = plt.subplots(2,4,figsize=(10,5))
idx=0
for fname in glob.glob('./imgs/fountain/*.png')[:8]:
    img = cv2.imread(fname)   
    plt.subplot(int("24{}".format(idx+1)))
    plt.imshow(img.T[[2,1,0]].T)
    idx+=1
plt.tight_layout()
plt.show()

images_dir = "./imgs/fountain/"
filenames = os.listdir(images_dir)
filenames.sort()

ff, axs = plt.subplots(2,4,figsize=(10,10))
idx=0
for fname in glob.glob('./imgs/*.JPEG')[:8]:
    gray = cv2.imread(fname)   
    plt.subplot(int("34{}".format(idx+1)))
    plt.imshow(gray)
    idx+=1
plt.tight_layout()
plt.show()

src_image_path1 = "{}{}".format(images_dir,filenames[0])
src_image_path2 = "{}{}".format(images_dir,filenames[1]

calibration_mat_path = "./calib.txt"
distortions_vec_path = None
debug = False

src_image_1 = cv2.imread(src_image_path1, cv2.IMREAD_COLOR)
src_image_2 = cv2.imread(src_image_path2, cv2.IMREAD_COLOR)

K = np.loadtxt(calibration_mat_path)

if distortions_vec_path:
    distortions = np.loadtxt(distortions_vec_path)
    src_image_1 = cv2.undistort(src_image_1, K, distortions, None, None)
    src_image_2 = cv2.undistort(src_image_2, K, distortions, None, None)

src_gray_1 = cv2.cvtColor(src_image_1 , cv2.COLOR_BGR2GRAY)
src_gray_2 = cv2.cvtColor(src_image_2 , cv2.COLOR_BGR2GRAY)

kp1, desc1 = computeDescriptions(src_gray_1, "feats/{}_feats.kp".format(os.path.basename(src_image_path1).split(".")[0]))
kp2, desc2 = computeDescriptions(src_gray_2, "feats/{}_feats.kp".format(os.path.basename(src_image_path2).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) <= 10 : #MIN_MATCH_COUNT=10
    print( "Not enough matches are found - {}/{}".format(len(good), MIN_MATCH_COUNT) )

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

#pts1 = np.array(pts1)
#pts2 = np.array(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]

Kinv = np.linalg.inv(K)

points_3d, P0, P1 = two_view_geometry( pts1, pts2, F, K )

kp_prev = kp2
desc_prev = desc2
src_image_prev = src_image_2
pts_prev = pts2
points_3d_prev = points_3d
pose_prev = P1

out_pts_3d = points_3d.copy()
out_colors = np.array([ src_image_1[pt[1],pt[0]] for pt in pts1 ])

#for bundle adjustment
out_pts_2d = pts_prev.copy()
rvec,_ = cv2.Rodrigues(P1[:,:3])
out_camera_params = np.hstack([rvec.ravel(), P1[:,3].ravel() ])

view_idx = 0
points_indexes = np.arange(points_3d_prev.shape[0] )
points_indexes_prev = points_indexes.copy()

camera_indexes = np.full( (pts2.shape[0],1), view_idx )

for fname in filenames[2:]:
    src_image_path = "{}{}".format(images_dir,fname)
    print( src_image_path )
    src_image = cv2.imread(src_image_path, cv2.IMREAD_COLOR)
    src_gray = cv2.cvtColor(src_image , cv2.COLOR_BGR2GRAY)
    kp, desc = computeDescriptions(src_gray, "feats/{}_feats.kp".format(os.path.basename(src_image_path).split(".")[0]))
    
    pts_prev_keys = [ ",".join( list(pt.astype(str) ) ) for pt in pts_prev ]
    points_3d_2d = dict( zip( pts_prev_keys, points_indexes_prev ))
    
    knn_matches = matcher.knnMatch(desc_prev, desc, 2)
    #-- Filter matches using the Lowe's ratio test
    ratio_thresh = 0.8
    good = []
    pts_a = []
    pts_b = []
    for m,n in knn_matches:
        if m.distance < ratio_thresh * n.distance:
            good.append(m)
            pts_b.append(kp[m.trainIdx].pt)
            pts_a.append(kp_prev[m.queryIdx].pt) 
    
    pts_a = np.array(pts_a)
    pts_b = np.array(pts_b)
    
    # Using Epiplolar Constraint to filter only inlier points
    F, mask = cv2.findFundamentalMat(pts_a, pts_b, cv2.FM_RANSAC )
    pts_a = pts_a[mask.ravel()==1]
    pts_b = pts_b[mask.ravel()==1]
    
    pts_a = np.int32(pts_a)
    
    
    #Select 3d points that were seen in previous image view
    points_3d_cur_index = np.array([ points_3d_2d.get( ",".join( list(pt.astype(str)) ) ) for pt in pts_a ])
    pts_mask = np.array([ True if idx != None else False for idx in points_3d_cur_index ])
    points_3d_cur = np.array([ out_pts_3d[idx] if idx != None else [0,0,0,1] for idx in points_3d_cur_index])
    
    retval, rvecs, tvecs, inliers = cv2.solvePnPRansac(points_3d_cur[ pts_mask ][:,:-1], pts_b[pts_mask] , K, None)
    
    pts_b = np.int32(pts_b)
    
    R_mat, _ = cv2.Rodrigues(rvecs)
    if not unitDeterminantCheck(R_mat):
        print("Not a rotation matrix")
        break
    
    #triangulate new 3d points
    pose = np.column_stack( [ R_mat, tvecs ] )
    points_3d_cur2, err =  TriangulatePoints( pts_a[ ~pts_mask ], pts_b[ ~pts_mask ], K, Kinv, pose_prev, pose )

    
    print("Reprojection error:", err)
    
    points_3d_cur[~pts_mask] = points_3d_cur2
    
    
    #for bundle adjustment
    out_camera_params = np.vstack( [ out_camera_params, np.hstack([rvecs.ravel(), tvecs.ravel() ]) ])
    out_pts_2d = np.concatenate( [ out_pts_2d, pts_b ], axis=0)
    #points_indexes_cur = np.concatenate([ points_3d_cur_index[ pts_mask ], out_pts_3d.shape[0] + np.arange(points_3d_cur2.shape[0]) ])  
    
    points_indexes_cur = points_3d_cur_index.copy()
    points_indexes_cur[ ~pts_mask ] = out_pts_3d.shape[0] + np.arange(points_3d_cur2.shape[0])   
    
    points_indexes = np.concatenate( [ points_indexes, points_indexes_cur ], axis=0)
   
     
    out_pts_3d = np.concatenate([out_pts_3d, points_3d_cur2 ], axis=0)
    #print(out_pts_3d.shape, points_3d_cur.shape)
    out_colors_cur = np.array([ src_image_prev[pt[1],pt[0]] for pt in pts_a[ ~pts_mask ]  ])
    out_colors = np.concatenate([out_colors, out_colors_cur],axis=0)
    camera_indexes = np.concatenate( [ camera_indexes, np.full( (pts_b.shape[0],1), view_idx+1 ) ] )
    
    
    
    kp_prev = kp
    desc_prev = desc
    src_image_prev = src_image
    #pts_prev = np.concatenate( [ pts_b[pts_mask],pts_b[~pts_mask]  ],axis=0 )
    pts_prev = pts_b
    #points_3d_prev = points_3d_cur
    points_indexes_prev = points_indexes_cur
    #points_3d_prev = np.concatenate( [ points_3d_cur[pts_mask],points_3d_cur[~pts_mask]  ],axis=0 )
    pose_prev = pose
    view_idx += 1 
    

"""
https://scipy-cookbook.readthedocs.io/items/bundle_adjustment.html
"""

def rotate(points, rot_vecs):
    """Rotate points by given rotation vectors.
    
    Rodrigues' rotation formula is used.
    """
    theta = np.linalg.norm(rot_vecs, axis=1)[:, np.newaxis]
    with np.errstate(invalid='ignore'):
        v = rot_vecs / theta
        v = np.nan_to_num(v)
    dot = np.sum(points * v, axis=1)[:, np.newaxis]
    cos_theta = np.cos(theta)
    sin_theta = np.sin(theta)

    return cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v

def project(points, camera_params, camera_intrinsics):
    """Convert 3-D points to 2-D by projecting onto images."""
    points_proj = rotate(points, camera_params[:, :3])
    points_proj += camera_params[:, 3:6]
    
    
    fx = camera_intrinsics[0, 0]
    fy = camera_intrinsics[1, 1]
    cx = camera_intrinsics[0, 2]
    cy = camera_intrinsics[1, 2]
    
    points_proj =  np.array([ points_proj[:,0]*fx + cx*points_proj[:,2], points_proj[:,1]*fy + cy*points_proj[:,2], points_proj[:,2] ]).T 
    points_proj = points_proj[:, :2] / points_proj[:, 2, np.newaxis]
    
    return points_proj

def fun(params, n_cameras, n_points, camera_indices, point_indices, points_2d, K):
    """Compute residuals.
    
    `params` contains camera parameters and 3-D coordinates.
    """
    camera_params = params[:n_cameras * 6].reshape((n_cameras, 6))
    points_3d = params[n_cameras * 6:].reshape((n_points, 3))
    points_proj = project(points_3d[point_indices], camera_params[camera_indices], K )
    return np.abs(points_proj - points_2d).ravel()

n_cameras = view_idx+1
n_points = out_pts_3d.shape[0]
camera_indices = camera_indexes.ravel()
point_indices = points_indexes.astype(np.int32)

x0 = np.hstack((out_camera_params.ravel(), out_pts_3d[:,:3].ravel()))
f0 = fun(x0, n_cameras, n_points, camera_indices, point_indices, out_pts_2d, K)
plt.plot(f0)

def isOutlier(f, res=3):
    #an outlier is any point that deivates from 3sdtev from mean
    mu = f.mean()
    dev = f0.std()
    return ( f > (mu + res * dev) ) | ( f < (mu - res * dev) )

f0_2d = f0.reshape(-1,2)
f0_2d_norm = np.array([ np.linalg.norm(pt) for pt in f0_2d ])
plt.plot(f0_2d_norm)
outlier = isOutlier(f0_2d_norm)

n_cameras = view_idx+1
n_points = out_pts_3d.shape[0]
camera_indices = camera_indexes[~outlier].ravel()
point_indices = points_indexes[~outlier].astype(np.int32)

x1 = np.hstack((out_camera_params.ravel(), out_pts_3d[:,:3].ravel()))
f1 = fun(x1, n_cameras, n_points, camera_indices, point_indices.copy(), out_pts_2d[~outlier], K)
plt.plot(f1)

from scipy.sparse import lil_matrix

#we are only trying to optimize camera extrinsics and the 3d points
def bundle_adjustment_sparsity(n_cameras, n_points, camera_indices, point_indices):
    m = camera_indices.size * 2
    n = n_cameras * 6 + n_points * 3
    A = lil_matrix((m, n), dtype=int)

    i = np.arange(camera_indices.size)
    for s in range(6):
        A[2 * i, camera_indices * 6 + s] = 1
        A[2 * i + 1, camera_indices * 6 + s] = 1

    for s in range(3):
        A[2 * i, n_cameras * 6 + point_indices * 3 + s] = 1
        A[2 * i + 1, n_cameras * 6 + point_indices * 3 + s] = 1

    return A

A = bundle_adjustment_sparsity(n_cameras, n_points, camera_indices, point_indices)

import time
from scipy.optimize import least_squares

t0 = time.time()
res = least_squares(fun, x1, jac_sparsity=A, verbose=2, x_scale='jac', ftol=1e-8, method='trf',
                    args=(n_cameras, n_points, camera_indices, point_indices, out_pts_2d[~outlier], K))
t1 = time.time()

camera_params = res.x[:n_cameras * 6].reshape((n_cameras, 6))
points_3d = res.x[n_cameras * 6:].reshape((n_points, 3))

plt.plot(res.fun)

write_ply("./out1.ply", points_3d, out_colors.T[[2,1,0]].T)