三维坐标系中,已知三个欧拉角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矩阵,用于三维空间坐标的旋转。
现在给定一幅二维图像如下,并且已知拍摄此图像的摄像机内参,根据输入的三个欧拉角,实现绕三个坐标轴的旋转。
绕x轴旋转30°,alpha=π/6;
绕y轴旋转30°, beta=π/6;
绕z轴旋转30°,gamma=π/6;
代码如下:代码中用到3DWarping技术(z==1),双线性插值等
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 }