在 KITTI 数据集中利用车辆位姿真值拼接 pcd 点云并滤波，得到一个准确的点云地图

在 KITTI 数据集中利用车辆位姿真值拼接 pcd 点云并滤波，得到一个准确的点云地图：

全部代码如下（C++）：

#include "stdlib.h"
#include 
#include 
#include 
#include 

#include 

#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

// ubuntu 系统下读取指定目录下的所有文件名及其绝对路径
void GetFileNames(string path,vector<string>& filenames)
{
    DIR *pDir;
    struct dirent* ptr;
    if(!(pDir = opendir(path.c_str())))
        return;
    while((ptr = readdir(pDir))!=0) {
        if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
            filenames.push_back(path + "/" + ptr->d_name);
    }
    closedir(pDir);
}

int main()
{

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr allcloud (new pcl::PointCloud<pcl::PointXYZRGB>); // 创建点云（指针）

    pcl::visualization::CloudViewer viewer("viewer");  // 用来显示拼接后的点云
    vector<Eigen::Isometry3d> Trans = {Eigen::Isometry3d::Identity()};
    Eigen::Isometry3d T12 = Eigen::Isometry3d::Identity();
    /*T12(0,0) = 1  ;T12(0,1) = 0  ;T12(0,2) = 0  ;
    T12(1,0) = 0  ;T12(1,1) = 0  ;T12(1,2) = -1 ;
    T12(2,0) = 0  ;T12(2,1) = 1   ;T12(2,2) = 0  ;*/
    Eigen::Isometry3d T_out = Eigen::Isometry3d::Identity();
    //从激光到相机坐标系的外参，从 KITTI 的 Odometry 数据集中的 calib 文件夹下的 sequence_01 序列得到
    T_out(0,0)= 4.2e-04  ;T_out(0,1) =  -9.99e-01  ;T_out(0,2) = -8.08e-03  ;T_out(0,3) = -1.198e-02  ;
    T_out(1,0) = -7.21e-03  ;T_out(1,1) = 8.08e-03  ;T_out(1,2) = -9.999e-01;  T_out(1,3) = -5.40e-02  ;
    T_out(2,0) = 9.9997e-01  ;T_out(2,1) =4.8594e-04 ;T_out(2,2) = -7.20e-03 ;T_out(2,3) = -2.92e-01;

    cout<<"T12 = "<<T12.matrix()<<endl;
    //读取GroundTruth文件（车辆位姿真值文件），在 KITTI 的 Odometry 数据集中的 poses 文件夹内得到，和 01 序列对应
    ifstream fin("/home/clzx/ZhangYong/01.txt");

    vector<string> filename;
    string path = "/home/clzx/ZhangYong/rgbd";  // 存放待拼接的 pcd 点云文件的目录
    GetFileNames(path, filename);

    string feature;
    float feat_onepoint;
    vector<float>lines;
    for(int i = 0; i < filename.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr downcloud (new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr outcloud (new pcl::PointCloud<pcl::PointXYZRGB>);
        Eigen::Isometry3d transformation = Eigen::Isometry3d::Identity();
        Eigen::Isometry3d transformation2 = Eigen::Isometry3d::Identity();
        getline(fin,feature); //一次读取一行数据
        stringstream stringin(feature); //使用串流实现对string的输入输出操作
        lines.clear();
        while(stringin >> feat_onepoint) //按空格一次读取一个数据存入feat_onePoint
            lines.push_back(feat_onepoint); //存储每行按空格分开的数据
        //cout<<"lines = "<
        for(int m=0;m<3;m++)
            for(int n=0;n<4;n++)
                transformation(m,n) = lines[m*4+n];
        cout<<"第"<<i<<"帧"<<endl;
        //cout<<" 原来的transformation = "<

		// 因为我的点云文件已经转换到了参考相机（00 相机）坐标系下，所以不需要再乘以 T_out（从激光坐标系到参考相机（00 相机）坐标系的外参）了，如果没有转换的则要乘以外参。
        //transformation2 = transformation*T_out;
        transformation2 = transformation;
        
        //cout<<" 转换后的transformation = "<
        pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename[i], *pointcloud);
        pcl::transformPointCloud( *pointcloud, *outcloud, transformation2.matrix() );
        //体素滤波
        pcl::VoxelGrid<pcl::PointXYZRGB> downSampled;  //创建滤波对象
        downSampled.setInputCloud (outcloud);            //设置需要过滤的点云给滤波对象
        downSampled.setLeafSize (0.2f,0.2f, 0.2f);  //设置滤波时创建的体素体积为1cm的立方体，三个参数表示体素栅格叶大小，分别表示体素在XYZ方向的尺寸
        downSampled.setDownsampleAllData(true); //设置是否对所有的字段进行下采样
        downSampled.filter (*downcloud);           //执行滤波处理，存储输出

        *allcloud = *allcloud + *downcloud;

        viewer.showCloud(allcloud);  // 刷新显示点云
    }
    pcl::io::savePCDFile("/home/clzx/ZhangYong/result.pcd", *allcloud); //拼接后的pcd文件保存目录
    cout<<"Final result saved."<<endl;
    while( !viewer.wasStopped() )
    {

    }
}

