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 /= 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);
  rotM(0, 3) = point.x;
  rotM(1, 3) = point.y;
  rotM(2, 3) = point.z;

  return rotM;
}

2，矩阵转欧拉角

Eigen::Vector3f pclfunction::MatrixtorToEulerAngles(Eigen::Matrix4f Matrix) {
  Eigen::Matrix3f AnglesMat = Matrix.block<3, 3>(0, 0);

  return AnglesMat.eulerAngles(2, 1, 0) * 180 / 3.1415926;
}

end