ploting.py

import numpy as np
import matplotlib.pyplot as plt

from common import Camera

def plot_points_cloud(X: np.ndarray, colors: np.ndarray=None, cameras: list = None) -> None:
    """
    Plot 3D points cloud
    :param X: 3D points (3xN)
    :param colors: Color of the points (3xN)
    :param cameras: List of cameras
    """
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.view_init(-60, -90)
    ax.set_xlim([-1.5, 1.5])
    ax.set_ylim([-3, 1])
    ax.set_zlim([-1, 5])
    if colors is None:
        # green color
        colors = np.zeros((3, X.shape[1]))
        colors[1] = 0.7
    ax.scatter(X[0], X[1], X[2], c=colors.T, marker='o', s=1)
    for camera in cameras:
        C = -camera.R.T @ camera.t
        ax.scatter(C[0], C[1], C[2], c='b', marker='o')
        # direction vector of the camera
        d = camera.get_optical_axis()
        ax.quiver(C[0], C[1], C[2], d[0], d[1], d[2], color='r', length=1, normalize=True)
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    plt.show()

def plot_rectified_images(img1_rect, img2_rect) -> None:
    plt.figure()
    plt.subplot(1, 2, 1)
    plt.imshow(img1_rect, cmap='gray')
    plt.title('Rectified image 1')
    plt.subplot(1, 2, 2)
    plt.imshow(img2_rect, cmap='gray')
    plt.title('Rectified image 2')
    plt.show()

def plot_disparity_map(disparity) -> None:
    plt.figure()
    plt.imshow(disparity, cmap='jet')
    plt.colorbar()
    plt.title('Disparity map')
    plt.show()

if __name__ == '__main__':
    # load point cloud
    import open3d as o3d
    point_cloud = o3d.io.read_point_cloud("scene/out_sparse.ply")

    # Convert to NumPy array
    X = np.asarray(point_cloud.points).T
    colors = np.asarray(point_cloud.colors).T

    # load cameras
    cameras = []
    for i in range(12):
        camera = Camera(id=i)
        camera.load(f"scene/cameras/camera{i}.txt")
        cameras.append(camera)

    # display point cloud
    plot_points_cloud(X, colors, cameras=cameras)