100Hz IMU Data from KITTI dataset (2011_09_26_drive_0028)

原始100Hz IMU数据获取

数据集获取地址 (这里选的是road 分类)：
http://www.cvlibs.net/datasets/kitti/raw_data.php?type=road

100Hz的IMU数据在 [unsynced+unrectified data] 中，而 [synced+rectified data] 保存的是修正后的10Hz数据。

100Hz IMU 数据展现与问题

这里下载使用的是 2011_09_26_drive_0028，包含43s 的公路行驶，公路为双向单车道。

写了一个matlab脚本：getNaviData_ext， 来获取IMU数据，并保持到TraceData.mat文件中以备分析和后用。
首先，检查时间戳是否有问题。
TraceData第一列为整理后的时间戳 t_acc=TraceData(:,1)，以第一个时刻为0时刻，剩余时刻为与第一个时刻的差。正常情况下，k时刻 t_acc(k) - t_acc(k-1) 应该为0.01s. 作图

1. t_dt = diff(TraceData(:,1));

但是发现，会有负值出现。

为分析为何出现负值，先找到对应的点，看它们的数据

1. kminus=find(t_dt<0)

kminus =1850191023072364237524252431251325192762288529443018302432353290341134453494357835853771402340294057409242154374

共有28处存在负值，大小为-0.05左右。

以第一处 k= 1850为例，发现k=1851~1856 与 k=1845~1850 数据出现重复，

再看k=1910附近：

看2307

以及剩下的

经过手工标记，除去重复数据点，得到数据间隔如下

free INS： 10Hz数据和100Hz数据解算结果对比

（1）10Hz pure INS result

（2）100Hz pure INS result

100Hz IMU 数据与校正后image数据 时间戳的同步

时间戳文件：
100Hz IMU: 2011_09_26_drive_0028_extract\oxts\timestamps.txt
校正后camera0: 2011_09_26_drive_0028_sync\image_00\timestamps.txt

以100Hz IMU的第一个时间戳为基准 （0时刻），得到剩余IMU数据的时间戳，以及camera0 图像的时间戳（保存）。

校正后图像的时间戳比较均匀（0.1s）, 而经过重复数据剔除后的100Hz IMU时间戳时间不均匀，也就是说每10个点时间戳相差并不是0.1s，有时候回出现更多的情况（由于有些时刻没有IMU数据），见下图。

由于数据图像是每0.1s采集，这就需要对IMU数据与图像数据进行时间配准。
10Hz Raw Data 同步方法：

In order to synchronize the sensors, we use the timestamps of the Velodyne 3D laser scanner as a reference and con- sider each spin as a frame. We mounted a reed contact at the bottom of the continuously rotating scanner, trigger- ing the cameras when facing forward. This minimizes the differences in the range and image observations caused by dynamic objects. Unfortunately, the GPS/IMU system can- not be synchronized thatway. Instead, as it provides updates at 100 Hz,we collect the information with the closest times- tamp to the laser scanner timestamp for a particular frame, resulting in a worst-case time difference of 5 ms between a GPS/IMU and a camera/Velodyne data package. Ref: Vision meets robotics: The KITTI dataset

这里有三个方法可供选择：

• 找到第一个大于当前图像时间戳的IMU时间戳，该时刻置 有image观测的标志 （正常情况0.01s 最大误差）。最简单，实时软件中也会出现的情形。
  
• 找到距离当前图像最近的IMU时间戳，该时刻置 有image观测的标志 （正常情况0.005s 最大误差）。适合离线处理。
• 找到第一个大于当前图像时间戳的IMU时间戳，结合前一时刻（及前前时刻）的INS导航解，插值出image时刻的导航解，再做融合。复杂些，也可用于实时软件。

另一个需要调整的地方在程序中的时间控制部分，因为程序中一些模块的触发是以计数的形式，且某些时间区间也是区间计数值与固定周期的乘积。在周期一定的规则数据下运行没有问题，但是现在100Hz IMU数据并不是规则周期的。

100Hz IMU Data from KITTI dataset (2011_09_26_drive_0101)

同样的方法，对2011_09_26_drive_0101_extract的100Hz IMU的数据进行规整，去除重复数据。
生成TraceData后。

显示上下俩时刻时间间隔

1. t_dt = diff(TraceData(:,1));

找到负值对应的时间点，在.mat数据中检查对应点附近数据。

1. kminus=find(t_dt<0)

    kminus =1004251125175538653871597337886593719377

开始手工标记重复数据：
（1）index=1004 附近：重复数据1001-1004

(2) index = 2511, 2517 附近: 重复数据 2508-2517

(3) index = 5538 附近: 重复数据 5538

(4) index = 6538 附近: 重复数据 6537-6538

(5) index = 7159 附近: 重复数据 7156-7159

(5) index = 7337 附近: 重复数据 7332-7337

(5) index = 8865 附近: 重复数据 8866-8871

(5) index = 9371, 9377 附近: 重复数据 9371-9377

除去重复数据点，

1. index_del =[1001:1004,2508:2517,5538,6537:6538,7156:7159,7332:7337,8866:8871,9371:9377];
2. TraceData(index_del,:)=[];

data number changed: 9725 -> 9685

得到数据间隔如下:

保存TraceData 数据， 以备后用。