aruco_pose: aruco_pose: marker.cpp Source File

00001 #include "aruco/marker.h"
00002 #include <cstdio>
00003 using namespace cv;
00004 namespace aruco {
00008 Marker::Marker()
00009 {
00010     id=-1;
00011     ssize=-1;
00012     Rvec.create(3,1,CV_32FC1);
00013     Tvec.create(3,1,CV_32FC1);
00014     for (int i=0;i<3;i++)
00015         Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999;
00016 }
00020 Marker::Marker(const Marker &M):std::vector<cv::Point2f>(M)
00021 {
00022     M.Rvec.copyTo(Rvec);
00023     M.Tvec.copyTo(Tvec);
00024     id=M.id;
00025     ssize=M.ssize;
00026 }
00030 void Marker::glGetModelViewMatrix(   double modelview_matrix[16])throw(cv::Exception)
00031 {
00032     //check if paremeters are valid
00033     bool invalid=false;
00034     for (int i=0;i<3 && !invalid ;i++)
00035     {
00036         if (Tvec.at<float>(i,0)!=-999999) invalid|=false;
00037         if (Rvec.at<float>(i,0)!=-999999) invalid|=false;
00038     }
00039     if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
00040     Mat Rot(3,3,CV_32FC1),Jacob;
00041     Rodrigues(Rvec, Rot, Jacob);
00042 
00043     double para[3][4];
00044     for (int i=0;i<3;i++)
00045         for (int j=0;j<3;j++) para[i][j]=Rot.at<float>(i,j);
00046     //now, add the translation
00047     para[0][3]=Tvec.at<float>(0,0);
00048     para[1][3]=Tvec.at<float>(1,0);
00049     para[2][3]=Tvec.at<float>(2,0);
00050     double scale=1;
00051 
00052     modelview_matrix[0 + 0*4] = para[0][0];
00053     // R1C2
00054     modelview_matrix[0 + 1*4] = para[0][1];
00055     modelview_matrix[0 + 2*4] = para[0][2];
00056     modelview_matrix[0 + 3*4] = para[0][3];
00057     // R2
00058     modelview_matrix[1 + 0*4] = para[1][0];
00059     modelview_matrix[1 + 1*4] = para[1][1];
00060     modelview_matrix[1 + 2*4] = para[1][2];
00061     modelview_matrix[1 + 3*4] = para[1][3];
00062     // R3
00063     modelview_matrix[2 + 0*4] = -para[2][0];
00064     modelview_matrix[2 + 1*4] = -para[2][1];
00065     modelview_matrix[2 + 2*4] = -para[2][2];
00066     modelview_matrix[2 + 3*4] = -para[2][3];
00067     modelview_matrix[3 + 0*4] = 0.0;
00068     modelview_matrix[3 + 1*4] = 0.0;
00069     modelview_matrix[3 + 2*4] = 0.0;
00070     modelview_matrix[3 + 3*4] = 1.0;
00071     if (scale != 0.0)
00072     {
00073         modelview_matrix[12] *= scale;
00074         modelview_matrix[13] *= scale;
00075         modelview_matrix[14] *= scale;
00076     }
00077 
00078 
00079 }
00080 
00081 
00082 void Marker::draw(Mat &in, Scalar color, int lineWidth ,bool writeId)
00083 {
00084     if (size()!=4) return;
00085     cv::line( in,(*this)[0],(*this)[1],color,lineWidth,CV_AA);
00086     cv::line( in,(*this)[1],(*this)[2],color,lineWidth,CV_AA);
00087     cv::line( in,(*this)[2],(*this)[3],color,lineWidth,CV_AA);
00088     cv::line( in,(*this)[3],(*this)[0],color,lineWidth,CV_AA);
00089     cv::rectangle( in,(*this)[0]-Point2f(2,2),(*this)[0]+Point2f(2,2),Scalar(0,0,255),lineWidth,CV_AA);
00090     cv::rectangle( in,(*this)[1]-Point2f(2,2),(*this)[1]+Point2f(2,2),Scalar(0,255,0),lineWidth,CV_AA);
00091     cv::rectangle( in,(*this)[2]-Point2f(2,2),(*this)[2]+Point2f(2,2),Scalar(255,0,0),lineWidth,CV_AA);
00092     if (writeId) {
00093         char cad[100];
00094         sprintf(cad,"id=%d",id);
00095         //determine the centroid
00096         Point cent(0,0);
00097         for (int i=0;i<4;i++)
00098         {
00099             cent.x+=(*this)[i].x;
00100             cent.y+=(*this)[i].y;
00101         }
00102         cent.x/=4.;
00103         cent.y/=4.;
00104         putText(in,cad, cent,FONT_HERSHEY_SIMPLEX, 0.5,  Scalar(255-color[0],255-color[1],255-color[2]),2);
00105     }
00106 }
00107 
00110 void Marker::calculateExtrinsics(float markerSize,const CameraParameters &CP)throw(cv::Exception)
00111 {
00112     if (!CP.isValid()) throw cv::Exception(9004,"!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
00113     calculateExtrinsics( markerSize,CP.CameraMatrix,CP.Distorsion);
00114 }
00115 
00118 void Marker::calculateExtrinsics(float markerSizeMeters,cv::Mat  camMatrix,cv::Mat distCoeff )throw(cv::Exception)
00119 {
00120     if (!isValid()) throw cv::Exception(9004,"!isValid(): invalid marker. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
00121     if (markerSizeMeters<=0)throw cv::Exception(9004,"markerSize<=0: invalid markerSize","calculateExtrinsics",__FILE__,__LINE__);
00122     if ( camMatrix.rows==0 || camMatrix.cols==0) throw cv::Exception(9004,"CameraMatrix is empty","calculateExtrinsics",__FILE__,__LINE__);
00123 
00124     double halfSize=markerSizeMeters/2.;
00125     CvMat* objPoints=cvCreateMat(4,3,CV_32FC1);
00126     cvSet2D(objPoints,1,0,cvScalar(-halfSize));
00127     cvSet2D(objPoints,1,1,cvScalar(halfSize));
00128     cvSet2D(objPoints,1,2,cvScalar(0));
00129     cvSet2D(objPoints,2,0,cvScalar(halfSize));
00130     cvSet2D(objPoints,2,1,cvScalar(halfSize));
00131     cvSet2D(objPoints,2,2,cvScalar(0));
00132     cvSet2D(objPoints,3,0,cvScalar(halfSize));
00133     cvSet2D(objPoints,3,1,cvScalar(-halfSize));
00134     cvSet2D(objPoints,3,2,cvScalar(0));
00135     cvSet2D(objPoints,0,0,cvScalar(-halfSize));
00136     cvSet2D(objPoints,0,1,cvScalar(-halfSize));
00137     cvSet2D(objPoints,0,2,cvScalar(0));
00138 
00139     CvMat *imagePoints=cvCreateMat(4,2,CV_32FC1);
00140     CvMat cvCamMatrix=camMatrix;
00141     CvMat cvDistCoeffs;
00142     Mat zeros=Mat::zeros(4,1,CV_32FC1);
00143     if (distCoeff.rows>=4)  cvDistCoeffs=distCoeff;
00144     else  cvDistCoeffs=zeros;
00145 
00146     //Set image points from the marker
00147     for (int c=0;c<4;c++)
00148     {
00149         cvSet2D( imagePoints,c,0,cvScalar((*this)[c%4].x));
00150         cvSet2D( imagePoints,c,1,cvScalar((*this)[c%4].y));
00151     }
00152     CvMat cvRvec=Rvec;
00153     CvMat cvTvec=Tvec;
00154     cvFindExtrinsicCameraParams2(objPoints, imagePoints, &cvCamMatrix, &cvDistCoeffs,&cvRvec,&cvTvec);
00155     //rotate the X axis so that Y is perpendicular to the marker plane
00156     rotateXAxis(Rvec);
00157     ssize=markerSizeMeters;
00158 
00159     cvReleaseMat(&objPoints);
00160     cvReleaseMat(&imagePoints);
00161 }
00162 
00163 
00167 void Marker::rotateXAxis(Mat &rotation)
00168 {
00169     cv::Mat R(3,3,CV_32FC1);
00170     Rodrigues(rotation, R);
00171     //create a rotation matrix for x axis
00172     cv::Mat RX=cv::Mat::eye(3,3,CV_32FC1);
00173     float angleRad=M_PI/2;
00174     RX.at<float>(1,1)=cos(angleRad);
00175     RX.at<float>(1,2)=-sin(angleRad);
00176     RX.at<float>(2,1)=sin(angleRad);
00177     RX.at<float>(2,2)=cos(angleRad);
00178     //now multiply
00179     R=R*RX;
00180     //finally, the the rodrigues back
00181     Rodrigues(R,rotation);
00182 }
00183 
00186 Mat Marker::createMarkerImage(int id,int size) throw (cv::Exception)
00187 {
00188     if (id>=1024) throw cv::Exception(9004,"id>=1024","createMarker",__FILE__,__LINE__);
00189     Mat marker(size,size, CV_8UC1);
00190     marker.setTo(Scalar(0));
00191     //for each line, create
00192     int swidth=size/7;
00193     int ids[4]={0x10,0x17,0x09,0x0e};
00194     for (int y=0;y<5;y++) {
00195         int index=(id>>2*(4-y)) & 0x0003;
00196         int val=ids[index];
00197         for (int x=0;x<5;x++) {
00198             Mat roi=marker(Rect((x+1)* swidth,(y+1)* swidth,swidth,swidth));
00199             if ( ( val>>(4-x) ) & 0x0001 ) roi.setTo(Scalar(255));
00200             else roi.setTo(Scalar(0));
00201         }
00202     }
00203     return marker;
00204 }
00205 }