(九) 画出KITTI本车行驶轨迹

一、 两帧间计算距离和偏航原理

        为了在RVIZ中画出本车的轨迹，需要计算当前帧（frame）在本车坐标系下，其他帧（frame-1….n）下本车坐标系经过移动后的位置。车辆行驶时的偏航和平移可以看作是车辆坐标系的旋转和平移。

        如下图所示，先不考虑旋转，平移时第（frame）帧和第（frame-1）帧始终差一个d，这个d为distance，是每两帧，即一个采样时间（0.1s）下车辆移动的距离。这个距离可以通过IMU记录的经纬度，或者车辆速度矢量与采样时间的乘积表示。

        而车辆在运动时，不仅只有平移，还有偏航，于是坐标系还有有旋转运动。如下图所示，坐标系先是旋转了一个θ角度，然后平移过一个d距离，移动方向正是车辆速度vf和vl的和向量方向，因此其与采样时间相乘即为移动距离。

        因此，记录车辆行驶轨迹，最主要是的d 和θ的获取，使用KITTI raw data自带的oxts文件，其中自带的dataformat.txt中详细说明了记录的每个数据的type ，θ使用其中的yaw即可获得当前的偏航角yaw和下一时刻的偏航角yaw（rad），当车辆逆时针转动时Δyaw为正值，当顺时针转动时Δyaw为负值。

        具体IMU数据格式可以参考https://blog.csdn.net/rhyijg/article/details/108368314

        d 的长度，可以通过两帧之间的经纬度(lat和lon) lat: latitude of the oxts-unit (deg)
lon: longitude of the oxts-unit (deg)的关系，使用大圆定理(Great Circle distance)进行求解，可以根据公式求出两点的最短距离。

大圆距离（英语：Great-circledistance）指的是从球面的一点A出发到达球面上另一点B，所经过的最短路径的长度。一般说来，球面上任意两点A和B都可以与球心确定唯一的大圆，这个大圆被称为黎曼圆，而在大圆上连接这两点的较短的一条弧的长度就是大圆距离。若这两点和球心正好都在球的直径上，则过这三点可以有无数大圆，但两点之间的弧长都相等，且等于该大圆周长的一半,r是球的半径。由于地球类似球体，地球上任何两点沿球面的最短距离都可以通过大圆距离的公式估算的出，这在航空和航海上都有很大作用。

        使用两个经纬度可以计算距离，python代码即：

# 使用经纬度计算两个经纬度之间的距离
def compute_great_circle_distance(lat1,lon1,lat2,lon2):
    '''
        Compute the great circle distance from two gps data
        Input: latitudes and longitudes in degree
        Output: distance in meter
    '''
    delta_sigma = float(np.sin(lat1*np.pi/180)*np.sin(lat2*np.pi/180)