KITTI数据集之点云建图 数据集之点云建图 本文描述了如何通过KITTI数据集，读取激光雷达点云数据，并通过ground truth，对前后两帧点云进行旋转变换，使得二者统一坐标系，不断叠加点云进行点云建图的过 程。使用的是KITTI odometry中的07号数据集。 其主要内容包括： 1）点云文件的格式转换 2）点云转换矩阵的推导 3）代码以及文件资源链接 有关KITTI数据集的介绍请看参考链接或者KITTI官网 参考链接 参考链接 1、坐标系的转换 2、KITTI数据集数据初体验 3、KITTI odometry数据集下载 4、本文数据下载 1、点云建图效果 、点云建图效果 图1 odometry数据集07 图2 map 细节图 

2、数据处理 、数据处理 我们的目的是利用 图3 readme file in the development kit int32_t num = 1000000; int distance_threshold=20; float *data = (float*)malloc(num * sizeof(float)); // float *px = data + 0; float *py = data + 1; float *pz = data + 2; float *pr = data + 3;// // FILE *stream; fopen_s(&stream, inptfile, "rb"); num = fread(data, sizeof(float), num, stream) / 4;// fclose(stream); int32_t after_erase_num = 0; int distance_threshold=20; float *px_t = px; float *py_t = py; float *pz_t = pz; float *pr_t = pr;// for (int32_t i = 0; i < num; i++) { //setting a threshold according to the distance between the point and centre to decrease the points double distance = sqrt((*px_t)*(*px_t) + (*py_t)*(*py_t) + (*pz_t)*(*pz_t)); if (distance < distance_threshold) { fprintf(writePCDStream, "%f %f %f %f\n", *px, *py, *pz, *pr); } px_t += 4; py_t += 4; pz_t += 4; pr_t += 4; } fclose(writePCDStream); 上述代码中，为了减小点云量，过滤掉比较远的误差大的点云，我设置了一个阈值，只保存距离中心点20m以内的点云。 处理完后，得到的点云文件见图4，它们可以通过notepad++直接打开，看到里面的点云数据。 图4 转换后点云图片 之后，我们就可以通过点云库pcl直接读取pcd文件，为我们后续的点云坐标转换以及建图做准备。 2.2、旋转矩阵的转换 、旋转矩阵的转换 根据kitti官网的数据集中的readme文件介绍，odometry中的 轴的顺序和方向都不一样，详细见下图： （ground truth）并不是以velodyne为坐标系的，而是以采集设备中的0号相机为坐标系，且二者x,y,z三 

图5 kitti采集车传感器标定图​ 根据上图，我们可以看到，如果我们要利用velodyne进行建图，则必须将 公式1 由pose组成的旋转矩阵 上述Ri和ti即第i个pose与第0个pose之间的旋转和位置平移关系。但是，上述旋转矩阵是基于相机坐标系的，我们需要将其转换到velodyne的坐标系下，根据图5，可以 得到cam坐标系和velo坐标系之间的关系： 公式2 相机到激光雷达的坐标转换矩阵 上面0.08表示的是相机坐标系与激光雷达坐标系中，z轴方向上的高度差，可以从图5看出，相机坐标系高度为1.65m，而激光雷达坐标系高度为1.73m，这个0.08就是补 上二者差值。 此外，我们还需要激光雷达到相机的坐标转换矩阵，如下： 公式3 激光雷达到相机的坐标转换矩阵 为了得到点云与点云之间的转换矩阵，我们需要对公式1中的 二者之间的关系为： 进行转换。假设我们首先将点云转换到相机坐标系下，那么转换后的点云设为 ,转换前的为 ,那么 将点云转换到相机坐标系下后，就可以而通过 中的T来转换点云，假设转换后的点云为 ,那么转换前后的关系为： 此后，我们将在相机坐标系下通过相机转换矩阵得到的点云再变换回激光雷达坐标系下，设转换后的为点云 变换关系为： 通过上面三个关系，我们可以得到两个pose的点云之间的变换矩阵，这样，我们就将以相机为坐标系的 转换为以激光雷达的为坐标系的 。设此转换矩阵为 ，则可得： 公式4 转换后，我们单独提取两个坐标系下的pose中的坐标值，并将散点图画出来，如下图所示： ​ ​ ​ ​ 

图6 pose转换后的GT散点对比图 上图中，蓝色为相机坐标系的 到。 ，绿色为激光雷达的 ，对比图5中的两个坐标系关系，我们可以发现，相机坐标系中的前向与激光雷达中的前向旋转90°得 在代码中，我们使用Eigen库中的矩阵，利用pcl库中的点云变换函数，遍历所有点云文件，即可得到完整的点云地图。主要代码如下： pcl::PointCloud::Ptr map(new pcl::PointCloud()); pcl::PointCloud::Ptr target(new pcl::PointCloud()); pcl::PointCloud::Ptr source(new pcl::PointCloud()); string load_path; // traverse point cloud to transform,the total is 1101,the interval is 10,you can change it by yourself. for (int i =0; i <1100;i+=10) { load_path = PCD_OUTPUT_PATH + pcd_filename[i]; //load pcd to a pointcloud name map if (pcl::io::loadPCDFile(load_path, *source) == -1) return (-1); //transform the pointcloud pose according the transform to the cloud_transfrom pcl::transformPointCloud(*source, *target, transform[i]); //add the cloud_transfrom to map *map = *map + *target; cout << "process the " << i << "th pointcloud" << endl; } //down sampling the points cloud to decrease the memeory pcl::VoxelGrid filter; filter.setInputCloud(map); filter.setLeafSize(0.1f, 0.1f, 0.1f); filter.filter(*map); //save the map pcl::io::savePCDFileASCII(PCD_MAP_PATH, *map); 其中，transform[i]即根据公式4的来。 Eigen::Matrix4d pose_data2; Eigen::Matrix4d velo2cam, cam2velo; cam2velo << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0.08, 0, 0, 0, 1; velo2cam << 0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, -0.08, 0, 0, 0, 1; ... pose_data2 = cam2velo * pose_data2 * velo2cam; 其中，pose_data2就是转换矩阵transform[i]。 最后，如果你还没有装pcl，但仍然想先看一看点云地图，那么你可以到我提供的资源链接里面下载，资源中包含本文中提到的点云地图数据以及数据集07中的pose，还 有完整的代码。点云地图可以安装cloudcompare软件显示。 总结总结 本文主要讲的是如何利用KITTI数据集中的GT以及点云创建点云地图的，其中最主要的部分是点云的格式转换和点云旋转矩阵的推导和使用。在这个过程中，旋转矩阵的 推导花了我很多心思，因为对数据集中的转换关系并不是很了解，踩了不少坑。其中一个坑就是误以为图5中的velo-to-cam这一旋转矩阵就是我上文提到的 ， 结果根据KITTI给的标定数据一直得不到想要的结果。以至于后来想通过ICP算法进行帧间匹配，然后得到旋转矩阵进行建图，说好听点，就是ICP-SLAM。但是这种帧间 匹配效果并不好，以至于变成一坨鬼一样的东西，没法用。最后没办法，再仔细想了一下坐标轴的变换关系，才搞定的。 资源链接 资源链接 点这里 作者：瞬冬恒夏