注意！！

1、KITTI 中的 Odometry 数据集中的 11 个序列与 KITTI 中的 raw data 数据集具有对应关系，对应关系可以参考：1、kitti数据集 Raw Data与00-10 Ground Truth的对应关系，2、KITTI 00 ~ KITTI10 下载地址，3、KITTI中的pose数据集使用

2、KITTI 的 Odometry 数据集中的 calib 文件夹内有 calib.txt 文件，

P0: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 0.000000000000e+00 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
P1: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.861448000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
P2: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 4.538225000000e+01 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 -1.130887000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.779761000000e-03
P3: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.372877000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 2.369057000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 4.915215000000e-03
Tr: 4.276802385584e-04 -9.999672484946e-01 -8.084491683471e-03 -1.198459927713e-02 -7.210626507497e-03 8.081198471645e-03 -9.999413164504e-01 -5.403984729748e-02 9.999738645903e-01 4.859485810390e-04 -7.206933692422e-03 -2.921968648686e-01

其中 0,1,2,3 代表相机的编号，0表示左边灰度相机，1右边灰度相机，2左边彩色相机，3右边彩色相机，P0、P1、P2、P3分别是四个相机的内参。Tr 表示从激光坐标系到参考相机坐标系的经过校正后的外参，参考相机坐标系就是 0 相机（左边灰度相机）坐标系。

P0 表示把激光点从参考相机坐标系投影到编号为 0 的相机采集的图像中，P1 表示把激光点从参考相机坐标系投影到编号为 0 的相机采集的图像中，P2 表示把激光点从参考相机坐标系投影到编号为 2 的相机采集的图像中，P3 表示把激光点从参考相机坐标系投影到编号为 3 的相机采集的图像中。

KITTI 的 raw 数据集中的 calib 文件夹内有 calib_velo_to_cam.txt 文件和 calib_cam_to_cam.txt 文件，
calib_velo_to_cam.txt 文件中的 R 和 T 分别代表从激光坐标系到参考相机（0相机）坐标系的旋转矩阵和平移向量，delta_f 和 delta_c 已废弃。

calib_time: 15-Mar-2012 11:45:23
R: 7.967514e-03 -9.999679e-01 -8.462264e-04 -2.771053e-03 8.241710e-04 -9.999958e-01 9.999644e-01 7.969825e-03 -2.764397e-03
T: -1.377769e-02 -5.542117e-02 -2.918589e-01
delta_f: 0.000000e+00 0.000000e+00
delta_c: 0.000000e+00 0.000000e+00

calib_cam_to_cam.txt 文件：

