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


lidar_camera_calibration
Author(s):
autogenerated on Sat Feb 6 2021 03:39:37