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、数据处理

我们的目的是利用（ground truth）中的旋转矩阵，直接读取velodyne采集得到的点云并根据进行旋转，不断叠加得到点云地图。

那么我们需要的数据有：

1）点云数据

2）以velodyne中心为坐标轴的旋转矩阵

需要的工具：

1）pcl点云库

2.1、点云数据处理

官网给出的点云数据是通过二进制的.bin文件格式保存的，而pcl库能处理的是以.pcd或.ply文件保存的。其.pcd文件格式见pcl官网介绍。因此，为了使用点云，我们需要对odometry中的velodyne数据进行转换，kitti官网已经给出我们bin转为pcd格式的代码了，详细的可以参考odometry中development kit里面的readme文件关于点云文件的描述，也可以参考我贴出的点云转换代码。 

图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进行建图，则必须将转换到以velodyne为中心的坐标系下,变为。其实不转换也可以，只是为了效果的话，我们直接将读取的点云根据进行旋转就行了。但是，为了严谨，以及了解标定中的坐标系之间的变换，我们还是按标准来。

对于的pose文件，它以txt文件存储，文件中一行数据代表一个pose，每个pose包含旋转和坐标位置两个部分。其中，第4、8、12代表了相机坐标系的x,y,z三个坐标值，也就是位置。1~3、5~7、9~12这九个数据按顺序组成了一个旋转矩阵，这个旋转矩阵描述了当前这一行的pose与起点pose之间的旋转关系（注意，不是前后两个连续的pose之间的旋转关系）。我们可以将两个部分合起来，并添加一行，形成4x4的旋转矩阵，如下公式1所示：

公式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。但是这种帧间匹配效果并不好，以至于变成一坨鬼一样的东西，没法用。最后没办法，再仔细想了一下坐标轴的变换关系，才搞定的。

 

资源链接

点这里