# 3D Visualization of calibrated cameras and a chessboard

This notebook visualizes the basics of perspective projection and pinhole cameras.    

**Subjects covered** 

1. **Camera calibration** using a chessboard and OpenCV's calibration functions. 
2. **Interchanging coordinate frames**  
    That is, switching between: 
    * *3D world coordinate frame* - a frame with standard basis vectors centered at the world origin.
    * *2D camera image plane* - a frame centered at a camera's projection center and rotated    
         to align the $z$-axis with the camera's principal axis, and the $x$ and $y$ axis with the camera's image plane. 
    * *2D camera pixel plane* - another camera-centric coordinate frame, but with appropriate scaling, skewing,    
        and translating to align coordinates of projected points with the camera's pixel coordinates of those points. 
3. **Visualizing camera poses and a chessboard in 3D**.  
4. **Superimposing an object into a scene**.   
5. **Back-projecting image points** to exhibit the ray along which corresponding 3D object points will lie. 
6. **Triangulating 3D world points** by finding the (near-)intersection of two rays of back-projected SIFT correspondences.
7. **Conclusion**
7. **Sources**

**Why are we interested in these subjects?**    
Real-world cameras can be set up to approximate a pinhole camera. Pinhole camera image formation is well modeled by the maths of perspective projection.    
This 2D image$\leftrightarrow$3D world correspondence enables us to infer world structure from images, and images from world structure.   
This gives rise to a range of useful applications, including: 
* measuring objects in the scene (photogrammetry) 
* collision avoidance for autonomous robots (depth perception)
* inserting objects into a scene (virtual reality)
* superimposing textures onto a scene (virtual reality)
* creating 3D models of real-world objects (metric reconstruction).

**Prerequisites**    
A basic familiarity with linear algebra, Python, and the camera matrix is assumed. For the camera matrix, reading [this OpenCV page](https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html ) should suffice. 

