NormalizeLandmark()、CalculateOrientation()和ConvertToPitchYawRoll()实现方法

好的，下面是三个函数的实现方法：

## NormalizeLandmark()

// 将关键点归一化为位于中心点的单位球上
cv::Vec3f NormalizeLandmark(const mediapipe::NormalizedLandmark& landmark) {
  const float x = landmark.x();
  const float y = landmark.y();
  const float z = landmark.z();

  // 计算关键点到中心点的距离
  const float distance = std::sqrt(x * x + y * y + z * z);

  // 将关键点归一化为单位球上的坐标
  const cv::Vec3f normalized_landmark(x / distance, y / distance, z / distance);
  return normalized_landmark;
}

在上述代码中，我们将传入的关键点坐标`(x, y, z)`归一化为单位球上的坐标`(x', y', z')`，并返回一个`cv::Vec3f`类型的向量表示。

## CalculateOrientation()

// 计算手臂的方向向量
cv::Vec3f CalculateOrientation(const cv::Vec3f& norm_landmark) {
  const cv::Vec3f origin(0.0f, 0.0f, 0.0f);
  const cv::Vec3f direction = norm_landmark - origin;
  return direction;
}

在上述代码中，我们计算手臂的方向向量，其中`norm_landmark`是通过`NormalizeLandmark()`函数归一化后的关键点坐标。

## ConvertToPitchYawRoll()

// 将方向向量转换为Pitch Yaw Roll
struct PitchYawRoll {
  float pitch; // X 轴旋转角度
  float yaw; // Y 轴旋转角度
  float roll; // Z 轴旋转角度
};

PitchYawRoll ConvertToPitchYawRoll(const cv::Vec3f& direction) {
  // 计算Pitch Yaw Roll
  const float pitch = std::asin(direction[2]);
  const float yaw = std::atan2(direction[1], direction[0]);
  const float roll = std::atan2(direction[2], std::sqrt(direction[0] * direction[0] + direction[1] * direction[1]));

  // 将弧度转换为角度
  const float rad2deg = 180.0f / M_PI;
  const PitchYawRoll pitch_yaw_roll = {
      pitch * rad2deg,
      yaw * rad2deg,
      roll * rad2deg,
  };
  return pitch_yaw_roll;
}

在上述代码中，我们将手臂的方向向量转换为Pitch Yaw Roll，其中`pitch`表示绕X轴旋转的角度，`yaw`表示绕Y轴旋转的角度，`roll`表示绕Z轴旋转的角度。最后，我们将弧度转换为角度，并将结果存储在一个`PitchYawRoll`结构体中返回。