aruco_test_markermap.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 <iostream>
29 #include <fstream>
30 #include <sstream>
31 #include <string>
32 #include <opencv2/highgui/highgui.hpp>
33 #include <opencv2/imgproc/imgproc.hpp>
34 #include "aruco.h"
35 using namespace cv;
36 using namespace aruco;
37 
39 bool The3DInfoAvailable = false;
40 float TheMarkerSize = -1;
41 VideoCapture TheVideoCapturer;
47 void cvTackBarEvents(int pos, void *);
50 int waitTime = 10;
51 std::map<int,cv::Mat> frame_pose_map;//set of poses and the frames they were detected
52 
53 class CmdLineParser{int argc; char **argv; public: CmdLineParser(int _argc,char **_argv):argc(_argc),argv(_argv){} bool operator[] ( string param ) {int idx=-1; for ( int i=0; i<argc && idx==-1; i++ ) if ( string ( argv[i] ) ==param ) idx=i; return ( idx!=-1 ) ; } string operator()(string param,string defvalue="-1"){int idx=-1; for ( int i=0; i<argc && idx==-1; i++ ) if ( string ( argv[i] ) ==param ) idx=i; if ( idx==-1 ) return defvalue; else return ( argv[ idx+1] ); }};
54 
55 void savePCDFile(string fpath,const aruco::MarkerMap &ms,const std::map<int,cv::Mat> frame_pose_map)throw(std::exception) ;
56 void savePosesToFile(string filename,const std::map<int,cv::Mat> &fmp);
57 
58 /************************************
59  *
60  *
61  *
62  *
63  ************************************/
64 
65 
66 
67 void processKey(char k) {
68  switch (k) {
69  case 's':
70  if (waitTime == 0)
71  waitTime = 10;
72  else
73  waitTime = 0;
74  break;
75 
76  }
77 }
78 
79 /************************************
80  *
81  *
82  *
83  *
84  ************************************/
85 int main(int argc, char **argv) {
86  try {
87  CmdLineParser cml(argc,argv);
88  if (argc < 3|| cml["-h"]) {
89  cerr << "Invalid number of arguments" << endl;
90  cerr << "Usage: (in.avi|live) marksetconfig.yml [optional_arguments] \n\t[-c camera_intrinsics.yml] \n\t[-s marker_size] \n\t[-pcd out_pcd_file_with_camera_poses] \n\t[-poses out_file_with_poses] \n\t[-corner <corner_refinement_method> (0: LINES(default),1 SUBPIX) ][-h]" << endl;
91  return false;
92  }
93  TheMarkerMapConfig.readFromFile(argv[2]);
94 
95  TheMarkerMapConfigFile = argv[2];
96  TheMarkerSize = stof( cml("-s","1"));
97  // read from camera or from file
98  if (string(argv[1])== "live") {
99  TheVideoCapturer.open(0);
100  } else TheVideoCapturer.open(argv[1]);
101  // check video is open
102  if (!TheVideoCapturer.isOpened()) throw std::runtime_error("Could not open video");
103 
104  // read first image to get the dimensions
106 
107  // read camera parameters if passed
108  if (cml["-c"]) {
109  TheCameraParameters.readFromXMLFile(cml("-c"));
110  TheCameraParameters.resize(TheInputImage.size());
111  }
112  //prepare the detector
113  string dict=TheMarkerMapConfig.getDictionary();//see if the dictrionary is already indicated in the configuration file. It should!
114  if(dict.empty()) dict="ARUCO";
115  TheMarkerDetector.setDictionary(dict);
116  if (stoi(cml("-corner","0"))==0)
117  TheMarkerDetector.setCornerRefinementMethod(MarkerDetector::LINES);
118  else{
119  MarkerDetector::Params params=TheMarkerDetector.getParams();
120  params._cornerMethod=MarkerDetector::SUBPIX;
121  params._subpix_wsize= (15./2000.)*float(TheInputImage.cols) ;//search corner subpix in a 5x5 widow area
122  TheMarkerDetector.setParams(params);
123  }
124 
125  //prepare the pose tracker if possible
126  //if the camera parameers are avaiable, and the markerset can be expressed in meters, then go
127 
128  if ( TheMarkerMapConfig.isExpressedInPixels() && TheMarkerSize>0)
129  TheMarkerMapConfig=TheMarkerMapConfig.convertToMeters(TheMarkerSize);
130 cout<<"TheCameraParameters.isValid()="<<TheCameraParameters.isValid()<<" "<<TheMarkerMapConfig.isExpressedInMeters()<<endl;
131  if (TheCameraParameters.isValid() && TheMarkerMapConfig.isExpressedInMeters() )
132  TheMSPoseTracker.setParams(TheCameraParameters,TheMarkerMapConfig);
133 
134 
135  // Create gui
136 
137  cv::namedWindow("thres", 1);
138  cv::namedWindow("in", 1);
139 
140  TheMarkerDetector.getThresholdParams(ThresParam1, ThresParam2);
143  cv::createTrackbar("ThresParam1", "in", &iThresParam1, 13, cvTackBarEvents);
144  cv::createTrackbar("ThresParam2", "in", &iThresParam2, 13, cvTackBarEvents);
145  char key = 0;
146  int index = 0;
147  // capture until press ESC or until the end of the video
148  cout<<"Press 's' to start/stop video"<<endl;
149  do {
150  TheVideoCapturer.retrieve(TheInputImage);
151  TheInputImage.copyTo(TheInputImageCopy);
152  index++; // number of images captured
153  // Detection of the board
154  vector<aruco::Marker> detected_markers=TheMarkerDetector.detect(TheInputImage);
155  //print the markers detected that belongs to the markerset
156  for(auto idx:TheMarkerMapConfig.getIndices(detected_markers))
157  detected_markers[idx].draw(TheInputImageCopy, Scalar(0, 0, 255), 2);
158  //detect 3d info if possible
159  if (TheMSPoseTracker.isValid()){
160  if ( TheMSPoseTracker.estimatePose(detected_markers)){
161  aruco::CvDrawingUtils::draw3dAxis(TheInputImageCopy, TheCameraParameters,TheMSPoseTracker.getRvec(),TheMSPoseTracker.getTvec(),TheMarkerMapConfig[0].getMarkerSize()*2);
162  frame_pose_map.insert(make_pair(index,TheMSPoseTracker.getRTMatrix() ));
163  cout<<"pose rt="<<TheMSPoseTracker.getRvec()<<" "<<TheMSPoseTracker.getTvec()<<endl;
164  }
165  }
166 
167  // show input with augmented information and the thresholded image
168  cv::imshow("in", TheInputImageCopy);
169  cv::imshow("thres",TheMarkerDetector.getThresholdedImage());
170 
171  key = cv::waitKey(waitTime); // wait for key to be pressed
172  processKey(key);
173 
174  } while (key != 27 && TheVideoCapturer.grab() );
175 
176 
177 
178  //save a beatiful pcd file (pcl library) showing the results (you can use pcl_viewer to see it)
179  if (cml["-pcd"]){
180  savePCDFile(cml("-pcd"),TheMarkerMapConfig,frame_pose_map);
181  }
182 
183  //save the poses to a file in tum rgbd data format
184  if (cml["-poses"]){
185  savePosesToFile(cml("-poses"),frame_pose_map);
186  }
187 
188  } catch (std::exception &ex)
189 
190  {
191  cout << "Exception :" << ex.what() << endl;
192  }
193 }
194 /************************************
195  *
196  *
197  *
198  *
199  ************************************/
200 
201 void cvTackBarEvents(int pos, void *) {
202  (void)(pos);
203  if (iThresParam1 < 3)
204  iThresParam1 = 3;
205  if (iThresParam1 % 2 != 1)
206  iThresParam1++;
207  if (ThresParam2 < 1)
208  ThresParam2 = 1;
211  TheMarkerDetector.setThresholdParams(ThresParam1, ThresParam2);
212 
213 
214 
215 
216  //detect, print, get pose, and print
217 
218  //detect
219  vector<aruco::Marker> detected_markers=TheMarkerDetector.detect(TheInputImage);
220  //print the markers detected that belongs to the markerset
221  for(auto idx:TheMarkerMapConfig.getIndices(detected_markers))
222  detected_markers[idx].draw(TheInputImageCopy, Scalar(0, 0, 255), 2);
223  //detect 3d info if possible
224  if (TheMSPoseTracker.isValid()){
225  TheMSPoseTracker.estimatePose(detected_markers);
226  aruco::CvDrawingUtils::draw3dAxis(TheInputImageCopy, TheCameraParameters,TheMSPoseTracker.getRvec(),TheMSPoseTracker.getTvec(),TheMarkerMapConfig[0].getMarkerSize()*2);
227  }
228 
229 
230  cv::imshow("in", TheInputImageCopy);
231  cv::imshow("thres",TheMarkerDetector.getThresholdedImage());
232 }
233 
234 inline float SIGN(float x) {return (x >= 0.0f) ? +1.0f : -1.0f;}
235 inline float NORM(float a, float b, float c, float d) {return sqrt(a * a + b * b + c * c + d * d);}
236 
237 
238 void getQuaternionAndTranslationfromMatrix44(const cv::Mat &M_in ,float &qx,float &qy,float &qz,float &qw,float &tx,float &ty,float &tz){
239  //get the 3d part of matrix and get quaternion
240  assert(M_in.total()==16);
241  cv::Mat M;M_in.convertTo(M,CV_32F);
242  //use now eigen
243  float r11=M.at<float>(0,0);
244  float r12=M.at<float>(0,1);
245  float r13=M.at<float>(0,2);
246  float r21=M.at<float>(1,0);
247  float r22=M.at<float>(1,1);
248  float r23=M.at<float>(1,2);
249  float r31=M.at<float>(2,0);
250  float r32=M.at<float>(2,1);
251  float r33=M.at<float>(2,2);
252 
253 
254 
255  double q0 = ( r11 + r22 + r33 + 1.0f) / 4.0f;
256  double q1 = ( r11 - r22 - r33 + 1.0f) / 4.0f;
257  double q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
258  double q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
259  if(q0 < 0.0f) q0 = 0.0f;
260  if(q1 < 0.0f) q1 = 0.0f;
261  if(q2 < 0.0f) q2 = 0.0f;
262  if(q3 < 0.0f) q3 = 0.0f;
263  q0 = sqrt(q0);
264  q1 = sqrt(q1);
265  q2 = sqrt(q2);
266  q3 = sqrt(q3);
267  if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
268  q0 *= +1.0f;
269  q1 *= SIGN(r32 - r23);
270  q2 *= SIGN(r13 - r31);
271  q3 *= SIGN(r21 - r12);
272  } else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
273  q0 *= SIGN(r32 - r23);
274  q1 *= +1.0f;
275  q2 *= SIGN(r21 + r12);
276  q3 *= SIGN(r13 + r31);
277  } else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
278  q0 *= SIGN(r13 - r31);
279  q1 *= SIGN(r21 + r12);
280  q2 *= +1.0f;
281  q3 *= SIGN(r32 + r23);
282  } else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
283  q0 *= SIGN(r21 - r12);
284  q1 *= SIGN(r31 + r13);
285  q2 *= SIGN(r32 + r23);
286  q3 *= +1.0f;
287  } else {
288  cerr<<"Coding error"<<endl;
289  }
290  double r = NORM(q0, q1, q2, q3);
291  qx =q0/ r;
292  qy =q1/ r;
293  qz =q2/ r;
294  qw =q3/ r;
295 
296 
297 
298  tx=M.at<float>(0,3);
299  ty=M.at<float>(1,3);
300  tz=M.at<float>(2,3);
301 
302 
303 }
304 void savePosesToFile(string filename,const std::map<int,cv::Mat> &fmp){
305  std::ofstream file(filename);
306  float qx,qy,qz,qw,tx,ty,tz;
307  for(auto frame:fmp){
308  if ( !frame.second.empty()){
309  getQuaternionAndTranslationfromMatrix44(frame.second,qx,qy,qz,qw,tx,ty,tz);
310  file<<frame.first<<" "<<tx<<" "<<ty<<" "<<tz<<" "<<qx<<" "<<qy<<" "<<qz<<" "<<qw<<endl;
311  }
312  }
313 }
std::map< int, cv::Mat > frame_pose_map
void setThresholdParams(double param1, double param2)
const cv::Mat getTvec() const
Definition: posetracker.h:104
filename
bool param(const std::string &param_name, T &param_val, const T &default_val)
void resize(cv::Size size)
Params getParams() const
void processKey(char k)
string TheMarkerMapConfigFile
f
bool isExpressedInPixels() const
Definition: markermap.h:90
void setCornerRefinementMethod(CornerRefinementMethod method, int val=-1)
bool isExpressedInMeters() const
Definition: markermap.h:87
const cv::Mat getRvec() const
Definition: posetracker.h:102
void getThresholdParams(double &param1, double &param2) const
double ThresParam1
CameraParameters TheCameraParameters
void setDictionary(std::string dict_type, float error_correction_rate=0)
int iThresParam1
std::vector< aruco::Marker > detect(const cv::Mat &input)
float TheMarkerSize
void readFromFile(string sfile)
Definition: markermap.cpp:89
static void draw3dAxis(cv::Mat &Image, const CameraParameters &CP, const cv::Mat &Rvec, const cv::Mat &Tvec, float axis_size)
cv::Mat getRTMatrix() const
bool estimatePose(const vector< Marker > &v_m)
float SIGN(float x)
void readFromXMLFile(string filePath)
void cvTackBarEvents(int pos, void *)
void setParams(const CameraParameters &cam_params, const MarkerMap &msconf, float markerSize=-1)
MarkerMapPoseTracker TheMSPoseTracker
void setParams(Params p)
int main(int argc, char **argv)
Mat TheInputImage
MarkerMap convertToMeters(float markerSize)
Definition: markermap.cpp:297
MarkerMap TheMarkerMapConfig
void savePCDFile(string fpath, const aruco::MarkerMap &ms, const std::map< int, cv::Mat > frame_pose_map)
Definition: pcdwriter.cpp:335
void getQuaternionAndTranslationfromMatrix44(const cv::Mat &M_in, float &qx, float &qy, float &qz, float &qw, float &tx, float &ty, float &tz)
int waitTime
Main class for marker detection.
VideoCapture TheVideoCapturer
double ThresParam2
const cv::Mat & getThresholdedImage()
Parameters of the camera.
std::string getDictionary() const
Definition: markermap.h:132
MarkerDetector TheMarkerDetector
bool The3DInfoAvailable
void savePosesToFile(string filename, const std::map< int, cv::Mat > &fmp)
std::vector< int > getIndices(vector< aruco::Marker > &markers)
Definition: markermap.cpp:364
const CwiseUnaryOp< internal::scalar_sqrt_op< Scalar >, const Derived > sqrt() const
float NORM(float a, float b, float c, float d)
CornerRefinementMethod _cornerMethod
Mat TheInputImageCopy
This class defines a set of markers whose locations are attached to a common reference system...
Definition: markermap.h:71
int iThresParam2


tuw_aruco
Author(s): Lukas Pfeifhofer
autogenerated on Mon Jun 10 2019 15:40:45