TP Calibration

Read, understand, and complete and run the following notebook. You must return the completed notebook, including your answers and illustrations (you may need to add cells to write your code or comments).

To execute a notebook, you will need to install jupyter. If you are using anaconda (strongly advised). If you cannot/don't want to use notebooks, you can return both your python code and a report in pdf. Note that all the TPs will use python2. If you are using python3, you might need to do a few changes to the provided code.

0. Imports

In [2]:
import numpy as np 
# this is the key library for manipulating arrays. Use the online ressources! http://www.numpy.org/

import matplotlib.pyplot as plt 
# used to read images, display and plot http://matplotlib.org/api/pyplot_api.html . 
#You can also check this simple intro to using ipython notebook with images https://matplotlib.org/users/image_tutorial.html

%matplotlib inline 
# to display directly in the notebook

import scipy.ndimage as ndimage
# one of several python libraries for image procession

import scipy.io as sio
# usefull for loading mat files

import scipy.optimize as optimize
# usefull for optimization

plt.rcParams['image.cmap'] = 'gray' 
# by default, the grayscale images are displayed with the jet colormap: use grayscale instead

Load the data

In [3]:
data=sio.loadmat('clean_points.mat')
pts2d=data['pts2d']
pts3d=data['pts3d']

1: Build the matrix that define the calibration equation

In [4]:
def calibr_mat(pts2d=pts2d, pts3d=pts3d):
    A = np.zeros((2*pts3d.shape[0], 12))
    
    for i in range(pts3d.shape[0]):
        A[2*i, 4:7] = -pts3d[i, :]
        A[2*i, 7] = -1
        A[2*i, 8:11] = pts2d[i, 1]*pts3d[i,:]
        A[2*i, 11] = pts2d[i, 1]
        
        A[2*i+1,:3] = pts3d[i, :]
        A[2*i+1, 3] = 1
        A[2*i+1, 8:11] = -pts2d[i, 0]*pts3d[i,:]
        A[2*i+1, 11] = -pts2d[i, 0]
    
    return A

A = calibr_mat()

2: Perform SVD (using np.linalg.svd). Check that the SVD is coherent and that one of the singular value is close to 0. Compute the projection matrix.

In [6]:
U, s, V = np.linalg.svd(A)

S = np.zeros((U.shape[1], V.shape[0]))
S[:12, :12] = np.diag(s)
print(np.isclose(np.dot(U, np.dot(S, V)), A))

P = V[-1,:].reshape(3, 4)
[[ True  True  True ...,  True  True  True]
 [ True  True  True ...,  True  True  True]
 [ True  True  True ...,  True  True  True]
 ..., 
 [ True  True  True ...,  True  True  True]
 [ True  True  True ...,  True  True  True]
 [ True  True  True ...,  True  True  True]]

3: Compute the error between the projected 3D points and the 2D points.

In [8]:
N = pts3d.shape[0]
proj_pts3d = np.hstack((pts3d, np.ones(N).reshape(N, 1)))
proj_pts2d = np.hstack((pts2d, np.ones(N).reshape(N, 1)))

sum((pts2d[i, 0] - np.dot(P[0], proj_pts3d[i, :]/np.dot(P[2], proj_pts3d[i, :])))**2 \
    + (pts2d[i, 1] - np.dot(P[1], proj_pts3d[i, :])/np.dot(P[2], proj_pts3d[i, :]))**2 for i in range(N))
Out[8]:
450.01475379292839

4: Visualize the projected 3D points and the 2D points on the same figure (use plt.scatter)

In [138]:
proj_pts = P.dot(proj_pts3d.T).T

fig = plt.figure(figsize=(10,8))


plt.scatter(*zip(*pts2d), label='2D points', s=60, c='y')
plt.scatter(*zip(*[(p[0]/p[2], p[1]/p[2]) for p in proj_pts]),\
            s=60, label='Projected 3D points', marker='+', c='red')
plt.legend(bbox_to_anchor=(1, 1.1))
plt.show()

5: Extract the intrinsic and extrinsic parameters of the camera from the matrix (use np.linalg.inv)

$$P ≝ \left[M \;\middle|\; b\right] = K \left[R \;\middle|\; t\right]$$

