kitti点云地图拼接

前言：这段时间在学习坐标系变换相关的知识，同时尝试了利用kitti公开点云数据集以及对应的真实位姿，拼接出全局地图，如下图所示，我采用了kitti点云数据集的00序列来测试拼接地图。下面大致记录下点云拼接过程以及基于C++和Matlab的拼接代码，备忘。

1. 关于kitti数据集以及坐标系

kitti数据集是一个为立体，光流，视觉里程计，3D目标检测和3D跟踪等任务而开发采集的基准数据集。它利用了Annieway自动驾驶平台，配备了1个OXTS RT 3003，1台Velodyne HDL-64E激光扫描仪，2台Point Grey Flea 2（FL2-14S3M-C）一百四十万像素灰度相机，2台一百四十万像素彩色摄像头，以及4个4~8mm的可变焦镜头Edmund Optics NT59-917。根据 Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite 这篇文章的描述，相机，激光扫描仪以及定位系统全部都经过了校准和时间同步，且里程计的真值由GPS/IMU定位单元的输出投影到校准后的左侧相机得到。也就是说，在kitti提供的里程计真值是基于相机坐标系的，而根据上面kitti链接中的介绍，相机坐标系为：向前为z，向右为x，向下为y；而velodyne激光扫描仪坐标系则是：向前x，向左y，向上z。（需要注意的是，点云数据是在激光雷达坐标系下，而真值则是在相机坐标系下。）
在从官网上下载的真值位姿具有以下格式（kitti总共有编号为00~20的21个数据集序列，其中只有00~10序列公开了真值，序列11~20仅用来做为算法评估使用）：

1.000000e+00 9.043680e-12 2.326809e-11 5.551115e-17 9.043683e-12 1.000000e+00 2.392370e-10 3.330669e-16 2.326810e-11 2.392370e-10 9.999999e-01 -4.440892e-16
9.999978e-01 5.272628e-04 -2.066935e-03 -4.690294e-02 -5.296506e-04 9.999992e-01 -1.154865e-03 -2.839928e-02 2.066324e-03 1.155958e-03 9.999971e-01 8.586941e-01
9.999910e-01 1.048972e-03 -4.131348e-03 -9.374345e-02 -1.058514e-03 9.999968e-01 -2.308104e-03 -5.676064e-02 4.128913e-03 2.312456e-03 9.999887e-01 1.716275e+00
9.999796e-01 1.566466e-03 -6.198571e-03 -1.406429e-01 -1.587952e-03 9.999927e-01 -3.462706e-03 -8.515762e-02 6.193102e-03 3.472479e-03 9.999747e-01 2.574964e+00

它们每一行有12个数据，它们记录了每一个时刻的位置和方向，对于上面的每一行，$p_0,p_1,p_2,...,p_{11}$; 它们与（旋转+平移）欧式矩阵对应关系如下：
$$T=[\begin{array}{cccc}{p}_0& p_1& p_2& \\ p_4& p_5& p_6& \\ p_8& p_9& p_{10}& \\ 0& 0& 0& \end{array}$$