二维图像的三维旋转

三维坐标系中，已知三个欧拉角alpha,beta,gamma，分别为绕x轴旋转alpha角度，绕y轴旋转beta角度，绕z轴旋转gamma角度。则旋转矩阵Rotation的求法如下：

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);

Rotation是3*3矩阵，用于三维空间坐标的旋转。

现在给定一幅二维图像如下，并且已知拍摄此图像的摄像机内参，根据输入的三个欧拉角，实现绕三个坐标轴的旋转。

1 #include <iostream>
2 #include <opencv.hpp>
3 #include <string>
4 #include <fstream>
5
6 using namespace std;
7 using namespace cv;
8
9 const float PI=3.1415926;
10
11 void main()
12 {
13     string imgPath="data/source_images/";
14     Mat srcImg=imread(imgPath+"moon.jpg");
15     pyrDown(srcImg, srcImg);
16     pyrDown(srcImg, srcImg);
17
18     namedWindow("show",0);
19     imshow("show", srcImg);
20     waitKey(0);
21
22     int imgHeight=srcImg.rows;
23     int imgWidth=srcImg.cols;
24
25     float alpha, beta, gamma;
26     alpha=0;
27     beta=0;
28     gamma=30*PI/180;
29     Mat Rot=Mat::eye(3,3,CV_32FC1);
30
31     Rot.at<float>(0, 0) = cos(beta) * cos(gamma);
32     Rot.at<float>(0, 1) = cos(beta) * sin(gamma);
33     Rot.at<float>(0, 2) = -sin(beta);
34     Rot.at<float>(1, 0) = sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma);
35     Rot.at<float>(1, 1) = sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma);
36     Rot.at<float>(1, 2) = sin(alpha) * cos(beta);
37     Rot.at<float>(2, 0) = cos(alpha) * sin(beta) * cos(gamma) + sin(alpha) * sin(gamma);
38     Rot.at<float>(2, 1) = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma);
39     Rot.at<float>(2, 2) = cos(alpha) * cos(beta);
40
41     Mat invRot;
42     invert(Rot, invRot, DECOMP_SVD);
43
44     float fx=930.965;
45     float fy=930.884;
46     float cx=513.823;
47     float cy=385.656;
48
49     Mat point3D=Mat::zeros(3, 1, CV_32FC1);
50     Mat oldPoint3D=Mat::zeros(3, 1, CV_32FC1);
51
52      Mat dstImg=srcImg.clone();
53     dstImg.setTo(0);
54
55     uchar* pImgData=(uchar*)srcImg.data;
56     uchar* pDstData=(uchar*)dstImg.data;
57     for (int j=0; j<imgHeight; j++)
58     {
59         for (int i=0; i<imgWidth; i++)
60         {
61             float X=(i-cx)/fx;
62             float Y=(j-cy)/fy;
63             float Z=1;
64
65             point3D.at<float>(0,0)=X;
66             point3D.at<float>(1,0)=Y;
67             point3D.at<float>(2,0)=Z;
68             //求旋转前坐标点
69             oldPoint3D=invRot*point3D;
70             float oldX=oldPoint3D.at<float>(0,0);
71             float oldY=oldPoint3D.at<float>(1,0);
72             float oldZ=oldPoint3D.at<float>(2,0);
73             //重投影到二维平面
74             if (oldZ>1e-3)
75             {
76                 float u= ((fx*oldX+cx*oldZ)/oldZ);
77                 float v= ((fy*oldY+cy*oldZ)/oldZ);
78
79                 int u0=floor(u);
80                 int v0=floor(v);
81                 int u1=u0+1;
82                 int v1=v0+1;
83
84                 if (u0>=0 && v0>=0 && u1<imgWidth && v1<imgHeight)
85                 {
86                     float dx=u-u0;
87                     float dy=v-v0;
88                     float weight1=(1-dx)*(1-dy);
89                     float weight2=dx*(1-dy);
90                     float weight3=(1-dx)*dy;
91                     float weight4=dx*dy;
92
93                     pDstData[j*imgWidth*3+i*3+0]=weight1*pImgData[v0*imgWidth*3+u0*3+0]+
94                         weight2*pImgData[v0*imgWidth*3+u1*3+0]+
95                         weight3*pImgData[v1*imgWidth*3+u0*3+0]+
96                         weight4*pImgData[v1*imgWidth*3+u1*3+0];
97
98                     pDstData[j*imgWidth*3+i*3+1]=weight1*pImgData[v0*imgWidth*3+u0*3+1]+
99                         weight2*pImgData[v0*imgWidth*3+u1*3+1]+
100                         weight3*pImgData[v1*imgWidth*3+u0*3+1]+
101                         weight4*pImgData[v1*imgWidth*3+u1*3+1];
102
103                     pDstData[j*imgWidth*3+i*3+2]=weight1*pImgData[v0*imgWidth*3+u0*3+2]+
104                         weight2*pImgData[v0*imgWidth*3+u1*3+2]+
105                         weight3*pImgData[v1*imgWidth*3+u0*3+2]+
106                         weight4*pImgData[v1*imgWidth*3+u1*3+2];
107                 }
108
109             }
110
111         }
112     }
113
114     imshow("show", dstImg);
115     waitKey(0);
116 }