[carla]关于odometry坐标中的角度坐标系 以及 到地图的映射问题

1.获取车辆的Odometry原始信息

在carla中，通过订阅/carla/ego_vecle/odometry 可以查看车辆的全局位置信息,例如：

> header: 
  seq: 118872
  stamp: 
    secs: 5946
    nsecs:   5720187
  frame_id: "map"
child_frame_id: "ego_vehicle"
pose: 
  pose: 
    position: 
      x: 96.5346984863
      y: -188.643692017
      z: 0.221679911017
    orientation: 
      x: 1.09737910971e-07
      y: 5.40849999846e-07
      z: 0.0368271997411
      w: 0.999321648599
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: -0.00358968405948
      y: -3.63046108104e-06
      z: 2.80475536936e-07
    angular: 
      x: 2.17012864219e-05
      y: -0.000262381265741
      z: -0.00483244467764
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

其中，pose.pose.position.x 和 pose.pose.position.y提供了全局x,y坐标，pose.pose.orientation提供了汽车的旋转角度。

1.1 全局坐标

全局坐标和GNSS传感器获得的经纬度坐标可以相互转换，统一地图内，两种坐标都可以用于车辆定位。

1.2 旋转角度

这里的旋转角度的坐标以N方向为零度，E为90度，W为-90度，S偏E侧为+180度，S偏W侧为-180度，如图所示：

这里值得注意的是：CARLA的odometry坐标系与真实世界的坐标系不同。正常坐标系是上北下南，左西右东，而CARLA里面是上北下南，左东右西。

该角度的具体计算的方法为：

//导入tf/transform_datatypes.h库
#include <tf/transform_datatypes.h>

//使用tf::getYaw获取四元数中的偏航角，然后转换成角度
tf::getYaw(carla_odometry.pose.pose.orientation) * 180.0 / M_PI

计算出来与显示界面的Heading是相同的。

2. 坐标系变换

我们想要把Odometry坐标系变换成下面的角度坐标系:

首先，Odometry坐标系需要逆时针旋转90度，那么对应的角度应当减去90度：
$${\theta }_1={\theta }_0-90°$$
然后，取值范围需要变换到0-360度，对于原角度位于[90°,180°]的，减去90°后已经是变换后的角度，对于[0,90°]，[-0,-180°]角度，减去90°后会变成负数，需要增加360°变成正数。

$${\theta }_2=\{\begin{array}{c}{\theta }_1,{\theta }_1>=0\\ {\theta }_1+2\pi ,{\theta }_1<0\end{array}$$
变换过程如图所示：

3.关于小地图截取时的注意事项

carla的地图有时候 并不是 上北下南 左西右东的，例如Town02地图

实际上，图片的上方是东，下发是西，左边是南，右边是北。我们想要使得截取的小地图始终朝向车头方向，就要将图片往反方向旋转相同的角度。