通过Eigen库实现机械手位姿转换为Isometry3f类型

初步接触这方面，根据自己的理解先简单实现效果。熟练掌握和使用后再进行优化。

因为百度搜索时，大部分答主描述的比较复杂，这里记录一下如何直接使用

//其中pose1是由机械手坐标X、Y、Z和Rx、Ry、Rz组成。
int Pose2Isometry3f(const std::vector<float>& pose1, Eigen::Isometry3f& matrix)
{
	float roll	= pose1[3] * PI / 180;
	float pitch = pose1[4] * PI / 180;
	float yaw	= pose1[5] * PI / 180;

	Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX());	  // 轴角表达公式
	Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY());  // 轴角表达公式
	Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ());	  // 轴角表达公式

	Eigen::Translation3f trans(pose1[0], pose1[1], pose1[2]);//位移量，录入当前位置
	matrix = trans * rotZ * rotY * rotX;

	std::cout << "Hand Matrix = " << matrix.matrix() << std::endl;//机器人位姿转换为矩阵的形式

	return 0;
}

int main()
{
    Eigen::Isometry3f PoseMat;
	std::vector<float> pose1 = {0.76227, 0.59453, 0.28947, -178.9, 0.87, 92.96};//X、Y、Z、Rx、Ry、Rz
	Pose2Isometry3f(pose1, PoseMat);
}

结果如下，跟halcon中的算子pose_to_hom_mat3d的输出结果相同