ぷろぐ((>ω<))

ぷろぐらみんぐ関係のメモ

VeloViewで出力した点群データ(CSV)をPCLで読み込む

Velodyne社のLIDAR「HDL-32e」や「HDL-64e」用のViewerにVeloViewというものがあります。VeloViewでは点群データをCSV形式でエクスポートできます。そのCSVデータをpcl::PointCloudとして読み込むコードのメモ。

CSVデータの1行目は、カラム名が出力されていて下記のようになっています。

  • Points:0
  • Points:1
  • Points:2
  • intensity
  • laser_id
  • azimuth
  • distance_m
  • timestamp

今回のコードはpcl::PointXYZIの形式で、intensityにはレーザの反射強度が入ります。

#include <fstream>
#include <pcl/pcl_base.h>
#include <pcl/point_cloud.h>

int loadPointCloudFromCSV(const std::string &file_path, pcl::PointCloud<pcl::PointXYZI> &cloud) {
	std::ifstream ifs(file_path, std::ios::in);
	if(!ifs)
		return 1;	// File open failed

	std::string buf;
	std::getline(ifs, buf);	// Skip first line ("Points:0","Points:1","Points:2","intensity","laser_id","azimuth","distance_m","timestamp")
	while(ifs && std::getline(ifs, buf)) {
		std::vector<std::string> v;
		boost::algorithm::split(v, buf, boost::is_any_of(","));
		if(v.size() < 4)
			continue;
		pcl::PointXYZI p;
		p.x = std::atof(v[0].c_str());
		p.y = std::atof(v[1].c_str());
		p.z = std::atof(v[2].c_str());
		p.intensity = std::atof(v[3].c_str());
		cloud.push_back(p);
	}
	return 0;
}