boarddetector.cpp
Go to the documentation of this file.
00001 #include "aruco/boarddetector.h"
00002 #include <cstdlib>
00003 #include <ctime>
00004 #include <cassert>
00005 #include <fstream>
00006 using namespace std;
00007 namespace aruco
00008 {
00013 float BoardDetector::detect(const vector<Marker> &detectedMarkers,const  BoardConfiguration &BConf, Board &Bdetected, CameraParameters cp, float markerSizeMeters  )throw (cv::Exception)
00014 {
00015     return detect(detectedMarkers, BConf,Bdetected,cp.CameraMatrix,cp.Distorsion,markerSizeMeters);
00016 }
00021 float BoardDetector::detect(const vector<Marker> &detectedMarkers,const  BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters)throw (cv::Exception)
00022 {
00023 // cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
00025     Mat detected(BConf._markersId.size(),CV_32SC1); //stores the indices of the makers
00026     detected.setTo(Scalar(-1));//-1 mean not detected
00027     int nMarkInBoard=0;//total number of markers detected
00028     for (unsigned int i=0;i<detectedMarkers.size();i++) {
00029         bool found=false;
00030         int id=detectedMarkers[i].id;
00031         //find it
00032         for (  int j=0;j<detected.size().height && ! found;j++)
00033             for (  int k=0;k<detected.size().width && ! found;k++)
00034                 if ( BConf._markersId.at<int>(j,k)==id) {
00035                     detected.at<int>(j,k)=i;
00036                     nMarkInBoard++;
00037                     found=true;
00038                     Bdetected.push_back(detectedMarkers[i]);
00039                     if (markerSizeMeters>0)
00040                         Bdetected.back().ssize=markerSizeMeters;
00041                 }
00042     }
00043     Bdetected.conf=BConf;
00044     if (markerSizeMeters!=-1)
00045         Bdetected.markerSizeMeters=markerSizeMeters;
00046 //calculate extrinsic if there is information for that
00047     if (camMatrix.rows!=0 && markerSizeMeters>0 && detectedMarkers.size()>1) {
00048         // now, create the matrices for finding the extrinsics
00049         Mat objPoints(4*nMarkInBoard,3,CV_32FC1);
00050         Mat imagePoints(4*nMarkInBoard,2,CV_32FC1);
00051         //size in meters of inter-marker distance
00052         double markerDistanceMeters= double(BConf._markerDistancePix) * markerSizeMeters / double(BConf._markerSizePix);
00053 
00054 
00055 
00056         int currIndex=0;
00057         for (  int y=0;y<detected.size().height;y++)
00058             for (  int x=0;x<detected.size().width;x++) {
00059                 if (  detected.at<int>(y,x)!=-1 ) {
00060 
00061                     vector<Point2f> points =detectedMarkers[ detected.at<int>(y,x) ];
00062                     //set first image points
00063                     for (int p=0;p<4;p++) {
00064                         imagePoints.at<float>(currIndex+p,0)=points[p].x;
00065                         imagePoints.at<float>(currIndex+p,1)=points[p].y;
00066                     }
00067 
00068                     //tranaltion to make the Ref System be in center
00069                     float TX=-(  ((detected.size().height-1)*(markerDistanceMeters+markerSizeMeters) +markerSizeMeters) /2) ;
00070                     float TY=-(  ((detected.size().width-1)*(markerDistanceMeters+markerSizeMeters) +markerSizeMeters)/2);
00071                     //points in real refernce system. We se the center in the bottom-left corner
00072                     float AY=x*(markerDistanceMeters+markerSizeMeters ) +TY;
00073                     float AX=y*(markerDistanceMeters+markerSizeMeters )+TX;
00074                     objPoints.at<float>( currIndex,0)= AX;
00075                     objPoints.at<float>( currIndex,1)= AY;
00076                     objPoints.at<float>( currIndex,2)= 0;
00077                     objPoints.at<float>( currIndex+1,0)= AX;
00078                     objPoints.at<float>( currIndex+1,1)= AY+markerSizeMeters;
00079                     objPoints.at<float>( currIndex+1,2)= 0;
00080                     objPoints.at<float>( currIndex+2,0)= AX+markerSizeMeters;
00081                     objPoints.at<float>( currIndex+2,1)= AY+markerSizeMeters;
00082                     objPoints.at<float>( currIndex+2,2)= 0;
00083                     objPoints.at<float>( currIndex+3,0)= AX+markerSizeMeters;
00084                     objPoints.at<float>( currIndex+3,1)= AY;
00085                     objPoints.at<float>( currIndex+3,2)= 0;
00086                     currIndex+=4;
00087                 }
00088             }
00089 
00090         CvMat cvCamMatrix=camMatrix;
00091         CvMat cvDistCoeffs;
00092         Mat zeros=Mat::zeros(4,1,CV_32FC1);
00093         if (distCoeff.rows>=4)  cvDistCoeffs=distCoeff;
00094         else  cvDistCoeffs=zeros;
00095         CvMat cvImgPoints=imagePoints;
00096         CvMat cvObjPoints=objPoints;
00097 
00098         CvMat cvRvec=Bdetected.Rvec;
00099         CvMat cvTvec=Bdetected.Tvec;
00100         cvFindExtrinsicCameraParams2(&cvObjPoints, &cvImgPoints, &cvCamMatrix, &cvDistCoeffs,&cvRvec,&cvTvec);
00101         //now, rotate 90 deg in X so that Y axis points up
00102         rotateXAxis(Bdetected.Rvec);
00103         //cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<Bdetected.Rvec.at<float>(2,0)<<endl;
00104         //cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<Bdetected.Tvec.at<float>(2,0)<<endl;
00105     }
00106     return double(nMarkInBoard)/double( BConf._markersId.size().width*BConf._markersId.size().height);
00107 }
00108 
00109 void BoardDetector::rotateXAxis(Mat &rotation)
00110 {
00111     cv::Mat R(3,3,CV_32FC1);
00112     Rodrigues(rotation, R);
00113     //create a rotation matrix for x axis
00114     cv::Mat RX=cv::Mat::eye(3,3,CV_32FC1);
00115     float angleRad=M_PI/2;
00116     RX.at<float>(1,1)=cos(angleRad);
00117     RX.at<float>(1,2)=-sin(angleRad);
00118     RX.at<float>(2,1)=sin(angleRad);
00119     RX.at<float>(2,2)=cos(angleRad);
00120     //now multiply
00121     R=R*RX;
00122     //finally, the the rodrigues back
00123     Rodrigues(R,rotation);
00124 
00125 }
00126 
00127 
00128 void Board::glGetModelViewMatrix(   double modelview_matrix[16])throw(cv::Exception)
00129 {
00130     //check if paremeters are valid
00131     bool invalid=false;
00132     for (int i=0;i<3 && !invalid ;i++)
00133     {
00134         if (Tvec.at<float>(i,0)!=-999999) invalid|=false;
00135         if (Rvec.at<float>(i,0)!=-999999) invalid|=false;
00136     }
00137     if (invalid) throw cv::Exception(9002,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
00138     Mat Rot(3,3,CV_32FC1),Jacob;
00139     Rodrigues(Rvec, Rot, Jacob);
00140 
00141     double para[3][4];
00142     for (int i=0;i<3;i++)
00143         for (int j=0;j<3;j++) para[i][j]=Rot.at<float>(i,j);
00144     //now, add the translation
00145     para[0][3]=Tvec.at<float>(0,0);
00146     para[1][3]=Tvec.at<float>(1,0);
00147     para[2][3]=Tvec.at<float>(2,0);
00148     double scale=1;
00149 
00150     modelview_matrix[0 + 0*4] = para[0][0];
00151     // R1C2
00152     modelview_matrix[0 + 1*4] = para[0][1];
00153     modelview_matrix[0 + 2*4] = para[0][2];
00154     modelview_matrix[0 + 3*4] = para[0][3];
00155     // R2
00156     modelview_matrix[1 + 0*4] = para[1][0];
00157     modelview_matrix[1 + 1*4] = para[1][1];
00158     modelview_matrix[1 + 2*4] = para[1][2];
00159     modelview_matrix[1 + 3*4] = para[1][3];
00160     // R3
00161     modelview_matrix[2 + 0*4] = -para[2][0];
00162     modelview_matrix[2 + 1*4] = -para[2][1];
00163     modelview_matrix[2 + 2*4] = -para[2][2];
00164     modelview_matrix[2 + 3*4] = -para[2][3];
00165     modelview_matrix[3 + 0*4] = 0.0;
00166     modelview_matrix[3 + 1*4] = 0.0;
00167     modelview_matrix[3 + 2*4] = 0.0;
00168     modelview_matrix[3 + 3*4] = 1.0;
00169     if (scale != 0.0)
00170     {
00171         modelview_matrix[12] *= scale;
00172         modelview_matrix[13] *= scale;
00173         modelview_matrix[14] *= scale;
00174     }
00175 
00176 
00177 }
00178 
00179 };
00180 


aruco_pose
Author(s): Julian Brunner
autogenerated on Mon Oct 6 2014 08:32:33