<jupyter_start><jupyter_text>Machine VisionAssignment 8 - Triangulation Personal details* **Name(s):** ``* **Student ID(s):** `` 1. IntroductionIn this assignment we will use a pair of stereo images to triangulate points in 3D. Let us first display the test images and 2D point correspondences. We also load 3D points mainly for testing purposes. The data is from __http://www.robots.ox.ac.uk/~vgg/data/data-mview.html__.<jupyter_code>import cv2
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
%matplotlib inline

# Load image pair and convert to RGB
left = cv2.imread('left.jpg')
right = cv2.imread('right.jpg')
left = cv2.cvtColor(left,cv2.COLOR_BGR2RGB)
right = cv2.cvtColor(right,cv2.COLOR_BGR2RGB)

# Load 2D points (2xN matrices)
pts1 = np.load('pts1.npy')
pts2 = np.load('pts2.npy')

# Load 3D points (3xN matrix)
pts3D = np.load('pts3D.npy')

# Display 2D point correspondences
plt.figure(figsize=(14,8))
plt.subplot(121)
plt.imshow(left)
plt.plot(pts1[0,:],pts1[1,:], '.r')
plt.title('Left image')
plt.axis('off')
plt.subplot(122)
plt.imshow(right)
plt.plot(pts2[0,:],pts2[1,:], '.r')
plt.title('Right image')
plt.axis('off')<jupyter_output><empty_output><jupyter_text>The camera projection matrices $\mathbf{C}$ and $\mathbf{C}'$ are also provided for both views.<jupyter_code># Load 3x4 camera projection matrices
C1 = np.load('C1.npy')
C2 = np.load('C2.npy')<jupyter_output><empty_output><jupyter_text>2. From 3D points to 2D pointsWith the camera matrix $\mathbf{C} = \mathbf{K}[\mathbf{R} | \mathbf{t}]$ we can project 3D points to the 2D points on the image plane:$$s \begin{pmatrix}u \\ v \\ 1 \end{pmatrix} = \mathbf{C} \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix} \qquad \qquad (1)$$Note that we are using homogeneous coordinates.**2.1. Project 3D points** $\quad$ (0.50 points)Complete the function `projectPts()` by following the instructions below. The function should project 3D points `pts3D` to 2D image points. The result should look like the previous figure.<jupyter_code># INPUT   
# pts3D : 3D points (X,Y,Z) (3xN matrix)
# C     : Camera projection matrix (3x4 matrix)
#
# OUTPUT  
# pts2D : 2D points (x,y) (2xN matrix)
#
def projectPts(pts3D,C):
    
    N = pts3D.shape[1] # Number of points
    
    # ---------- YOUR CODE STARTS HERE -----------
    
    # This line can be removed
    pts2D = np.zeros((2,N), dtype=np.float_)
        
    # 1. Convert 3D points from Euclidean to 
    #    homogeneous coordinates (4xN matrix)
    newarray = np.append(pts3D,np.ones((1,N), dtype=np.float_))
    hom3D = newarray.reshape(pts3D.shape[0]+1,N)
    
    # 2. Project points using Equation 1
    projectpoints = np.matmul(C,hom3D)
    for i in range(N):
        projectpoints[0,i] = np.divide(projectpoints[0,i],projectpoints[2,i])
        projectpoints[1,i] = np.divide(projectpoints[1,i],projectpoints[2,i])
        projectpoints[2,i] = 1
        
    # 3. Covert 2D points from homogeneous to 
    #    Euclidean coordinates (2xN matrix)
    hom2D = np.append(projectpoints[0,:],projectpoints[1,:])
    pts2D = hom2D.reshape(2,N)
    
    # ----------- YOUR CODE ENDS HERE ------------

    return pts2D


# Project 3D points and visualize the result
points1 = projectPts(pts3D,C1)
points2 = projectPts(pts3D,C2)

