cameraparameters.cpp
Go to the documentation of this file.
00001 /*****************************
00002 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification, are
00005 permitted provided that the following conditions are met:
00006 
00007    1. Redistributions of source code must retain the above copyright notice, this list of
00008       conditions and the following disclaimer.
00009 
00010    2. Redistributions in binary form must reproduce the above copyright notice, this list
00011       of conditions and the following disclaimer in the documentation and/or other materials
00012       provided with the distribution.
00013 
00014 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00015 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00019 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00020 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00021 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00022 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00023 
00024 The views and conclusions contained in the software and documentation are those of the
00025 authors and should not be interpreted as representing official policies, either expressed
00026 or implied, of Rafael Muñoz Salinas.
00027 ********************************/
00028 #include <aruco/cameraparameters.h>
00029 #include <fstream>
00030 #include <iostream>
00031 #include <opencv/cv.h>
00032 using namespace std;
00033 namespace aruco
00034 {
00035 
00036 
00037 CameraParameters::CameraParameters() {
00038     CameraMatrix=cv::Mat();
00039     Distorsion=cv::Mat();
00040     CamSize=cv::Size(-1,-1);
00041 }
00047 CameraParameters::CameraParameters(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception) {
00048     setParams(cameraMatrix,distorsionCoeff,size);
00049 }
00052 CameraParameters::CameraParameters(const CameraParameters &CI) {
00053     CI.CameraMatrix.copyTo(CameraMatrix);
00054     CI.Distorsion.copyTo(Distorsion);
00055     CamSize=CI.CamSize;
00056 }
00057 
00060 CameraParameters & CameraParameters::operator=(const CameraParameters &CI) {
00061     CI.CameraMatrix.copyTo(CameraMatrix);
00062     CI.Distorsion.copyTo(Distorsion);
00063     CamSize=CI.CamSize;
00064     return *this;
00065 }
00068 void CameraParameters::setParams(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception)
00069 {
00070     if (cameraMatrix.rows!=3 || cameraMatrix.cols!=3)
00071         throw cv::Exception(9000,"invalid input cameraMatrix","CameraParameters::setParams",__FILE__,__LINE__);
00072     cameraMatrix.convertTo(CameraMatrix,CV_32FC1);
00073     if (  distorsionCoeff.total()<4 ||  distorsionCoeff.total()>=7 )
00074         throw cv::Exception(9000,"invalid input distorsionCoeff","CameraParameters::setParams",__FILE__,__LINE__);
00075     cv::Mat auxD;
00076 
00077     distorsionCoeff.convertTo( Distorsion,CV_32FC1);
00078 
00079 //     Distorsion.create(1,4,CV_32FC1);
00080 //     for (int i=0;i<4;i++)
00081 //         Distorsion.ptr<float>(0)[i]=auxD.ptr<float>(0)[i];
00082 
00083     CamSize=size;
00084 
00085 }
00086 
00089 cv::Point3f CameraParameters::getCameraLocation(cv::Mat Rvec,cv::Mat Tvec)
00090 {
00091     cv::Mat m33(3,3,CV_32FC1);
00092     cv::Rodrigues(Rvec, m33)  ;
00093 
00094     cv::Mat m44=cv::Mat::eye(4,4,CV_32FC1);
00095     for (int i=0;i<3;i++)
00096         for (int j=0;j<3;j++)
00097             m44.at<float>(i,j)=m33.at<float>(i,j);
00098 
00099     //now, add translation information
00100     for (int i=0;i<3;i++)
00101         m44.at<float>(i,3)=Tvec.at<float>(0,i);
00102     //invert the matrix
00103     m44.inv();
00104     return  cv::Point3f( m44.at<float>(0,0),m44.at<float>(0,1),m44.at<float>(0,2));
00105 
00106 }
00107 
00110 void CameraParameters::readFromFile(string path)throw(cv::Exception)
00111 {
00112 
00113     ifstream file(path.c_str());
00114     if (!file)  throw cv::Exception(9005,"could not open file:"+path,"CameraParameters::readFromFile",__FILE__,__LINE__);
00115 //Create the matrices
00116     Distorsion=cv::Mat::zeros(4,1,CV_32FC1);
00117     CameraMatrix=cv::Mat::eye(3,3,CV_32FC1);
00118     char line[1024];
00119     while (!file.eof()) {
00120         file.getline(line,1024);
00121         char cmd[20];
00122         float fval;
00123         if ( sscanf(line,"%s = %f",cmd,&fval)==2) {
00124             string scmd(cmd);
00125             if (scmd=="fx") CameraMatrix.at<float>(0,0)=fval;
00126             else if (scmd=="cx") CameraMatrix.at<float>(0,2)=fval;
00127             else if (scmd=="fy") CameraMatrix.at<float>(1,1)=fval;
00128             else if (scmd=="cy") CameraMatrix.at<float>(1,2)=fval;
00129             else if (scmd=="k1") Distorsion.at<float>(0,0)=fval;
00130             else if (scmd=="k2") Distorsion.at<float>(1,0)=fval;
00131             else if (scmd=="p1") Distorsion.at<float>(2,0)=fval;
00132             else if (scmd=="p2") Distorsion.at<float>(3,0)=fval;
00133             else if (scmd=="width") CamSize.width=fval;
00134             else if (scmd=="height") CamSize.height=fval;
00135         }
00136     }
00137 }
00140 void CameraParameters::saveToFile(string path,bool inXML)throw(cv::Exception)
00141 {
00142     if (!isValid())  throw cv::Exception(9006,"invalid object","CameraParameters::saveToFile",__FILE__,__LINE__);
00143     if (!inXML) {
00144         ofstream file(path.c_str());
00145         if (!file)  throw cv::Exception(9006,"could not open file:"+path,"CameraParameters::saveToFile",__FILE__,__LINE__);
00146         file<<"# Aruco 1.0 CameraParameters"<<endl;
00147         file<<"fx = "<<CameraMatrix.at<float>(0,0)<<endl;
00148         file<<"cx = "<<CameraMatrix.at<float>(0,2)<<endl;
00149         file<<"fy = "<<CameraMatrix.at<float>(1,1)<<endl;
00150         file<<"cy = "<<CameraMatrix.at<float>(1,2)<<endl;
00151         file<<"k1 = "<<Distorsion.at<float>(0,0)<<endl;
00152         file<<"k2 = "<<Distorsion.at<float>(1,0)<<endl;
00153         file<<"p1 = "<<Distorsion.at<float>(2,0)<<endl;
00154         file<<"p2 = "<<Distorsion.at<float>(3,0)<<endl;
00155         file<<"width = "<<CamSize.width<<endl;
00156         file<<"height = "<<CamSize.height<<endl;
00157     }
00158     else {
00159         cv::FileStorage fs(path,cv::FileStorage::WRITE);
00160         fs<<"image_width" << CamSize.width;
00161         fs<<"image_height" << CamSize.height;
00162         fs<<"camera_matrix" << CameraMatrix;
00163         fs<<"distortion_coefficients" <<Distorsion;
00164     }
00165 }
00166 
00169 void CameraParameters::resize(cv::Size size)throw(cv::Exception)
00170 {
00171     if (!isValid())  throw cv::Exception(9007,"invalid object","CameraParameters::resize",__FILE__,__LINE__);
00172     if (size==CamSize) return;
00173     //now, read the camera size
00174     //resize the camera parameters to fit this image size
00175     float AxFactor= float(size.width)/ float(CamSize.width);
00176     float AyFactor= float(size.height)/ float(CamSize.height);
00177     CameraMatrix.at<float>(0,0)*=AxFactor;
00178     CameraMatrix.at<float>(0,2)*=AxFactor;
00179     CameraMatrix.at<float>(1,1)*=AyFactor;
00180     CameraMatrix.at<float>(1,2)*=AyFactor;
00181 }
00182 
00183 /****
00184  *
00185  *
00186  *
00187  *
00188  */
00189 void CameraParameters::readFromXMLFile(string filePath)throw(cv::Exception)
00190 {
00191     cv::FileStorage fs(filePath, cv::FileStorage::READ);
00192     int w=-1,h=-1;
00193     cv::Mat MCamera,MDist;
00194 
00195     fs["image_width"] >> w;
00196     fs["image_height"] >> h;
00197     fs["distortion_coefficients"] >> MDist;
00198     fs["camera_matrix"] >> MCamera;
00199 
00200     if (MCamera.cols==0 || MCamera.rows==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera matrix","CameraParameters::readFromXML",__FILE__,__LINE__);
00201     if (w==-1 || h==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera dimensions","CameraParameters::readFromXML",__FILE__,__LINE__);
00202 
00203     if (MCamera.type()!=CV_32FC1) MCamera.convertTo(CameraMatrix,CV_32FC1);
00204     else CameraMatrix=MCamera;
00205 
00206     if (MDist.total()<4) throw cv::Exception(9007,"File :"+filePath+" does not contains valid distortion_coefficients","CameraParameters::readFromXML",__FILE__,__LINE__);
00207     //convert to 32 and get the 4 first elements only
00208     cv::Mat mdist32;
00209     MDist.convertTo(mdist32,CV_32FC1);
00210 //     Distorsion.create(1,4,CV_32FC1);
00211 //     for (int i=0;i<4;i++)
00212 //         Distorsion.ptr<float>(0)[i]=mdist32.ptr<float>(0)[i];
00213 
00214     Distorsion.create(1,5,CV_32FC1);
00215     for (int i=0;i<5;i++)
00216         Distorsion.ptr<float>(0)[i]=mdist32.ptr<float>(0)[i];
00217     CamSize.width=w;
00218     CamSize.height=h;
00219 }
00220 /****
00221  *
00222  */
00223 void CameraParameters::glGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert   )throw(cv::Exception)
00224 {
00225 
00226     if (cv::countNonZero(Distorsion)!=0) std::cerr<< "CameraParameters::glGetProjectionMatrix :: The camera has distortion coefficients " <<__FILE__<<" "<<__LINE__<<endl;
00227     if (isValid()==false) throw cv::Exception(9100,"invalid camera parameters","CameraParameters::glGetProjectionMatrix",__FILE__,__LINE__);
00228 
00229     //Deterime the rsized info
00230     double Ax=double(size.width)/double(orgImgSize.width);
00231     double Ay=double(size.height)/double(orgImgSize.height);
00232     double _fx=CameraMatrix.at<float>(0,0)*Ax;
00233     double _cx=CameraMatrix.at<float>(0,2)*Ax;
00234     double _fy=CameraMatrix.at<float>(1,1)*Ay;
00235     double _cy=CameraMatrix.at<float>(1,2)*Ay;
00236     double cparam[3][4] =
00237     {
00238         {
00239             _fx,  0,  _cx,  0
00240         },
00241         {0,          _fy,  _cy, 0},
00242         {0,      0,      1,      0}
00243     };
00244 
00245     argConvGLcpara2( cparam, size.width, size.height, gnear, gfar, proj_matrix, invert );
00246 
00247 }
00248 
00249 /*******************
00250  *
00251  *
00252  *******************/
00253 double CameraParameters::norm( double a, double b, double c )
00254 {
00255     return( sqrt( a*a + b*b + c*c ) );
00256 }
00257 
00258 /*******************
00259  *
00260  *
00261  *******************/
00262 
00263 double CameraParameters::dot( double a1, double a2, double a3,
00264                               double b1, double b2, double b3 )
00265 {
00266     return( a1 * b1 + a2 * b2 + a3 * b3 );
00267 }
00268 
00269 /*******************
00270  *
00271  *
00272  *******************/
00273 
00274 void CameraParameters::argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception)
00275 {
00276 
00277     double   icpara[3][4];
00278     double   trans[3][4];
00279     double   p[3][3], q[4][4];
00280     int      i, j;
00281 
00282     cparam[0][2] *= -1.0;
00283     cparam[1][2] *= -1.0;
00284     cparam[2][2] *= -1.0;
00285 
00286     if ( arParamDecompMat(cparam, icpara, trans) < 0 )
00287         throw cv::Exception(9002,"parameter error","MarkerDetector::argConvGLcpara2",__FILE__,__LINE__);
00288 
00289     for ( i = 0; i < 3; i++ )
00290     {
00291         for ( j = 0; j < 3; j++ )
00292         {
00293             p[i][j] = icpara[i][j] / icpara[2][2];
00294         }
00295     }
00296     q[0][0] = (2.0 * p[0][0] / width);
00297     q[0][1] = (2.0 * p[0][1] / width);
00298     q[0][2] = ((2.0 * p[0][2] / width)  - 1.0);
00299     q[0][3] = 0.0;
00300 
00301     q[1][0] = 0.0;
00302     q[1][1] = (2.0 * p[1][1] / height);
00303     q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
00304     q[1][3] = 0.0;
00305 
00306     q[2][0] = 0.0;
00307     q[2][1] = 0.0;
00308     q[2][2] = (gfar + gnear)/(gfar - gnear);
00309     q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
00310 
00311     q[3][0] = 0.0;
00312     q[3][1] = 0.0;
00313     q[3][2] = 1.0;
00314     q[3][3] = 0.0;
00315 
00316     for ( i = 0; i < 4; i++ )
00317     {
00318         for ( j = 0; j < 3; j++ )
00319         {
00320             m[i+j*4] = q[i][0] * trans[0][j]
00321                        + q[i][1] * trans[1][j]
00322                        + q[i][2] * trans[2][j];
00323         }
00324         m[i+3*4] = q[i][0] * trans[0][3]
00325                    + q[i][1] * trans[1][3]
00326                    + q[i][2] * trans[2][3]
00327                    + q[i][3];
00328     }
00329 
00330     if (!invert)
00331     {
00332         m[13]=-m[13] ;
00333         m[1]=-m[1];
00334         m[5]=-m[5];
00335         m[9]=-m[9];
00336     }
00337 
00338 }
00339 /*******************
00340  *
00341  *
00342  *******************/
00343 
00344 int CameraParameters::arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception)
00345 {
00346     int       r, c;
00347     double    Cpara[3][4];
00348     double    rem1, rem2, rem3;
00349 
00350     if ( source[2][3] >= 0 )
00351     {
00352         for ( r = 0; r < 3; r++ )
00353         {
00354             for ( c = 0; c < 4; c++ )
00355             {
00356                 Cpara[r][c] = source[r][c];
00357             }
00358         }
00359     }
00360     else
00361     {
00362         for ( r = 0; r < 3; r++ )
00363         {
00364             for ( c = 0; c < 4; c++ )
00365             {
00366                 Cpara[r][c] = -(source[r][c]);
00367             }
00368         }
00369     }
00370 
00371     for ( r = 0; r < 3; r++ )
00372     {
00373         for ( c = 0; c < 4; c++ )
00374         {
00375             cpara[r][c] = 0.0;
00376         }
00377     }
00378     cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] );
00379     trans[2][0] = Cpara[2][0] / cpara[2][2];
00380     trans[2][1] = Cpara[2][1] / cpara[2][2];
00381     trans[2][2] = Cpara[2][2] / cpara[2][2];
00382     trans[2][3] = Cpara[2][3] / cpara[2][2];
00383 
00384     cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],
00385                        Cpara[1][0], Cpara[1][1], Cpara[1][2] );
00386     rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];
00387     rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];
00388     rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];
00389     cpara[1][1] = norm( rem1, rem2, rem3 );
00390     trans[1][0] = rem1 / cpara[1][1];
00391     trans[1][1] = rem2 / cpara[1][1];
00392     trans[1][2] = rem3 / cpara[1][1];
00393 
00394     cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],
00395                        Cpara[0][0], Cpara[0][1], Cpara[0][2] );
00396     cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],
00397                        Cpara[0][0], Cpara[0][1], Cpara[0][2] );
00398     rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];
00399     rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];
00400     rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
00401     cpara[0][0] = norm( rem1, rem2, rem3 );
00402     trans[0][0] = rem1 / cpara[0][0];
00403     trans[0][1] = rem2 / cpara[0][0];
00404     trans[0][2] = rem3 / cpara[0][0];
00405 
00406     trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1];
00407     trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]
00408                    - cpara[0][2]*trans[2][3]) / cpara[0][0];
00409 
00410     for ( r = 0; r < 3; r++ )
00411     {
00412         for ( c = 0; c < 3; c++ )
00413         {
00414             cpara[r][c] /= cpara[2][2];
00415         }
00416     }
00417 
00418     return 0;
00419 }
00420 
00421 
00422 /******
00423  *
00424  */
00425 void CameraParameters::OgreGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, bool invert) throw(cv::Exception)
00426 {
00427     double temp_matrix[16];
00428     (*this).glGetProjectionMatrix(orgImgSize, size, temp_matrix, gnear, gfar, invert);
00429     proj_matrix[0]=-temp_matrix[0];
00430     proj_matrix[1]=-temp_matrix[4];
00431     proj_matrix[2]=-temp_matrix[8];
00432     proj_matrix[3]=temp_matrix[12];
00433 
00434     proj_matrix[4]=-temp_matrix[1];
00435     proj_matrix[5]=-temp_matrix[5];
00436     proj_matrix[6]=-temp_matrix[9];
00437     proj_matrix[7]=temp_matrix[13];
00438 
00439     proj_matrix[8]=-temp_matrix[2];
00440     proj_matrix[9]=-temp_matrix[6];
00441     proj_matrix[10]=-temp_matrix[10];
00442     proj_matrix[11]=temp_matrix[14];
00443 
00444     proj_matrix[12]=-temp_matrix[3];
00445     proj_matrix[13]=-temp_matrix[7];
00446     proj_matrix[14]=-temp_matrix[11];
00447     proj_matrix[15]=temp_matrix[15];
00448 }
00449 /******
00450  *
00451  */
00452 
00453     cv::Mat CameraParameters::getRTMatrix ( const cv::Mat &R_,const cv::Mat &T_ ,int forceType ) {
00454         cv::Mat M;
00455         cv::Mat R,T;
00456         R_.copyTo ( R );
00457         T_.copyTo ( T );
00458         if ( R.type() ==CV_64F ) {
00459             assert ( T.type() ==CV_64F );
00460             cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_64FC1 );
00461 
00462             cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
00463             if ( R.total() ==3 ) {
00464                 cv::Rodrigues ( R,R33 );
00465             } else if ( R.total() ==9 ) {
00466                 cv::Mat R64;
00467                 R.convertTo ( R64,CV_64F );
00468                 R.copyTo ( R33 );
00469             }
00470             for ( int i=0; i<3; i++ )
00471                 Matrix.at<double> ( i,3 ) =T.ptr<double> ( 0 ) [i];
00472             M=Matrix;
00473         } else if ( R.depth() ==CV_32F ) {
00474             cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_32FC1 );
00475             cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
00476             if ( R.total() ==3 ) {
00477                 cv::Rodrigues ( R,R33 );
00478             } else if ( R.total() ==9 ) {
00479                 cv::Mat R32;
00480                 R.convertTo ( R32,CV_32F );
00481                 R.copyTo ( R33 );
00482             }
00483 
00484             for ( int i=0; i<3; i++ )
00485                 Matrix.at<float> ( i,3 ) =T.ptr<float> ( 0 ) [i];
00486             M=Matrix;
00487         }
00488 
00489         if ( forceType==-1 ) return M;
00490         else {
00491             cv::Mat MTyped;
00492             M.convertTo ( MTyped,forceType );
00493             return MTyped;
00494         }
00495     }
00496 
00497 
00498 };


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sun Oct 5 2014 22:12:49