aruco_test_board_gl_mask.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 #ifdef __APPLE__
32 #include <GLUT/glut.h>
33 #elif _MSC_VER
34 //http://social.msdn.microsoft.com/Forums/eu/vcgeneral/thread/7d6e6fa5-afc2-4370-9a1f-991a76ccb5b7
35 #include <windows.h>
36 #include <GL/gl.h>
37 #include <GL/glut.h>
38 #else
39 #include <GL/gl.h>
40 #include <GL/glut.h>
41 #endif
42 
43 #include <opencv2/highgui/highgui.hpp>
44 #include <opencv2/imgproc/imgproc.hpp>
45 #include <opencv2/calib3d/calib3d.hpp>
46 #include "aruco.h"
47 #include "markermap.h"
48 
49 void __glGetModelViewMatrix(double modelview_matrix[16],const cv::Mat &Rvec,const cv::Mat &Tvec) throw(cv::Exception) ;
50 using namespace cv;
51 using namespace aruco;
52 
53  bool The3DInfoAvailable=false;
54 float TheMarkerSize=-1;
56 VideoCapture TheVideoCapturer;
57 vector<Marker> TheMarkers;
64 bool TheCaptureFlag=true;
65 void vDrawScene();
66 void vIdle();
67 void vResize( GLsizei iWidth, GLsizei iHeight );
68 void vMouse(int b,int s,int x,int y);
69 
70 
71 
72 /************************************
73  *
74  *
75  *
76  *
77  ************************************/
78 
79 int main(int argc,char **argv)
80 {
81  try
82  {
83  if (argc!=5) {
84  cerr<<"Invalid number of arguments"<<endl;
85  cerr<<"Usage: (in.avi|live) markermap_config.yml intrinsics.yml size "<<endl;
86  cerr<<"WARNING: this test creates a synthetic mask consisting of a single rectangle. "<<endl;
87  cerr<<"WARNING: The only purpose is to show how to create an AR application with mask in OpenGL "<<endl;
88  return false;
89  }
90  TheCameraParams.readFromXMLFile(argv[3]);
91  //read board configuration
92  TheMMConfig.readFromFile(argv[2]);
93  TheMarkerSize=atof(argv[4]);
94  if (TheMMConfig.isExpressedInPixels())
95  TheMMConfig=TheMMConfig.convertToMeters(TheMarkerSize);
96 
97  MMPoseTracker.setParams(TheCameraParams,TheMMConfig);
98  //Open video input source
99  if (string(argv[1])=="live") //read from camera
100  TheVideoCapturer.open(0);
101  else TheVideoCapturer.open(argv[1]);
102 
103  if (!TheVideoCapturer.isOpened()) throw std::runtime_error("could not open video");
104 
105  //read first image
106  TheVideoCapturer>>TheInputImage;
107  //read camera paramters if passed
108  TheCameraParams.readFromXMLFile(argv[3]);
109  TheCameraParams.resize( TheInputImage.size());
110 
111  TheMarkerDetector.setThresholdParams(25,7);
112 
113  glutInit(&argc, argv);
114  glutInitWindowPosition( 0, 0);
115  glutInitWindowSize(TheInputImage.size().width,TheInputImage.size().height);
116  glutInitDisplayMode( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE );
117  glutCreateWindow( "ArUco" );
118  glutDisplayFunc( vDrawScene );
119  glutIdleFunc( vIdle );
120  glutReshapeFunc( vResize );
121  glutMouseFunc(vMouse);
122  glClearColor( 0.0, 0.0, 0.0, 1.0 );
123  glClearDepth( 1.0 );
124 
125  // these two are necesary for the mask effect
126  glEnable( GL_ALPHA_TEST );
127  glAlphaFunc( GL_GREATER, 0.5 );
128 
129  TheGlWindowSize=TheInputImage.size();
130  vResize(TheGlWindowSize.width,TheGlWindowSize.height);
131  glutMainLoop();
132 
133  } catch (std::exception &ex)
134 
135  {
136  cout<<"Exception :"<<ex.what()<<endl;
137  }
138 
139 }
140 
141 
142 
143 /************************************
144  *
145  *
146  *
147  *
148  ************************************/
149 
150 cv::Mat createSyntheticMask(const cv::Mat &img) {
151  // just create a mask consisting of a rectangle in the middle of the image
152  // very simple but enough to show how to employ mask in opengl
153  cv::Mat mask(img.size(), CV_8UC1, cv::Scalar::all(255)); // 255 means it is hidden
154  cv::rectangle(mask,cv::Rect(img.cols/4, img.rows/4, img.cols/2, img.rows/2), cv::Scalar(0), CV_FILLED); //create visible (0) rectangle
155  return mask;
156 }
157 
158 
159 
160 
161 /************************************
162  *
163  *
164  *
165  *
166  ************************************/
167 
168 cv::Mat createMultiChannelMask(const cv::Mat &img, const cv::Mat &mask)
169 {
170  cv::Mat out(img.size(), CV_8UC4, cv::Scalar::all(0));
171  for(int i=0; i<img.total(); i++) {
172  for(int j=0; j<3; j++) out.ptr<cv::Vec4b>()[i][j] = img.ptr<cv::Vec3b>()[i][j];
173  if(mask.size()==img.size()) out.ptr<cv::Vec4b>()[i][3] = mask.ptr<unsigned char>()[i];
174  }
175  return out;
176 }
177 
178 
179 
180 
181 
182 
183 
184 
185 /************************************
186  *
187  *
188  *
189  *
190  ************************************/
191 
192 void vMouse(int b,int s,int x,int y)
193 {
194  if (b==GLUT_LEFT_BUTTON && s==GLUT_DOWN) {
195  TheCaptureFlag=!TheCaptureFlag;
196  }
197 
198 }
199 
200 /************************************
201  *
202  *
203  *
204  *
205  ************************************/
206 void axis(float size)
207 {
208  glColor3f (1,0,0 );
209  glBegin(GL_LINES);
210  glVertex3f(0.0f, 0.0f, 0.0f); // origin of the line
211  glVertex3f(size,0.0f, 0.0f); // ending point of the line
212  glEnd( );
213 
214  glColor3f ( 0,1,0 );
215  glBegin(GL_LINES);
216  glVertex3f(0.0f, 0.0f, 0.0f); // origin of the line
217  glVertex3f( 0.0f,size, 0.0f); // ending point of the line
218  glEnd( );
219 
220 
221  glColor3f (0,0,1 );
222  glBegin(GL_LINES);
223  glVertex3f(0.0f, 0.0f, 0.0f); // origin of the line
224  glVertex3f(0.0f, 0.0f, size); // ending point of the line
225  glEnd( );
226 
227 
228 }
229 /************************************
230  *
231  *
232  *
233  *
234  ************************************/
236 {
237  if (TheResizedImage.rows==0) //prevent from going on until the image is initialized
238  return;
240  glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
242  glMatrixMode(GL_MODELVIEW);
243  glLoadIdentity();
244  glMatrixMode(GL_PROJECTION);
245  glLoadIdentity();
246  glOrtho(0, TheGlWindowSize.width, 0, TheGlWindowSize.height, -1.0, 1.0);
247  glViewport(0, 0, TheGlWindowSize.width , TheGlWindowSize.height);
248  glDisable(GL_TEXTURE_2D);
249  glPixelZoom( 1, -1);
250  glRasterPos3f( 0, TheGlWindowSize.height - 0.5, -1.0 );
251  glDrawPixels ( TheGlWindowSize.width , TheGlWindowSize.height , GL_RGB , GL_UNSIGNED_BYTE , TheResizedImage.ptr(0) );
253  //like the real camera (without distorsion)
254  glMatrixMode(GL_PROJECTION);
255  double proj_matrix[16];
256  TheCameraParams.glGetProjectionMatrix(TheInputImage.size(),TheGlWindowSize,proj_matrix,0.05,10);
257  glLoadIdentity();
258  glLoadMatrixd(proj_matrix);
259  glLineWidth(2);
260  //now, for each marker,
261  double modelview_matrix[16];
262 
263 // for (unsigned int m=0;m<TheMarkers.size();m++)
264 // {
265 // TheMarkers[m].glGetModelViewMatrix(modelview_matrix);
266 // glMatrixMode(GL_MODELVIEW);
267 // glLoadIdentity();
268 // glLoadMatrixd(modelview_matrix);
269 // // axis(TheMarkerSize);
270 // glColor3f(1,0.4,0.4);
271 // glTranslatef(0, TheMarkerSize/2,0);
272 // glPushMatrix();
273 // glutWireCube( TheMarkerSize );
274 //
275 // glPopMatrix();
276 // }
277  //If the board is detected with enough probability
278  if (!MMPoseTracker.getRTMatrix().empty()) {
279  __glGetModelViewMatrix(modelview_matrix,MMPoseTracker.getRvec(),MMPoseTracker.getTvec());
280  glMatrixMode(GL_MODELVIEW);
281  glLoadIdentity();
282  glLoadMatrixd(modelview_matrix);
283  glColor3f(0,1,0);
284  axis(TheMarkerSize);
285  cerr<<TheMarkerSize<<endl;cin.ignore();
286 // if(TheMarkerDetector.isYPerpendicular()) glTranslatef(0,TheMarkerSize/2,0);
287 // else
288  glTranslatef(0,0,TheMarkerSize/2);
289  glPushMatrix();
290  // glutWireCube( TheMarkerSize );
291  glutWireTeapot(2*TheMarkerSize);
292  glPopMatrix();
293  }
294 
295  // After drawing everything, now draw mask
296  cv::Mat multiChannelMask = createMultiChannelMask(TheResizedImage, TheMask);
297  // now redraw to create the mask effect, and thats all
298  glDrawPixels ( TheGlWindowSize.width , TheGlWindowSize.height , GL_RGBA , GL_UNSIGNED_BYTE , multiChannelMask.ptr(0) );
299 
300  glutSwapBuffers();
301 
302 }
303 
304 
305 /************************************
306  *
307  *
308  *
309  *
310  ************************************/
311 void vIdle()
312 {
313  if (TheCaptureFlag) {
314  //capture image
315  TheVideoCapturer.grab();
316  TheVideoCapturer.retrieve( TheInputImage);
317  TheUndInputImage.create(TheInputImage.size(),CV_8UC3);
318  //by deafult, opencv works in BGR, so we must convert to RGB because OpenGL in windows preffer
319  cv::cvtColor(TheInputImage,TheInputImage,CV_BGR2RGB);
320  //remove distorion in image
321  cv::undistort(TheInputImage,TheUndInputImage, TheCameraParams.CameraMatrix,TheCameraParams.Distorsion);
322  //detect markers
323  TheMarkers=MDetector.detect(TheUndInputImage);
324  MMPoseTracker.estimatePose(TheMarkers);
325  //resize the image to the size of the GL window
326  cv::resize(TheUndInputImage,TheResizedImage,TheGlWindowSize);
327  // create mask. It is a syntetic mask consisting of a simple rectangle, just to show a example of opengl with mask
328  TheMask = createSyntheticMask(TheResizedImage); // lets create with the same size of the resized image, i.e. the size of the opengl window
329  }
330  glutPostRedisplay();
331 }
332 
333 
334 /************************************
335  *
336  *
337  *
338  *
339  ************************************/
340 void vResize( GLsizei iWidth, GLsizei iHeight )
341 {
342  TheGlWindowSize=Size(iWidth,iHeight);
343  //not all sizes are allowed. OpenCv images have padding at the end of each line in these that are not aligned to 4 bytes
344  if (iWidth*3%4!=0) {
345  iWidth+=iWidth*3%4;//resize to avoid padding
346  vResize(iWidth,TheGlWindowSize.height);
347  }
348  else {
349  //resize the image to the size of the GL window
350  if (TheUndInputImage.rows!=0)
351  cv::resize(TheUndInputImage,TheResizedImage,TheGlWindowSize);
352  }
353 }
354 
355 
356 void __glGetModelViewMatrix(double modelview_matrix[16],const cv::Mat &Rvec,const cv::Mat &Tvec) throw(cv::Exception) {
357  assert(Tvec.type()==CV_32F);
358  // check if paremeters are valid
359  Mat Rot(3, 3, CV_32FC1), Jacob;
360  Rodrigues(Rvec, Rot, Jacob);
361 
362  double para[3][4];
363  for (int i = 0; i < 3; i++)
364  for (int j = 0; j < 3; j++)
365  para[i][j] = Rot.at< float >(i, j);
366  // now, add the translation
367  para[0][3] = Tvec.ptr< float >(0)[0];
368  para[1][3] = Tvec.ptr< float >(0)[1];
369  para[2][3] = Tvec.ptr< float >(0)[2];
370  double scale = 1;
371 
372  modelview_matrix[0 + 0 * 4] = para[0][0];
373  // R1C2
374  modelview_matrix[0 + 1 * 4] = para[0][1];
375  modelview_matrix[0 + 2 * 4] = para[0][2];
376  modelview_matrix[0 + 3 * 4] = para[0][3];
377  // R2
378  modelview_matrix[1 + 0 * 4] = para[1][0];
379  modelview_matrix[1 + 1 * 4] = para[1][1];
380  modelview_matrix[1 + 2 * 4] = para[1][2];
381  modelview_matrix[1 + 3 * 4] = para[1][3];
382  // R3
383  modelview_matrix[2 + 0 * 4] = -para[2][0];
384  modelview_matrix[2 + 1 * 4] = -para[2][1];
385  modelview_matrix[2 + 2 * 4] = -para[2][2];
386  modelview_matrix[2 + 3 * 4] = -para[2][3];
387  modelview_matrix[3 + 0 * 4] = 0.0;
388  modelview_matrix[3 + 1 * 4] = 0.0;
389  modelview_matrix[3 + 2 * 4] = 0.0;
390  modelview_matrix[3 + 3 * 4] = 1.0;
391  if (scale != 0.0) {
392  modelview_matrix[12] *= scale;
393  modelview_matrix[13] *= scale;
394  modelview_matrix[14] *= scale;
395  }
396 }
MarkerMap TheMMConfig
void vResize(GLsizei iWidth, GLsizei iHeight)
void setThresholdParams(double param1, double param2)
const cv::Mat getTvec() const
Definition: posetracker.h:104
Size TheGlWindowSize
Mat TheResizedImage
void resize(cv::Size size)
f
bool isExpressedInPixels() const
Definition: markermap.h:90
int main(int argc, char **argv)
const cv::Mat getRvec() const
Definition: posetracker.h:102
void glGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, bool invert=false)
void vDrawScene()
std::vector< aruco::Marker > detect(const cv::Mat &input)
void readFromFile(string sfile)
Definition: markermap.cpp:89
cv::Mat getRTMatrix() const
bool estimatePose(const vector< Marker > &v_m)
void readFromXMLFile(string filePath)
bool The3DInfoAvailable
void setParams(const CameraParameters &cam_params, const MarkerMap &msconf, float markerSize=-1)
MarkerDetector TheMarkerDetector
float TheMarkerSize
cv::Mat createSyntheticMask(const cv::Mat &img)
MarkerMap convertToMeters(float markerSize)
Definition: markermap.cpp:297
void __glGetModelViewMatrix(double modelview_matrix[16], const cv::Mat &Rvec, const cv::Mat &Tvec)
Main class for marker detection.
void axis(float size)
Parameters of the camera.
vector< Marker > TheMarkers
MarkerMapPoseTracker MMPoseTracker
Mat TheUndInputImage
VideoCapture TheVideoCapturer
cv::Mat createMultiChannelMask(const cv::Mat &img, const cv::Mat &mask)
This class defines a set of markers whose locations are attached to a common reference system...
Definition: markermap.h:71
MarkerDetector MDetector
CameraParameters TheCameraParams
void vMouse(int b, int s, int x, int y)
bool TheCaptureFlag


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