Camera

3D to 2D Projection

Given a point in 3D. How does the corresponging projection onto the image plane look like.

Theorem on intersecting lines:

$$\frac{\tilde{x}}{f} = \frac{X}{Z} \qquad \frac{\tilde{y}}{f} = \frac{Y}{Z}$$

Physicall location of the pixel. $X$ and $Z$ are in $[m]$. $f$ is in $[\mu m]$.

$$\tilde{x} = f \frac{X}{Z} \qquad \tilde{y} = f \frac{Y}{Z}$$

To get the value in pixels.

$$x = \frac{\tilde{x}}{d_x} + x_0 \qquad y = \frac{\tilde{y}}{d_y} + y_0$$

$dx$ and $dy$ are there to scale $\tilde{x}$ and $\tilde{y}$ by physical dimensions of a pixel.

Written in matrix form we get the Camera Matrix:

$$K = \begin{bmatrix} \alpha_x & 0 & x_0 \\ 0 & \alpha_y & y_0 \\ 0 & 0 & 1 \\ \end{bmatrix}$$

Where:

$$\alpha_x = \frac{f}{d_x} \qquad \alpha_y = \frac{f}{d_y}$$

$$\begin{bmatrix} \frac{f}{d_x} & 0 & x_0 \\ 0 & \frac{f}{d_y} & y_0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ \end{bmatrix} = \begin{bmatrix} \frac{f}{d_x} X_c + x_0 Z_c \\ \frac{f}{d_y} Y_c + y_0 Z_c \\ Z_c \end{bmatrix} \begin{bmatrix} \frac{f}{d_x} \frac{X_c}{Z_c} + x_0 \\ \frac{f}{d_y} \frac{Y_c}{Z_c} + y_0 \\ 1 \end{bmatrix}$$

Python

def project_3d_to_2d(K, T, points):
    points_3d_homo = np.vstack([points, np.ones((1, points.shape[1]))])
    K_3x4 = np.hstack([K, np.zeros((3, 1))])
    C = K_3x4 @ T_inv
    points_2d_homo = C @ points_3d_homo
    points_2d = points_2d_homo[:2, :] / points_2d_homo[2, :]
    return points_2d

C++

Eigen::MatrixXf Project3dTo2d(const Eigen::Matrix3f& K, const Eigen::Matrix4f& T_inv, const Eigen::MatrixXf& points) {
    Eigen::MatrixXf points_3d_homo(4, points.cols());
    points_3d_homo << points, Eigen::RowVectorXf::Ones(points.cols());
    Eigen::Matrix<float, 3, 4> K_3x4;
    K_3x4.leftCols<3>() = K;
    K_3x4.rightCols<1>().setZero();
    Eigen::MatrixXf C = K_3x4 * T_inv;
    Eigen::MatrixXf points_2d_homo = C * points_3d_homo;
    Eigen::MatrixXf points_2d(2, points_2d_homo.cols());
    for (int i = 0; i < points_2d_homo.cols(); ++i) {
        points_2d.col(i) = points_2d_homo.col(i).head<2>() / points_2d_homo(2, i);
    }
    return points_2d;
}

Project 2D to 3D

Python

def project_2d_to_3d(K, T, points, depth=1.0):
    points_2d_homo = np.vstack([points, np.ones((1, points.shape[1]))])
    rays_3d_camera = np.linalg.inv(K) @ points_2d_homo
    rays_3d_camera /= np.linalg.norm(rays_3d_camera, axis=0)
    rays_3d_world = rays_3d_camera[:3] * depth
    rays_3d_world_homo = np.vstack([rays_3d_world, np.ones((1, points.shape[1]))])
    return (T @ rays_3d_world_homo)[:3]

C++

Eigen::MatrixXf Project2dTo3d(const Eigen::Matrix3f& K, const Eigen::Matrix4f& T, const Eigen::MatrixXf& points) {
    Eigen::MatrixXf points_2d_homo(3, points.cols());
    points_2d_homo << points, Eigen::RowVectorXf::Ones(points.cols());
    Eigen::MatrixXf rays_3d_camera = K.inverse() * points_2d_homo;
    for (int i = 0; i < rays_3d_camera.cols(); ++i) {
        rays_3d_camera.col(i).normalize();
    }
    Eigen::MatrixXf rays_3d_camera_homo(4, points.cols());
    rays_3d_camera_homo << rays_3d_camera, Eigen::RowVectorXf::Ones(points.cols());
    return T * rays_3d_camera_homo.topRows<3>();
}

Draw camera

# Define camera parameters
K = np.array([[500, 0, 320], [0, 500, 240], [0, 0, 1]])  # Example intrinsic matrix
T = np.eye(4)  # Example extrinsic matrix (identity matrix for simplicity)

# Define the corners of the image plane in normalized device coordinates
ndc_corners = np.array([[-1, -1], [1, -1], [1, 1], [-1, 1], [-1, -1]]).T

# Define near and far plane distances
near_plane, far_plane = 1, 3

# Project corners into 3D for both near and far planes
corners_near = project_2d_to_3d(K, T, ndc_corners, near_plane)
corners_far = project_2d_to_3d(K, T, ndc_corners, far_plane)

# Plotting
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

# Draw near and far plane rectangles
ax.plot(corners_near[0, :], corners_near[1, :], corners_near[2, :], color='r')
ax.plot(corners_far[0, :], corners_far[1, :], corners_far[2, :], color='b')

# Draw lines connecting the rectangles
for i in range(4):
    ax.plot([corners_near[0, i], corners_far[0, i]], 
            [corners_near[1, i], corners_far[1, i]], 
            [corners_near[2, i], corners_far[2, i]], color='g')