cameraparameters.cpp
Go to the documentation of this file.
00001 #include "aruco/cameraparameters.h"
00002 #include <fstream>
00003 #include <opencv/cv.h>
00004 using namespace std;
00005 namespace aruco
00006 {
00007 
00008 
00009 CameraParameters::CameraParameters() {
00010     CameraMatrix=cv::Mat();
00011     Distorsion=cv::Mat();
00012     CamSize=cv::Size(-1,-1);
00013 }
00019 CameraParameters::CameraParameters(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception) {
00020     if (cameraMatrix.rows!=3 || cameraMatrix.cols!=3)
00021         throw cv::Exception(9000,"invalid input cameraMatrix","CameraParameters::CameraParameters",__FILE__,__LINE__);
00022     CameraMatrix=cameraMatrix;
00023     if ( (distorsionCoeff.rows!=0 && distorsionCoeff.rows!=4)  ||( distorsionCoeff.cols!=0 && distorsionCoeff.cols!=1))
00024         throw cv::Exception(9000,"invalid input distorsionCoeff","CameraParameters::CameraParameters",__FILE__,__LINE__);
00025     Distorsion=distorsionCoeff;
00026     CamSize=size;
00027 }
00030 CameraParameters::CameraParameters(const CameraParameters &CI) {
00031     CI.CameraMatrix.copyTo(CameraMatrix);
00032     CI.Distorsion.copyTo(Distorsion);
00033     CamSize=CI.CamSize;
00034 }
00035  
00038 CameraParameters & CameraParameters::operator=(const CameraParameters &CI) {
00039     CI.CameraMatrix.copyTo(CameraMatrix);
00040     CI.Distorsion.copyTo(Distorsion);
00041     CamSize=CI.CamSize;
00042     return *this;
00043 }
00046 cv::Point3f CameraParameters::getCameraLocation(cv::Mat Rvec,cv::Mat Tvec)
00047 {
00048     cv::Mat m33(3,3,CV_32FC1);
00049     cv::Rodrigues(Rvec, m33)  ;
00050 
00051     cv::Mat m44=cv::Mat::eye(4,4,CV_32FC1);
00052     for (int i=0;i<3;i++)
00053         for (int j=0;j<3;j++)
00054             m44.at<float>(i,j)=m33.at<float>(i,j);
00055 
00056     //now, add translation information
00057     for (int i=0;i<3;i++)
00058         m44.at<float>(i,3)=Tvec.at<float>(0,i);
00059     //invert the matrix
00060     m44.inv();
00061     return  cv::Point3f( m44.at<float>(0,0),m44.at<float>(0,1),m44.at<float>(0,2));
00062 
00063 }
00064 
00067 void CameraParameters::readFromFile(string path)throw(cv::Exception)
00068 {
00069 
00070     ifstream file(path.c_str());
00071     if (!file)  throw cv::Exception(9005,"could not open file:"+path,"CameraParameters::readFromFile",__FILE__,__LINE__);
00072 //Create the matrices
00073     Distorsion=cv::Mat::zeros(4,1,CV_32FC1);
00074     CameraMatrix=cv::Mat::eye(3,3,CV_32FC1);
00075     char line[1024];
00076     while (!file.eof()) {
00077         file.getline(line,1024);
00078         char cmd[20];
00079         float fval;
00080         if ( sscanf(line,"%s = %f",cmd,&fval)==2) {
00081             string scmd(cmd);
00082             if (scmd=="fx") CameraMatrix.at<float>(0,0)=fval;
00083             else if (scmd=="cx") CameraMatrix.at<float>(0,2)=fval;
00084             else if (scmd=="fy") CameraMatrix.at<float>(1,1)=fval;
00085             else if (scmd=="cy") CameraMatrix.at<float>(1,2)=fval;
00086             else if (scmd=="k1") Distorsion.at<float>(0,0)=fval;
00087             else if (scmd=="k2") Distorsion.at<float>(1,0)=fval;
00088             else if (scmd=="p1") Distorsion.at<float>(2,0)=fval;
00089             else if (scmd=="p2") Distorsion.at<float>(3,0)=fval;
00090             else if (scmd=="width") CamSize.width=fval;
00091             else if (scmd=="height") CamSize.height=fval;
00092         }
00093     }
00094 }
00097 void CameraParameters::saveToFile(string path)throw(cv::Exception)
00098 {
00099     if (!isValid())  throw cv::Exception(9006,"invalid object","CameraParameters::saveToFile",__FILE__,__LINE__);
00100     ofstream file(path.c_str());
00101     if (!file)  throw cv::Exception(9006,"could not open file:"+path,"CameraParameters::saveToFile",__FILE__,__LINE__);
00102     file<<"# Aruco 1.0 CameraParameters"<<endl;
00103     file<<"fx = "<<CameraMatrix.at<float>(0,0)<<endl;
00104     file<<"cx = "<<CameraMatrix.at<float>(0,2)<<endl;
00105     file<<"fy = "<<CameraMatrix.at<float>(1,1)<<endl;
00106     file<<"cy = "<<CameraMatrix.at<float>(1,2)<<endl;
00107     file<<"k1 = "<<Distorsion.at<float>(0,0)<<endl;
00108     file<<"k2 = "<<Distorsion.at<float>(1,0)<<endl;
00109     file<<"p1 = "<<Distorsion.at<float>(2,0)<<endl;
00110     file<<"p2 = "<<Distorsion.at<float>(3,0)<<endl;
00111     file<<"width = "<<CamSize.width<<endl;
00112     file<<"height = "<<CamSize.height<<endl;
00113 }
00114 
00117 void CameraParameters::resize(cv::Size size)throw(cv::Exception)
00118 {
00119     if (!isValid())  throw cv::Exception(9007,"invalid object","CameraParameters::resize",__FILE__,__LINE__);
00120     if (size==CamSize) return;
00121     //now, read the camera size
00122     //resize the camera parameters to fit this image size
00123     float AxFactor= float(size.width)/ float(CamSize.width);
00124     float AyFactor= float(size.height)/ float(CamSize.height);
00125     CameraMatrix.at<float>(0,0)*=AxFactor;
00126     CameraMatrix.at<float>(0,2)*=AxFactor;
00127     CameraMatrix.at<float>(1,1)*=AyFactor;
00128     CameraMatrix.at<float>(1,2)*=AyFactor;
00129 }
00130 
00131 /****
00132  * 
00133  * 
00134  * 
00135  * 
00136  */
00137 void CameraParameters::readFromXMLFile(string filePath)throw(cv::Exception)
00138 {
00139     cv::FileStorage fs(filePath, cv::FileStorage::READ);
00140     int w=-1,h=-1;
00141     cv::Mat MCamera,MDist;
00142     
00143     fs["image_width"] >> w;
00144     fs["image_height"] >> h;
00145     fs["distortion_coefficients"] >> MDist;
00146     fs["camera_matrix"] >> MCamera;
00147     
00148     if (MCamera.cols==0 || MCamera.rows==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera matrix","CameraParameters::readFromXML",__FILE__,__LINE__);
00149     if (w==-1 || h==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera dimensions","CameraParameters::readFromXML",__FILE__,__LINE__);
00150     
00151     if (MCamera.type()!=CV_32FC1) MCamera.convertTo(CameraMatrix,CV_32FC1);
00152     else CameraMatrix=MCamera;
00153     if (MDist.type()!=CV_32FC1) MDist.convertTo(Distorsion,CV_32FC1);
00154     else Distorsion=MDist;
00155     CamSize.width=w;
00156     CamSize.height=h;
00157 }
00158  
00159 };
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


aruco_pose
Author(s): Julian Brunner
autogenerated on Thu May 23 2013 12:15:46