board.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 "board.h"
29 #include <opencv2/calib3d/calib3d.hpp>
30 #include <fstream>
31 using namespace std;
32 using namespace cv;
33 namespace aruco {
34 
39 BoardConfiguration::BoardConfiguration() { mInfoType = NONE; }
44 BoardConfiguration::BoardConfiguration(string filePath) throw(cv::Exception) {
45  mInfoType = NONE;
46  readFromFile(filePath);
47 }
52 BoardConfiguration::BoardConfiguration(const BoardConfiguration &T) : vector< MarkerInfo >(T) {
53  // MarkersInfo=T.MarkersInfo;
54  mInfoType = T.mInfoType;
55 }
56 
62  // MarkersInfo=T.MarkersInfo;
63  vector< MarkerInfo >::operator=(T);
64  mInfoType = T.mInfoType;
65  return *this;
66 }
71 void BoardConfiguration::saveToFile(string sfile) throw(cv::Exception) {
72 
73  cv::FileStorage fs(sfile, cv::FileStorage::WRITE);
74  saveToFile(fs);
75 }
78 void BoardConfiguration::saveToFile(cv::FileStorage &fs) throw(cv::Exception) {
79  fs << "aruco_bc_nmarkers" << (int)size();
80  fs << "aruco_bc_mInfoType" << (int)mInfoType;
81  fs << "aruco_bc_markers"
82  << "[";
83  for (size_t i = 0; i < size(); i++) {
84  fs << "{:"
85  << "id" << at(i).id;
86 
87  fs << "corners"
88  << "[:";
89  for (int c = 0; c < at(i).size(); c++)
90  fs << at(i)[c];
91  fs << "]";
92  fs << "}";
93  }
94  fs << "]";
95 }
96 
101 void BoardConfiguration::readFromFile(string sfile) throw(cv::Exception) {
102  try {
103  cv::FileStorage fs(sfile, cv::FileStorage::READ);
104  readFromFile(fs);
105  } catch (std::exception &ex) {
106  throw cv::Exception(81818, "BoardConfiguration::readFromFile", ex.what() + string(" file=)") + sfile, __FILE__, __LINE__);
107  }
108 }
109 
110 
113 void BoardConfiguration::readFromFile(cv::FileStorage &fs) throw(cv::Exception) {
114  int aux = 0;
115  // look for the nmarkers
116  if (fs["aruco_bc_nmarkers"].name() != "aruco_bc_nmarkers")
117  throw cv::Exception(81818, "BoardConfiguration::readFromFile", "invalid file type", __FILE__, __LINE__);
118  fs["aruco_bc_nmarkers"] >> aux;
119  resize(aux);
120  fs["aruco_bc_mInfoType"] >> mInfoType;
121  cv::FileNode markers = fs["aruco_bc_markers"];
122  int i = 0;
123  for (FileNodeIterator it = markers.begin(); it != markers.end(); ++it, i++) {
124  at(i).id = (*it)["id"];
125  FileNode FnCorners = (*it)["corners"];
126  for (FileNodeIterator itc = FnCorners.begin(); itc != FnCorners.end(); ++itc) {
127  vector< float > coordinates3d;
128  (*itc) >> coordinates3d;
129  if (coordinates3d.size() != 3)
130  throw cv::Exception(81818, "BoardConfiguration::readFromFile", "invalid file type 3", __FILE__, __LINE__);
131  cv::Point3f point(coordinates3d[0], coordinates3d[1], coordinates3d[2]);
132  at(i).push_back(point);
133  }
134  }
135 }
136 
140 
141  for (size_t i = 0; i < size(); i++)
142  if (at(i).id == id)
143  return i;
144  return -1;
145 }
146 
149 const MarkerInfo &BoardConfiguration::getMarkerInfo(int id) const throw(cv::Exception) {
150  for (size_t i = 0; i < size(); i++)
151  if (at(i).id == id)
152  return at(i);
153  throw cv::Exception(111, "BoardConfiguration::getMarkerInfo", "Marker with the id given is not found", __FILE__, __LINE__);
154 }
155 
156 
159 void Board::glGetModelViewMatrix(double modelview_matrix[16]) throw(cv::Exception) {
160  // check if paremeters are valid
161  bool invalid = false;
162  for (int i = 0; i < 3 && !invalid; i++) {
163  if (Tvec.at< float >(i, 0) != -999999)
164  invalid |= false;
165  if (Rvec.at< float >(i, 0) != -999999)
166  invalid |= false;
167  }
168  if (invalid)
169  throw cv::Exception(9002, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__);
170  Mat Rot(3, 3, CV_32FC1), Jacob;
171  Rodrigues(Rvec, Rot, Jacob);
172 
173  double para[3][4];
174  for (int i = 0; i < 3; i++)
175  for (int j = 0; j < 3; j++)
176  para[i][j] = Rot.at< float >(i, j);
177  // now, add the translation
178  para[0][3] = Tvec.at< float >(0, 0);
179  para[1][3] = Tvec.at< float >(1, 0);
180  para[2][3] = Tvec.at< float >(2, 0);
181  double scale = 1;
182 
183  modelview_matrix[0 + 0 * 4] = para[0][0];
184  // R1C2
185  modelview_matrix[0 + 1 * 4] = para[0][1];
186  modelview_matrix[0 + 2 * 4] = para[0][2];
187  modelview_matrix[0 + 3 * 4] = para[0][3];
188  // R2
189  modelview_matrix[1 + 0 * 4] = para[1][0];
190  modelview_matrix[1 + 1 * 4] = para[1][1];
191  modelview_matrix[1 + 2 * 4] = para[1][2];
192  modelview_matrix[1 + 3 * 4] = para[1][3];
193  // R3
194  modelview_matrix[2 + 0 * 4] = -para[2][0];
195  modelview_matrix[2 + 1 * 4] = -para[2][1];
196  modelview_matrix[2 + 2 * 4] = -para[2][2];
197  modelview_matrix[2 + 3 * 4] = -para[2][3];
198  modelview_matrix[3 + 0 * 4] = 0.0;
199  modelview_matrix[3 + 1 * 4] = 0.0;
200  modelview_matrix[3 + 2 * 4] = 0.0;
201  modelview_matrix[3 + 3 * 4] = 1.0;
202  if (scale != 0.0) {
203  modelview_matrix[12] *= scale;
204  modelview_matrix[13] *= scale;
205  modelview_matrix[14] *= scale;
206  }
207 }
208 
209 
210 /****
211  *
212  */
213 void Board::OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception) {
214  // check if paremeters are valid
215  bool invalid = false;
216  for (int i = 0; i < 3 && !invalid; i++) {
217  if (Tvec.at< float >(i, 0) != -999999)
218  invalid |= false;
219  if (Rvec.at< float >(i, 0) != -999999)
220  invalid |= false;
221  }
222  if (invalid)
223  throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__);
224 
225  // calculate position vector
226  position[0] = -Tvec.ptr< float >(0)[0];
227  position[1] = -Tvec.ptr< float >(0)[1];
228  position[2] = +Tvec.ptr< float >(0)[2];
229 
230  // now calculare orientation quaternion
231  cv::Mat Rot(3, 3, CV_32FC1);
232  cv::Rodrigues(Rvec, Rot);
233 
234  // calculate axes for quaternion
235  double stAxes[3][3];
236  // x axis
237  stAxes[0][0] = -Rot.at< float >(0, 0);
238  stAxes[0][1] = -Rot.at< float >(1, 0);
239  stAxes[0][2] = +Rot.at< float >(2, 0);
240  // y axis
241  stAxes[1][0] = -Rot.at< float >(0, 1);
242  stAxes[1][1] = -Rot.at< float >(1, 1);
243  stAxes[1][2] = +Rot.at< float >(2, 1);
244  // for z axis, we use cross product
245  stAxes[2][0] = stAxes[0][1] * stAxes[1][2] - stAxes[0][2] * stAxes[1][1];
246  stAxes[2][1] = -stAxes[0][0] * stAxes[1][2] + stAxes[0][2] * stAxes[1][0];
247  stAxes[2][2] = stAxes[0][0] * stAxes[1][1] - stAxes[0][1] * stAxes[1][0];
248 
249  // transposed matrix
250  double axes[3][3];
251  axes[0][0] = stAxes[0][0];
252  axes[1][0] = stAxes[0][1];
253  axes[2][0] = stAxes[0][2];
254 
255  axes[0][1] = stAxes[1][0];
256  axes[1][1] = stAxes[1][1];
257  axes[2][1] = stAxes[1][2];
258 
259  axes[0][2] = stAxes[2][0];
260  axes[1][2] = stAxes[2][1];
261  axes[2][2] = stAxes[2][2];
262 
263  // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
264  // article "Quaternion Calculus and Fast Animation".
265  double fTrace = axes[0][0] + axes[1][1] + axes[2][2];
266  double fRoot;
267 
268  if (fTrace > 0.0) {
269  // |w| > 1/2, may as well choose w > 1/2
270  fRoot = sqrt(fTrace + 1.0); // 2w
271  orientation[0] = 0.5 * fRoot;
272  fRoot = 0.5 / fRoot; // 1/(4w)
273  orientation[1] = (axes[2][1] - axes[1][2]) * fRoot;
274  orientation[2] = (axes[0][2] - axes[2][0]) * fRoot;
275  orientation[3] = (axes[1][0] - axes[0][1]) * fRoot;
276  } else {
277  // |w| <= 1/2
278  static unsigned int s_iNext[3] = {1, 2, 0};
279  unsigned int i = 0;
280  if (axes[1][1] > axes[0][0])
281  i = 1;
282  if (axes[2][2] > axes[i][i])
283  i = 2;
284  unsigned int j = s_iNext[i];
285  unsigned int k = s_iNext[j];
286 
287  fRoot = sqrt(axes[i][i] - axes[j][j] - axes[k][k] + 1.0);
288  double *apkQuat[3] = {&orientation[1], &orientation[2], &orientation[3]};
289  *apkQuat[i] = 0.5 * fRoot;
290  fRoot = 0.5 / fRoot;
291  orientation[0] = (axes[k][j] - axes[j][k]) * fRoot;
292  *apkQuat[j] = (axes[j][i] + axes[i][j]) * fRoot;
293  *apkQuat[k] = (axes[k][i] + axes[i][k]) * fRoot;
294  }
295 }
296 
299 void Board::draw(cv::Mat &im, cv::Scalar color, int lineWidth, bool writeId) {
300  for (size_t i = 0; i < size(); i++) {
301  at(i).draw(im, color, lineWidth, writeId);
302  }
303 }
304 
307 void Board::saveToFile(string filePath) throw(cv::Exception) {
308  cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
309 
310  fs << "aruco_bo_rvec" << Rvec;
311  fs << "aruco_bo_tvec" << Tvec;
312  // now, the markers
313  fs << "aruco_bo_nmarkers" << (int)size();
314  fs << "aruco_bo_markers"
315  << "[";
316  for (size_t i = 0; i < size(); i++) {
317  fs << "{:"
318  << "id" << at(i).id;
319  fs << "corners"
320  << "[:";
321  for (int c = 0; c < at(i).size(); c++)
322  fs << at(i)[c];
323  fs << "]";
324  fs << "}";
325  }
326  fs << "]";
327  // save configuration file
328  conf.saveToFile(fs);
329 
330 
331 
332  // readFromFile(fs);
333 }
336 void Board::readFromFile(string filePath) throw(cv::Exception) {
337  cv::FileStorage fs(filePath, cv::FileStorage::READ);
338  if (fs["aruco_bo_nmarkers"].name() != "aruco_bo_nmarkers")
339  throw cv::Exception(81818, "Board::readFromFile", "invalid file type:", __FILE__, __LINE__);
340 
341 
342 
343  int aux = 0;
344  // look for the nmarkers
345  fs["aruco_bo_nmarkers"] >> aux;
346  resize(aux);
347  fs["aruco_bo_rvec"] >> Rvec;
348  fs["aruco_bo_tvec"] >> Tvec;
349 
350  cv::FileNode markers = fs["aruco_bo_markers"];
351  int i = 0;
352  for (FileNodeIterator it = markers.begin(); it != markers.end(); ++it, i++) {
353  at(i).id = (*it)["id"];
354  int ncorners = (*it)["ncorners"];
355  at(i).resize(ncorners);
356  FileNode FnCorners = (*it)["corners"];
357  int c = 0;
358  for (FileNodeIterator itc = FnCorners.begin(); itc != FnCorners.end(); ++itc, c++) {
359  vector< float > coordinates2d;
360  (*itc) >> coordinates2d;
361  if (coordinates2d.size() != 2)
362  throw cv::Exception(81818, "Board::readFromFile", "invalid file type 2", __FILE__, __LINE__);
363  cv::Point2f point;
364  point.x = coordinates2d[0];
365  point.y = coordinates2d[1];
366  at(i).push_back(point);
367  }
368  }
369 
370  conf.readFromFile(fs);
371 }
372 
375 void BoardConfiguration::getIdList(std::vector< int > &ids, bool append) const {
376  if (!append)
377  ids.clear();
378  for (size_t i = 0; i < size(); i++)
379  ids.push_back(at(i).id);
380 }
381 };
void saveToFile(string filePath)
Definition: board.cpp:307
BoardConfiguration & operator=(const BoardConfiguration &T)
Definition: board.cpp:61
void readFromFile(string filePath)
Definition: board.cpp:336
void readFromFile(string sfile)
Definition: board.cpp:101
int getIndexOfMarkerId(int id) const
Definition: board.cpp:139
const MarkerInfo & getMarkerInfo(int id) const
Definition: board.cpp:149
void getIdList(vector< int > &ids, bool append=true) const
Definition: board.cpp:375
This class defines a board with several markers. A Board contains several markers so that they are mo...
Definition: board.h:69
void glGetModelViewMatrix(double modelview_matrix[16])
Definition: board.cpp:159
void saveToFile(string sfile)
Definition: board.cpp:71
void draw(cv::Mat &im, cv::Scalar color, int lineWidth=1, bool writeId=true)
Definition: board.cpp:299
void OgreGetPoseParameters(double position[3], double orientation[4])
Definition: board.cpp:213


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