boarddetector.cpp
Go to the documentation of this file.
00001 #pragma GCC diagnostic warning "-Wuninitialized"
00002 /*****************************
00003 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without modification, are
00006 permitted provided that the following conditions are met:
00007 
00008    1. Redistributions of source code must retain the above copyright notice, this list of
00009       conditions and the following disclaimer.
00010 
00011    2. Redistributions in binary form must reproduce the above copyright notice, this list
00012       of conditions and the following disclaimer in the documentation and/or other materials
00013       provided with the distribution.
00014 
00015 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00016 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00017 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00018 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00019 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00020 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00021 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00022 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00023 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00024 
00025 The views and conclusions contained in the software and documentation are those of the
00026 authors and should not be interpreted as representing official policies, either expressed
00027 or implied, of Rafael Muñoz Salinas.
00028 ********************************/
00029 #include <aruco/boarddetector.h>
00030 #define _USE_MATH_DEFINES
00031 #include <math.h>
00032 #include <cstdlib>
00033 #include <ctime>
00034 #include <cassert>
00035 #include <fstream>
00036 using namespace std;
00037 using namespace cv;
00038 namespace aruco
00039 {
00040 
00041   BoardDetector::BoardDetector(bool setYPerpendicular)
00042     : _setYPerpendicular(setYPerpendicular),
00043       _areParamsSet(false),
00044       _markerSize(0)
00045   {}
00049   void BoardDetector::setParams(const BoardConfiguration &bc,const CameraParameters &cp, float markerSizeMeters)
00050   {
00051     _camParams = cp;
00052     _markerSize = markerSizeMeters;
00053     _bconf = bc;
00054     _areParamsSet = true;
00055   }
00056 
00057   void BoardDetector::setParams(const BoardConfiguration &bc)
00058   {
00059     _bconf = bc;
00060     _areParamsSet = true;
00061   }
00062 
00063 
00064   float  BoardDetector::detect(const cv::Mat &im)throw (cv::Exception)
00065   {
00066     _mdetector.detect(im,_vmarkers);
00067 
00068     float res;
00069 
00070     if (_camParams.isValid())
00071       res=detect(_vmarkers,_bconf,_boardDetected,_camParams.CameraMatrix,_camParams.Distorsion,_markerSize);
00072     else res=detect(_vmarkers,_bconf,_boardDetected);
00073     return res;
00074   }
00075 
00076   float BoardDetector::detect ( const vector<Marker> &detectedMarkers,const  BoardConfiguration &BConf, Board &Bdetected,const CameraParameters &cp, float markerSizeMeters ) throw ( cv::Exception )
00077   {
00078     return detect ( detectedMarkers, BConf,Bdetected,cp.CameraMatrix,cp.Distorsion,markerSizeMeters );
00079   }
00080 
00081   float BoardDetector::detect ( const vector<Marker> &detectedMarkers,const  BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters ) throw ( cv::Exception )
00082   {
00083     if (BConf.size()==0) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty",__FILE__,__LINE__);
00084     if (BConf[0].size()<2) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty 2",__FILE__,__LINE__);
00085     //compute the size of the markers in meters, which is used for some routines(mostly drawing)
00086     float ssize;
00087     if ( BConf.mInfoType==BoardConfiguration::PIX && markerSizeMeters>0 ) ssize=markerSizeMeters;
00088     else if ( BConf.mInfoType==BoardConfiguration::METERS )
00089     {
00090       ssize=cv::norm ( BConf[0][0]-BConf[0][1] );
00091     }
00092 
00093     // cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
00094     Bdetected.clear();
00096     for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00097     {
00098       int idx=BConf.getIndexOfMarkerId(detectedMarkers[i].id);
00099       if (idx!=-1) {
00100         Bdetected.push_back ( detectedMarkers[i] );
00101         Bdetected.back().ssize=ssize;
00102       }
00103     }
00104     //copy configuration
00105     Bdetected.conf=BConf;
00106     //
00107 
00108     bool hasEnoughInfoForRTvecCalculation=false;
00109     if ( Bdetected.size() >=1 )
00110     {
00111       if ( camMatrix.rows!=0 )
00112       {
00113         if ( markerSizeMeters>0 && BConf.mInfoType==BoardConfiguration::PIX ) hasEnoughInfoForRTvecCalculation=true;
00114         else if ( BConf.mInfoType==BoardConfiguration::METERS ) hasEnoughInfoForRTvecCalculation=true;
00115       }
00116     }
00117 
00118     //calculate extrinsic if there is information for that
00119     if ( hasEnoughInfoForRTvecCalculation )
00120     {
00121       //calculate the size of the markers in meters if expressed in pixels
00122       double marker_meter_per_pix=0;
00123       if ( BConf.mInfoType==BoardConfiguration::PIX ) marker_meter_per_pix=markerSizeMeters /  cv::norm ( BConf[0][0]-BConf[0][1] );
00124       else marker_meter_per_pix=1;//to avoind interferring the process below
00125 
00126       // now, create the matrices for finding the extrinsics
00127       Mat objPoints ( 4*Bdetected.size(),3,CV_32FC1 );
00128       Mat imagePoints ( 4*Bdetected.size(),2,CV_32FC1 );
00129       //size in meters of inter-marker distance
00130 
00131       for ( size_t i=0;i<Bdetected.size();i++ )
00132       {
00133         int idx=Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
00134         assert(idx!=-1);
00135         for ( int p=0;p<4;p++ )
00136         {
00137           imagePoints.at<float> ( ( i*4 ) +p,0 ) =Bdetected[i][p].x;
00138           imagePoints.at<float> ( ( i*4 ) +p,1 ) =Bdetected[i][p].y;
00139           const aruco::MarkerInfo &Minfo=Bdetected.conf.getMarkerInfo( Bdetected[i].id);
00140 
00141           objPoints.at<float> ( ( i*4 ) +p,0 ) = Minfo[p].x*marker_meter_per_pix;
00142           objPoints.at<float> ( ( i*4 ) +p,1 ) = Minfo[p].y*marker_meter_per_pix;
00143           objPoints.at<float> ( ( i*4 ) +p,2 ) = Minfo[p].z*marker_meter_per_pix;
00144           //            cout<<objPoints.at<float>( (i*4)+p,0)<<" "<<objPoints.at<float>( (i*4)+p,1)<<" "<<objPoints.at<float>( (i*4)+p,2)<<endl;
00145         }
00146       }
00147       if (distCoeff.total()==0) distCoeff=cv::Mat::zeros(1,4,CV_32FC1 );
00148 
00149       cv::Mat rvec,tvec;
00150       cv::solvePnP(objPoints,imagePoints,camMatrix,distCoeff,rvec,tvec );
00151       rvec.convertTo(Bdetected.Rvec,CV_32FC1);
00152       tvec.convertTo(Bdetected.Tvec,CV_32FC1);
00153       //now, rotate 90 deg in X so that Y axis points up
00154       if (_setYPerpendicular)
00155         rotateXAxis ( Bdetected.Rvec );
00156       //         cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
00157       //         cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
00158     }
00159 
00160     float prob=float( Bdetected.size() ) /double ( Bdetected.conf.size() );
00161     return prob;
00162   }
00163 
00164   void BoardDetector::rotateXAxis ( Mat &rotation )
00165   {
00166     cv::Mat R ( 3,3,CV_32FC1 );
00167     Rodrigues ( rotation, R );
00168     //create a rotation matrix for x axis
00169     cv::Mat RX=cv::Mat::eye ( 3,3,CV_32FC1 );
00170     float angleRad=-M_PI/2;
00171     RX.at<float> ( 1,1 ) =cos ( angleRad );
00172     RX.at<float> ( 1,2 ) =-sin ( angleRad );
00173     RX.at<float> ( 2,1 ) =sin ( angleRad );
00174     RX.at<float> ( 2,2 ) =cos ( angleRad );
00175     //now multiply
00176     R=R*RX;
00177     //finally, the the rodrigues back
00178     Rodrigues ( R,rotation );
00179 
00180   }
00181 
00182 }
00183 


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