绕X 轴 Y轴 Z轴旋转的结果

void warp_perspect_3_angle(cv::Mat face, float roll, float yaw, float pitch) {

cv::Mat face_img = face.clone();
int imgHeight = face_img.rows;
int imgWidth = face_img.cols;

float alpha, beta, gamma;
alpha = pitch * 3.1415926 / 180;
beta = yaw* 3.1415926 / 180;
gamma = roll * 3.1415926 / 180;
Mat Rot = Mat::eye(3, 3, CV_32FC1);

Rot.at<float>(0, 0) = cos(beta) * cos(gamma);
Rot.at<float>(0, 1) = cos(beta) * sin(gamma);
Rot.at<float>(0, 2) = -sin(beta);
Rot.at<float>(1, 0) = sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma);
Rot.at<float>(1, 1) = sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma);
Rot.at<float>(1, 2) = sin(alpha) * cos(beta);
Rot.at<float>(2, 0) = cos(alpha) * sin(beta) * cos(gamma) + sin(alpha) * sin(gamma);
Rot.at<float>(2, 1) = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma);
Rot.at<float>(2, 2) = cos(alpha) * cos(beta);

Mat invRot;
invert(Rot, invRot, DECOMP_SVD);

float fx = imgWidth/2;
float fy = imgHeight/2;
float cx = imgWidth / 2;
float cy = imgHeight / 2;

Mat point3D = Mat::zeros(3, 1, CV_32FC1);
Mat oldPoint3D = Mat::zeros(3, 1, CV_32FC1);

Mat dstImg = face_img.clone();
dstImg.setTo(0);

uchar* pImgData = (uchar*)face_img.data;
uchar* pDstData = (uchar*)dstImg.data;
for (int j = 0; j < imgHeight; j++)
{
for (int i = 0; i < imgWidth; i++)
{
float X = (i - cx) / fx;
float Y = (j - cy) / fy;
float Z = 1;

point3D.at<float>(0, 0) = X;
point3D.at<float>(1, 0) = Y;
point3D.at<float>(2, 0) = Z;
//求旋转前坐标点
oldPoint3D = invRot*point3D;
float oldX = oldPoint3D.at<float>(0, 0);
float oldY = oldPoint3D.at<float>(1, 0);
float oldZ = oldPoint3D.at<float>(2, 0);
//重投影到二维平面
if (oldZ > 1e-3)
{
float u = ((fx*oldX + cx*oldZ) / oldZ);
float v = ((fy*oldY + cy*oldZ) / oldZ);

int u0 = floor(u);
int v0 = floor(v);
int u1 = u0 + 1;
int v1 = v0 + 1;

if (u0 >= 0 && v0 >= 0 && u1 < imgWidth && v1 < imgHeight)
{
float dx = u - u0;
float dy = v - v0;
float weight1 = (1 - dx)*(1 - dy);
float weight2 = dx*(1 - dy);
float weight3 = (1 - dx)*dy;
float weight4 = dx*dy;

pDstData[j*imgWidth * 3 + i * 3 + 0] = weight1*pImgData[v0*imgWidth * 3 + u0 * 3 + 0] +
weight2*pImgData[v0*imgWidth * 3 + u1 * 3 + 0] +
weight3*pImgData[v1*imgWidth * 3 + u0 * 3 + 0] +
weight4*pImgData[v1*imgWidth * 3 + u1 * 3 + 0];

pDstData[j*imgWidth * 3 + i * 3 + 1] = weight1*pImgData[v0*imgWidth * 3 + u0 * 3 + 1] +
weight2*pImgData[v0*imgWidth * 3 + u1 * 3 + 1] +
weight3*pImgData[v1*imgWidth * 3 + u0 * 3 + 1] +
weight4*pImgData[v1*imgWidth * 3 + u1 * 3 + 1];

pDstData[j*imgWidth * 3 + i * 3 + 2] = weight1*pImgData[v0*imgWidth * 3 + u0 * 3 + 2] +
weight2*pImgData[v0*imgWidth * 3 + u1 * 3 + 2] +
weight3*pImgData[v1*imgWidth * 3 + u0 * 3 + 2] +
weight4*pImgData[v1*imgWidth * 3 + u1 * 3 + 2];
}
}
}
}
imshow("show", dstImg);

--------------------- 

转载于:https://www.cnblogs.com/ly570/p/11278002.html