marker.cpp
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
28 #include "marker.h"
29 #define _USE_MATH_DEFINES
30 #include <math.h>
31 #include <cstdio>
32 #include <opencv2/calib3d/calib3d.hpp>
33 #include <opencv2/highgui/highgui.hpp>
34 #include <opencv2/imgproc/imgproc.hpp>
35 using namespace cv;
36 namespace aruco {
40 Marker::Marker() {
41  id = -1;
42  ssize = -1;
43  Rvec.create(3, 1, CV_32FC1);
44  Tvec.create(3, 1, CV_32FC1);
45  for (int i = 0; i < 3; i++)
46  Tvec.at< float >(i, 0) = Rvec.at< float >(i, 0) = -999999;
47 }
51 Marker::Marker(const Marker &M) : std::vector< cv::Point2f >(M) {
52  M.Rvec.copyTo(Rvec);
53  M.Tvec.copyTo(Tvec);
54  id = M.id;
55  ssize = M.ssize;
56 }
57 
61 Marker::Marker(const std::vector< cv::Point2f > &corners, int _id) : std::vector< cv::Point2f >(corners) {
62  id = _id;
63  ssize = -1;
64  Rvec.create(3, 1, CV_32FC1);
65  Tvec.create(3, 1, CV_32FC1);
66  for (int i = 0; i < 3; i++)
67  Tvec.at< float >(i, 0) = Rvec.at< float >(i, 0) = -999999;
68 }
69 
73 void Marker::glGetModelViewMatrix(double modelview_matrix[16]) throw(cv::Exception) {
74  // check if paremeters are valid
75  bool invalid = false;
76  for (int i = 0; i < 3 && !invalid; i++) {
77  if (Tvec.at< float >(i, 0) != -999999)
78  invalid |= false;
79  if (Rvec.at< float >(i, 0) != -999999)
80  invalid |= false;
81  }
82  if (invalid)
83  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);
86 
87  double para[3][4];
88  for (int i = 0; i < 3; i++)
89  for (int j = 0; j < 3; j++)
90  para[i][j] = Rot.at< float >(i, j);
91  // now, add the translation
92  para[0][3] = Tvec.at< float >(0, 0);
93  para[1][3] = Tvec.at< float >(1, 0);
94  para[2][3] = Tvec.at< float >(2, 0);
95  double scale = 1;
96 
97  modelview_matrix[0 + 0 * 4] = para[0][0];
98  // R1C2
99  modelview_matrix[0 + 1 * 4] = para[0][1];
100  modelview_matrix[0 + 2 * 4] = para[0][2];
101  modelview_matrix[0 + 3 * 4] = para[0][3];
102  // R2
103  modelview_matrix[1 + 0 * 4] = para[1][0];
104  modelview_matrix[1 + 1 * 4] = para[1][1];
105  modelview_matrix[1 + 2 * 4] = para[1][2];
106  modelview_matrix[1 + 3 * 4] = para[1][3];
107  // R3
108  modelview_matrix[2 + 0 * 4] = -para[2][0];
109  modelview_matrix[2 + 1 * 4] = -para[2][1];
110  modelview_matrix[2 + 2 * 4] = -para[2][2];
111  modelview_matrix[2 + 3 * 4] = -para[2][3];
112  modelview_matrix[3 + 0 * 4] = 0.0;
113  modelview_matrix[3 + 1 * 4] = 0.0;
114  modelview_matrix[3 + 2 * 4] = 0.0;
115  modelview_matrix[3 + 3 * 4] = 1.0;
116  if (scale != 0.0) {
117  modelview_matrix[12] *= scale;
118  modelview_matrix[13] *= scale;
119  modelview_matrix[14] *= scale;
120  }
121 }
122 
123 
124 
125 /****
126  *
127  */
128 void Marker::OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception) {
129 
130  // check if paremeters are valid
131  bool invalid = false;
132  for (int i = 0; i < 3 && !invalid; i++) {
133  if (Tvec.at< float >(i, 0) != -999999)
134  invalid |= false;
135  if (Rvec.at< float >(i, 0) != -999999)
136  invalid |= false;
137  }
138  if (invalid)
139  throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__);
140 
141  // calculate position vector
142  position[0] = -Tvec.ptr< float >(0)[0];
143  position[1] = -Tvec.ptr< float >(0)[1];
144  position[2] = +Tvec.ptr< float >(0)[2];
145 
146  // now calculare orientation quaternion
147  cv::Mat Rot(3, 3, CV_32FC1);
148  cv::Rodrigues(Rvec, Rot);
149 
150  // calculate axes for quaternion
151  double stAxes[3][3];
152  // x axis
153  stAxes[0][0] = -Rot.at< float >(0, 0);
154  stAxes[0][1] = -Rot.at< float >(1, 0);
155  stAxes[0][2] = +Rot.at< float >(2, 0);
156  // y axis
157  stAxes[1][0] = -Rot.at< float >(0, 1);
158  stAxes[1][1] = -Rot.at< float >(1, 1);
159  stAxes[1][2] = +Rot.at< float >(2, 1);
160  // for z axis, we use cross product
161  stAxes[2][0] = stAxes[0][1] * stAxes[1][2] - stAxes[0][2] * stAxes[1][1];
162  stAxes[2][1] = -stAxes[0][0] * stAxes[1][2] + stAxes[0][2] * stAxes[1][0];
163  stAxes[2][2] = stAxes[0][0] * stAxes[1][1] - stAxes[0][1] * stAxes[1][0];
164 
165  // transposed matrix
166  double axes[3][3];
167  axes[0][0] = stAxes[0][0];
168  axes[1][0] = stAxes[0][1];
169  axes[2][0] = stAxes[0][2];
170 
171  axes[0][1] = stAxes[1][0];
172  axes[1][1] = stAxes[1][1];
173  axes[2][1] = stAxes[1][2];
174 
175  axes[0][2] = stAxes[2][0];
176  axes[1][2] = stAxes[2][1];
177  axes[2][2] = stAxes[2][2];
178 
179  // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
180  // article "Quaternion Calculus and Fast Animation".
181  double fTrace = axes[0][0] + axes[1][1] + axes[2][2];
182  double fRoot;
183 
184  if (fTrace > 0.0) {
185  // |w| > 1/2, may as well choose w > 1/2
186  fRoot = sqrt(fTrace + 1.0); // 2w
187  orientation[0] = 0.5 * fRoot;
188  fRoot = 0.5 / fRoot; // 1/(4w)
189  orientation[1] = (axes[2][1] - axes[1][2]) * fRoot;
190  orientation[2] = (axes[0][2] - axes[2][0]) * fRoot;
191  orientation[3] = (axes[1][0] - axes[0][1]) * fRoot;
192  } else {
193  // |w| <= 1/2
194  static unsigned int s_iNext[3] = {1, 2, 0};
195  unsigned int i = 0;
196  if (axes[1][1] > axes[0][0])
197  i = 1;
198  if (axes[2][2] > axes[i][i])
199  i = 2;
200  unsigned int j = s_iNext[i];
201  unsigned int k = s_iNext[j];
202 
203  fRoot = sqrt(axes[i][i] - axes[j][j] - axes[k][k] + 1.0);
204  double *apkQuat[3] = {&orientation[1], &orientation[2], &orientation[3]};
205  *apkQuat[i] = 0.5 * fRoot;
206  fRoot = 0.5 / fRoot;
207  orientation[0] = (axes[k][j] - axes[j][k]) * fRoot;
208  *apkQuat[j] = (axes[j][i] + axes[i][j]) * fRoot;
209  *apkQuat[k] = (axes[k][i] + axes[i][k]) * fRoot;
210  }
211 }
212 
213 
214 
215 void Marker::draw(Mat &in, Scalar color, int lineWidth, bool writeId) const {
216  if (size() != 4)
217  return;
218  cv::line(in, (*this)[0], (*this)[1], color, lineWidth, CV_AA);
219  cv::line(in, (*this)[1], (*this)[2], color, lineWidth, CV_AA);
220  cv::line(in, (*this)[2], (*this)[3], color, lineWidth, CV_AA);
221  cv::line(in, (*this)[3], (*this)[0], color, lineWidth, CV_AA);
222  cv::rectangle(in, (*this)[0] - Point2f(2, 2), (*this)[0] + Point2f(2, 2), Scalar(0, 0, 255, 255), lineWidth, CV_AA);
223  cv::rectangle(in, (*this)[1] - Point2f(2, 2), (*this)[1] + Point2f(2, 2), Scalar(0, 255, 0, 255), lineWidth, CV_AA);
224  cv::rectangle(in, (*this)[2] - Point2f(2, 2), (*this)[2] + Point2f(2, 2), Scalar(255, 0, 0, 255), lineWidth, CV_AA);
225  if (writeId) {
226  char cad[100];
227  sprintf(cad, "id=%d", id);
228  // determine the centroid
229  Point cent(0, 0);
230  for (int i = 0; i < 4; i++) {
231  cent.x += (*this)[i].x;
232  cent.y += (*this)[i].y;
233  }
234  cent.x /= 4.;
235  cent.y /= 4.;
236  putText(in, cad, cent, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255 - color[0], 255 - color[1], 255 - color[2], 255), 2);
237  }
238 }
239 
242 void Marker::calculateExtrinsics(float markerSize, const CameraParameters &CP, bool setYPerpendicular) throw(cv::Exception) {
243  if (!CP.isValid())
244  throw cv::Exception(9004, "!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics", "calculateExtrinsics", __FILE__,
245  __LINE__);
246  calculateExtrinsics(markerSize, CP.CameraMatrix, CP.Distorsion, setYPerpendicular);
247 }
248 
249 void print(cv::Point3f p, string cad) { cout << cad << " " << p.x << " " << p.y << " " << p.z << endl; }
252 void Marker::calculateExtrinsics(float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff, bool setYPerpendicular) throw(cv::Exception) {
253  if (!isValid())
254  throw cv::Exception(9004, "!isValid(): invalid marker. It is not possible to calculate extrinsics", "calculateExtrinsics", __FILE__, __LINE__);
255  if (markerSizeMeters <= 0)
256  throw cv::Exception(9004, "markerSize<=0: invalid markerSize", "calculateExtrinsics", __FILE__, __LINE__);
257  if (camMatrix.rows == 0 || camMatrix.cols == 0)
258  throw cv::Exception(9004, "CameraMatrix is empty", "calculateExtrinsics", __FILE__, __LINE__);
259 
260  double halfSize = markerSizeMeters / 2.;
261  cv::Mat ObjPoints(4, 3, CV_32FC1);
262  ObjPoints.at< float >(1, 0) = -halfSize;
263  ObjPoints.at< float >(1, 1) = halfSize;
264  ObjPoints.at< float >(1, 2) = 0;
265  ObjPoints.at< float >(2, 0) = halfSize;
266  ObjPoints.at< float >(2, 1) = halfSize;
267  ObjPoints.at< float >(2, 2) = 0;
268  ObjPoints.at< float >(3, 0) = halfSize;
269  ObjPoints.at< float >(3, 1) = -halfSize;
270  ObjPoints.at< float >(3, 2) = 0;
271  ObjPoints.at< float >(0, 0) = -halfSize;
272  ObjPoints.at< float >(0, 1) = -halfSize;
273  ObjPoints.at< float >(0, 2) = 0;
274 
275  cv::Mat ImagePoints(4, 2, CV_32FC1);
276 
277  // Set image points from the marker
278  for (int c = 0; c < 4; c++) {
279  ImagePoints.at< float >(c, 0) = ((*this)[c].x);
280  ImagePoints.at< float >(c, 1) = ((*this)[c].y);
281  }
282 
283  cv::Mat raux, taux;
284  cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff, raux, taux);
285  raux.convertTo(Rvec, CV_32F);
286  taux.convertTo(Tvec, CV_32F);
287  // rotate the X axis so that Y is perpendicular to the marker plane
288  if (setYPerpendicular)
289  rotateXAxis(Rvec);
290  ssize = markerSizeMeters;
291  // cout<<(*this)<<endl;
292 }
293 
294 
298 void Marker::rotateXAxis(Mat &rotation) {
299  cv::Mat R(3, 3, CV_32F);
300  Rodrigues(rotation, R);
301  // create a rotation matrix for x axis
302  cv::Mat RX = cv::Mat::eye(3, 3, CV_32F);
303  float angleRad = M_PI / 2;
304  RX.at< float >(1, 1) = cos(angleRad);
305  RX.at< float >(1, 2) = -sin(angleRad);
306  RX.at< float >(2, 1) = sin(angleRad);
307  RX.at< float >(2, 2) = cos(angleRad);
308  // now multiply
309  R = R * RX;
310  // finally, the the rodrigues back
311  Rodrigues(R, rotation);
312 }
313 
314 
315 
318 cv::Point2f Marker::getCenter() const {
319  cv::Point2f cent(0, 0);
320  for (size_t i = 0; i < size(); i++) {
321  cent.x += (*this)[i].x;
322  cent.y += (*this)[i].y;
323  }
324  cent.x /= float(size());
325  cent.y /= float(size());
326  return cent;
327 }
328 
331 float Marker::getArea() const {
332  assert(size() == 4);
333  // use the cross products
334  cv::Point2f v01 = (*this)[1] - (*this)[0];
335  cv::Point2f v03 = (*this)[3] - (*this)[0];
336  float area1 = fabs(v01.x * v03.y - v01.y * v03.x);
337  cv::Point2f v21 = (*this)[1] - (*this)[2];
338  cv::Point2f v23 = (*this)[3] - (*this)[2];
339  float area2 = fabs(v21.x * v23.y - v21.y * v23.x);
340  return (area2 + area1) / 2.;
341 }
344 float Marker::getPerimeter() const {
345  assert(size() == 4);
346  float sum = 0;
347  for (int i = 0; i < 4; i++)
348  sum += norm((*this)[i] - (*this)[(i + 1) % 4]);
349  return sum;
350 }
351 }
void rotateXAxis(cv::Mat &rotation)
Definition: marker.cpp:298
bool isValid() const
Definition: marker.h:64
cv::Mat Rvec
Definition: marker.h:48
void calculateExtrinsics(float markerSize, const CameraParameters &CP, bool setYPerpendicular=true)
Definition: marker.cpp:242
float getArea() const
Definition: marker.cpp:331
This class represents a marker. It is a vector of the fours corners ot the marker.
Definition: marker.h:41
Parameters of the camera.
float ssize
Definition: marker.h:46
void glGetModelViewMatrix(double modelview_matrix[16])
Definition: marker.cpp:73
cv::Point2f getCenter() const
Definition: marker.cpp:318
cv::Mat Tvec
Definition: marker.h:48
void draw(cv::Mat &in, cv::Scalar color, int lineWidth=1, bool writeId=true) const
Definition: marker.cpp:215
void print(cv::Point3f p, string cad)
Definition: marker.cpp:249
float getPerimeter() const
Definition: marker.cpp:344
void OgreGetPoseParameters(double position[3], double orientation[4])
Definition: marker.cpp:128


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Meißner Pascal, Qattan Mohamad
autogenerated on Mon Jun 10 2019 12:40:21