向量到欧拉角

记录学习过程，向量到欧拉角
目的是将机器人工具指向目标平面法线

1. 先通过点云拟合出平面，得到平面方程，相应得到平面法向量（A,B,C）

		typedef pcl::PointXYZ PointT;
		typedef pcl::PointCloud<PointT> PointCloudT;
		cout << "->正在估计平面..." << endl;
		pcl::SampleConsensusModelPlane<PointT>::Ptr model_plane(new pcl::SampleConsensusModelPlane<PointT>(cloud_transformed));	//选择拟合点云与几何模型
		pcl::RandomSampleConsensus<PointT> ransac(model_plane);	//创建随机采样一致性对象
		ransac.setDistanceThreshold(0.01);	//设置距离阈值，与平面距离小于0.01的点作为内点
		ransac.computeModel();				//执行模型估计

		PointCloudT::Ptr cloud_plane(new PointCloudT);

		vector<int> inliers;				//存储内点索引的向量
		ransac.getInliers(inliers);			//提取内点对应的索引
		pcl::copyPointCloud<PointT>(*cloud_transformed, inliers, *cloud_plane);

		//输出模型参数Ax+By+Cz+D=0
		Eigen::VectorXf coefficient;
		ransac.getModelCoefficients(coefficient);
		cout << "平面方程为：\n"
		<< coefficient[0] << "x + "
		<< coefficient[1] << "y + "
		<< coefficient[2] << "z + "
		<< coefficient[3] << " = 0"
		<< endl;

2. 固定法线方向，因为法线有可能会为反向

		if (coefficient[2] > 0)
		{
			coefficient[0] = 0 - coefficient[0];
			coefficient[1] = 0 - coefficient[1];
			coefficient[2] = 0 - coefficient[2];
			cout << "翻转：\n" << endl;
		}

3. 向量转换欧拉角，需要确定两个方向向量，如果只有一个方向，则存在不确定性，这里主要是让Z指向平面法向量，x就固定在45°位置

		Eigen::Matrix3f rotM = Eigen::Matrix3f::Identity();
		/*计算完都需要归一化处理
		* 如果放进去的Z方向和X方向不垂直，那就先计算出Y方向，在重新算出X方向
		* 例子
			curZ(0, 1, 0)；将z指向本体坐标系下的Y方向
			curX(0, 0, 1); 将x指向本体坐标系下的Z方向
		*/
		Eigen::Vector3f curZ(coefficient[0], coefficient[1], coefficient[2]);//定义z的指向
		Eigen::Vector3f curX(1, 1, 0);//定义x的指向
		curX /= curX.norm();//归一化
		Eigen::Vector3f curY = curZ.cross(curX);
		curY /= curY.norm();
		//重新算出X方向
		curX = curY.cross(curZ);
		curX /= curX.norm();

		rotM(0, 0) = curX(0);
		rotM(0, 1) = curY(0);
		rotM(0, 2) = curZ(0);
		rotM(1, 0) = curX(1);
		rotM(1, 1) = curY(1);
		rotM(1, 2) = curZ(1);
		rotM(2, 0) = curX(2);
		rotM(2, 1) = curY(2);
		rotM(2, 2) = curZ(2);
		std::cout << rotM << std::endl;
		std::cout << rotM.eulerAngles(2, 1, 0) * 180 / 3.1415926 << std::endl;