plt.figure(figsize=(14,8))
plt.subplot(121)
plt.imshow(left)
plt.plot(points1[0,:],points1[1,:], '.r')
plt.title('Left image')
plt.axis('off')
plt.subplot(122)
plt.imshow(right)
plt.plot(points2[0,:],points2[1,:], '.r')
plt.title('Right image')
plt.axis('off')<jupyter_output><empty_output><jupyter_text>3. From 2D points to 3D pointsThe lecture notes describe a linear method to triangulate a point observed in two cameras (slide 196).Given a point $\mathbf{X} = (X,Y,Z)^{\top}$ the projection equations are:$$\begin{pmatrix}s u \\ s v \\s \end{pmatrix} = \begin{pmatrix}c_{11} & c_{12} & c_{13} & c_{14} \\ c_{21} & c_{22} & c_{23} & c_{24} \\c_{31} & c_{32} & c_{33} & c_{34} \end{pmatrix}\begin{pmatrix}X \\ Y \\Z \\1 \end{pmatrix}$$$$\begin{pmatrix}t u' \\ t v' \\t \end{pmatrix} = \begin{pmatrix}c_{11}' & c_{12}' & c_{13}' & c_{14}' \\ c_{21}' & c_{22}' & c_{23}' & c_{24}' \\c_{31}' & c_{32}' & c_{33}' & c_{34}' \end{pmatrix}\begin{pmatrix}X \\ Y \\Z \\1 \end{pmatrix}$$Here we have used the same notation as in the exercises. Eliminating $s$ and $t$ we obtain the system of equations:$$(c_{31} u - c_{11}) X + (c_{32} u - c_{12}) Y + (c_{33} u - c_{13}) Z = c_{14} - c_{34} u \\(c_{31} v - c_{21}) X + (c_{32} v - c_{22}) Y + (c_{33} v - c_{23}) Z = c_{24} - c_{34} v \\(c_{31}' u' - c_{11}') X + (c_{32}' u' - c_{12}') Y + (c_{33}' u' - c_{13}') Z = c_{14}' - c_{34}' u' \\(c_{31}' v' - c_{21}') X + (c_{32}' v' - c_{22}') Y + (c_{33}' v' - c_{23}') Z = c_{24}' - c_{34}' v'$$which can be expressed in a linear system of the form: $\mathbf{Ax} = \mathbf{b}$ and solved using the least squares method. *See the Exercise 8 (Q2)*. Notice also that equations are different from the ones presented in the lecture slides. **3.1. Triangulate** $\quad$ (1.50 points)Complete the function `triangulatePts()`. Estimate 3D points given 2D points `pts1` and `pts2` and projection matrices `C1` and `C2`. For each point, form a linear system $\mathbf{Ax} = \mathbf{b}$, where $\mathbf{A}$ is a $4 \times 3$ matrix and $\mathbf{b}$ is a $4 \times 1$ vector. The least-squares solution can be obtained by $\hat{\mathbf{x}} = (\mathbf{A}^{\top} \mathbf{A})^{-1} \mathbf{A}^{\top} \mathbf{b}$, where $\hat{\mathbf{x}}$ contains the coordinates of the 3D point $(X,Y,Z)$. Once you have completed the function, execute the following code cell. The implementation is correct if the estimated 3D points `points3D` overlap with given 3D points `pts3D`.<jupyter_code># INPUT   
# pts1 : 2D points from the first image (2xN matrix)
# pts2 : 2D points from the second image (2xN matrix)
# C1   : Camera matrix for the first image (3x4 matrix)
# C2   : Camera matrix for the second image (3x4 matrix)
#
# OUTPUT  
# pts3D : Triangulated 3D points (X,Y,Z) (3xN matrix)
#
from numpy.linalg import inv
def triangulatePts(pts1,pts2,C1,C2):
    
    N = pts1.shape[1] # Number of points
    pts3D = np.zeros((3,N),dtype=np.float_)
    
    # ---------- YOUR CODE STARTS HERE -----------
    
    # 1. For each point i, form A and b
    
    A = np.zeros((4,3),dtype=np.float_)
    
    b = np.zeros((4,1),dtype=np.float_)
    
    
    for i in range(N):
        
        A[0,0]=C1[2][0]*pts1[0,i]-C1[0][0]
        A[0,1]=C1[2][1]*pts1[0,i]-C1[0][1]
        A[0,2]=C1[2][2]*pts1[0,i]-C1[0][2]
        
        
        A[1,0]=C1[2][0] * pts1[1,i]-C1[1][0]
        A[1,1]=C1[2][1] * pts1[1,i]-C1[1][1]
        A[1,2]=C1[2][2] * pts1[1,i]-C1[1][2]
        
        A[2,0]=C2[2][0]*pts2[0,i]-C2[0][0]
        A[2,1]=C2[2][1]*pts2[0,i]-C2[0][1]
        A[2,2]=C2[2][2]*pts2[0,i]-C2[0][2]
        
        A[3,0]=C2[2][0] * pts2[1,i]-C2[1][0]
        A[3,1]=C2[2][1] * pts2[1,i]-C2[1][1]
        A[3,2]=C2[2][2] * pts2[1,i]-C2[1][2]
        
        b[0,0]=C1[0][3]-C1[2][3]*pts1[0,i]
        b[1,0]=C1[1][3]-C1[2][3]*pts1[1,i]
        b[2,0]=C2[0][3]-C2[2][3]*pts2[0,i]
        b[3,0]=C2[1][3]-C2[2][3]*pts2[1,i]
        
        # 2. Find solution to linear system Ax=b

        points = np.zeros((3,1),dtype=np.float_)  
        points = np.matmul( np.matmul(inv(np.matmul(np.transpose(A) , A )), np.transpose(A)), b)
        
        # 3. Save triangulated point to pts3D[i,:]
        pts3D[0,i]=points[0]
        pts3D[1,i]=points[1]
        pts3D[2,i]=points[2]        
    

    # ----------- YOUR CODE ENDS HERE ------------

    return pts3D


# Triangulate points and compare to given 3D points
points3D = triangulatePts(pts1,pts2,C1,C2)

fig = plt.figure(figsize=(12,12))
ax = fig.add_subplot(111, projection='3d')
ax.scatter(-pts3D[1,:],pts3D[2,:],-pts3D[0,:], 
           color='blue', label='Given 3D points')
ax.scatter(-points3D[1,:],points3D[2,:],-points3D[0,:], 
           color='red', label='Estimated 3D points')
ax.set_xlabel('Y')
ax.set_ylabel('Z')
ax.set_zlabel('X')
ax.set_title('Comparison of 3D points')
ax.legend()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.19390513639714918, 'fraction_numerical': 0.0625460801179651, 'mean_word_length': 3.4402618657937807, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 17, 'https://': 0, 'lorem ipsum': 0, 'www.': 1, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '11883', 'license': 'BSD-4-Clause-UC', 'n_tokens_mistral': 3451, 'n_tokens_neox': 3236, 'n_words': 787, 'path': 'A8-Triangulation/MV_A8.ipynb', 'repo_name': 'Haoban/Machine-Vision', 'size': '8138'}