calib_time: 09-Jan-2012 14:00:15
corner_dist: 9.950000e-02
S_00: 1.392000e+03 5.120000e+02
K_00: 9.799200e+02 0.000000e+00 6.900000e+02 0.000000e+00 9.741183e+02 2.486443e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_00: -3.745594e-01 2.049385e-01 1.110145e-03 1.379375e-03 -7.084798e-02
R_00: 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00
T_00: -9.251859e-17 8.326673e-17 -7.401487e-17
S_rect_00: 1.241000e+03 3.760000e+02
R_rect_00: 9.999454e-01 7.259129e-03 -7.519551e-03 -7.292213e-03 9.999638e-01 -4.381729e-03 7.487471e-03 4.436324e-03 9.999621e-01
P_rect_00: 7.188560e+02 0.000000e+00 6.071928e+02 0.000000e+00 0.000000e+00 7.188560e+02 1.852157e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
S_01: 1.392000e+03 5.120000e+02
K_01: 9.903522e+02 0.000000e+00 7.020000e+02 0.000000e+00 9.855674e+02 2.607319e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_01: -3.712084e-01 1.978723e-01 -3.709831e-05 -3.440494e-04 -6.724045e-02
R_01: 9.993440e-01 1.814887e-02 -3.134011e-02 -1.842595e-02 9.997935e-01 -8.575221e-03 3.117801e-02 9.147067e-03 9.994720e-01
T_01: -5.370000e-01 5.964270e-03 -1.274584e-02
S_rect_01: 1.241000e+03 3.760000e+02
R_rect_01: 9.996568e-01 -1.110284e-02 2.372712e-02 1.099810e-02 9.999292e-01 4.539964e-03 -2.377585e-02 -4.277453e-03 9.997082e-01
P_rect_01: 7.188560e+02 0.000000e+00 6.071928e+02 -3.861448e+02 0.000000e+00 7.188560e+02 1.852157e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
S_02: 1.392000e+03 5.120000e+02
K_02: 9.601149e+02 0.000000e+00 6.947923e+02 0.000000e+00 9.548911e+02 2.403547e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_02: -3.685917e-01 1.928022e-01 4.069233e-04 7.247536e-04 -6.276909e-02
R_02: 9.999788e-01 -5.008404e-03 -4.151018e-03 4.990516e-03 9.999783e-01 -4.308488e-03 4.172506e-03 4.287682e-03 9.999821e-01
T_02: 5.954406e-02 -7.675338e-04 3.582565e-03
S_rect_02: 1.241000e+03 3.760000e+02
R_rect_02: 9.999191e-01 1.228161e-02 -3.316013e-03 -1.228209e-02 9.999246e-01 -1.245511e-04 3.314233e-03 1.652686e-04 9.999945e-01
P_rect_02: 7.188560e+02 0.000000e+00 6.071928e+02 4.538225e+01 0.000000e+00 7.188560e+02 1.852157e+02 -1.130887e-01 0.000000e+00 0.000000e+00 1.000000e+00 3.779761e-03
S_03: 1.392000e+03 5.120000e+02
K_03: 9.049931e+02 0.000000e+00 6.957698e+02 0.000000e+00 9.004945e+02 2.389820e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_03: -3.735725e-01 2.066816e-01 -6.133284e-04 -1.193269e-04 -7.600861e-02
R_03: 9.995578e-01 1.656369e-02 -2.469315e-02 -1.663353e-02 9.998582e-01 -2.625576e-03 2.464616e-02 3.035149e-03 9.996916e-01
T_03: -4.738786e-01 5.991982e-03 -3.215069e-03
S_rect_03: 1.241000e+03 3.760000e+02
R_rect_03: 9.998092e-01 -9.354781e-03 1.714961e-02 9.382303e-03 9.999548e-01 -1.525064e-03 -1.713457e-02 1.685675e-03 9.998518e-01
P_rect_03: 7.188560e+02 0.000000e+00 6.071928e+02 -3.372877e+02 0.000000e+00 7.188560e+02 1.852157e+02 2.369057e+00 0.000000e+00 0.000000e+00 1.000000e+00 4.915215e-03

如果想把雷达点云投影到 02 相机（左侧彩色相机）拍摄的图片上，需要的参数有：

1. calib_velo_to_cam.txt 中的 R（旋转矩阵）
2. calib_velo_to_cam.txt 中的 T（平移向量）
3. calib_cam_to_cam.txt 中的 R_rect_00（3x3 纠正旋转矩阵（使图像平面共面，kitti特有的））
4. calib_cam_to_cam.txt 中的 P_rect_02（02 相机内参，可以把激光点从参考相机坐标系投影到编号为 2 的相机采集的图像中）

公式：

• RT_velo_to_cam * x ：是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机（参考相机）坐标系中
• R_rect00 *RT_velo_to_cam * x ：是将 Velodyne 坐标中的点 x 投影到编号为0的相机（参考相机）坐标系中, 再以参考相机 0 为基础进行图像共面对齐修正(这是使用KITTI数据集的进行3D投影的必要操作)
• P_rect_00 * R_rect00 *RT_velo_to_cam * x ：是将 Velodyne 坐标中的点 x 投影到编号为0的相机（参考相机）坐标系中, 再以参考相机 0 为基础进行图像共面对齐修正, 然后投影到相机 0 的像素坐标系中. 如果将 P_rect_00 改成 P_rect_02, 就是从参考相机 0 坐标系投影到相机 2 的像素坐标系中(其他相机相对与相机0有偏移b(i)).

其中，R_rect00 * RT_velo_to_cam 的结果与 KITTI 的 Odometry 数据集中的 calib 文件夹内的 calib.txt 文件的 Tr 矩阵一模一样！！
而 P_rect_00、P_rect_01、P_rect_02、P_rect_03 也分别与 KITTI 的 Odometry 数据集中的 calib 文件夹内的 calib.txt 文件的 P0、P1、P2、P3 一模一样！！

参考文献：
1、python_kitti_激光点云变换到图像平面并保存成int16灰度图
2、KITTI数据集 激光雷达-图像坐标系转换关系
3、KITTI数据集测试 - 3 calib 相机参数