（五）carla中世界坐标系与相机坐标系、像素坐标系变换

由于实验需要将 $carla$ 中世界坐标系下的坐标点画到相机采集的前视图上，故需要进行坐标系之间的转换。

转换中遇到的问题

参考这篇博客中给出的方法: link

发现将世界坐标系转到像素坐标系下显示的不对。因此仔细从头到尾仔细分析一下，原来是 $carla$ 中 $get\_transform().get\_matrix()$ 函数的问题。具体解释如下。

转换过程

1.世界坐标系到相机坐标系

首先，车辆在 $carla$ 中运行时可以获取实时的世界坐标，例如下图所示 $location$ 中的 $x,y$ 坐标。
获取方式为:

#创建了一辆车my_vehicle
t = my_vehicle.get_transform()
global_location_x = t.location.x
global_location_y = t.location.y
global_location_z = t.location.z
global_rotation_yaw = t.rotation.yaw
global_rotation_pitch = t.rotation.pitch
global_rotation_roll = t.rotation.roll

获取到世界坐标系后，首先需要转到相机坐标系下。
这篇博客 link 中说相机的外参矩阵直接可以通过下方函数来获得。

camera.get_transform().get_matrix()

但是，经过测试发现不是。
$carla$ 官方文档：

文档中说 $transform$ 计算以当前点为原点的坐标系 $A$ 与世界坐标系 $B$ 之间的变换。

$get\_matrix()$ 函数获取从当前点为原点的坐标系 $A$ 到世界坐标系 $B$ 之间的变换矩阵。
重点： 但是，$get\_matrix()$ 默认相机位置为原点的这个坐标系的 $x,y,z$ 轴与世界坐标系的 $x,y,z$ 轴是相同方向的，没有考虑轴旋转。
而实际上，相机坐标系是相机的正前方，光心方向总是 $z$ 轴方向。

世界坐标系是这样的:

这两个坐标系轴的关系存在一个旋转矩阵。
$camera.get\_transform().get\_matrix()$ 获得的是不考虑轴旋转的局部到全局的变换矩阵。
将 $camera.get\_transform().get\_matrix()$ 获得的矩阵取逆得到的是不考虑轴旋转的全局到局部的变换矩阵。
而世界坐标系到相机局部坐标系考虑轴旋转关系的对应矩阵为:
$$\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 1& 0& 0\end{array}\right]$$
因此将 $camera.get\_transform().get\_matrix()$ 获得的矩阵取逆再乘以世界坐标系轴与相机坐标系轴之间的旋转矩阵，就真正得到世界坐标系到相机坐标系的变换矩阵。
得到变换矩阵 $R$ 后，假设世界坐标系下点的坐标是 $P_w=(x_w,y_w,z_w{)}^T$，
$$P_c=R.P_w$$
注:如果使用上面世界坐标系到相机局部坐标系考虑轴旋转关系的对应矩阵发现效果不对时，可能是由于光轴是朝向相机正后方。

世界坐标系到相机局部坐标系考虑轴旋转关系的对应矩阵为:
$$\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& 1\\ -1& 0& 0\end{array}\right]$$

2.相机坐标系到像素坐标系

参考这篇博客链接 link

参考这篇博客 link

在 $carla$ 中没有找到内参获取函数，但是根据上述两篇博客中的解释，内参可以估计得到。
在参考文档中 $Sensorsrefernece/RGBcamera/Advancedcameraattributes$ 中，可以得到相机的焦距。

光心一般是相机采集图片的中心。根据这些信息可以估计出相机内参。
假设相机坐标系下点的坐标是 $P_c=(x_c,y_c,z_c{)}^T$，相机内参是 $K$，
$$P_p=K.P_c$$

转换效果

首先记录车辆行驶过程中的轨迹点，然后将当前位置后20帧车辆的世界坐标系下坐标转化到当前图像坐标系下，转换结果如下：

程序后续整理上传到我的 $github$ 中。