Convert .bin to .pcd file in Velodyne points in KITTI Road Dataset C++ Code

2019.11.26

 

딥러닝을 이용하여 도로 영역 검출을 하기위해 KITTI Dataset을 사용하고 있습니다. KITTI dataset을 제공하는 공식 홈페이지에 가보면 쉽게 다운로드 받을 수 있습니다. 저에게 필요한 LiDAR point cloud 데이터를 확인해보았는데, .bin 파일로 되어있어 안에 내용을 쉽게 볼 수가 없습니다. 파일 시스템을 사용해서 .txt 파일로 저장하고 PCL을 사용해서 실제 활용이 가능한 .pcd 파일로 변환할 수 있겠지만 PCL을 사용하지 않고 c++ 코드로만 사용해서 .bin 파일을 .pcd 파일로 바로 변환을 해주려고 합니다. 

 

아래 글은 PCL 공식 홈페이지에 가보면 PCD 파일의 포멧 헤더 정보가 어떻게 이루어지고 있는지 설명해주고 있습니다.

File format header

Each PCD file contains a header that identifies and declares certain properties of the point cloud data stored in the file. The header of a PCD must be encoded in ASCII.

Note

Each header entry as well as ascii point data (see below) specified in a PCD file, is separated using new lines (\n).

As of version 0.7, the PCD header contains the following entries:

• VERSION - specifies the PCD file version

• FIELDS - specifies the name of each dimension/field that a point can have. Examples:

  FIELDS x y z # XYZ data FIELDS x y z rgb # XYZ + colors FIELDS x y z normal_x normal_y normal_z # XYZ + surface normals FIELDS j1 j2 j3 # moment invariants ...

• SIZE - specifies the size of each dimension in bytes. Examples:

  • unsigned char/char has 1 byte

  • unsigned short/short has 2 bytes

  • unsigned int/int/float has 4 bytes

  • double has 8 bytes

• TYPE - specifies the type of each dimension as a char. The current accepted types are:

  • I - represents signed types int8 (char), int16 (short), and int32 (int)

  • U - represents unsigned types uint8 (unsigned char), uint16 (unsigned short), uint32 (unsigned int)

  • F - represents float types

• COUNT - specifies how many elements does each dimension have. For example, x data usually has 1 element, but a feature descriptor like the VFH has 308. Basically this is a way to introduce n-D histogram descriptors at each point, and treating them as a single contiguous block of memory. By default, if COUNT is not present, all dimensions’ count is set to 1.

• WIDTH - specifies the width of the point cloud dataset in the number of points. WIDTH has two meanings:

  • it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets;

  • it can specify the width (total number of points in a row) of an organized point cloud dataset.

  Also see HEIGHT.

  Note

  An organized point cloud dataset is the name given to point clouds that resemble an organized image (or matrix) like structure, where the data is split into rows and columns. Examples of such point clouds include data coming from stereo cameras or Time Of Flight cameras. The advantages of a organized dataset is that by knowing the relationship between adjacent points (e.g. pixels), nearest neighbor operations are much more efficient, thus speeding up the computation and lowering the costs of certain algorithms in PCL.

  Examples:

  WIDTH 640 # there are 640 points per line

• HEIGHT - specifies the height of the point cloud dataset in the number of points. HEIGHT has two meanings:

  • it can specify the height (total number of rows) of an organized point cloud dataset;

  • it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not).

  Example:

  WIDTH 640 # Image-like organized structure, with 480 rows and 640 columns, HEIGHT 480 # thus 640*480=307200 points total in the dataset

  Example:

  WIDTH 307200 HEIGHT 1 # unorganized point cloud dataset with 307200 points

• VIEWPOINT - specifies an acquisition viewpoint for the points in the dataset. This could potentially be later on used for building transforms between different coordinate systems, or for aiding with features such as surface normals, that need a consistent orientation.

  The viewpoint information is specified as a translation (tx ty tz) + quaternion (qw qx qy qz). The default value is:

  VIEWPOINT 0 0 0 1 0 0 0

• POINTS - specifies the total number of points in the cloud. As of version 0.7, its purpose is a bit redundant, so we’re expecting this to be removed in future versions.

  Example:

  POINTS 307200 # the total number of points in the cloud

