aruco: cameraparameters.cpp Source File

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     CamSize.width=w;
00215     CamSize.height=h;
00216 }
00217 /****
00218  *
00219  */
00220 void CameraParameters::glGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert   )throw(cv::Exception)
00221 {
00222 
00223     if (cv::countNonZero(Distorsion)!=0) std::cerr<< "CameraParameters::glGetProjectionMatrix :: The camera has distortion coefficients " <<__FILE__<<" "<<__LINE__<<endl;
00224     if (isValid()==false) throw cv::Exception(9100,"invalid camera parameters","CameraParameters::glGetProjectionMatrix",__FILE__,__LINE__);
00225 
00226     //Deterime the rsized info
00227     double Ax=double(size.width)/double(orgImgSize.width);
00228     double Ay=double(size.height)/double(orgImgSize.height);
00229     double _fx=CameraMatrix.at<float>(0,0)*Ax;
00230     double _cx=CameraMatrix.at<float>(0,2)*Ax;
00231     double _fy=CameraMatrix.at<float>(1,1)*Ay;
00232     double _cy=CameraMatrix.at<float>(1,2)*Ay;
00233     double cparam[3][4] =
00234     {
00235         {
00236             _fx,  0,  _cx,  0
00237         },
00238         {0,          _fy,  _cy, 0},
00239         {0,      0,      1,      0}
00240     };
00241 
00242     argConvGLcpara2( cparam, size.width, size.height, gnear, gfar, proj_matrix, invert );
00243 
00244 }
00245 
00246 /*******************
00247  *
00248  *
00249  *******************/
00250 double CameraParameters::norm( double a, double b, double c )
00251 {
00252     return( sqrt( a*a + b*b + c*c ) );
00253 }
00254 
00255 /*******************
00256  *
00257  *
00258  *******************/
00259 
00260 double CameraParameters::dot( double a1, double a2, double a3,
00261                               double b1, double b2, double b3 )
00262 {
00263     return( a1 * b1 + a2 * b2 + a3 * b3 );
00264 }
00265 
00266 /*******************
00267  *
00268  *
00269  *******************/
00270 
00271 void CameraParameters::argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception)
00272 {
00273 
00274     double   icpara[3][4];
00275     double   trans[3][4];
00276     double   p[3][3], q[4][4];
00277     int      i, j;
00278 
00279     cparam[0][2] *= -1.0;
00280     cparam[1][2] *= -1.0;
00281     cparam[2][2] *= -1.0;
00282 
00283     if ( arParamDecompMat(cparam, icpara, trans) < 0 )
00284         throw cv::Exception(9002,"parameter error","MarkerDetector::argConvGLcpara2",__FILE__,__LINE__);
00285 
00286     for ( i = 0; i < 3; i++ )
00287     {
00288         for ( j = 0; j < 3; j++ )
00289         {
00290             p[i][j] = icpara[i][j] / icpara[2][2];
00291         }
00292     }
00293     q[0][0] = (2.0 * p[0][0] / width);
00294     q[0][1] = (2.0 * p[0][1] / width);
00295     q[0][2] = ((2.0 * p[0][2] / width)  - 1.0);
00296     q[0][3] = 0.0;
00297 
00298     q[1][0] = 0.0;
00299     q[1][1] = (2.0 * p[1][1] / height);
00300     q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
00301     q[1][3] = 0.0;
00302 
00303     q[2][0] = 0.0;
00304     q[2][1] = 0.0;
00305     q[2][2] = (gfar + gnear)/(gfar - gnear);
00306     q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
00307 
00308     q[3][0] = 0.0;
00309     q[3][1] = 0.0;
00310     q[3][2] = 1.0;
00311     q[3][3] = 0.0;
00312 
00313     for ( i = 0; i < 4; i++ )
00314     {
00315         for ( j = 0; j < 3; j++ )
00316         {
00317             m[i+j*4] = q[i][0] * trans[0][j]
00318                        + q[i][1] * trans[1][j]
00319                        + q[i][2] * trans[2][j];
00320         }
00321         m[i+3*4] = q[i][0] * trans[0][3]
00322                    + q[i][1] * trans[1][3]
00323                    + q[i][2] * trans[2][3]
00324                    + q[i][3];
00325     }
00326 
00327     if (!invert)
00328     {
00329         m[13]=-m[13] ;
00330         m[1]=-m[1];
00331         m[5]=-m[5];
00332         m[9]=-m[9];
00333     }
00334 
00335 }
00336 /*******************
00337  *
00338  *
00339  *******************/
00340 
00341 int CameraParameters::arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception)
00342 {
00343     int       r, c;
00344     double    Cpara[3][4];
00345     double    rem1, rem2, rem3;
00346 
00347     if ( source[2][3] >= 0 )
00348     {
00349         for ( r = 0; r < 3; r++ )
00350         {
00351             for ( c = 0; c < 4; c++ )
00352             {
00353                 Cpara[r][c] = source[r][c];
00354             }
00355         }
00356     }
00357     else
00358     {
00359         for ( r = 0; r < 3; r++ )
00360         {
00361             for ( c = 0; c < 4; c++ )
00362             {
00363                 Cpara[r][c] = -(source[r][c]);
00364             }
00365         }
00366     }
00367 
00368     for ( r = 0; r < 3; r++ )
00369     {
00370         for ( c = 0; c < 4; c++ )
00371         {
00372             cpara[r][c] = 0.0;
00373         }
00374     }
00375     cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] );
00376     trans[2][0] = Cpara[2][0] / cpara[2][2];
00377     trans[2][1] = Cpara[2][1] / cpara[2][2];
00378     trans[2][2] = Cpara[2][2] / cpara[2][2];
00379     trans[2][3] = Cpara[2][3] / cpara[2][2];
00380 
00381     cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],
00382                        Cpara[1][0], Cpara[1][1], Cpara[1][2] );
00383     rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];
00384     rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];
00385     rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];
00386     cpara[1][1] = norm( rem1, rem2, rem3 );
00387     trans[1][0] = rem1 / cpara[1][1];
00388     trans[1][1] = rem2 / cpara[1][1];
00389     trans[1][2] = rem3 / cpara[1][1];
00390 
00391     cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],
00392                        Cpara[0][0], Cpara[0][1], Cpara[0][2] );
00393     cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],
00394                        Cpara[0][0], Cpara[0][1], Cpara[0][2] );
00395     rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];
00396     rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];
00397     rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
00398     cpara[0][0] = norm( rem1, rem2, rem3 );
00399     trans[0][0] = rem1 / cpara[0][0];
00400     trans[0][1] = rem2 / cpara[0][0];
00401     trans[0][2] = rem3 / cpara[0][0];
00402 
00403     trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1];
00404     trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]
00405                    - cpara[0][2]*trans[2][3]) / cpara[0][0];
00406 
00407     for ( r = 0; r < 3; r++ )
00408     {
00409         for ( c = 0; c < 3; c++ )
00410         {
00411             cpara[r][c] /= cpara[2][2];
00412         }
00413     }
00414 
00415     return 0;
00416 }
00417 
00418 
00419 /******
00420  *
00421  */
00422 void CameraParameters::OgreGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, bool invert) throw(cv::Exception)
00423 {
00424     double temp_matrix[16];
00425     (*this).glGetProjectionMatrix(orgImgSize, size, temp_matrix, gnear, gfar, invert);
00426     proj_matrix[0]=-temp_matrix[0];
00427     proj_matrix[1]=-temp_matrix[4];
00428     proj_matrix[2]=-temp_matrix[8];
00429     proj_matrix[3]=temp_matrix[12];
00430 
00431     proj_matrix[4]=-temp_matrix[1];
00432     proj_matrix[5]=-temp_matrix[5];
00433     proj_matrix[6]=-temp_matrix[9];
00434     proj_matrix[7]=temp_matrix[13];
00435 
00436     proj_matrix[8]=-temp_matrix[2];
00437     proj_matrix[9]=-temp_matrix[6];
00438     proj_matrix[10]=-temp_matrix[10];
00439     proj_matrix[11]=temp_matrix[14];
00440 
00441     proj_matrix[12]=-temp_matrix[3];
00442     proj_matrix[13]=-temp_matrix[7];
00443     proj_matrix[14]=-temp_matrix[11];
00444     proj_matrix[15]=temp_matrix[15];
00445 }
00446 
00447 
00448 
00449 
00450 };