**Further reading**   
For a comprehensive treatment of perspective projection, see the first four chapters of [Multiple view geometry in computer vision](https://www.robots.ox.ac.uk/~vgg/hzbook/) by Richard Hartley and Andrew Zisserman. For a less thorough but more merciful introduction, watch [the second lecture](https://www.youtube.com/watch?v=iL4WhR0hzns&list=PL05umP7R6ij35L2MHGzis8AEHz7mg381_&ab_channel=T%C3%BCbingenMachineLearning) of Andreas Geiger's computer vision course.

## Imports


```python
import matplotlib.pyplot as plt
import ipyvolume as ipv
import numpy as np
import cv2
```

## Calibrate cameras
[Camera calibration](https://www.mathworks.com/help/vision/ug/camera-calibration.html) refers to the process of determining the extrinsic and intrinsic parameters of a camera. Extrinsic parameters model the pose of a camera in 3D space. Viewed as a transformation, the extrinsic matrix maps 3D points to the camera's image plane. Intrinsic parameters model how these 2D points in the image plane relate to the 2D pixel coordinates in the camera's image. Without knowing the intrinsics, one cannot infer object size, angles, parallelism, and other useful scene properties. Now we will see how to retrieve the intrinsic and extrinsic parameters.

As the camera extrinsics and instrinsics encode a 2D pixel$\leftrightarrow$3D world relation, knowing 2D$\leftrightarrow$3D correspondences lets us compute the parameters. To get 2D$\leftrightarrow$3D correspondences, a calibration object with known geometry such as a chessboard can be used. The OpenCV library has a function [findChessboardCorners](https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#ga93efa9b0aa890de240ca32b11253dd4a) that supports a chessboard setup. This means we do not need to manually label pixels to find 2D pixel$\leftrightarrow$3D world correspondences. To then calculate the intrinsics from correspondences, OpenCV's [calibrateCamera function](https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d) is used, which implements [Zhang's camera calibration algorithm](https://ieeexplore.ieee.org/abstract/document/888718/). For a high-level summary of this algorithm, see the appendix at the end of this notebook.  


```python
def calibrate_cameras() :
    """ Calibrates cameras from chessboard images. 
    
    Returns: 
        images (list[np.ndarray]): Images containing the chessboard.
        intrinsics (np.ndarray): An upper triangular 4x4 full-rank matrix containing camera intrinsics.
        distortions (np.ndarray): Radial distortion coefficients.
        rotation_vectors (list[np.ndarray]): Rodrigues rotation vectors.
        translation_vectors (list[np.ndarray]): Translation vectors.
        object_points: (np.ndarray): A (4, 54) point array, representing the [x,y,z,w]
            of 54 chessboard points (homogenous coordiantes).
    """
    images = list() 
    
    # Read images
    for i in range(11): 
        img = cv2.imread(f'./images/{i}.jpg')
        img = cv2.resize(img, None, fx=0.25, fy=0.25)
        images.append(img) 

    # The default opencv chessboard has 6 rows, 9 columns 
    shape = (6, 9) 

    # List to store vectors of 3D world points for every checkerboard image
    object_points_all = []
    
    # List to store vectors of 2D projected points for every checkerboard image
    image_points_all = [] 
    
    # Flags for chessboard corner search. Taken from opencv docs.
    flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_NORMALIZE_IMAGE
    
    # Criteria for termination of the iterative corner refinement. Taken from opencv docs.
    refinement_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    # List for images in which chessboard is be found by search
    images_filtered = list()
    
    # Object points in a single image. Simply a row iterated list of z=0 3d points. 
    # E.g. [[0. 0. 0.] [1. 0. 0.] ... [0, 1, 0], [1, 1, 0], ... ]
    object_points = np.zeros((1, shape[0] * shape[1], 3), np.float32)
    object_points[0, :, :2] = np.mgrid[0:shape[0], 0:shape[1]].T.reshape(-1, 2)
    
    # For each image, store the object points and image points of chessboard corners.
    for idx, image in enumerate(images): 
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        succes, corners = cv2.findChessboardCorners(image=gray, 
                                                    patternSize=shape,
                                                    flags=flags) 
        if succes:
            images_filtered.append(image)
            corners = cv2.cornerSubPix(image=gray, 
                                       corners=corners, 
                                       winSize=(11,11), 
                                       zeroZone=(-1,-1), 
                                       criteria=refinement_criteria)
            object_points_all.append(object_points)
            image_points_all.append(corners)    
            
    images = images_filtered
    
    # Calibrate the cameras by using the 3D <-> 2D point correspondences.
    ret, intrinsics, distortions, rotation_vectors, translation_vectors = cv2.calibrateCamera(object_points_all, image_points_all, gray.shape[::-1], None, None)
    
    # Make intrinsic matrix 4x4 full-rank to ease manipulation.
    intrinsics = np.hstack([intrinsics, np.zeros((3, 1))])
    intrinsics = np.vstack([intrinsics, [[0, 0, 0, 1]]])

    # Convert chessboard object points to homogeneous coordinates to ease later use.
    object_points = object_points[0].reshape((-1, 3)).T
    object_points = np.vstack([object_points, np.ones((1, object_points.shape[1]))])
    
    return images, intrinsics, distortions, rotation_vectors, translation_vectors, object_points
    
images, intrinsics, distortions, rotation_vectors, translation_vectors, object_points = calibrate_cameras()

print(f'\n\nNumber of calibration images: {len(images)}')
print(f'Number of calibration points: {object_points.shape[1]}')
print(f'\n\nSample of imags used')
plt.figure(figsize=(20,20))
_ = plt.imshow(cv2.hconcat([cv2.cvtColor(im, cv2.COLOR_RGB2BGR) for im in images[:5]]))
```

## Convert calibration output to extrinsic matrices
The calibration method returns regular translations and rotations encoded as [Rodrigues rotation](https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula) vectors. After converting these rotation vectors into rotation matrices we can form $3 \times 4$ camera extrinsic matrices $[\mathbf{R}| \mathbf{t}] = \mathbf{E}$. Matrix multiplying the extrinsic matrix with a homogenous 3D point returns the point projected to the camera's image plane (a camera-centeric coordinate frame where $z=1$).  

$$ \text{point in camera image plane} = [\mathbf{R}| \mathbf{t}] \cdot \text{point in world ref.}$$

The camera projection center is the origin of this camera-centric coordinate frame. This is the point mapped to zero by the extrinsics, i.e. the extrinsics' null space. Keep in mind that $\mathbf{EC} = \mathbf{RC} + \mathbf{T}$, that is, applying the extrinsic matrix to a homogenous point is the same as applying the rotation and adding a translation separately.

$$ \begin{align}
 0 &= \mathbf{EC}\\
 0 &= \mathbf{RC} + \mathbf{T}\\
-\mathbf{RC} &= \mathbf{T}\\
-\mathbf{R^T} \cdot -\mathbf{RC} &= -\mathbf{R^T}\mathbf{T}\\
 \mathbf{C} &= -\mathbf{R^T}\mathbf{T}
 \end{align}$$
 
This formula will now be used to infer the camera centers. 


```python
def extrinsics_from_calibration(rotation_vectors, translation_vectors):
    """ Calculates extrinsic matrices from calibration output. 
        
    Args: 
        rotation_vectors (list[np.ndarray]): Rodrigues rotation vectors.
        translation_vectors (list[np.ndarray]): Translation vectors.
    Returns: 
        extrinsics (list[np.ndarray]): A list of camera extrinsic matrices.
            These matrices are 4x4 full-rank.
    """
    
    rotation_matrices = list() 
    for rot in rotation_vectors:
        rotation_matrices.append(cv2.Rodrigues(rot)[0]) 

    extrinsics = list()
    for rot, trans in zip(rotation_matrices, translation_vectors): 
        extrinsic = np.concatenate([rot, trans], axis=1)
        extrinsic = np.vstack([extrinsic, [[0,0,0,1]]]) 
        extrinsics.append(extrinsic)
    
    return extrinsics

def camera_centers_from_extrinsics(extrinsics):
    """ Calculates camera centers from extrinsic matrices. 
    
    Args: 
        extrinsics (list[np.ndarray]):  A list of camera extrinsic matrices.
    Returns: 
        camera_centers (list[np.ndarray]): Homogenous coordinates of camera centers in 
            3D world coordinate frame.
    """
    camera_centers = list() 

    for extrinsic in extrinsics:
        rot = extrinsic[:3, :3]
        trans = extrinsic[:3, 3]
        center = -rot.T @ trans
        center = np.append(center, 1)
        camera_centers.append(center)
    
    return camera_centers

extrinsics = extrinsics_from_calibration(rotation_vectors, translation_vectors)
camera_centers = camera_centers_from_extrinsics(extrinsics)

print(f'Shape of an extrinsic matrix: {extrinsics[0].shape}')
print(f'Shape of the intrinsic matrix: {intrinsics.shape}')
print(f'Number of cameras in the scene: {len(camera_centers)}')
```

## Invertible perspective projection 
Perspective projection of a pinhole camera is performed in two steps. First, the 3D point is projected to the camera's image plane using the extrinsic matrix. Then, the point on the image plane is scaled, skewed, and tranlsated to align with the camera's image pixel coordinates using the intrinsic matrix.

$$ \begin{align}
\text{2D pixel point} &= \text{Align with pixels} (\text{Proj to cam image plane}(\text{3D world point})) \\
\mathbf{x}&=\mathbf{KEX} \\
\begin{bmatrix} x\\ y\\ w \end{bmatrix} &= \begin{bmatrix} f_{x} & \alpha     & c_{x0} \\
                                                            0    & f_{y} & c_{y0} \\
                                                            0    & 0     & 1  \end{bmatrix} 
                                           \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_{x} \\
                                                           r_{24} & r_{25} & r_{26} & t_{y} \\
                                                           r_{36} & r_{37} & r_{38} & t_{z} \end{bmatrix}
                                           \begin{bmatrix} X\\ Y\\ Z\\ W \end{bmatrix}
\end{align}
$$
Often we refer to product $\mathbf{KE}$ as the projection matrix $\mathbf{P}$.  

As one may notice, the extrinsic matrix is not square in this description, while in the code it is. The reason is that it's useful to be able to invert the projection matrix and its components, i.e. $\mathbf{P^{-1}}, \mathbf{E^{-1}}, \mathbf{K^{-1}}$. This allows easy switching between coordinate frames using matrix multiplication. To achieve this, add the row $(0, 0, 0, 1)$ to make $\mathbf{E}$ square and full-rank. Similarly, make $\mathbf{K}$ full-rank $4\times4$.
  
$$\mathbf{\tilde{P}} = \begin{bmatrix} \mathbf{K} &  0 \\ 0^{T} & 1 \end{bmatrix}
              \begin{bmatrix} \mathbf{R} &  t \\ 0^{T} & 1 \end{bmatrix} = \mathbf{\tilde{K}\tilde{E}}$$ 
              
The 4x4 projection matrix $\mathbf{\tilde{P}}$ can now be used to map directly from 3D world coordinates $\mathbf{\bar{p}} = (x_w, y_w, z_w,1)$ to screen coordinates plus disparity, $\mathbf{x_s} = (x_s, y_s, 1, d)$  

$$ \mathbf{x_s} \sim \mathbf{\tilde{P}\bar{p}_w} $$

where $\sim$ indicates equality up to scale. Note that after multiplication by $\mathbf{\tilde{P}}$, the vector must be divided by the third element of the vector to obtain this normalized form $\mathbf{x_s} = (x_s, y_s, 1, d)$.    

Whenever we don't want the full expressivity of a $4\times4$ projection matrix we can always slice the intrinsics and extrincs back to their original size. E.g. when we do not want to specify a disparity.

Partial excerpt taken from Computer Vision Algorithms and Applications by Richard Szeliski pg.49, section 'Camera matrix'.

## Plot chessboard and cameras
Given the extrinsics and a priori defined chessboard object points, a 3D visualization can be created. The [Ipyvolume](https://github.com/maartenbreddels/ipyvolume) library is used for this.


```python
cam_sphere_size = 1

def init_3d_plot():
    """ Initializes a ipyvolume 3d plot and centers the 
        world view around the center of the chessboard.
        
    Returns: 
        fig (ipyvolume.pylab.figure): A 3D plot. 
    """
    chessboard_x_center = 2.5
    chessboard_y_center = 4
    fig = ipv.pylab.figure(figsize=(15, 15), width=800)
    ipv.xlim(2.5 - 30, 2.5 + 30)
    ipv.ylim(4 - 30, 4 + 30)
    ipv.zlim(-50, 10)
    ipv.pylab.view(azimuth=40, elevation=-150)
    return fig

def plot_chessboard(object_points):
    """ Plots a 3D chessboard and highlights the 
        objects points with green spheres. 
        
    Args:  A (4, 54) point array, representing the [x,y,z,w]
           of 54 chessboard points (homogenous coordiantes).
    """
    img = cv2.imread('./images/chessboard.jpg')
    img_height, img_width, _ = img.shape
    chessboard_rows, chessboard_cols = 7, 10
    xx, yy = np.meshgrid(np.linspace(0, chessboard_rows, img_height), 
                         np.linspace(0, chessboard_cols, img_width)) 
    zz = np.zeros_like(yy)
    img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) / 255
    # -1 is used as the start of the board images x and y coord, 
    # such that the first inner corner appear as coord (0, 0, 0) 
    ipv.plot_surface(xx-1, yy-1, zz, color=cv2.transpose(img))
    xs, ys, zs, _ = object_points
    ipv.scatter(xs, ys, zs, size=1, marker='sphere', color='lime')
    
def plot_camera_axis(cam_center, inv_extrinsic, vis_scale):
    """ Plots the x, y, and z basis vectors of the camera coordinate frame.
        This is achieved by  transforming the basis vectors into the world 
        coordinate frame and plotting them. 
    
    Args:
        cam_center (np.ndarray): Coordinates of camera centers in 
            3D world coordinate frame.                                                                                                                                                   :
        inv_extrinsic (np.ndarray): The (pseudo)-inverse of camera extrinsic matrix.
        vis_scale (int): A visualization size multiplyer to make cameras 
            appear larger and easier to see.
    """
    x, y, z, _ = cam_center
    x_arrow = inv_extrinsic @ (1*vis_scale, 0, 0,  1)
    y_arrow = inv_extrinsic @ (0, 1*vis_scale, 0,  1)
    z_arrow = inv_extrinsic @ (0, 0, 1*vis_scale,  1)
    
    ipv.plot([x, x_arrow[0]], [y, x_arrow[1]], [z, x_arrow[2]], color='red')
    ipv.plot([x, y_arrow[0]], [y, y_arrow[1]], [z, y_arrow[2]], color='blue')
    ipv.plot([x, z_arrow[0]], [y, z_arrow[1]], [z, z_arrow[2]], color='green')
    ipv.scatter(np.array([x]), np.array([y]), np.array([z]), size=cam_sphere_size, marker="sphere", color='blue')
```


```python
init_3d_plot()
plot_chessboard(object_points)

# Determines the visual scale of the plotted cameras 
vis_scale = 5

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images): 
    height, width, _ = image.shape
    inv_extrinsic = np.linalg.inv(extrinsic)
    plot_camera_axis(cam_center, inv_extrinsic, vis_scale)
    
ipv.show()
```

## Plot cameras as viewport wireframes
Instead of camera-centric coordinate frames, we can plot a slightly more intuitive wireframe of each camera's viewport.


```python
# Visual dimension of the camera in the 3D plot. 
height, width, _ = images[0].shape
camera_aspect_ratio = width / height
# A length of 1 corresponds to the length of 1 chessboard cell.
# This is because a chessboard points have been defined as such.
# Set height of camera viewport to 1.
vis_cam_height = 1 
vis_cam_width = vis_cam_height * camera_aspect_ratio
wire_frame_depth = 1.2

def plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic):
    """ Plots the wireframe of a camera's viewport. """
    x, y, z, _ = cam_center 
    
    # Get left/right top/bottom wireframe coordinates
    # Use the inverse of the camera's extrinsic matrix to convert 
    # coordinates relative to the camera to world coordinates.
    lt = inv_extrinsic @ np.array((-vis_cam_width/2, -vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale
    rt = inv_extrinsic @ np.array((vis_cam_width/2,  -vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale
    lb = inv_extrinsic @ np.array((-vis_cam_width/2,  vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale
    rb = inv_extrinsic @ np.array((vis_cam_width/2,   vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale

    # Connect camera projective center to wireframe extremities
    ipv.plot([x, lt[0]], [y, lt[1]], [z, lt[2]], color='blue')
    ipv.plot([x, rt[0]], [y, rt[1]], [z, rt[2]], color='blue')
    ipv.plot([x, lb[0]], [y, lb[1]], [z, lb[2]], color='blue')
    ipv.plot([x, rb[0]], [y, rb[1]], [z, rb[2]], color='blue')
    
    # Connect wireframe corners with a rectangle
    ipv.plot([lt[0], rt[0]], [lt[1], rt[1]], [lt[2], rt[2]], color='blue')
    ipv.plot([rt[0], rb[0]], [rt[1], rb[1]], [rt[2], rb[2]], color='blue')
    ipv.plot([rb[0], lb[0]], [rb[1], lb[1]], [rb[2], lb[2]], color='blue')
    ipv.plot([lb[0], lt[0]], [lb[1], lt[1]], [lb[2], lt[2]], color='blue')
    ipv.scatter(np.array([x]), np.array([y]), np.array([z]), size=cam_sphere_size, marker="sphere", color='blue')
```


```python
init_3d_plot()
plot_chessboard(object_points)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images): 
    inv_extrinsic = np.linalg.pinv(extrinsic)
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)

ipv.show()
```

## Plot images in the camera wireframes
To enhance the viewport visualization, let us
* add the real-world images to the viewport 
* project the chessboard calibration points to the images


```python
def plot_picture(image, inv_extrinsic, vis_scale):
    """ Plots a real-world image its respective 3D camera wireframe. """ 
    image = image.copy()
    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) 
    image = cv2.resize(image, None, fx=0.1, fy=0.1) / 255
    img_height, img_width, _ = image.shape

    xx, yy = np.meshgrid(np.linspace(-vis_cam_width/2 * vis_scale,  vis_cam_width/2 * vis_scale,  img_width), 
                         np.linspace(-vis_cam_height/2 * vis_scale, vis_cam_height/2 * vis_scale, img_height))
    zz = np.ones_like(yy) * wire_frame_depth * vis_scale
    coords = np.stack([xx, yy, zz, np.ones_like(zz)]) 
    coords = coords.reshape(4, -1) 
     
    # Convert canera relative coordinates to world relative coordinates
    coords = inv_extrinsic @ coords
    xx, yy, zz, ones = coords.reshape(4, img_height, img_width) 
    ipv.plot_surface(xx, yy, zz, color=image)
    
    
def project_points_to_picture(image, object_points, intrinsics, extrinsic):
    """ Perspective projects points to an image and draws them green. """
    image = image.copy()
    proj_matrix = intrinsics @ extrinsic
    object_points = proj_matrix @ object_points
    xs, ys, ones, disparity = object_points / object_points[2]
    
    for idx, (x, y) in enumerate(zip(xs, ys)):
        x = round(x)
        y = round(y)
        if (0 < y < image.shape[0] and
            0 < x < image.shape[1]):
            # Each point occupies a 20x20 pixel area in the image.
            image[y-10:y+10, x-10:x+10] = [0, 255, 0] 
    
    return image
```


```python
init_3d_plot()
plot_chessboard(object_points)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    image = project_points_to_picture(image, object_points, intrinsics, extrinsic)
    inv_extrinsic = np.linalg.pinv(extrinsic)
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    
ipv.show()
```

## Place bunny model in scene and project to cameras


Now we can insert artificial 3D objects into the scene and project them to the cameras.    
This is done for the infamous [Stanford bunny](https://www.cc.gatech.edu/~turk/bunny/bunny.html).     
The bunny appears flat in the images because shading is not implemented and is out of this notebook's scope.


```python
def plot_bunny():
    """Plots the Stanford bunny pointcloud and returns its points. 
    
    Returns: 
        bunny_coords (np.ndarray): a 4xn homogenous point cloud array.
    """
    bunny_coords = np.load(open('data/bunny_point_cloud.npy', 'rb'))
    b_xs, b_ys, b_zs = bunny_coords[:3]
    bunny_point_size = 0.5
    ipv.scatter(b_xs, b_ys, b_zs, size=bunny_point_size, marker="sphere", color='lime')
    return bunny_coords
```


```python
init_3d_plot()
bunny_coords = plot_bunny() 
plot_chessboard(object_points)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    image = project_points_to_picture(image, object_points, intrinsics, extrinsic)
    image = project_points_to_picture(image, bunny_coords, intrinsics, extrinsic)
    inv_extrinsic = np.linalg.pinv(extrinsic)
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    
ipv.show()
```

## Plot camera optical axes
By using the inverse of the projection matrix, $\mathbf{P^{-1}}$, one can back-project the camera ray for a given image pixel.   
In the following code, we back-project the center pixel of each camera. This should roughly equate to the camera's 'principal' or 'optical' axis.    
Keep the reversed order of the extrinsics and intrinsics in mind.
$$\mathbf{P} = \mathbf{KE}$$
$$\mathbf{P^{-1}} = \mathbf{E^{-1} K^{-1}}$$ 



```python
init_3d_plot()
plot_chessboard(object_points)
dir_vec_length = 50
inv_intrinsics = np.linalg.inv(intrinsics)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    image_center = np.array([image.shape[1]/2, image.shape[0]/2, 1, 1]).astype(int)

    # Color the center of the image green.
    x, y, _, _ = image_center
    image = image.copy()
    image[y-50:y+50, x-50:x+50] = [0, 255, 0]

    inv_extrinsic = np.linalg.inv(extrinsic)
    dir_vec_in_cam_ref = inv_intrinsics @ image_center
    
    # Increase the length of the ray (only x,y,z not w).
    # Scaling the whole homogenous vector would not change it.
    dir_vec_in_cam_ref[:3] = dir_vec_in_cam_ref[:3] * dir_vec_length
    dir_vec_in_world = inv_extrinsic @ dir_vec_in_cam_ref
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    cam_x, cam_y, cam_z, _ = cam_center
    ipv.plot([cam_x, dir_vec_in_world[0]], [cam_y, dir_vec_in_world[1]], [cam_z, dir_vec_in_world[2]], color='red')

ipv.show()
```


```python
cx = intrinsics[0,2]
cy = intrinsics[1,2]
print(f'Optical center according to the intrinsic matrix.\n x:{cx:.1f} y:{cy:.1f}\n')
print(f'Optical center according to the images.\n x:{x:.1f} y:{y:.1f} (assuming optical center is in middle)\n')
print(f'Discrepancy: \n x:{abs(cx-x):.1f} y:{abs(cy-y):.1f}')
```

## Plot all camera rays towards the first inner corner of the chessboard
Note that because we 
* keep track of the points 'disparity' after projection 
* use a full rank $4\times4$ invertible projection matrix      

we can back-project a camera ray with a length that reaches the appropriate world point. 


```python
init_3d_plot()
plot_chessboard(object_points)
chess_corner = np.array([0, 0, 0, 1])

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    proj_matrix = intrinsics @ extrinsic
    chess_corner_proj = proj_matrix @ chess_corner
    inv_extrinsic = np.linalg.inv(extrinsic)
    dir_vec_in_cam_ref = inv_intrinsics @ chess_corner_proj
    dir_vec_in_world = inv_extrinsic @ dir_vec_in_cam_ref
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    cam_x, cam_y, cam_z, _ = cam_center
    ipv.plot([cam_x, dir_vec_in_world[0]], [cam_y, dir_vec_in_world[1]], [cam_z, dir_vec_in_world[2]], color='red')

ipv.show()
```

## Triangulate 3D points on the table with image point correspondences

[Triangulation](https://en.wikipedia.org/wiki/Triangulation) in computer vision refers to the process of determining the location of a world point by finding near-intersections of camera rays. In this section we will:
* find [SIFT](https://en.wikipedia.org/wiki/Scale-invariant_feature_transform) point correspondences in a pair of images.
* back-project the image points into rays.
* find the triangulated point where the rays intersect.

## Handpick correct SIFT correspondences
Normally, one would use an automatic and robust method to find correspondences across multiple images. 
Due to the 
* large difference in camera poses
* poor exposure of the images 
* absence of texture features on the table  

valid point correspondences will be hand-picked in this code cell. 


```python
camera_1_idx = 3
camera_2_idx = 0 
image_1 = images[camera_1_idx].copy()
image_2 = images[camera_2_idx].copy()

# Initialize SIFT detector
sift = cv2.SIFT_create()

# Find the keypoints and descriptors with SIFT
keypoints_1, descriptors_1 = sift.detectAndCompute(image_1, None) # queryImage
keypoints_2, descriptors_2 = sift.detectAndCompute(image_2, None) # trainimage

# Match descriptors
bf = cv2.BFMatcher()
matches = bf.knnMatch(descriptors_1, descriptors_2, k=2)

# Apply ratio test 
good = []
for best_match, second_best_match in matches:
    if best_match.distance < 0.75 * second_best_match.distance:
        good.append([best_match])

# Sort matches according to descriptor distance
dists = [g[0].distance for g in good]
good = list(sorted(zip(dists, good)))
good = [list(g) for g in zip(*good)][1]

# Select manually validated matches
hand_picked_matches = [2, 9, 15, 16, 18, 19, 22, 23, 24]
good = np.array(good, dtype=object)[hand_picked_matches]

# Plot matches 
match_image = cv2.drawMatchesKnn(image_1, keypoints_1, image_2, keypoints_2, good, None, [0, 255, 0])
match_image = cv2.cvtColor(match_image, cv2.COLOR_RGB2BGR)
plt.figure(figsize=(20,20))
plt.imshow(match_image)
plt.show()
```

## Encode and plot matches as image points 


```python
match_coords_1 = list()
match_coords_2 = list()

for i in good:
    i = i[0]
    keypoint_1 = keypoints_1[i.queryIdx]
    keypoint_2 = keypoints_2[i.trainIdx]
    keypoint_1_center = np.array(keypoint_1.pt)
    keypoint_2_center = np.array(keypoint_2.pt)
    x1, y1 = keypoint_1_center
    x2, y2 = keypoint_2_center
    match_coords_1.append([x1, y1, 1, 1])
    match_coords_2.append([x2, y2, 1, 1])
    color = [0, 0, 255]
    image1 = cv2.circle(image_1, keypoint_1_center.astype(int), 10, color, -1)
    image2 = cv2.circle(image_2, keypoint_2_center.astype(int), 10, color, -1)

match_coords_1 = np.array(match_coords_1)
match_coords_2 = np.array(match_coords_2)

image_1 = cv2.cvtColor(image_1, cv2.COLOR_BGR2RGB)
image_2 = cv2.cvtColor(image_2, cv2.COLOR_BGR2RGB)

image_concat = cv2.hconcat([image_1, image_2])
plt.figure(figsize=(20,20))
plt.imshow(image_concat)
plt.show() 
```

## Draw back-projected rays of point correspondences
Rays from the first camera are drawn in red, those from the second camera are drawn in green.


```python
camera_1_idx = 3
camera_2_idx = 0 

init_3d_plot()
plot_chessboard(object_points)

for idx, (cam_center, extrinsic, image) in enumerate(zip(camera_centers, extrinsics, images)):
    
    if idx == camera_1_idx: 
        points = match_coords_1
        color = 'red'
    elif idx == camera_2_idx:
        points = match_coords_2
        color = 'lime'
    else: 
        continue 
    
    inv_extrinsic = np.linalg.inv(extrinsic)
    dir_vec_in_cam_ref = inv_intrinsics @ points.T
    dir_vec_in_cam_ref[:3] *= 55
    dir_vec_in_world = inv_extrinsic @ dir_vec_in_cam_ref
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    cam_x, cam_y, cam_z, _ = cam_center
    
    for vec in dir_vec_in_world.T:
        ipv.plot([cam_x, vec[0]], [cam_y, vec[1]], [cam_z, vec[2]], color=color)

ipv.show()
```

# Triangulate, i.e. find the shortest line between two lines in 3D
Given the 
* stereo point correspondences 
* intrinsic matrix of both cameras
* extrinsic matrix of both cameras    

pixel points can be back-projected into rays. The intersections of these corresponding rays are the triangulated 3D point.   
The mathematics of finding this shortest line between two lines in 3D is detailed (with code examples) at http://paulbourke.net/geometry/pointlineplane/. 
We will take the midpoint of the shortest line joining two rays as the triangulated point, denoted here as $\mathbf{m}$.



Here $P_{a}$ and $P_{b}$ are the desired closest points on lines $a$ and $b$ respectively.
$$ P_{a} = P_{1} + \mu_{a} (P2 - P1)$$
$$ P_{b} = P_{3} + \mu_{b} (P4 - P3)$$

In our case, $P_{1}$ and $P_{3}$ are the first and second camera centers.
$P_{2}$ and $P_{4}$ are the back-projected correspondence pixels in the first and second camera.
  
Solving for $\mu_{a}$ and $\mu_{b}$ provides the desired points on the lines. $\mu_{a}$ and $\mu_{b}$ have the following closed-form solution.
Let:  
$$P_{12} \stackrel{\text{def}}{=} P_{1} - P_{2} $$
$$d_{1234} \stackrel{\text{def}}{=} P_{12} \cdot P_{34} $$

Then:  
$$ \mu_{a} = \frac{d_{1343} d_{4321} - d_{1321} d_{4343}}{d_{2121} d_{4343} - d_{4321} d_{4321}}$$   

$$ \mu_{b} = \frac{ d_{1343} + \mu_{a} d_{4321}}  {d_{4343}}   $$

See [this link](http://paulbourke.net/geometry/pointlineplane/) for the derivation. Note that the derivation is not complete, as it does not show the expansion of terms. This can be easily shown, however, by finding the reduced row echelon form of the equations that are derived.


```python
def triangulate(p1, p2, p3, p4):
    """ Calculates the point triangulated by two lines. 
        Also returns that points projection onto line 1 and 2, 
        know as Pa and Pb in the above description. 
    """
    # Strip potential scale factor of homogenous coord
    p1 = p1[:3]
    p2 = p2[:3]
    p3 = p3[:3]
    p4 = p4[:3]
    
    p13 = p1 - p3
    p21 = p2 - p1
    p43 = p4 - p3
    
    d1321 = np.dot(p13, p21)
    d1343 = np.dot(p13, p43)
    d2121 = np.dot(p21, p21)
    d4321 = np.dot(p43, p21) 
    d4343 = np.dot(p43, p43) 
    
    mu_a = (d1343 * d4321 - d1321 * d4343) / (d2121 * d4343 - d4321 * d4321)
    mu_b = (d1343 + mu_a * d4321) / d4343
    
    point_on_line_1 = p1 + mu_a * p21
    point_on_line_2 = p3 + mu_b * p43
    
    adjoining_line = point_on_line_2 - point_on_line_1
    midpoint = adjoining_line / 2 
    
    triangulated_point = point_on_line_1 + midpoint
    
    return triangulated_point, point_on_line_1, point_on_line_2
```

## Plot triangulated points


```python
image_1 = images[camera_1_idx]
image_2 = images[camera_2_idx] 
extrinsic_1 = extrinsics[camera_1_idx]
extrinsic_2 = extrinsics[camera_2_idx]
inv_extrinsic_1 = np.linalg.inv(extrinsic_1)
inv_extrinsic_2 = np.linalg.inv(extrinsic_2)
cam_center_1 = camera_centers[camera_1_idx]
cam_center_2 = camera_centers[camera_2_idx]

for coord_1, coord_2 in zip(match_coords_1, match_coords_2): 
    dir_vec_in_cam_ref_1 = inv_intrinsics @ coord_1
    dir_vec_in_cam_ref_2 = inv_intrinsics @ coord_2
    vec_1 = inv_extrinsic_1 @ dir_vec_in_cam_ref_1
    vec_2 = inv_extrinsic_2 @ dir_vec_in_cam_ref_2
    triangulated, _, _ = triangulate(cam_center_1, vec_1, cam_center_2, vec_2)
    x, y, z = triangulated
    print(f'x: {x:.1f},  \ty: {y:.1f},  \tz: {z:.1f}') 
    ipv.scatter(np.array([x]), np.array([y]), np.array([z]), marker="sphere")
    
ipv.show()
```

#### 

## Conclusion
The triangulation was a success!    
The rays align as expected, with the height of points on the table being consistently slightly under the chessboard.   
This presents many possibilities. By knowing the real-world length of the chessboard, given enough point correspondences, we can:
* infer the size and shape of the table, candles, and other objects in the scene.
* insert objects into the scene and make them appear geometrically consistent in the cameras (as done with the bunny).
* project patterns onto objects in a geometrically consistent manner (like dynamic selfie filters).

While this method of triangulation is convenient for our current setup, it assumes cameras poses and point correspondences are given and correct. 
A more convenient method for determining 3D world points would allow:
* iteratively refining camera poses and 3D world points simultaneously.
* utilizing the point correspondences in more than 2 images for preciser and more robust triangulation.        

Bundle adjustment provides these benefits and will be demonstrated in notebook 4.

This concludes the first notebook.
Having a visualization throughout each step in this notebook has made for a pleasant journey. Rather than taking the matrix manipulations and geometry theory at face value, we get to see and interact with it. Also, this can save quite some time when debugging. This is feasible thanks to [Ipyvolume](https://github.com/maartenbreddels/ipyvolume), which provides efficient OpenGL interfacing (unfortunately, Matplotlib doesn't support z-buffering in 3D mode).    

Next up: a notebook on unit quaternions as 3D rotation representations. 

## Sources

1. [Szeliski, Richard. "Computer vision: algorithms and applications." Springer Science & Business Media, 2010](https://szeliski.org/Book/)
2. [Zisserman, Richard Hartley Andrew. "Multiple view geometry in computer vision." 2004.](https://www.robots.ox.ac.uk/~vgg/hzbook/)
3. [Zhang, Zhengyou. "A flexible new technique for camera calibration." IEEE, 2000](https://ieeexplore.ieee.org/abstract/document/888718)
4. [OpenCV Documentaiton](https://docs.opencv.org/4.5.4/) 
5. [Paul Bourke Geometry Website](http://paulbourke.net/geometry/pointlineplane/)
6. [Ipyvolume by Maarten Breddels](https://github.com/maartenbreddels/ipyvolume) 
7. [Wikipedia](https://www.wikipedia.org/). Article links included in text.

## Appendix

### Zhang's camera calibration algorithm
Zhang's algorithm models camera intrinsics and extrinsics by: 
* Imposing constraints on the camera matrices by associating known world points on a calibration object with imaged pixel coordinates. 
* Imposing the constraint of a shared intrinsic matrix across all cameras.
* Using an analytical solution to gain an initial estimate of each camera's pose and shared intrinsics. Distortions are not yet considered.
* Refining the initial estimate with an iterative non-linear optimization of the reprojection error of the calibration points. This step also does not consider distortions.    
    The Levenberg-Marquardt optimization algorithm is used. This algorithm iteratively 
    1. Computes the derivatives (Jacobian $J$) of the squared reprojection error w.r.t the camera parameters up to the second-order. Zhang used [Minpack](https://devernay.github.io/cminpack/), so numerical differentiation was used. For numerical differentiation, the second-order derivatives (Hessian $H$) are estimated using the [Jacobian product](https://math.stackexchange.com/questions/2349026/why-is-the-approximation-of-hessian-jtj-reasonable) $H = J^{T}J$.
    2. Set the derivative of the squared error function to be zero. This is the analytical least-squares solution. That is, the reprojection error function is being modeled as a locally quadratic, ideally convex, function. This loss function will have the lowest loss where the derivative is zero, that is, where the loss lies in a valley, or a saddlepoint in the non-ideal case. This solution tends to miss or overshoot the optimal parameters when the initial guess of camera parameters is poor. This is due to the non-quadratic and even non-euclidean nature of the reprojection error w.r.t camera parameters. To solve this see step 3.
    3. Determine a dampening factor $\lambda$. This factor will determine the tradeoff between the analytical least-squares solution and the more stable but less optimal gradient-descent update for camera parameters. When $\lambda=0$ only the analytical least-squares solution is used. This is known as [Guass-Newton optimization](https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm). For a greater $\lambda$, the update is closer to a gradient-descent step. This is known as [Levenberg-Marquardt optimization](https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm).
    4. Update the camera parameters. Observe the new points projections and reprojection errors. Iterate until convergence.
* Finally, estimate radial distortion by another round of Levenberg-Marquardt minimization of the reprojection error. That is, introduce distortion parameters into the image formation model and reoptimize.

44b52360744aa77c3d423056843b9fe2b7d38bd9
ipynb
Jupyter Notebook
1_camera_basics.ipynb
maxcrous/multiview_notebooks
bea2f87b8c78c5819337a496a0d330c255b492d1
MIT
2021-12-05T16:12:01.000Z
2022-03-28T12:18:23.000Z
1_camera_basics.ipynb
maxcrous/multiview_notebooks
bea2f87b8c78c5819337a496a0d330c255b492d1
MIT
1_camera_basics.ipynb
maxcrous/multiview_notebooks
bea2f87b8c78c5819337a496a0d330c255b492d1
MIT
2021-12-05T18:48:06.000Z
2022-03-26T02:19:43.000Z
Qwen/Qwen-72B
1. YES
2. YES