【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
原文地址:https://blog.csdn.net/m0_74758366/article/details/140669808
免责声明:本站文章内容转载自网络资源,如本站内容侵犯了原著者的合法权益,可联系本站删除。更多内容请关注自学内容网(zxcms.com)!