00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "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):
00051 std::vector<cv::Point2f>(M)
00052 {
00053 M.Rvec.copyTo(Rvec);
00054 M.Tvec.copyTo(Tvec);
00055 id=M.id;
00056 ssize=M.ssize;
00057 }
00058
00062 Marker::Marker(const std::vector<cv::Point2f> &corners,int _id):
00063 std::vector<cv::Point2f>(corners)
00064 {
00065 id=_id;
00066 ssize=-1;
00067 Rvec.create(3,1,CV_32FC1);
00068 Tvec.create(3,1,CV_32FC1);
00069 for (int i=0;i<3;i++)
00070 Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999;
00071 }
00072
00073
00077 Marker::Marker(const std::vector<cv::Point2f> &corners,int _id, float _ssize):
00078 std::vector<cv::Point2f>(corners)
00079 {
00080 id=_id;
00081 ssize=_ssize;
00082 Rvec.create(3,1,CV_32FC1);
00083 Tvec.create(3,1,CV_32FC1);
00084 for (int i=0;i<3;i++)
00085 Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999;
00086 }
00087
00088
00092 void Marker::glGetModelViewMatrix( double modelview_matrix[16])throw(cv::Exception)
00093 {
00094
00095 bool invalid=false;
00096 for (int i=0;i<3 && !invalid ;i++)
00097 {
00098 if (Tvec.at<float>(i,0)!=-999999) invalid|=false;
00099 if (Rvec.at<float>(i,0)!=-999999) invalid|=false;
00100 }
00101 if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
00102 Mat Rot(3,3,CV_32FC1),Jacob;
00103 Rodrigues(Rvec, Rot, Jacob);
00104
00105 double para[3][4];
00106 for (int i=0;i<3;i++)
00107 for (int j=0;j<3;j++) para[i][j]=Rot.at<float>(i,j);
00108
00109 para[0][3]=Tvec.at<float>(0,0);
00110 para[1][3]=Tvec.at<float>(1,0);
00111 para[2][3]=Tvec.at<float>(2,0);
00112 double scale=1;
00113
00114 modelview_matrix[0 + 0*4] = para[0][0];
00115
00116 modelview_matrix[0 + 1*4] = para[0][1];
00117 modelview_matrix[0 + 2*4] = para[0][2];
00118 modelview_matrix[0 + 3*4] = para[0][3];
00119
00120 modelview_matrix[1 + 0*4] = para[1][0];
00121 modelview_matrix[1 + 1*4] = para[1][1];
00122 modelview_matrix[1 + 2*4] = para[1][2];
00123 modelview_matrix[1 + 3*4] = para[1][3];
00124
00125 modelview_matrix[2 + 0*4] = -para[2][0];
00126 modelview_matrix[2 + 1*4] = -para[2][1];
00127 modelview_matrix[2 + 2*4] = -para[2][2];
00128 modelview_matrix[2 + 3*4] = -para[2][3];
00129 modelview_matrix[3 + 0*4] = 0.0;
00130 modelview_matrix[3 + 1*4] = 0.0;
00131 modelview_matrix[3 + 2*4] = 0.0;
00132 modelview_matrix[3 + 3*4] = 1.0;
00133 if (scale != 0.0)
00134 {
00135 modelview_matrix[12] *= scale;
00136 modelview_matrix[13] *= scale;
00137 modelview_matrix[14] *= scale;
00138 }
00139
00140
00141 }
00142
00143
00144
00145
00146
00147
00148 void Marker::OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception)
00149 {
00150
00151
00152 bool invalid=false;
00153 for (int i=0;i<3 && !invalid ;i++)
00154 {
00155 if (Tvec.at<float>(i,0)!=-999999) invalid|=false;
00156 if (Rvec.at<float>(i,0)!=-999999) invalid|=false;
00157 }
00158 if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
00159
00160
00161 position[0] = -Tvec.ptr<float>(0)[0];
00162 position[1] = -Tvec.ptr<float>(0)[1];
00163 position[2] = +Tvec.ptr<float>(0)[2];
00164
00165
00166 cv::Mat Rot(3,3,CV_32FC1);
00167 cv::Rodrigues(Rvec, Rot);
00168
00169
00170 double stAxes[3][3];
00171
00172 stAxes[0][0] = -Rot.at<float>(0,0);
00173 stAxes[0][1] = -Rot.at<float>(1,0);
00174 stAxes[0][2] = +Rot.at<float>(2,0);
00175
00176 stAxes[1][0] = -Rot.at<float>(0,1);
00177 stAxes[1][1] = -Rot.at<float>(1,1);
00178 stAxes[1][2] = +Rot.at<float>(2,1);
00179
00180 stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1];
00181 stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0];
00182 stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0];
00183
00184
00185 double axes[3][3];
00186 axes[0][0] = stAxes[0][0];
00187 axes[1][0] = stAxes[0][1];
00188 axes[2][0] = stAxes[0][2];
00189
00190 axes[0][1] = stAxes[1][0];
00191 axes[1][1] = stAxes[1][1];
00192 axes[2][1] = stAxes[1][2];
00193
00194 axes[0][2] = stAxes[2][0];
00195 axes[1][2] = stAxes[2][1];
00196 axes[2][2] = stAxes[2][2];
00197
00198
00199
00200 double fTrace = axes[0][0]+axes[1][1]+axes[2][2];
00201 double fRoot;
00202
00203 if ( fTrace > 0.0 )
00204 {
00205
00206 fRoot = sqrt(fTrace + 1.0);
00207 orientation[0] = 0.5*fRoot;
00208 fRoot = 0.5/fRoot;
00209 orientation[1] = (axes[2][1]-axes[1][2])*fRoot;
00210 orientation[2] = (axes[0][2]-axes[2][0])*fRoot;
00211 orientation[3] = (axes[1][0]-axes[0][1])*fRoot;
00212 }
00213 else
00214 {
00215
00216 static unsigned int s_iNext[3] = { 1, 2, 0 };
00217 unsigned int i = 0;
00218 if ( axes[1][1] > axes[0][0] )
00219 i = 1;
00220 if ( axes[2][2] > axes[i][i] )
00221 i = 2;
00222 unsigned int j = s_iNext[i];
00223 unsigned int k = s_iNext[j];
00224
00225 fRoot = sqrt(axes[i][i]-axes[j][j]-axes[k][k] + 1.0);
00226 double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] };
00227 *apkQuat[i] = 0.5*fRoot;
00228 fRoot = 0.5/fRoot;
00229 orientation[0] = (axes[k][j]-axes[j][k])*fRoot;
00230 *apkQuat[j] = (axes[j][i]+axes[i][j])*fRoot;
00231 *apkQuat[k] = (axes[k][i]+axes[i][k])*fRoot;
00232 }
00233
00234
00235 }
00236
00237
00238
00239 void Marker::draw(Mat &in, Scalar color, int lineWidth ,bool writeId)const
00240 {
00241 if (size()!=4) return;
00242 cv::line( in,(*this)[0],(*this)[1],color,lineWidth,CV_AA);
00243 cv::line( in,(*this)[1],(*this)[2],color,lineWidth,CV_AA);
00244 cv::line( in,(*this)[2],(*this)[3],color,lineWidth,CV_AA);
00245 cv::line( in,(*this)[3],(*this)[0],color,lineWidth,CV_AA);
00246 cv::rectangle( in,(*this)[0]-Point2f(2,2),(*this)[0]+Point2f(2,2),Scalar(0,0,255,255),lineWidth,CV_AA);
00247 cv::rectangle( in,(*this)[1]-Point2f(2,2),(*this)[1]+Point2f(2,2),Scalar(0,255,0,255),lineWidth,CV_AA);
00248 cv::rectangle( in,(*this)[2]-Point2f(2,2),(*this)[2]+Point2f(2,2),Scalar(255,0,0,255),lineWidth,CV_AA);
00249 if (writeId) {
00250 char cad[100];
00251 sprintf(cad,"id=%d",id);
00252
00253 Point cent(0,0);
00254 for (int i=0;i<4;i++)
00255 {
00256 cent.x+=(*this)[i].x;
00257 cent.y+=(*this)[i].y;
00258 }
00259 cent.x/=4.;
00260 cent.y/=4.;
00261 putText(in,cad, cent,FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255-color[0],255-color[1],255-color[2],255),2);
00262 }
00263 }
00264
00265
00266
00267
00268 void Marker::draw_size(Mat &in, Scalar color, int lineWidth ,bool writeId)const
00269 {
00270 if (size()!=4) return;
00271 cv::line( in,(*this)[0],(*this)[1],color,lineWidth,CV_AA);
00272 cv::line( in,(*this)[1],(*this)[2],color,lineWidth,CV_AA);
00273 cv::line( in,(*this)[2],(*this)[3],color,lineWidth,CV_AA);
00274 cv::line( in,(*this)[3],(*this)[0],color,lineWidth,CV_AA);
00275 cv::rectangle( in,(*this)[0]-Point2f(2,2),(*this)[0]+Point2f(2,2),Scalar(0,0,255,255),lineWidth,CV_AA);
00276 cv::rectangle( in,(*this)[1]-Point2f(2,2),(*this)[1]+Point2f(2,2),Scalar(0,255,0,255),lineWidth,CV_AA);
00277 cv::rectangle( in,(*this)[2]-Point2f(2,2),(*this)[2]+Point2f(2,2),Scalar(255,0,0,255),lineWidth,CV_AA);
00278 if (writeId) {
00279 char cad[100];
00280 sprintf(cad,"id=%d (%1.3f m)",id,ssize);
00281
00282 Point cent(0,0);
00283 for (int i=0;i<4;i++)
00284 {
00285 cent.x+=(*this)[i].x;
00286 cent.y+=(*this)[i].y;
00287 }
00288 cent.x/=4.;
00289 cent.y/=4.;
00290 putText(in,cad, cent,FONT_HERSHEY_SIMPLEX, 0.3, Scalar(255-color[0],255-color[1],255-color[2],255),1);
00291 }
00292 }
00293
00294
00297 void Marker::calculateExtrinsics(float markerSize,const CameraParameters &CP,bool setYPerperdicular)throw(cv::Exception)
00298 {
00299 if (!CP.isValid()) throw cv::Exception(9004,"!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
00300 calculateExtrinsics( markerSize,CP.CameraMatrix,CP.Distorsion,setYPerperdicular);
00301 }
00302
00303 void print(cv::Point3f p,string cad){
00304
00305 }
00308 void Marker::calculateExtrinsics(float markerSizeMeters,cv::Mat camMatrix,cv::Mat distCoeff ,bool setYPerperdicular)throw(cv::Exception)
00309 {
00310 if (!isValid()) throw cv::Exception(9004,"!isValid(): invalid marker. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
00311 if (markerSizeMeters<=0)throw cv::Exception(9004,"markerSize<=0: invalid markerSize","calculateExtrinsics",__FILE__,__LINE__);
00312 if ( camMatrix.rows==0 || camMatrix.cols==0) throw cv::Exception(9004,"CameraMatrix is empty","calculateExtrinsics",__FILE__,__LINE__);
00313
00314 double halfSize=markerSizeMeters/2.;
00315 cv::Mat ObjPoints(4,3,CV_32FC1);
00316 ObjPoints.at<float>(1,0)=-halfSize;
00317 ObjPoints.at<float>(1,1)=halfSize;
00318 ObjPoints.at<float>(1,2)=0;
00319 ObjPoints.at<float>(2,0)=halfSize;
00320 ObjPoints.at<float>(2,1)=halfSize;
00321 ObjPoints.at<float>(2,2)=0;
00322 ObjPoints.at<float>(3,0)=halfSize;
00323 ObjPoints.at<float>(3,1)=-halfSize;
00324 ObjPoints.at<float>(3,2)=0;
00325 ObjPoints.at<float>(0,0)=-halfSize;
00326 ObjPoints.at<float>(0,1)=-halfSize;
00327 ObjPoints.at<float>(0,2)=0;
00328
00329 cv::Mat ImagePoints(4,2,CV_32FC1);
00330
00331
00332 for (int c=0;c<4;c++)
00333 {
00334 ImagePoints.at<float>(c,0)=((*this)[c%4].x);
00335 ImagePoints.at<float>(c,1)=((*this)[c%4].y);
00336 }
00337
00338 cv::Mat raux,taux;
00339 cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux);
00340 raux.convertTo(Rvec,CV_32F);
00341 taux.convertTo(Tvec ,CV_32F);
00342
00343 if (setYPerperdicular) rotateXAxis(Rvec);
00344 ssize=markerSizeMeters;
00345
00346
00347 }
00348
00349
00353 void Marker::rotateXAxis(Mat &rotation)
00354 {
00355 cv::Mat R(3,3,CV_32F);
00356 Rodrigues(rotation, R);
00357
00358 cv::Mat RX=cv::Mat::eye(3,3,CV_32F);
00359 float angleRad=M_PI/2;
00360 RX.at<float>(1,1)=cos(angleRad);
00361 RX.at<float>(1,2)=-sin(angleRad);
00362 RX.at<float>(2,1)=sin(angleRad);
00363 RX.at<float>(2,2)=cos(angleRad);
00364
00365 R=R*RX;
00366
00367 Rodrigues(R,rotation);
00368 }
00369
00370
00371
00374 cv::Point2f Marker::getCenter()const
00375 {
00376 cv::Point2f cent(0,0);
00377 for(size_t i=0;i<size();i++){
00378 cent.x+=(*this)[i].x;
00379 cent.y+=(*this)[i].y;
00380 }
00381 cent.x/=float(size());
00382 cent.y/=float(size());
00383 return cent;
00384 }
00385
00388 float Marker::getArea()const
00389 {
00390 assert(size()==4);
00391
00392 cv::Point2f v01=(*this)[1]-(*this)[0];
00393 cv::Point2f v03=(*this)[3]-(*this)[0];
00394 float area1=fabs(v01.x*v03.y - v01.y*v03.x);
00395 cv::Point2f v21=(*this)[1]-(*this)[2];
00396 cv::Point2f v23=(*this)[3]-(*this)[2];
00397 float area2=fabs(v21.x*v23.y - v21.y*v23.x);
00398 return (area2+area1)/2.;
00399
00400
00401 }
00404 float Marker::getPerimeter()const
00405 {
00406 assert(size()==4);
00407 float sum=0;
00408 for(int i=0;i<4;i++)
00409 sum+=norm( (*this)[i]-(*this)[(i+1)%4]);
00410 return sum;
00411 }
00412
00413
00414 }