• DATA - specifies the data type that the point cloud data is stored in. As of version 0.7, two data types are supported: ascii and binary. See the next section for more details.

# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 213
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 213
DATA ascii
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06
0.81915 0.32 0 4.2108e+06
0.97192 0.278 0 4.2108e+06

위의 내용의 설명처럼 .bin 파일을 읽어 .pcd 파일로 변환해주려면 먼저 .pcd 파일의 헤더 내용을 저장하고 포인트 데이터를 파싱하여 입력해주면 됩니다. 아래는 KITTI Velodyne 데이터가 저장되어있는 위치에서 모든 .bin 파일 리스트를 읽어들여 각 .bin 파일들을 .pcd 파일로 파싱해주고 새로운 경로에 저장해주는 코드가 됩니다.

#include <Windows.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>

using namespace std;

// Variable and function for reading file list.
typedef std::wstring str_t;

vector<str_t> get_files_in_folder(str_t folder, str_t file_type = L"*.*");

int main()
{
	// File List in specific directory
	vector<str_t> list;
	vector<string> w2s_list;

	str_t path = L"KITTI Datasets\\data_road_velodyne\\training\\velodyne\\";
    string load_path.assign(path.begin(), path.end());
	list = get_files_in_folder(path);

	for (int i = 0; i < list.size(); i++) {
		string temp;

		w2s_list.push_back(temp.assign(list[i].begin(), list[i].end()));
	}

	for (int index = 0; index < w2s_list.size(); index++) {
		// Read point cloud data
		int32_t num = 1000000;
		float *data = (float*)malloc(num * sizeof(float));

		// point
		float *px = data + 0;
		float *py = data + 1;
		float *pz = data + 2;
		float *pr = data + 3; // reflection intensity

		FILE *stream;
		fopen_s(&stream, (load_path + w2s_list[index]).c_str(), "rb");
		num = fread(data, sizeof(float), num, stream) / 4; // Read point cloud data, about 100,000+ points
		fclose(stream);

		string save_path = "KITTI Datasets\\data_road_velodyne\\training\\velodyne_pcd\\";
		int extension = w2s_list[index].find(".bin");
		string save_name = w2s_list[index].substr(0, extension);
		string pcdExtension = ".pcd";

		/* converted to PCD */
		// Write a file statement
		FILE *writePCDStream;
		fopen_s(&writePCDStream, (save_path + save_name + pcdExtension).c_str(), "wb");
		fprintf(writePCDStream, "VERSION 0.7\n"); // Release Description
		fprintf(writePCDStream, "FIELDS x y z\n"); // Dimension Description
		fprintf(writePCDStream, "SIZE 4 4 4\n"); // Occupy Byte Description
		fprintf(writePCDStream, "TYPE F F F\n"); // Specific data type definition
		fprintf(writePCDStream, "WIDTH %d\n", num); // number of points
		fprintf(writePCDStream, "HEIGHT 1\n"); // Unordered point cloud defaults to 1
		fprintf(writePCDStream, "POINTS %d\n", num); // Number of points
		fprintf(writePCDStream, "DATA ascii\n"); // Document uses the character type shuom

		// Write point cloud data
		for (int32_t i = 0; i < num; i++) {
			fprintf(writePCDStream, "%f %f %f\n", *px, *py, *pz);
			px += 4; py += 4; pz += 4; pr += 4;
		}

		// Because of using 'data' at every iteration, must do free malloc memory.
		free(data);

		fclose(writePCDStream);
	}	

	return 0;
}

vector<str_t> get_files_in_folder(str_t folder, str_t file_type) {
	vector<str_t> names;
	wchar_t search_path[200];
	wsprintf(search_path, L"%s/%s", folder.c_str(), file_type.c_str());
	WIN32_FIND_DATA fd;
	HANDLE hFind = ::FindFirstFile(search_path, &fd);
	if (hFind != INVALID_HANDLE_VALUE) {
		do {
			// read all (real) files in current folder
			// , delete '!' read other 2 default folder. and ..
			if (!(fd.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)) {
				names.push_back(fd.cFileName);
			}
		} while (::FindNextFile(hFind, &fd));
		::FindClose(hFind);
	}

	return names;
}