c++ pcl 法向量 转机器人欧拉角

一个向量是无法计算出机器人的姿态的 ，可以将该法向量作为机器人的z方向向量  ，然后指定x方向向量 ，一般为(0,0,-1)用于焊接姿态，具体需要什么姿态调节x的向量即可，

然后根据右手定则知道y方向向量，最后调用eulerAngles 方法计算出欧拉角，具体代码如下:

1,向量转换矩阵

Eigen::Matrix4f pclfunction::vectorToMatrix(const Eigen::Vector3f AxisX,
                                            const Eigen::Vector3f AxisZ,
                                            pcl::PointXYZ point) {
  Eigen::Matrix4f rotM = Eigen::Matrix4f::Identity();

  Eigen::Vector3f curZ(AxisZ(0), AxisZ(1),
                       AxisZ(2));  // 定义z的指向
  Eigen::Vector3f curX(AxisX(0), AxisX(1),
                       AxisX(2));  // 定义x的指向

  curX /= curX.norm();  // 归一化
  Eigen::Vector3f curY = curZ.cross(curX);
  curY /= curY.norm();
  // 重新算出X方向
  curX = curY.cross(curZ);
  curX /= c