//c++实现
cv::Mat eulerAnglesToRotationMatrix(cv::Vec3f& theta)
{
// Calculate rotation about x axis
cv::Mat R_x = (cv::Mat_<double>(3, 3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);
// Calculate rotation about y axis
cv::Mat R_y = (cv::Mat_<double>(3, 3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);
// Calculate rotation about z axis
cv::Mat R_z = (cv::Mat_<double>(3, 3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1);
// Combined rotation matrix
cv::Mat R = R_z * R_y * R_x;
return R;
}
opencv函数:
cv::Mat_<float> r = (cv::Mat_<float>(3, 1) << theta(2), theta(1), theta(0));
cv::Mat R;
Rodrigues(r, R);