marker.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/marker.h>
00029 #define _USE_MATH_DEFINES
00030 #include <math.h>
00031 #include <cstdio>
00032 
00033 using namespace cv;
00034 namespace aruco {
00038   Marker::Marker()
00039   {
00040     id=-1;
00041     ssize=-1;
00042     Rvec.create(3,1,CV_32FC1);
00043     Tvec.create(3,1,CV_32FC1);
00044     for (int i=0;i<3;i++)
00045       Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999;
00046   }
00050   Marker::Marker(const Marker &M):std::vector<cv::Point2f>(M)
00051   {
00052     M.Rvec.copyTo(Rvec);
00053     M.Tvec.copyTo(Tvec);
00054     id=M.id;
00055     ssize=M.ssize;
00056   }
00057 
00061   Marker::Marker(const  std::vector<cv::Point2f> &corners,int _id):std::vector<cv::Point2f>(corners)
00062   {
00063     id=_id;
00064     ssize=-1;
00065     Rvec.create(3,1,CV_32FC1);
00066     Tvec.create(3,1,CV_32FC1);
00067     for (int i=0;i<3;i++)
00068       Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999;
00069   }
00070 
00074   void Marker::glGetModelViewMatrix(   double modelview_matrix[16])throw(cv::Exception)
00075   {
00076     //check if paremeters are valid
00077     bool invalid=false;
00078     for (int i=0;i<3 && !invalid ;i++)
00079     {
00080       if (Tvec.at<float>(i,0)!=-999999) invalid|=false;
00081       if (Rvec.at<float>(i,0)!=-999999) invalid|=false;
00082     }
00083     if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
00084     Mat Rot(3,3,CV_32FC1),Jacob;
00085     Rodrigues(Rvec, Rot, Jacob);
00086 
00087     double para[3][4];
00088     for (int i=0;i<3;i++)
00089       for (int j=0;j<3;j++) para[i][j]=Rot.at<float>(i,j);
00090     //now, add the translation
00091     para[0][3]=Tvec.at<float>(0,0);
00092     para[1][3]=Tvec.at<float>(1,0);
00093     para[2][3]=Tvec.at<float>(2,0);
00094     double scale=1;
00095 
00096     modelview_matrix[0 + 0*4] = para[0][0];
00097     // R1C2
00098     modelview_matrix[0 + 1*4] = para[0][1];
00099     modelview_matrix[0 + 2*4] = para[0][2];
00100     modelview_matrix[0 + 3*4] = para[0][3];
00101     // R2
00102     modelview_matrix[1 + 0*4] = para[1][0];
00103     modelview_matrix[1 + 1*4] = para[1][1];
00104     modelview_matrix[1 + 2*4] = para[1][2];
00105     modelview_matrix[1 + 3*4] = para[1][3];
00106     // R3
00107     modelview_matrix[2 + 0*4] = -para[2][0];
00108     modelview_matrix[2 + 1*4] = -para[2][1];
00109     modelview_matrix[2 + 2*4] = -para[2][2];
00110     modelview_matrix[2 + 3*4] = -para[2][3];
00111     modelview_matrix[3 + 0*4] = 0.0;
00112     modelview_matrix[3 + 1*4] = 0.0;
00113     modelview_matrix[3 + 2*4] = 0.0;
00114     modelview_matrix[3 + 3*4] = 1.0;
00115     if (scale != 0.0)
00116     {
00117       modelview_matrix[12] *= scale;
00118       modelview_matrix[13] *= scale;
00119       modelview_matrix[14] *= scale;
00120     }
00121   }
00122 
00123 
00124 
00125   /****
00126  *
00127  */
00128   void Marker::OgreGetPoseParameters(double position[3], double orientation[4]) 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(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
00138     
00139     // calculate position vector
00140     position[0] = Tvec.ptr<float>(0)[0];
00141     position[1] = Tvec.ptr<float>(0)[1];
00142     position[2] = +Tvec.ptr<float>(0)[2];
00143     
00144     // now calculare orientation quaternion
00145     cv::Mat Rot(3,3,CV_32FC1);
00146     cv::Rodrigues(Rvec, Rot);
00147     
00148     // calculate axes for quaternion
00149     double stAxes[3][3];
00150     // x axis
00151     stAxes[0][0] = -Rot.at<float>(0,0);
00152     stAxes[0][1] = -Rot.at<float>(1,0);
00153     stAxes[0][2] = +Rot.at<float>(2,0);
00154     // y axis
00155     stAxes[1][0] = -Rot.at<float>(0,1);
00156     stAxes[1][1] = -Rot.at<float>(1,1);
00157     stAxes[1][2] = +Rot.at<float>(2,1);
00158     // for z axis, we use cross product
00159     stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1];
00160     stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0];
00161     stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0];
00162     
00163     // transposed matrix
00164     double axes[3][3];
00165     axes[0][0] = stAxes[0][0];
00166     axes[1][0] = stAxes[0][1];
00167     axes[2][0] = stAxes[0][2];
00168     
00169     axes[0][1] = stAxes[1][0];
00170     axes[1][1] = stAxes[1][1];
00171     axes[2][1] = stAxes[1][2];
00172     
00173     axes[0][2] = stAxes[2][0];
00174     axes[1][2] = stAxes[2][1];
00175     axes[2][2] = stAxes[2][2];
00176     
00177     // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
00178     // article "Quaternion Calculus and Fast Animation".
00179     double fTrace = axes[0][0]+axes[1][1]+axes[2][2];
00180     double fRoot;
00181 
00182     if ( fTrace > 0.0 )
00183     {
00184       // |w| > 1/2, may as well choose w > 1/2
00185       fRoot = sqrt(fTrace + 1.0);  // 2w
00186       orientation[0] = 0.5*fRoot;
00187       fRoot = 0.5/fRoot;  // 1/(4w)
00188       orientation[1] = (axes[2][1]-axes[1][2])*fRoot;
00189       orientation[2] = (axes[0][2]-axes[2][0])*fRoot;
00190       orientation[3] = (axes[1][0]-axes[0][1])*fRoot;
00191     }
00192     else
00193     {
00194       // |w| <= 1/2
00195       static unsigned int s_iNext[3] = { 1, 2, 0 };
00196       unsigned int i = 0;
00197       if ( axes[1][1] > axes[0][0] )
00198         i = 1;
00199       if ( axes[2][2] > axes[i][i] )
00200         i = 2;
00201       unsigned int j = s_iNext[i];
00202       unsigned int k = s_iNext[j];
00203 
00204       fRoot = sqrt(axes[i][i]-axes[j][j]-axes[k][k] + 1.0);
00205       double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] };
00206       *apkQuat[i] = 0.5*fRoot;
00207       fRoot = 0.5/fRoot;
00208       orientation[0] = (axes[k][j]-axes[j][k])*fRoot;
00209       *apkQuat[j] = (axes[j][i]+axes[i][j])*fRoot;
00210       *apkQuat[k] = (axes[k][i]+axes[i][k])*fRoot;
00211     }
00212   }
00213 
00214 
00215 
00216   void Marker::draw(Mat &in, Scalar color, int lineWidth ,bool writeId)const
00217   {
00218     if (size()!=4) return;
00219     cv::line( in,(*this)[0],(*this)[1],color,lineWidth,CV_AA);
00220     cv::line( in,(*this)[1],(*this)[2],color,lineWidth,CV_AA);
00221     cv::line( in,(*this)[2],(*this)[3],color,lineWidth,CV_AA);
00222     cv::line( in,(*this)[3],(*this)[0],color,lineWidth,CV_AA);
00223     cv::rectangle( in,(*this)[0]-Point2f(2,2),(*this)[0]+Point2f(2,2),Scalar(0,0,255,255),lineWidth,CV_AA);
00224     cv::rectangle( in,(*this)[1]-Point2f(2,2),(*this)[1]+Point2f(2,2),Scalar(0,255,0,255),lineWidth,CV_AA);
00225     cv::rectangle( in,(*this)[2]-Point2f(2,2),(*this)[2]+Point2f(2,2),Scalar(255,0,0,255),lineWidth,CV_AA);
00226     if (writeId) {
00227       char cad[100];
00228       sprintf(cad,"id=%d",id);
00229       //determine the centroid
00230       Point cent(0,0);
00231       for (int i=0;i<4;i++)
00232       {
00233         cent.x+=(*this)[i].x;
00234         cent.y+=(*this)[i].y;
00235       }
00236       cent.x/=4.;
00237       cent.y/=4.;
00238       putText(in,cad, cent,FONT_HERSHEY_SIMPLEX, 0.5,  Scalar(255-color[0],255-color[1],255-color[2],255),2);
00239     }
00240   }
00241 
00242   void Marker::calculateExtrinsics(float markerSize,const CameraParameters &CP,bool setYPerpendicular)throw(cv::Exception)
00243   {
00244     if (!CP.isValid()) throw cv::Exception(9004,"!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
00245     calculateExtrinsics( markerSize,CP.CameraMatrix,CP.Distorsion,setYPerpendicular);
00246   }
00247 
00248   void print(cv::Point3f p,string cad)
00249   {
00250     cout<<cad<<" "<<p.x<<" "<<p.y<< " "<<p.z<<endl;
00251   }
00252 
00253   void Marker::calculateExtrinsics(float markerSizeMeters, cv::Mat  camMatrix, cv::Mat distCoeff , bool setYPerpendicular)throw(cv::Exception)
00254   {
00255     if (!isValid()) throw cv::Exception(9004,"!isValid(): invalid marker. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
00256     if (markerSizeMeters<=0)throw cv::Exception(9004,"markerSize<=0: invalid markerSize","calculateExtrinsics",__FILE__,__LINE__);
00257     if ( camMatrix.rows==0 || camMatrix.cols==0) throw cv::Exception(9004,"CameraMatrix is empty","calculateExtrinsics",__FILE__,__LINE__);
00258 
00259     double halfSize=markerSizeMeters/2.;
00260     cv::Mat ObjPoints(4,3,CV_32FC1);
00261     ObjPoints.at<float>(1,0)=-halfSize;
00262     ObjPoints.at<float>(1,1)=halfSize;
00263     ObjPoints.at<float>(1,2)=0;
00264     ObjPoints.at<float>(2,0)=halfSize;
00265     ObjPoints.at<float>(2,1)=halfSize;
00266     ObjPoints.at<float>(2,2)=0;
00267     ObjPoints.at<float>(3,0)=halfSize;
00268     ObjPoints.at<float>(3,1)=-halfSize;
00269     ObjPoints.at<float>(3,2)=0;
00270     ObjPoints.at<float>(0,0)=-halfSize;
00271     ObjPoints.at<float>(0,1)=-halfSize;
00272     ObjPoints.at<float>(0,2)=0;
00273 
00274     cv::Mat ImagePoints(4,2,CV_32FC1);
00275 
00276     //Set image points from the marker
00277     for (int c=0;c<4;c++)
00278     {
00279       ImagePoints.at<float>(c,0)=((*this)[c%4].x);
00280       ImagePoints.at<float>(c,1)=((*this)[c%4].y);
00281     }
00282     
00283     cv::Mat raux,taux;
00284     cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux);
00285     raux.convertTo(Rvec,CV_32F);
00286     taux.convertTo(Tvec ,CV_32F);
00287     //rotate the X axis so that Y is perpendicular to the marker plane
00288     if (setYPerpendicular) rotateXAxis(Rvec);
00289     ssize=markerSizeMeters;
00290   }
00291 
00292   void Marker::rotateXAxis(Mat &rotation)
00293   {
00294     cv::Mat R(3,3,CV_32F);
00295     cv::Rodrigues(rotation, R);
00296     //create a rotation matrix for x axis
00297     cv::Mat RX=cv::Mat::eye(3,3,CV_32F);
00298     float angleRad=M_PI/2;
00299     RX.at<float>(1,1)=cos(angleRad);
00300     RX.at<float>(1,2)=-sin(angleRad);
00301     RX.at<float>(2,1)=sin(angleRad);
00302     RX.at<float>(2,2)=cos(angleRad);
00303     //now multiply
00304     R=R*RX;
00305     //finally, the the rodrigues back
00306     cv::Rodrigues(R,rotation);
00307   }
00308 
00309   cv::Point2f Marker::getCenter()const
00310   {
00311     cv::Point2f cent(0,0);
00312     for(size_t i=0;i<size();i++){
00313       cent.x+=(*this)[i].x;
00314       cent.y+=(*this)[i].y;
00315     }
00316     cent.x/=float(size());
00317     cent.y/=float(size());
00318     return cent;
00319   }
00320 
00321   float Marker::getArea()const
00322   {
00323     assert(size()==4);
00324     //use the cross products
00325     cv::Point2f v01=(*this)[1]-(*this)[0];
00326     cv::Point2f v03=(*this)[3]-(*this)[0];
00327     float area1=fabs(v01.x*v03.y - v01.y*v03.x);
00328     cv::Point2f v21=(*this)[1]-(*this)[2];
00329     cv::Point2f v23=(*this)[3]-(*this)[2];
00330     float area2=fabs(v21.x*v23.y - v21.y*v23.x);
00331     return (area2+area1)/2.;
00332   }
00333 
00334   float Marker::getPerimeter()const
00335   {
00336     assert(size()==4);
00337     float sum=0;
00338     for(int i=0;i<4;i++)
00339       sum+=norm( (*this)[i]-(*this)[(i+1)%4]);
00340     return sum;
00341   }
00342 
00343 }


aruco
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Thu Jun 6 2019 17:52:15