28 #include <aruco/marker.h> 29 #define _USE_MATH_DEFINES 42 Rvec.create(3,1,CV_32FC1);
43 Tvec.create(3,1,CV_32FC1);
45 Tvec.at<
float>(i,0)=Rvec.at<
float>(i,0)=-999999;
50 Marker::Marker(
const Marker &M):std::vector<cv::Point2f>(M)
61 Marker::Marker(
const std::vector<cv::Point2f> &corners,
int _id):std::vector<cv::Point2f>(corners)
65 Rvec.create(3,1,CV_32FC1);
66 Tvec.create(3,1,CV_32FC1);
68 Tvec.at<
float>(i,0)=Rvec.at<
float>(i,0)=-999999;
74 void Marker::glGetModelViewMatrix(
double modelview_matrix[16])
throw(cv::Exception)
78 for (
int i=0;i<3 && !invalid ;i++)
80 if (Tvec.at<
float>(i,0)!=-999999) invalid|=
false;
81 if (Rvec.at<
float>(i,0)!=-999999) invalid|=
false;
83 if (invalid)
throw cv::Exception(9003,
"extrinsic parameters are not set",
"Marker::getModelViewMatrix",__FILE__,__LINE__);
84 Mat Rot(3,3,CV_32FC1),Jacob;
85 Rodrigues(Rvec, Rot, Jacob);
89 for (
int j=0;j<3;j++) para[i][j]=Rot.at<
float>(i,j);
91 para[0][3]=Tvec.at<
float>(0,0);
92 para[1][3]=Tvec.at<
float>(1,0);
93 para[2][3]=Tvec.at<
float>(2,0);
96 modelview_matrix[0 + 0*4] = para[0][0];
98 modelview_matrix[0 + 1*4] = para[0][1];
99 modelview_matrix[0 + 2*4] = para[0][2];
100 modelview_matrix[0 + 3*4] = para[0][3];
102 modelview_matrix[1 + 0*4] = para[1][0];
103 modelview_matrix[1 + 1*4] = para[1][1];
104 modelview_matrix[1 + 2*4] = para[1][2];
105 modelview_matrix[1 + 3*4] = para[1][3];
107 modelview_matrix[2 + 0*4] = -para[2][0];
108 modelview_matrix[2 + 1*4] = -para[2][1];
109 modelview_matrix[2 + 2*4] = -para[2][2];
110 modelview_matrix[2 + 3*4] = -para[2][3];
111 modelview_matrix[3 + 0*4] = 0.0;
112 modelview_matrix[3 + 1*4] = 0.0;
113 modelview_matrix[3 + 2*4] = 0.0;
114 modelview_matrix[3 + 3*4] = 1.0;
117 modelview_matrix[12] *= scale;
118 modelview_matrix[13] *= scale;
119 modelview_matrix[14] *= scale;
128 void Marker::OgreGetPoseParameters(
double position[3],
double orientation[4])
throw(cv::Exception)
132 for (
int i=0;i<3 && !invalid ;i++)
134 if (Tvec.at<
float>(i,0)!=-999999) invalid|=
false;
135 if (Rvec.at<
float>(i,0)!=-999999) invalid|=
false;
137 if (invalid)
throw cv::Exception(9003,
"extrinsic parameters are not set",
"Marker::getModelViewMatrix",__FILE__,__LINE__);
140 position[0] = Tvec.ptr<
float>(0)[0];
141 position[1] = Tvec.ptr<
float>(0)[1];
142 position[2] = +Tvec.ptr<
float>(0)[2];
145 cv::Mat Rot(3,3,CV_32FC1);
146 cv::Rodrigues(Rvec, Rot);
151 stAxes[0][0] = -Rot.at<
float>(0,0);
152 stAxes[0][1] = -Rot.at<
float>(1,0);
153 stAxes[0][2] = +Rot.at<
float>(2,0);
155 stAxes[1][0] = -Rot.at<
float>(0,1);
156 stAxes[1][1] = -Rot.at<
float>(1,1);
157 stAxes[1][2] = +Rot.at<
float>(2,1);
159 stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1];
160 stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0];
161 stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0];
165 axes[0][0] = stAxes[0][0];
166 axes[1][0] = stAxes[0][1];
167 axes[2][0] = stAxes[0][2];
169 axes[0][1] = stAxes[1][0];
170 axes[1][1] = stAxes[1][1];
171 axes[2][1] = stAxes[1][2];
173 axes[0][2] = stAxes[2][0];
174 axes[1][2] = stAxes[2][1];
175 axes[2][2] = stAxes[2][2];
179 double fTrace = axes[0][0]+axes[1][1]+axes[2][2];
185 fRoot = sqrt(fTrace + 1.0);
186 orientation[0] = 0.5*fRoot;
188 orientation[1] = (axes[2][1]-axes[1][2])*fRoot;
189 orientation[2] = (axes[0][2]-axes[2][0])*fRoot;
190 orientation[3] = (axes[1][0]-axes[0][1])*fRoot;
195 static unsigned int s_iNext[3] = { 1, 2, 0 };
197 if ( axes[1][1] > axes[0][0] )
199 if ( axes[2][2] > axes[i][i] )
201 unsigned int j = s_iNext[i];
202 unsigned int k = s_iNext[j];
204 fRoot = sqrt(axes[i][i]-axes[j][j]-axes[k][k] + 1.0);
205 double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] };
206 *apkQuat[i] = 0.5*fRoot;
208 orientation[0] = (axes[k][j]-axes[j][k])*fRoot;
209 *apkQuat[j] = (axes[j][i]+axes[i][j])*fRoot;
210 *apkQuat[k] = (axes[k][i]+axes[i][k])*fRoot;
216 void Marker::draw(Mat &in, Scalar color,
int lineWidth ,
bool writeId)
const 218 if (size()!=4)
return;
219 cv::line( in,(*
this)[0],(*
this)[1],color,lineWidth,LINE_AA);
220 cv::line( in,(*
this)[1],(*
this)[2],color,lineWidth,LINE_AA);
221 cv::line( in,(*
this)[2],(*
this)[3],color,lineWidth,LINE_AA);
222 cv::line( in,(*
this)[3],(*
this)[0],color,lineWidth,LINE_AA);
223 cv::rectangle( in,(*
this)[0]-Point2f(2,2),(*
this)[0]+Point2f(2,2),Scalar(0,0,255,255),lineWidth,LINE_AA);
224 cv::rectangle( in,(*
this)[1]-Point2f(2,2),(*
this)[1]+Point2f(2,2),Scalar(0,255,0,255),lineWidth,LINE_AA);
225 cv::rectangle( in,(*
this)[2]-Point2f(2,2),(*
this)[2]+Point2f(2,2),Scalar(255,0,0,255),lineWidth,LINE_AA);
228 sprintf(cad,
"id=%d",
id);
231 for (
int i=0;i<4;i++)
233 cent.
x+=(*this)[i].x;
234 cent.
y+=(*this)[i].y;
238 putText(in,cad, cent,FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255-color[0],255-color[1],255-color[2],255),2);
242 void Marker::calculateExtrinsics(
float markerSize,
const CameraParameters &CP,
bool setYPerpendicular)
throw(cv::Exception)
244 if (!CP.isValid())
throw cv::Exception(9004,
"!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics",
"calculateExtrinsics",__FILE__,__LINE__);
245 calculateExtrinsics( markerSize,CP.CameraMatrix,CP.Distorsion,setYPerpendicular);
248 void print(cv::Point3f p,
string cad)
250 cout<<cad<<
" "<<p.x<<
" "<<p.y<<
" "<<p.z<<endl;
253 void Marker::calculateExtrinsics(
float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff ,
bool setYPerpendicular)
throw(cv::Exception)
255 if (!isValid())
throw cv::Exception(9004,
"!isValid(): invalid marker. It is not possible to calculate extrinsics",
"calculateExtrinsics",__FILE__,__LINE__);
256 if (markerSizeMeters<=0)
throw cv::Exception(9004,
"markerSize<=0: invalid markerSize",
"calculateExtrinsics",__FILE__,__LINE__);
257 if ( camMatrix.rows==0 || camMatrix.cols==0)
throw cv::Exception(9004,
"CameraMatrix is empty",
"calculateExtrinsics",__FILE__,__LINE__);
259 double halfSize=markerSizeMeters/2.;
260 cv::Mat ObjPoints(4,3,CV_32FC1);
261 ObjPoints.at<
float>(1,0)=-halfSize;
262 ObjPoints.at<
float>(1,1)=halfSize;
263 ObjPoints.at<
float>(1,2)=0;
264 ObjPoints.at<
float>(2,0)=halfSize;
265 ObjPoints.at<
float>(2,1)=halfSize;
266 ObjPoints.at<
float>(2,2)=0;
267 ObjPoints.at<
float>(3,0)=halfSize;
268 ObjPoints.at<
float>(3,1)=-halfSize;
269 ObjPoints.at<
float>(3,2)=0;
270 ObjPoints.at<
float>(0,0)=-halfSize;
271 ObjPoints.at<
float>(0,1)=-halfSize;
272 ObjPoints.at<
float>(0,2)=0;
274 cv::Mat ImagePoints(4,2,CV_32FC1);
277 for (
int c=0;c<4;c++)
279 ImagePoints.at<
float>(c,0)=((*
this)[c%4].x);
280 ImagePoints.at<
float>(c,1)=((*
this)[c%4].y);
284 cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux);
285 raux.convertTo(Rvec,CV_32F);
286 taux.convertTo(Tvec ,CV_32F);
288 if (setYPerpendicular) rotateXAxis(Rvec);
289 ssize=markerSizeMeters;
294 void Marker::rotateXAxis(Mat &rotation)
296 cv::Mat R(3,3,CV_32F);
297 cv::Rodrigues(rotation, R);
299 cv::Mat RX=cv::Mat::eye(3,3,CV_32F);
300 float angleRad=M_PI/2;
301 RX.at<
float>(1,1)=cos(angleRad);
302 RX.at<
float>(1,2)=-sin(angleRad);
303 RX.at<
float>(2,1)=sin(angleRad);
304 RX.at<
float>(2,2)=cos(angleRad);
308 cv::Rodrigues(R,rotation);
311 cv::Point2f Marker::getCenter()
const 313 cv::Point2f cent(0,0);
314 for(
size_t i=0;i<size();i++){
315 cent.x+=(*this)[i].x;
316 cent.y+=(*this)[i].y;
318 cent.x/=float(size());
319 cent.y/=float(size());
323 float Marker::getArea()
const 327 cv::Point2f v01=(*this)[1]-(*this)[0];
328 cv::Point2f v03=(*this)[3]-(*this)[0];
329 float area1=fabs(v01.x*v03.y - v01.y*v03.x);
330 cv::Point2f v21=(*this)[1]-(*this)[2];
331 cv::Point2f v23=(*this)[3]-(*this)[2];
332 float area2=fabs(v21.x*v23.y - v21.y*v23.x);
333 return (area2+area1)/2.;
336 float Marker::getPerimeter()
const 341 sum+=norm( (*
this)[i]-(*
this)[(i+1)%4]);
void print(cv::Point3f p, string cad)