samples/cpp/Qt_sample/qt_opengl.cpp

   1 // Yannick Verdie 2010
   2 // --- Please read help() below: ---
   3
   4 #include <iostream>
   5 #include <vector>
   6
   7 #include <opencv2/calib3d/calib3d.hpp>
   8 #include <opencv2/calib3d/calib3d_c.h>
   9 #include <opencv2/core/core.hpp>
  10 #include <opencv2/highgui/highgui.hpp>
  11 #include <opencv2/imgproc/imgproc.hpp>
  12 #include <opencv2/legacy/compat.hpp>
  13
  14 #ifdef __APPLE__
  15 #include <OpenGL/gl.h>
  16 #else
  17 #include <GL/gl.h>
  18 #endif
  19
  20 using namespace std;
  21 using namespace cv;
  22
  23 static void help()
  24 {
  25     cout << "This demo demonstrates the use of the Qt enhanced version of the highgui GUI interface\n"
  26             "and dang if it doesn't throw in the use of of the POSIT 3D tracking algorithm too\n"
  27             "It works off of the video: cube4.avi\n"
  28             "Using OpenCV version " << CV_VERSION << "\n\n"
  29
  30             " 1) This demo is mainly based on work from Javier Barandiaran Martirena\n"
  31             "    See this page http://code.opencv.org/projects/opencv/wiki/Posit.\n"
  32             " 2) This is a demo to illustrate how to use **OpenGL Callback**.\n"
  33             " 3) You need Qt binding to compile this sample with OpenGL support enabled.\n"
  34             " 4) The features' detection is very basic and could highly be improved\n"
  35             "    (basic thresholding tuned for the specific video) but 2).\n"
  36             " 5) Thanks to Google Summer of Code 2010 for supporting this work!\n" << endl;
  37 }
  38
  39 #define FOCAL_LENGTH 600
  40 #define CUBE_SIZE 0.5
  41
  42 static void renderCube(float size)
  43 {
  44     glBegin(GL_QUADS);
  45     // Front Face
  46     glNormal3f( 0.0f, 0.0f, 1.0f);
  47     glVertex3f( 0.0f,  0.0f,  0.0f);
  48     glVertex3f( size,  0.0f,  0.0f);
  49     glVertex3f( size,  size,  0.0f);
  50     glVertex3f( 0.0f,  size,  0.0f);
  51     // Back Face
  52     glNormal3f( 0.0f, 0.0f,-1.0f);
  53     glVertex3f( 0.0f,  0.0f, size);
  54     glVertex3f( 0.0f,  size, size);
  55     glVertex3f( size,  size, size);
  56     glVertex3f( size,  0.0f, size);
  57     // Top Face
  58     glNormal3f( 0.0f, 1.0f, 0.0f);
  59     glVertex3f( 0.0f,  size,  0.0f);
  60     glVertex3f( size,  size,  0.0f);
  61     glVertex3f( size,  size, size);
  62     glVertex3f( 0.0f,  size, size);
  63     // Bottom Face
  64     glNormal3f( 0.0f,-1.0f, 0.0f);
  65     glVertex3f( 0.0f,  0.0f,  0.0f);
  66     glVertex3f( 0.0f,  0.0f, size);
  67     glVertex3f( size,  0.0f, size);
  68     glVertex3f( size,  0.0f,  0.0f);
  69     // Right face
  70     glNormal3f( 1.0f, 0.0f, 0.0f);
  71     glVertex3f( size,  0.0f, 0.0f);
  72     glVertex3f( size,  0.0f, size);
  73     glVertex3f( size,  size, size);
  74     glVertex3f( size,  size, 0.0f);
  75     // Left Face
  76     glNormal3f(-1.0f, 0.0f, 0.0f);
  77     glVertex3f( 0.0f,  0.0f, 0.0f);
  78     glVertex3f( 0.0f,  size, 0.0f);
  79     glVertex3f( 0.0f,  size, size);
  80     glVertex3f( 0.0f,  0.0f, size);
  81     glEnd();
  82 }
  83
  84 static void on_opengl(void* param)
  85 {
  86     //Draw the object with the estimated pose
  87     glLoadIdentity();
  88     glScalef( 1.0f, 1.0f, -1.0f);
  89     glMultMatrixf( (float*)param );
  90     glEnable( GL_LIGHTING );
  91     glEnable( GL_LIGHT0 );
  92     glEnable( GL_BLEND );
  93     glBlendFunc(GL_SRC_ALPHA, GL_ONE);
  94     renderCube( CUBE_SIZE );
  95     glDisable(GL_BLEND);
  96     glDisable( GL_LIGHTING );
  97 }
  98
  99 static void initPOSIT(std::vector<CvPoint3D32f> * modelPoints)
 100 {
 101     // Create the model pointss
 102     modelPoints->push_back(cvPoint3D32f(0.0f, 0.0f, 0.0f)); // The first must be (0, 0, 0)
 103     modelPoints->push_back(cvPoint3D32f(0.0f, 0.0f, CUBE_SIZE));
 104     modelPoints->push_back(cvPoint3D32f(CUBE_SIZE, 0.0f, 0.0f));
 105     modelPoints->push_back(cvPoint3D32f(0.0f, CUBE_SIZE, 0.0f));
 106 }
 107
 108 static void foundCorners(vector<CvPoint2D32f> * srcImagePoints, const Mat & source, Mat & grayImage)
 109 {
 110     cvtColor(source, grayImage, COLOR_RGB2GRAY);
 111     GaussianBlur(grayImage, grayImage, Size(11, 11), 0, 0);
 112     normalize(grayImage, grayImage, 0, 255, NORM_MINMAX);
 113     threshold(grayImage, grayImage, 26, 255, THRESH_BINARY_INV); //25
 114
 115     Mat MgrayImage = grayImage;
 116     vector<vector<Point> > contours;
 117     vector<Vec4i> hierarchy;
 118     findContours(MgrayImage, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
 119
 120     Point p;
 121     vector<CvPoint2D32f> srcImagePoints_temp(4, cvPoint2D32f(0, 0));
 122
 123     if (contours.size() == srcImagePoints_temp.size())
 124     {
 125         for (size_t i = 0; i < contours.size(); i++ )
 126         {
 127             p.x = p.y = 0;
 128
 129             for (size_t j = 0 ; j < contours[i].size(); j++)
 130                 p += contours[i][j];
 131
 132             srcImagePoints_temp.at(i) = cvPoint2D32f(float(p.x) / contours[i].size(), float(p.y) / contours[i].size());
 133         }
 134
 135         // Need to keep the same order
 136         // > y = 0
 137         // > x = 1
 138         // < x = 2
 139         // < y = 3
 140
 141         // get point 0;
 142         size_t index = 0;
 143         for (size_t i = 1 ; i<srcImagePoints_temp.size(); i++)
 144             if (srcImagePoints_temp.at(i).y > srcImagePoints_temp.at(index).y)
 145                 index = i;
 146         srcImagePoints->at(0) = srcImagePoints_temp.at(index);
 147
 148         // get point 1;
 149         index = 0;
 150         for (size_t i = 1 ; i<srcImagePoints_temp.size(); i++)
 151             if (srcImagePoints_temp.at(i).x > srcImagePoints_temp.at(index).x)
 152                 index = i;
 153         srcImagePoints->at(1) = srcImagePoints_temp.at(index);
 154
 155         // get point 2;
 156         index = 0;
 157         for (size_t i = 1 ; i<srcImagePoints_temp.size(); i++)
 158             if (srcImagePoints_temp.at(i).x < srcImagePoints_temp.at(index).x)
 159                 index = i;
 160         srcImagePoints->at(2) = srcImagePoints_temp.at(index);
 161
 162         // get point 3;
 163         index = 0;
 164         for (size_t i = 1 ; i<srcImagePoints_temp.size(); i++ )
 165             if (srcImagePoints_temp.at(i).y < srcImagePoints_temp.at(index).y)
 166                 index = i;
 167         srcImagePoints->at(3) = srcImagePoints_temp.at(index);
 168
 169         Mat Msource = source;
 170         stringstream ss;
 171         for (size_t i = 0; i<srcImagePoints_temp.size(); i++ )
 172         {
 173             ss << i;
 174             circle(Msource, srcImagePoints->at(i), 5, Scalar(0, 0, 255));
 175             putText(Msource, ss.str(), srcImagePoints->at(i), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255));
 176             ss.str("");
 177
 178             // new coordinate system in the middle of the frame and reversed (camera coordinate system)
 179             srcImagePoints->at(i) = cvPoint2D32f(srcImagePoints_temp.at(i).x - source.cols / 2,
 180                                                  source.rows / 2 - srcImagePoints_temp.at(i).y);
 181         }
 182     }
 183 }
 184
 185 static void createOpenGLMatrixFrom(float * posePOSIT, const CvMatr32f & rotationMatrix,
 186                                    const CvVect32f & translationVector)
 187 {
 188     // coordinate system returned is relative to the first 3D input point
 189     for (int f = 0; f < 3; f++)
 190         for (int c = 0; c < 3; c++)
 191             posePOSIT[c * 4 + f] = rotationMatrix[f * 3 + c]; // transposed
 192
 193     posePOSIT[3] = translationVector[0];
 194     posePOSIT[7] = translationVector[1];
 195     posePOSIT[11] = translationVector[2];
 196     posePOSIT[12] = 0.0f;
 197     posePOSIT[13] = 0.0f;
 198     posePOSIT[14] = 0.0f;
 199     posePOSIT[15] = 1.0f;
 200 }
 201
 202 int main(void)
 203 {
 204     help();
 205
 206     string fileName = "cube4.avi";
 207     VideoCapture video(fileName);
 208     if (!video.isOpened())
 209     {
 210         cerr << "Video file " << fileName << " could not be opened" << endl;
 211         return EXIT_FAILURE;
 212     }
 213
 214     Mat source, grayImage;
 215     video >> source;
 216
 217     namedWindow("Original", WINDOW_AUTOSIZE | WINDOW_FREERATIO);
 218     namedWindow("POSIT", WINDOW_OPENGL | WINDOW_FREERATIO);
 219     resizeWindow("POSIT", source.cols, source.rows);
 220
 221     displayOverlay("POSIT", "We lost the 4 corners' detection quite often (the red circles disappear).\n"
 222                    "This demo is only to illustrate how to use OpenGL callback.\n"
 223                    " -- Press ESC to exit.", 10000);
 224
 225     float OpenGLMatrix[] = { 0, 0, 0, 0,
 226                              0, 0, 0, 0,
 227                              0, 0, 0, 0,
 228                              0, 0, 0, 0 };
 229     setOpenGlContext("POSIT");
 230     setOpenGlDrawCallback("POSIT", on_opengl, OpenGLMatrix);
 231
 232     vector<CvPoint3D32f> modelPoints;
 233     initPOSIT(&modelPoints);
 234
 235     // Create the POSIT object with the model points
 236     CvPOSITObject* positObject = cvCreatePOSITObject( &modelPoints[0], (int)modelPoints.size());
 237
 238     CvMatr32f rotation_matrix = new float[9];
 239     CvVect32f translation_vector = new float[3];
 240     CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 100, 1e-4f);
 241     vector<CvPoint2D32f> srcImagePoints(4, cvPoint2D32f(0, 0));
 242
 243     while (waitKey(33) != 27)
 244     {
 245         video >> source;
 246         if (source.empty())
 247             break;
 248
 249         imshow("Original", source);
 250
 251         foundCorners(&srcImagePoints, source, grayImage);
 252         cvPOSIT(positObject, &srcImagePoints[0], FOCAL_LENGTH, criteria, rotation_matrix, translation_vector);
 253         createOpenGLMatrixFrom(OpenGLMatrix, rotation_matrix, translation_vector);
 254
 255         updateWindow("POSIT");
 256
 257         if (video.get(CAP_PROP_POS_AVI_RATIO) > 0.99)
 258             video.set(CAP_PROP_POS_AVI_RATIO, 0);
 259     }
 260
 261     setOpenGlDrawCallback("POSIT", NULL, NULL);
 262     destroyAllWindows();
 263     cvReleasePOSITObject(&positObject);
 264
 265     delete[]rotation_matrix;
 266     delete[]translation_vector;
 267
 268     return EXIT_SUCCESS;
 269 }