$search
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 };