mediapipe 0.8.9计算右肩的Pitch Yaw Roll c++
时间: 2024-02-19 15:03:49 浏览: 23
以下是使用C++代码实现通过mediapipe 0.8.9计算右肩姿态的Pitch、Yaw和Roll的示例:
```
#include <iostream>
#include "mediapipe/framework/calculator_framework.h"
#include "mediapipe/framework/formats/landmark.pb.h"
#include "mediapipe/framework/formats/landmark_util.h"
#include "mediapipe/framework/formats/image_frame.h"
#include "mediapipe/framework/formats/image_frame_opencv.h"
#include "mediapipe/framework/port/opencv_highgui_inc.h"
#include "mediapipe/framework/port/opencv_imgproc_inc.h"
namespace mediapipe {
void CalculateShoulderRotation(const NormalizedLandmarkList& landmarks) {
// Find the right shoulder landmark.
const auto& right_shoulder = landmarks.landmark(12);
// Find the right elbow landmark.
const auto& right_elbow = landmarks.landmark(14);
// Find the right wrist landmark.
const auto& right_wrist = landmarks.landmark(16);
// Calculate the shoulder, elbow and wrist positions in 3D space.
cv::Vec3d shoulder_pos(right_shoulder.x(), right_shoulder.y(), right_shoulder.z());
cv::Vec3d elbow_pos(right_elbow.x(), right_elbow.y(), right_elbow.z());
cv::Vec3d wrist_pos(right_wrist.x(), right_wrist.y(), right_wrist.z());
// Calculate the vector from the shoulder to the elbow.
cv::Vec3d shoulder_to_elbow = elbow_pos - shoulder_pos;
// Calculate the vector from the elbow to the wrist.
cv::Vec3d elbow_to_wrist = wrist_pos - elbow_pos;
// Calculate the rotation matrix from the shoulder to the wrist.
cv::Matx33d rotation_matrix = cv::Matx33d::zeros();
cv::Vec3d axis = shoulder_to_elbow.cross(elbow_to_wrist);
double angle = std::acos(shoulder_to_elbow.dot(elbow_to_wrist) / (cv::norm(shoulder_to_elbow) * cv::norm(elbow_to_wrist)));
cv::Rodrigues(axis * angle, rotation_matrix);
// Extract the Pitch, Yaw and Roll angles from the rotation matrix.
double pitch = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2));
double yaw = std::atan2(-rotation_matrix(2,0), std::sqrt(rotation_matrix(2,1)*rotation_matrix(2,1) + rotation_matrix(2,2)*rotation_matrix(2,2)));
double roll = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0));
// Output the results.
std::cout << "Right Shoulder Rotation: Pitch=" << pitch << ", Yaw=" << yaw << ", Roll=" << roll << std::endl;
}
} // namespace mediapipe
int main() {
// Load the image and landmarks.
cv::Mat image = cv::imread("image.jpg");
NormalizedLandmarkList landmarks = ...; // Load or calculate the landmark data.
// Calculate the right shoulder rotation.
mediapipe::CalculateShoulderRotation(landmarks);
return 0;
}
```
在这个示例中,我们使用mediapipe的`NormalizedLandmarkList`格式存储关键点(landmarks)数据,使用OpenCV进行向量和矩阵计算,并最终输出右肩的Pitch、Yaw和Roll这三个旋转角度。需要注意的是,这只是一个简单的示例,实际应用中还需要考虑更多的因素,如姿态变化、噪声和误差等。