where

  • $$K ≝ \begin{pmatrix}α & γ & u_0 \\ 0 & β & v_0 \\ 0 & 0 & 1\end{pmatrix}$$
  • $$M = KR ⟹ R = K^{-1}M$$
  • $$Kt = b ⟹ t = K^{-1}b$$

$$MM^T = KK^T = \begin{pmatrix}α^2 + γ^2 + u_0^2 & γβ+u_0 v_0 & u_0 \\ γβ+u_0 v_0 & β^2 + v_0^2 & v_0 \\ u_0 & v_0 & 1\end{pmatrix}$$

In [114]:
# Intrinsic parameters: alpha, beta, gamma, u_0, v_0

MM_T = np.linalg.inv(P[:, :-1]).dot(np.linalg.inv(P[:, :-1]).T)
# normalizing
MM_T /= MM_T[2,2]

u_0, v_0 = MM_T[0,2], MM_T[1,2]
beta = np.sqrt(MM_T[1,1]-v_0**2)
gamma = (MM_T[0,1]-u_0*v_0)/beta
alpha = np.sqrt(MM_T[0,0]-u_0**2-gamma**2)
K = np.array([[alpha, gamma, u_0], [0, beta, v_0], [0, 0, 1]])

# Extrinsic parameters: R, t

K_inv = np.linalg.inv(K)
R = K_inv.dot(P[:, :-1])
t = K_inv.dot(P[:,-1])

Rt = np.hstack((R,t.reshape([-1, 1])))

np.isclose(P,K.dot(Rt))
Out[114]:
array([[ True,  True,  True,  True],
       [ True,  True,  True,  True],
       [ True,  True,  True,  True]], dtype=bool)

6: Improve the estimate of the camera matrix using non linear least square (use optimize.leastsq which implements a variant of Levenberg-Marquardt) Compare the reprojection error before and after the optimization.

In [152]:
def error(P):
    return np.array([pts2d[i, 0] - np.dot(P[0:4], proj_pts3d[i, :])/np.dot(P[8:12], proj_pts3d[i, :]) for i in range(N)]\
                    + [pts2d[i, 1] - np.dot(P[4:8], proj_pts3d[i, :])/np.dot(P[8:12], proj_pts3d[i, :]) for i in range(N)])

P_new = optimize.leastsq(error, P.flatten())[0].reshape((3,4))
In [153]:
proj_pts = P.dot(proj_pts3d.T).T
proj_pts_new = P_new.dot(proj_pts3d.T).T


fig = plt.figure(figsize=(10,8))


plt.scatter(*zip(*pts2d), label='2D points', s=60, c='y')
plt.scatter(*zip(*[(p[0]/p[2], p[1]/p[2]) for p in proj_pts]),\
            s=60, label='Old projected 3D points', marker='+', c='purple')
plt.scatter(*zip(*[(p[0]/p[2], p[1]/p[2]) for p in proj_pts_new]),\
            s=60, label='New projected 3D points (Levenberg-Marquardt)', marker='x', c='red')
plt.legend(bbox_to_anchor=(1, 1.1))
plt.show()
In [156]:
# Markdown
from IPython.display import display, Markdown

err_before = sum((pts2d[i, 0] - np.dot(P[0], proj_pts3d[i, :]/np.dot(P[2], proj_pts3d[i, :])))**2 \
    + (pts2d[i, 1] - np.dot(P[1], proj_pts3d[i, :])/np.dot(P[2], proj_pts3d[i, :]))**2 for i in range(N))

err_after = sum((pts2d[i, 0] - np.dot(P_new[0], proj_pts3d[i, :]/np.dot(P_new[2], proj_pts3d[i, :])))**2 \
    + (pts2d[i, 1] - np.dot(P_new[1], proj_pts3d[i, :])/np.dot(P_new[2], proj_pts3d[i, :]))**2 for i in range(N))

display(Markdown("### Error between the projected 3D points and the 2D points:"))

display(Markdown("- **Before:** `{}` \n"\
         "- **After Levenberg-Marquardt**: `{}` \n"\
         "> Decrease: `{}` \n"\
                 .format(err_before, err_after, err_before-err_after)))

Error between the projected 3D points and the 2D points:

  • Before: 450.0147537929284
  • After Levenberg-Marquardt: 448.90377777447014

    Decrease: 1.1109760184582456