使用GPS和velodyne 64拼接地图

输入：GPS X Y Z，roll pitch yaw；时间对齐的velodyne 64E 激光数据

输出：拼接成的激光点云地图

注意事项：

1、激光数据是X 右，Y前，Z上，惯导记录的数据是，X前， Y左，Z是上（全局的X，Y，Z分别指向东、北、天，偏航角是与东向的夹角。车辆初始位置朝东，此时朝向近似为0度，往前（东）走，X增加；接着左转（往北），Y增加。

由此得出的激光数据和位置数据的关系，先将激光的方向投影成和全局方向一致，即调整所得到的点云x=y;y=-x）

2、所存储的GPS数据是惯导的数据，激光安装位置在惯导前1.5米，所以x,y坐标要根据车辆的偏航角进行一个平移

#include
#include
#include
#include
#include
//#include 
#define NOMINMAX
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#define  M_PI		3.14159265358979323846


using namespace Eigen;
using namespace std;
typedef float T;
typedef Eigen::Matrix Vector3t;
typedef Eigen::Matrix Matrix4t;
typedef Eigen::Matrix VectorXt;//列向量，列数是1
typedef Eigen::Quaternion Quaterniont;


void getFileNames(string dir, vector &fileName);
VectorXt f(const VectorXt& state, const VectorXt& control, float dt);


template Type string2num(const string& str) {

	istringstream iss(str);
	Type num;
	iss >> num;
	return num;
}
template string num2string(const Type &num) {
	stringstream ss;
	string str;
	ss << num;
	ss >> str;
	return str;
}

