二维图像的三维旋转

三维坐标系中，已知三个欧拉角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 }
View Code

 

posted @ 2017-08-19 22:04  一度逍遥  阅读(6823)  评论(1)  编辑  收藏  举报