KITTI Row Data 中的bin转pcd文件

KITTI Row Data 中的bin转pcd文件

代码功能：C++将文件夹下的bin文件批量转为pcd文件
KITTI Row Data：http://www.cvlibs.net/datasets/kitti/raw_data.php

#include <boost/program_options.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/point_operators.h>
#include <pcl/common/io.h>
#include <pcl/search/organized.h>
#include <pcl/search/octree.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>
#include <pcl/filters/voxel_grid.h>

#include <iostream>
#include <fstream>

using namespace pcl;
using namespace std;

namespace po = boost::program_options;

void getFiles(string path, vector<string>& files)
{
	//文件句柄  
	long   hFile = 0;
	//文件信息  
	struct _finddata_t fileinfo;
	string p;
	if ((hFile = _findfirst(p.assign(path).append("\\*").c_str(), &fileinfo)) != -1)
	{
		do
		{
			//如果是目录,迭代之  
			//如果不是,加入列表  
			if ((fileinfo.attrib &  _A_SUBDIR))
			{
				if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)
					getFiles(p.assign(path).append("\\").append(fileinfo.name), files);
			}
			else
			{
				files.push_back(p.assign(path).append("\\").append(fileinfo.name));
			}
		} while (_findnext(hFile, &fileinfo) == 0);
		_findclose(hFile);
	}
}

int main() {

	vector<string> files;
	char * filePath = "E:\\000\\KITTI(raw)\\2011_09_26\\data";
	////获取该路径下的所有文件  
	getFiles(filePath, files);
	int size = files.size();

	for (int j = 0; j < size; j++)
	{
		///The file to read from.
		string infile = files[j];

		///The file to output to.
		string file_type = ".pcd";
		string outfile = files[j].substr(0, files[j].rfind(".")) + file_type;

		// load point cloud
		fstream input(infile.c_str(), ios::in | ios::binary);
		if (!input.good()) {
			cerr << "Could not read file: " << infile << endl;
			exit(EXIT_FAILURE);
		}
		input.seekg(0, ios::beg);

		pcl::PointCloud<PointXYZI>::Ptr points(new pcl::PointCloud<PointXYZI>);

		int i;
		for (i = 0; input.good() && !input.eof(); i++) {
			PointXYZI point;
			input.read((char *)&point.x, 3 * sizeof(float));
			input.read((char *)&point.intensity, sizeof(float));
			points->push_back(point);
		}
		input.close();

		cout << "Read KTTI point cloud with " << i << " points, writing to " << outfile << endl;

		pcl::PCDWriter writer;

		// Save DoN features
		writer.write<PointXYZI>(outfile, *points, false);
	}
}