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

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

代码中写了详细注释。

import numpy as npdef 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/180alpha = (image_width / 2.0) / np.tan(field_of_view_rad / 2.)alpha_u = alpha_v = alpha# 主点通常位于图像中央u_0 = image_width / 2v_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 Kraise NotImplementedError# 投影折线
def project_polyline(polyline_world, trafo_world_to_cam, K):"""Returns array uv which contains the pixel coordinates of the polyline.返回包含了折线像素坐标的数组uvParameters----------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 polylineFirst 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' = KPpolyline_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 coordinatespolyline_world_hom = np.hstack((polyline_world, np.ones((polyline_world.shape[0], 1))))# Transform world coordinates to camera coordinatespolyline_cam_hom = (trafo_world_to_cam @ polyline_world_hom.T).T# Only keep the x, y, z coordinatespolyline_cam = polyline_cam_hom[:, :3]# Project 3D camera coordinates to 2D image coordinates using the intrinsic matrixpolyline_img_hom = (K @ polyline_cam.T).T# Normalize the homogeneous coordinates to get pixel coordinatesuv = polyline_img_hom[:, :2] / polyline_img_hom[:, 2, np.newaxis]return uv'''raise NotImplementedError