【github项目】Algorithms-for-Automated-Driving自动驾驶算法（1）

 本文解决项目中“Basics of Image Formation”部分的问题，也就是实现从世界坐标向像素坐标的转换。

文件位置：Algorithms-for-Automated-Driving-master \code\exercises\lane_detection\camera_geometry.py

代码中写了详细注释。

import numpy as np

def get_intrinsic_matrix(field_of_view_deg, image_width, image_height):
    """
    Returns intrinsic matrix K.
    """
    # For our Carla camera alpha_u = alpha_v = alpha
    # alpha 可以由所给相机的视场计算
    field_of_view_rad = field_of_view_deg * np.pi/180
    alpha = (image_width / 2.0) / np.tan(field_of_view_rad / 2.)
    alpha_u = alpha_v = alpha
    # 主点通常位于图像中央
    u_0 = image_width / 2
    v_0 = image_height / 2
    # TODO step 1: Complete this function
    # 注意每行数组写完之后要加上逗号
    K = np.array([
            [alpha_u, 0, u_0],
            [0, alpha_v, v_0],
            [0,       0,  1 ]
        ])
    return K
    raise NotImplementedError

# 投影折线
def project_polyline(polyline_world, trafo_world_to_cam, K):
    """
    Returns array uv which contains the pixel coordinates of the polyline.
    返回包含了折线像素坐标的数组uv

    Parameters
    ----------
    polyline_world : array_like, shape (M,3)
        Each row of this array is a vertex (x,y,z) of the polyline.
        这个数组的每一行都是折线的一个顶点(x,y,z)
        整个数组的形状是M行3列
    trafo_world_to_cam : array_like, shape (4,4)
        Transformation matrix, that maps vectors (x_world, y_world, z_world, 1) 
        to vectors (x_cam, y_cam, z_cam, 1).
        变换矩阵（实际上就是外参矩阵），实现从世界坐标向量到相机坐标向量的映射
    K: array_like, shape (3,3)
        Intrinsic matrix of  the camera.   
        相机内参矩阵

    Returns:
    --------
    uv : ndarray, shape (M,2)
        Pixel coordinates of the projected polyline
        First column is u, second column is v
        投影折线的像素坐标，第一行为u，第二行为v
    """
    
    # 总公式：polyline_pixel = K·E·polyline_world

    # TODO step 1: Write this function
    # 首先可以把世界坐标系转换成相机坐标系，也就是乘上外参矩阵
    # 由于外参矩阵是(4,4)，故需要齐次化，即对世界坐标升维，变成(M,4)
    # np.hstack()用于水平堆叠两个数组，比如[1,2,3] 与 [2] 堆叠：[1,2,3,2]
    #                                    [3,4,5]   [3]       [3,4,5,3]
    # np.ones(x,y)用于建立一个x行y列的全1矩阵
    # 注意：np.hstack()函数括号里只能传递一个参数进去，因此需要加上括号
    polyline_world_qicihua = np.hstack((polyline_world, np.ones((polyline_world.shape[0], 1))))
    # 矩阵相乘，得到相机坐标系下的坐标，形状为[M,4]
    # 对于式子里的多个转置符号进行解释：
    # 首先，由公式 P' = EP 可知，外参矩阵放在矩阵乘法的左侧，
    # 为了可以进行矩阵运算，需要转置形状为[M,4]的polyline_world_qicihua向量
    # [4, 4]·[4, M] = [4, M]，再一次转置将其变为[M, 4]（也可以不变，只需对后面的式子相应做出改变即可）
    polyline_camera = (np.dot(trafo_world_to_cam, polyline_world_qicihua.T)).T
    # 齐次化，降维polyline_camera然后乘内参矩阵,得到图像坐标系下的坐标，形状为[M,3]
    polyline_camera_qicihua = polyline_camera[: , :3]
    # 公式 P' = KP
    polyline_img = (np.dot(K, polyline_camera_qicihua.T)).T
    # 得到像素坐标系下的坐标，形状为[M,2]
    # 下面这一步将前两列 (x, y) 的坐标值除以相应点的第三列 w 值。
    # 这个操作可以理解为对每个点的坐标 (x, y) 进行透视除法，
    # 以获得其在投影平面上的真实坐标。
    uv = polyline_img[:, :2] / polyline_img[:, 2, np.newaxis]
    return uv

    
    '''
    下面这段代码运行无误，可以作为参考之一
    # Convert polyline_world to homogeneous coordinates
    polyline_world_hom = np.hstack((polyline_world, np.ones((polyline_world.shape[0], 1))))
    # Transform world coordinates to camera coordinates
    polyline_cam_hom = (trafo_world_to_cam @ polyline_world_hom.T).T
    # Only keep the x, y, z coordinates
    polyline_cam = polyline_cam_hom[:, :3]
    # Project 3D camera coordinates to 2D image coordinates using the intrinsic matrix
    polyline_img_hom = (K @ polyline_cam.T).T
    # Normalize the homogeneous coordinates to get pixel coordinates
    uv = polyline_img_hom[:, :2] / polyline_img_hom[:, 2, np.newaxis]
    return uv
    '''
    raise NotImplementedError