三维坐标系中,已知三个欧拉角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),双线性插值等
![](https://i-blog.csdnimg.cn/blog_migrate/8f900a89c6347c561fdf2122f13be562.gif)
![](https://i-blog.csdnimg.cn/blog_migrate/961ddebeb323a10fe0623af514929fc1.gif)
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 }