board.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/board.h>
00029 #include <fstream>
00030 using namespace std;
00031 using namespace cv;
00032 namespace aruco
00033 {
00034 
00039 BoardConfiguration::BoardConfiguration()
00040 {
00041     mInfoType=NONE;
00042 }
00047 BoardConfiguration::BoardConfiguration ( const BoardConfiguration  &T ): vector<MarkerInfo>(T)
00048 {
00049 //     MarkersInfo=T.MarkersInfo;
00050     mInfoType=T.mInfoType;
00051 }
00052 
00057 BoardConfiguration & BoardConfiguration ::operator=(const BoardConfiguration  &T) {
00058 //     MarkersInfo=T.MarkersInfo;
00059     vector<MarkerInfo>::operator=(T);
00060     mInfoType=T.mInfoType;
00061     return *this;
00062 }
00067 void BoardConfiguration::saveToFile ( string sfile ) throw ( cv::Exception )
00068 {
00069 
00070     cv::FileStorage fs ( sfile,cv::FileStorage::WRITE );
00071     saveToFile(fs);
00072 
00073 }
00076 void BoardConfiguration::saveToFile(cv::FileStorage &fs)throw (cv::Exception) {
00077     fs<<"aruco_bc_nmarkers"<< ( int ) size();
00078     fs<<"aruco_bc_mInfoType"<< ( int ) mInfoType;
00079     fs<<"aruco_bc_markers"<<"[";
00080     for ( size_t i=0;i<size();i++ )
00081     {
00082         fs << "{:" << "id" << at(i).id ;
00083 
00084         fs<<"corners"<< "[:";
00085         for (size_t c=0;c<at(i).size();++c)
00086             fs<<at(i)[c];
00087         fs<<"]";
00088         fs <<  "}";
00089     }
00090     fs << "]";
00091 }
00092 
00097 void BoardConfiguration::readFromFile ( string sfile ) throw ( cv::Exception )
00098 {
00099     cv::FileStorage fs ( sfile,cv::FileStorage::READ );
00100     readFromFile(fs);
00101 
00102 }
00103 
00104 
00107 void BoardConfiguration::readFromFile(cv::FileStorage &fs)throw (cv::Exception)
00108 {
00109     int aux=0;
00110     //look for the nmarkers
00111     if ( fs["aruco_bc_nmarkers"].name() !="aruco_bc_nmarkers" )
00112         throw cv::Exception ( 81818,"BoardConfiguration::readFromFile","invalid file type" ,__FILE__,__LINE__ );
00113     fs["aruco_bc_nmarkers"]>>aux;
00114     resize ( aux );
00115     fs["aruco_bc_mInfoType"]>>mInfoType;
00116     cv::FileNode markers=fs["aruco_bc_markers"];
00117     int i=0;
00118     for (FileNodeIterator it = markers.begin();it!=markers.end();++it,i++) {
00119         at(i).id=(*it)["id"];
00120         FileNode FnCorners=(*it)["corners"];
00121         for (FileNodeIterator itc = FnCorners.begin();itc!=FnCorners.end();++itc) {
00122             vector<float> coordinates3d;
00123             (*itc)>>coordinates3d;
00124             if(coordinates3d.size()!=3)
00125                throw cv::Exception ( 81818,"BoardConfiguration::readFromFile","invalid file type 3" ,__FILE__,__LINE__ );
00126             cv::Point3f point(coordinates3d[0],coordinates3d[1],coordinates3d[2]);
00127             at(i).push_back(point);
00128         }
00129     }
00130 }
00131 
00134 int BoardConfiguration::getIndexOfMarkerId(int id)const
00135 {
00136  
00137  for(size_t i=0;i<size();i++)
00138    if( at(i).id==id)return i;
00139  return -1;   
00140 }
00141 
00144 const MarkerInfo& BoardConfiguration::getMarkerInfo(int id)const throw (cv::Exception)
00145 {
00146  for(size_t i=0;i<size();i++)
00147    if( at(i).id ==id) return at(i);
00148  throw cv::Exception(111,"BoardConfiguration::getMarkerInfo","Marker with the id given is not found",__FILE__,__LINE__);
00149   
00150 }
00151 
00152 
00155 void Board::glGetModelViewMatrix ( double modelview_matrix[16] ) throw ( cv::Exception )
00156 {
00157     //check if paremeters are valid
00158     bool invalid=false;
00159     for ( int i=0;i<3 && !invalid ;i++ )
00160     {
00161         if ( Tvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00162         if ( Rvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00163     }
00164     if ( invalid ) throw cv::Exception ( 9002,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__ );
00165     Mat Rot ( 3,3,CV_32FC1 ),Jacob;
00166     Rodrigues ( Rvec, Rot, Jacob );
00167 
00168     double para[3][4];
00169     for ( int i=0;i<3;i++ )
00170         for ( int j=0;j<3;j++ ) para[i][j]=Rot.at<float> ( i,j );
00171     //now, add the translation
00172     para[0][3]=Tvec.at<float> ( 0,0 );
00173     para[1][3]=Tvec.at<float> ( 1,0 );
00174     para[2][3]=Tvec.at<float> ( 2,0 );
00175     double scale=1;
00176 
00177     modelview_matrix[0 + 0*4] = para[0][0];
00178     // R1C2
00179     modelview_matrix[0 + 1*4] = para[0][1];
00180     modelview_matrix[0 + 2*4] = para[0][2];
00181     modelview_matrix[0 + 3*4] = para[0][3];
00182     // R2
00183     modelview_matrix[1 + 0*4] = para[1][0];
00184     modelview_matrix[1 + 1*4] = para[1][1];
00185     modelview_matrix[1 + 2*4] = para[1][2];
00186     modelview_matrix[1 + 3*4] = para[1][3];
00187     // R3
00188     modelview_matrix[2 + 0*4] = -para[2][0];
00189     modelview_matrix[2 + 1*4] = -para[2][1];
00190     modelview_matrix[2 + 2*4] = -para[2][2];
00191     modelview_matrix[2 + 3*4] = -para[2][3];
00192     modelview_matrix[3 + 0*4] = 0.0;
00193     modelview_matrix[3 + 1*4] = 0.0;
00194     modelview_matrix[3 + 2*4] = 0.0;
00195     modelview_matrix[3 + 3*4] = 1.0;
00196     if ( scale != 0.0 )
00197     {
00198         modelview_matrix[12] *= scale;
00199         modelview_matrix[13] *= scale;
00200         modelview_matrix[14] *= scale;
00201     }
00202 
00203 
00204 }
00205 
00206 
00207 /****
00208  *
00209  */
00210 void Board::OgreGetPoseParameters ( double position[3], double orientation[4] ) throw ( cv::Exception )
00211 {
00212     //check if paremeters are valid
00213     bool invalid=false;
00214     for ( int i=0;i<3 && !invalid ;i++ )
00215     {
00216         if ( Tvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00217         if ( Rvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00218     }
00219     if ( invalid ) throw cv::Exception ( 9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__ );
00220 
00221     // calculate position vector
00222     position[0] = -Tvec.ptr<float> ( 0 ) [0];
00223     position[1] = -Tvec.ptr<float> ( 0 ) [1];
00224     position[2] = +Tvec.ptr<float> ( 0 ) [2];
00225 
00226     // now calculare orientation quaternion
00227     cv::Mat Rot ( 3,3,CV_32FC1 );
00228     cv::Rodrigues ( Rvec, Rot );
00229 
00230     // calculate axes for quaternion
00231     double stAxes[3][3];
00232     // x axis
00233     stAxes[0][0] = -Rot.at<float> ( 0,0 );
00234     stAxes[0][1] = -Rot.at<float> ( 1,0 );
00235     stAxes[0][2] = +Rot.at<float> ( 2,0 );
00236     // y axis
00237     stAxes[1][0] = -Rot.at<float> ( 0,1 );
00238     stAxes[1][1] = -Rot.at<float> ( 1,1 );
00239     stAxes[1][2] = +Rot.at<float> ( 2,1 );
00240     // for z axis, we use cross product
00241     stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1];
00242     stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0];
00243     stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0];
00244 
00245     // transposed matrix
00246     double axes[3][3];
00247     axes[0][0] = stAxes[0][0];
00248     axes[1][0] = stAxes[0][1];
00249     axes[2][0] = stAxes[0][2];
00250 
00251     axes[0][1] = stAxes[1][0];
00252     axes[1][1] = stAxes[1][1];
00253     axes[2][1] = stAxes[1][2];
00254 
00255     axes[0][2] = stAxes[2][0];
00256     axes[1][2] = stAxes[2][1];
00257     axes[2][2] = stAxes[2][2];
00258 
00259     // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
00260     // article "Quaternion Calculus and Fast Animation".
00261     double fTrace = axes[0][0]+axes[1][1]+axes[2][2];
00262     double fRoot;
00263 
00264     if ( fTrace > 0.0 )
00265     {
00266         // |w| > 1/2, may as well choose w > 1/2
00267         fRoot = sqrt ( fTrace + 1.0 );  // 2w
00268         orientation[0] = 0.5*fRoot;
00269         fRoot = 0.5/fRoot;  // 1/(4w)
00270         orientation[1] = ( axes[2][1]-axes[1][2] ) *fRoot;
00271         orientation[2] = ( axes[0][2]-axes[2][0] ) *fRoot;
00272         orientation[3] = ( axes[1][0]-axes[0][1] ) *fRoot;
00273     }
00274     else
00275     {
00276         // |w| <= 1/2
00277         static unsigned int s_iNext[3] = { 1, 2, 0 };
00278         unsigned int i = 0;
00279         if ( axes[1][1] > axes[0][0] )
00280             i = 1;
00281         if ( axes[2][2] > axes[i][i] )
00282             i = 2;
00283         unsigned int j = s_iNext[i];
00284         unsigned int k = s_iNext[j];
00285 
00286         fRoot = sqrt ( axes[i][i]-axes[j][j]-axes[k][k] + 1.0 );
00287         double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] };
00288         *apkQuat[i] = 0.5*fRoot;
00289         fRoot = 0.5/fRoot;
00290         orientation[0] = ( axes[k][j]-axes[j][k] ) *fRoot;
00291         *apkQuat[j] = ( axes[j][i]+axes[i][j] ) *fRoot;
00292         *apkQuat[k] = ( axes[k][i]+axes[i][k] ) *fRoot;
00293     }
00294 
00295 
00296 }
00297 
00298 
00301 void Board::saveToFile(string filePath)throw(cv::Exception)
00302 {
00303  cv::FileStorage fs ( filePath,cv::FileStorage::WRITE );
00304   
00305     fs<<"aruco_bo_rvec"<< Rvec;
00306     fs<<"aruco_bo_tvec"<< Tvec;
00307     //now, the markers
00308     fs<<"aruco_bo_nmarkers"<< ( int )  size();
00309     fs<<"aruco_bo_markers"<< "[";
00310     for ( size_t i=0;i<size();++i )
00311     {
00312         fs << "{:" << "id" << at(i).id ;
00313         fs<<"corners"<< "[:";
00314         for (size_t c=0;c<at(i).size();++c)
00315               fs<<at(i)[c];
00316         fs<<"]";
00317         fs <<  "}";
00318     }
00319     fs << "]";
00320   //save configuration file
00321   conf.saveToFile(fs);
00322  
00323  
00324  
00325 //  readFromFile(fs);
00326 
00327 }
00330 void Board::readFromFile(string filePath)throw(cv::Exception)
00331 {
00332  cv::FileStorage fs ( filePath,cv::FileStorage::READ );
00333     if ( fs["aruco_bo_nmarkers"].name() !="aruco_bo_nmarkers" )
00334         throw cv::Exception ( 81818,"Board::readFromFile","invalid file type:" ,__FILE__,__LINE__ );
00335 
00336     
00337     
00338     int aux=0;
00339     //look for the nmarkers
00340     fs["aruco_bo_nmarkers"]>>aux;
00341     resize ( aux );
00342     fs["aruco_bo_rvec"]>> Rvec;
00343     fs["aruco_bo_tvec"]>> Tvec;
00344     
00345     cv::FileNode markers=fs["aruco_bo_markers"];
00346     int i=0;
00347     for (FileNodeIterator it = markers.begin();it!=markers.end();++it,i++) {
00348         at(i).id=(*it)["id"];   
00349         int ncorners=(*it)["ncorners"];
00350         at(i).resize(ncorners);
00351         FileNode FnCorners=(*it)["corners"];
00352         int c=0;
00353         for (FileNodeIterator itc = FnCorners.begin();itc!=FnCorners.end();++itc,c++) {
00354             vector<float> coordinates2d;
00355             (*itc)>>coordinates2d;
00356             if(coordinates2d.size()!=2) 
00357               throw cv::Exception ( 81818,"Board::readFromFile","invalid file type 2" ,__FILE__,__LINE__ );
00358             cv::Point2f point;
00359             point.x=coordinates2d[0];
00360             point.y=coordinates2d[1];
00361             at(i).push_back(point);
00362         }
00363     }
00364     
00365  conf.readFromFile(fs);
00366  
00367  
00368 }
00369 
00372 void BoardConfiguration::getIdList(std::vector< int >& ids, bool append) const
00373 {
00374  if (!append) ids.clear();
00375  for(size_t i=0;i<size();i++)
00376    ids.push_back(at(i).id);
00377 }
00378 };


aruco
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Thu Aug 27 2015 12:26:19