void loadGps(string gps, vector &gps_vec)
{
	float temp;
	string str, tempstr;
	str = gps;
	//cout<(tempstr);
		gps_vec.push_back(temp);
		//cout<& filists, std::string type)
{
	if (filists.empty())return;

	std::sort(filists.begin(), filists.end(), computePairNum);
}

void readIMUData(vector &gps_vec, VectorXt &control, const string &imuFileName, float timeSec)
{
	static float x0 = 0, y0 = 0, z0 = 0;
	static bool count = 0;

	ifstream imuFile;
	string imuString;
	imuFile.open(imuFileName.c_str());
	getline(imuFile, imuString);
	//读取数据到gps_vec中
	loadGps(imuString, gps_vec);

	double x, y, z, roll, pitch, yaw, vx, vy, vz, ax, ay, az, rollv, pitchv, yawv;
	if (count == 0)
	{
		x0 = gps_vec[1];
		y0 = gps_vec[2];
		z0 = gps_vec[3];
		count++;
	}
	x = gps_vec[1] - x0;
	y = gps_vec[2] - y0;
	z = gps_vec[3] - z0;

	vx = gps_vec[7];
	vy = gps_vec[8];
	vz = gps_vec[9];

	//这里ax加了一个负号，转换为正方向,他妈的不能加
	ax = gps_vec[10] * 100;
	ay = gps_vec[11] * 100;
	az = gps_vec[12] * 100;

	//醉了，这里惯导顺序是roll,pitch,yaw，分别是绕y,x,z轴旋转的结果,注意！！！
	//但是在录激光数据的时候，存放的顺序是yaw pitch roll！！fuck
	//要转换为弧度制

	//惯导下的角度,pitch 是绕x ,roll是绕y，这里做一个转换，变为roll绕x,pitch绕y
	yawv = gps_vec[13] * 100;
	pitchv = gps_vec[15] * 100;
	rollv = gps_vec[14] * 100;
	//还是角度变换的锅！！！
	yaw = M_PI / 2 - gps_vec[4] / 180 * M_PI;
	pitch = gps_vec[6] / 180 * M_PI;
	roll = gps_vec[5] / 180 * M_PI;

	//车长
	float L = 1.5;
	gps_vec[1] = x + L * cos(yaw);
	gps_vec[2] = y + L * sin(yaw);
	gps_vec[3] = z;
	gps_vec[4] = roll;
	gps_vec[5] = pitch;
	gps_vec[6] = yaw;

	control.head<3>() = Eigen::Vector3f(ax, ay, az);
	control.tail<3>() = Eigen::Vector3f(rollv, pitchv, yawv);

	imuFile.close();
	/*cout << "pos:  " << x << " " << y << " " << z << "   "
	<< "angle:  " << roll / 180 * M_PI << " " << pitch / 180 * M_PI << " " << yaw / 180 * M_PI << "  "
	<< "vel:  " << vx << "," << vy << "," << vz << " "
	<< "acc:  " << ax << "," << ay << "," << az << "  "
	<< "angelRate:    " << rollv << "," << pitchv << "," << yawv << endl;*/
	//cout << "acc:  " << ax << "," << ay << "," << az << "  " << endl;
}

void rgb2xyzi(pcl::PointCloud::Ptr cloudI, const pcl::PointCloud::Ptr cloudRGB) {

	int m = cloudRGB->points.size();
	for (int i = 0; i < m; i++) {
		pcl::PointXYZI p;
		/*p.x = cloudRGB->points[i].y;
		p.y = -cloudRGB->points[i].x;
		p.z = cloudRGB->points[i].z;*/
		p.x = cloudRGB->points[i].y;
		p.y = -cloudRGB->points[i].x;
		p.z = cloudRGB->points[i].z;
		p.intensity = cloudRGB->points[i].r;
		cloudI->points.push_back(p);
	}
	cloudI->width = m;
	cloudI->height = 1;
}
//加载XYZRGB格式
void readCollectData(std::string &in_file, pcl::PointCloud::Ptr points_xyzi)
{
	pcl::PointCloud::Ptr cloudXYZRGB(new pcl::PointCloud);
	pcl::PCDReader reader;
	reader.read(in_file, *cloudXYZRGB);
	rgb2xyzi(points_xyzi, cloudXYZRGB);
}


int main(int argc, char **argv)
{
	//文件名后一定要记得加/,不然文件读不到
	std::string imu_path, pcd_path;
	imu_path = "J:\\schoolData\\20190106\\siyuan2\\IMU\\";
	pcd_path = "J:\\schoolData\\20190106\\siyuan2\\HDL\\";

	//        imu_path="/media/localization/Localization/tu/HDL/";
	//nh.getParam("PCD_directory",imu_path);
	cout << "数据文件夹： " << imu_path << endl;


	//时间戳
	vector time_list;
	//原始文件名列表
	std::vector file_lists, pcd_lists;
	//根据前后两帧时间差,处理过后的文件名列表
	std::vector sample_file_lists, sample_pcd_lists;
	//读取文件名
	getFileNames(imu_path, file_lists);
	sort_filelists(file_lists, "txt");

	cout << "file_lists size " << file_lists.size() << endl;


	//读取时间到time_list中
	for (int i = 0; i < file_lists.size(); ++i)
	{
		string time_string = file_lists[i].substr(0, file_lists[i].length() - 4) + " ";
		vector time_vec;
		string str, tempstr;
		str = time_string;
		for (int j = 0; j < 8; j++)
		{
			tempstr = str.substr(0, str.find_first_of(" "));

			str = str.substr(str.find_first_of(" ") + 1);

			if (tempstr == " ")
				continue;
			time_vec.push_back(string2num(tempstr));
		}
		float hour = time_vec[4];
		float minute = time_vec[5];
		float second = time_vec[6];
		float uSecond = time_vec[7];
		float timeSeq = hour * 3600 + minute * 60 + second + uSecond / 1000;

		if (i == 0)
		{
			time_list.push_back(timeSeq);
			sample_file_lists.push_back(file_lists[0]);
		}
		else
		{   //如果前后帧大于0.095
			if (timeSeq - time_list.back() >= 0.98)
			{
				time_list.push_back(timeSeq);
				sample_file_lists.push_back(file_lists[i]);
			}
		}
	}
	//所有地图
	pcl::PointCloud::Ptr all_GPSMap(new pcl::PointCloud());
	//for (int i = 1; i < 10; ++i)
	for (int i = 1; i < sample_file_lists.size(); ++i)
	{
		std::string imuFileName = imu_path + sample_file_lists[i];
		std::string pcdFileName = pcd_path + sample_file_lists[i].substr(0,sample_file_lists[i].length()-4)+".pcd";

		pcl::PointCloud::Ptr cloudXYZI(new pcl::PointCloud);
		//读数据
		pcl::PCDReader reader;
		readCollectData(pcdFileName, cloudXYZI);

		vector imu_vec;
		VectorXt control(6);
		readIMUData(imu_vec, control, imuFileName, time_list[i]);
		float dt = time_list[i] - time_list[i - 1];

		//旋转矩阵
		Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
		//x y z 
		transform_2.translation() << imu_vec[1], imu_vec[2], imu_vec[3];
		//roll pitch yaw 
		transform_2.rotate(Eigen::AngleAxisf(imu_vec[4], Eigen::Vector3f::UnitX()));
		transform_2.rotate(Eigen::AngleAxisf(imu_vec[5], Eigen::Vector3f::UnitY()));
		transform_2.rotate(Eigen::AngleAxisf(imu_vec[6], Eigen::Vector3f::UnitZ()));
		printf("\nMethod #2: using an Affine3f\n");
		std::cout << transform_2.matrix() << std::endl;

		// Executing the transformation
		pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud());
		// You can either apply transform_1 or transform_2; they are the same
		pcl::transformPointCloud(*cloudXYZI, *transformed_cloud, transform_2);
		*all_GPSMap += *transformed_cloud;
	}
	all_GPSMap->width = all_GPSMap->points.size();
	all_GPSMap->height = 1;

	//降采样存储地图
	pcl::PointCloud::Ptr map_filtered(new pcl::PointCloud());
	pcl::VoxelGrid downSizeFilter;
	downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
	downSizeFilter.setInputCloud(all_GPSMap);
	downSizeFilter.filter(*map_filtered);
	pcl::io::savePCDFile("GPS_Map_downFilter_offset.pcd", *map_filtered, true);


	return 0;
}


////博客：C/C++遍历某个单一目录下的所有文件，目录下若有目录则会出现错误，目录最后要加\\ https://www.cnblogs.com/collectionne/p/6792301.html

void getFileNames(string dir, vector &fileName)
{
	dir += "*.*";
	HANDLE hFind;
	WIN32_FIND_DATA findData;
	LARGE_INTEGER size;
	hFind = FindFirstFile(dir.c_str(), &findData);
	if (hFind == INVALID_HANDLE_VALUE)
	{
		cout << "Failed to find first file!\n";
		return;
	}
	do
	{
		// 忽略"."和".."两个结果 
		if (strcmp(findData.cFileName, ".") == 0 || strcmp(findData.cFileName, "..") == 0)
			continue;
		if (findData.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)    // 是否是目录 
		{
			cout << findData.cFileName << "\t\n";
		}
		else
		{
			size.LowPart = findData.nFileSizeLow;
			size.HighPart = findData.nFileSizeHigh;
			fileName.push_back(findData.cFileName);
			//cout << findData.cFileName << "\t" << size.QuadPart << " bytes\n";
		}
	} while (FindNextFile(hFind, &findData));
}