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 <opencv2/calib3d/calib3d.hpp>
00030 #include <fstream>
00031 using namespace std;
00032 using namespace cv;
00033 namespace aruco {
00034 
00039     BoardConfiguration::BoardConfiguration() {
00040         mInfoType=NONE;
00041     }
00046     BoardConfiguration::BoardConfiguration ( string filePath ) throw ( cv::Exception ) {
00047         mInfoType=NONE;
00048         readFromFile ( filePath );
00049     }
00054     BoardConfiguration::BoardConfiguration ( const BoardConfiguration  &T ) : vector<MarkerInfo> ( T ) {
00055 //     MarkersInfo=T.MarkersInfo;
00056         mInfoType=T.mInfoType;
00057     }
00058 
00063     BoardConfiguration & BoardConfiguration ::operator= ( const BoardConfiguration  &T ) {
00064 //     MarkersInfo=T.MarkersInfo;
00065         vector<MarkerInfo>::operator= ( T );
00066         mInfoType=T.mInfoType;
00067         return *this;
00068     }
00073     void BoardConfiguration::saveToFile ( string sfile ) throw ( cv::Exception ) {
00074 
00075         cv::FileStorage fs ( sfile,cv::FileStorage::WRITE );
00076         saveToFile ( fs );
00077 
00078     }
00081     void BoardConfiguration::saveToFile ( cv::FileStorage &fs ) throw ( cv::Exception ) {
00082         fs<<"aruco_bc_nmarkers"<< ( int ) size();
00083         fs<<"aruco_bc_mInfoType"<< ( int ) mInfoType;
00084         fs<<"aruco_bc_markers"<<"[";
00085         for ( size_t i=0; i<size(); i++ ) {
00086             fs << "{:" << "id" << at ( i ).id ;
00087 
00088             fs<<"corners"<< "[:";
00089             for ( int c=0; c<at ( i ).size(); c++ )
00090                 fs<<at ( i ) [c];
00091             fs<<"]";
00092             fs <<  "}";
00093         }
00094         fs << "]";
00095     }
00096 
00101     void BoardConfiguration::readFromFile ( string sfile ) throw ( cv::Exception ) {
00102       try{
00103         cv::FileStorage fs ( sfile,cv::FileStorage::READ );
00104         readFromFile ( fs );
00105       }catch (std::exception &ex){
00106         throw  cv::Exception ( 81818,"BoardConfiguration::readFromFile",ex.what()+string(" file=)")+sfile ,__FILE__,__LINE__ );
00107       }
00108     }
00109 
00110 
00113     void BoardConfiguration::readFromFile ( cv::FileStorage &fs ) throw ( cv::Exception ) {
00114         int aux=0;
00115         //look for the nmarkers
00116         if ( fs["aruco_bc_nmarkers"].name() !="aruco_bc_nmarkers" )
00117             throw cv::Exception ( 81818,"BoardConfiguration::readFromFile","invalid file type" ,__FILE__,__LINE__ );
00118         fs["aruco_bc_nmarkers"]>>aux;
00119         resize ( aux );
00120         fs["aruco_bc_mInfoType"]>>mInfoType;
00121         cv::FileNode markers=fs["aruco_bc_markers"];
00122         int i=0;
00123         for ( FileNodeIterator it = markers.begin(); it!=markers.end(); ++it,i++ ) {
00124             at ( i ).id= ( *it ) ["id"];
00125             FileNode FnCorners= ( *it ) ["corners"];
00126             for ( FileNodeIterator itc = FnCorners.begin(); itc!=FnCorners.end(); ++itc ) {
00127                 vector<float> coordinates3d;
00128                 ( *itc ) >>coordinates3d;
00129                 if ( coordinates3d.size() !=3 )
00130                     throw cv::Exception ( 81818,"BoardConfiguration::readFromFile","invalid file type 3" ,__FILE__,__LINE__ );
00131                 cv::Point3f point ( coordinates3d[0],coordinates3d[1],coordinates3d[2] );
00132                 at ( i ).push_back ( point );
00133             }
00134         }
00135     }
00136 
00139     int BoardConfiguration::getIndexOfMarkerId ( int id ) const
00140     {
00141 
00142         for ( size_t i=0; i<size(); i++ )
00143             if ( at ( i ).id==id ) return i;
00144         return -1;
00145     }
00146 
00149     const MarkerInfo& BoardConfiguration::getMarkerInfo ( int id ) const throw ( cv::Exception ) {
00150         for ( size_t i=0; i<size(); i++ )
00151             if ( at ( i ).id ==id ) return at ( i );
00152         throw cv::Exception ( 111,"BoardConfiguration::getMarkerInfo","Marker with the id given is not found",__FILE__,__LINE__ );
00153 
00154     }
00155 
00156 
00159     void Board::glGetModelViewMatrix ( double modelview_matrix[16] ) throw ( cv::Exception ) {
00160         //check if paremeters are valid
00161         bool invalid=false;
00162         for ( int i=0; i<3 && !invalid ; i++ ) {
00163             if ( Tvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00164             if ( Rvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00165         }
00166         if ( invalid ) throw cv::Exception ( 9002,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__ );
00167         Mat Rot ( 3,3,CV_32FC1 ),Jacob;
00168         Rodrigues ( Rvec, Rot, Jacob );
00169 
00170         double para[3][4];
00171         for ( int i=0; i<3; i++ )
00172             for ( int j=0; j<3; j++ ) para[i][j]=Rot.at<float> ( i,j );
00173         //now, add the translation
00174         para[0][3]=Tvec.at<float> ( 0,0 );
00175         para[1][3]=Tvec.at<float> ( 1,0 );
00176         para[2][3]=Tvec.at<float> ( 2,0 );
00177         double scale=1;
00178 
00179         modelview_matrix[0 + 0*4] = para[0][0];
00180         // R1C2
00181         modelview_matrix[0 + 1*4] = para[0][1];
00182         modelview_matrix[0 + 2*4] = para[0][2];
00183         modelview_matrix[0 + 3*4] = para[0][3];
00184         // R2
00185         modelview_matrix[1 + 0*4] = para[1][0];
00186         modelview_matrix[1 + 1*4] = para[1][1];
00187         modelview_matrix[1 + 2*4] = para[1][2];
00188         modelview_matrix[1 + 3*4] = para[1][3];
00189         // R3
00190         modelview_matrix[2 + 0*4] = -para[2][0];
00191         modelview_matrix[2 + 1*4] = -para[2][1];
00192         modelview_matrix[2 + 2*4] = -para[2][2];
00193         modelview_matrix[2 + 3*4] = -para[2][3];
00194         modelview_matrix[3 + 0*4] = 0.0;
00195         modelview_matrix[3 + 1*4] = 0.0;
00196         modelview_matrix[3 + 2*4] = 0.0;
00197         modelview_matrix[3 + 3*4] = 1.0;
00198         if ( scale != 0.0 ) {
00199             modelview_matrix[12] *= scale;
00200             modelview_matrix[13] *= scale;
00201             modelview_matrix[14] *= scale;
00202         }
00203 
00204 
00205     }
00206 
00207 
00208     /****
00209      *
00210      */
00211     void Board::OgreGetPoseParameters ( double position[3], double orientation[4] ) throw ( cv::Exception ) {
00212         //check if paremeters are valid
00213         bool invalid=false;
00214         for ( int i=0; i<3 && !invalid ; i++ ) {
00215             if ( Tvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00216             if ( Rvec.at<float> ( i,0 ) !=-999999 ) invalid|=false;
00217         }
00218         if ( invalid ) throw cv::Exception ( 9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__ );
00219 
00220         // calculate position vector
00221         position[0] = -Tvec.ptr<float> ( 0 ) [0];
00222         position[1] = -Tvec.ptr<float> ( 0 ) [1];
00223         position[2] = +Tvec.ptr<float> ( 0 ) [2];
00224 
00225         // now calculare orientation quaternion
00226         cv::Mat Rot ( 3,3,CV_32FC1 );
00227         cv::Rodrigues ( Rvec, Rot );
00228 
00229         // calculate axes for quaternion
00230         double stAxes[3][3];
00231         // x axis
00232         stAxes[0][0] = -Rot.at<float> ( 0,0 );
00233         stAxes[0][1] = -Rot.at<float> ( 1,0 );
00234         stAxes[0][2] = +Rot.at<float> ( 2,0 );
00235         // y axis
00236         stAxes[1][0] = -Rot.at<float> ( 0,1 );
00237         stAxes[1][1] = -Rot.at<float> ( 1,1 );
00238         stAxes[1][2] = +Rot.at<float> ( 2,1 );
00239         // for z axis, we use cross product
00240         stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1];
00241         stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0];
00242         stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0];
00243 
00244         // transposed matrix
00245         double axes[3][3];
00246         axes[0][0] = stAxes[0][0];
00247         axes[1][0] = stAxes[0][1];
00248         axes[2][0] = stAxes[0][2];
00249 
00250         axes[0][1] = stAxes[1][0];
00251         axes[1][1] = stAxes[1][1];
00252         axes[2][1] = stAxes[1][2];
00253 
00254         axes[0][2] = stAxes[2][0];
00255         axes[1][2] = stAxes[2][1];
00256         axes[2][2] = stAxes[2][2];
00257 
00258         // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
00259         // article "Quaternion Calculus and Fast Animation".
00260         double fTrace = axes[0][0]+axes[1][1]+axes[2][2];
00261         double fRoot;
00262 
00263         if ( fTrace > 0.0 ) {
00264             // |w| > 1/2, may as well choose w > 1/2
00265             fRoot = sqrt ( fTrace + 1.0 );  // 2w
00266             orientation[0] = 0.5*fRoot;
00267             fRoot = 0.5/fRoot;  // 1/(4w)
00268             orientation[1] = ( axes[2][1]-axes[1][2] ) *fRoot;
00269             orientation[2] = ( axes[0][2]-axes[2][0] ) *fRoot;
00270             orientation[3] = ( axes[1][0]-axes[0][1] ) *fRoot;
00271         } else {
00272             // |w| <= 1/2
00273             static unsigned int s_iNext[3] = { 1, 2, 0 };
00274             unsigned int i = 0;
00275             if ( axes[1][1] > axes[0][0] )
00276                 i = 1;
00277             if ( axes[2][2] > axes[i][i] )
00278                 i = 2;
00279             unsigned int j = s_iNext[i];
00280             unsigned int k = s_iNext[j];
00281 
00282             fRoot = sqrt ( axes[i][i]-axes[j][j]-axes[k][k] + 1.0 );
00283             double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] };
00284             *apkQuat[i] = 0.5*fRoot;
00285             fRoot = 0.5/fRoot;
00286             orientation[0] = ( axes[k][j]-axes[j][k] ) *fRoot;
00287             *apkQuat[j] = ( axes[j][i]+axes[i][j] ) *fRoot;
00288             *apkQuat[k] = ( axes[k][i]+axes[i][k] ) *fRoot;
00289         }
00290 
00291 
00292     }
00293 
00296     void Board::draw ( cv::Mat &im,cv::Scalar color, int lineWidth,bool writeId ) {
00297         for ( size_t i=0; i<size(); i++ ) {
00298             at ( i ).draw ( im,color,lineWidth,writeId );
00299         }
00300     }
00301 
00304     void Board::saveToFile ( string filePath ) throw ( cv::Exception ) {
00305         cv::FileStorage fs ( filePath,cv::FileStorage::WRITE );
00306 
00307         fs<<"aruco_bo_rvec"<< Rvec;
00308         fs<<"aruco_bo_tvec"<< Tvec;
00309         //now, the markers
00310         fs<<"aruco_bo_nmarkers"<< ( int )  size();
00311         fs<<"aruco_bo_markers"<< "[";
00312         for ( size_t i=0; i<size(); i++ ) {
00313             fs << "{:" << "id" << at ( i ).id ;
00314             fs<<"corners"<< "[:";
00315             for ( int c=0; c<at ( i ).size(); c++ )
00316                 fs<<at ( i ) [c];
00317             fs<<"]";
00318             fs <<  "}";
00319         }
00320         fs << "]";
00321         //save configuration file
00322         conf.saveToFile ( fs );
00323 
00324 
00325 
00326 //  readFromFile(fs);
00327 
00328     }
00331     void Board::readFromFile ( string filePath ) throw ( cv::Exception ) {
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 
00379 };


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