glpclview.c
Go to the documentation of this file.
1 /*
2  * This file is part of the OpenKinect Project. http://www.openkinect.org
3  *
4  * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
5  * for details.
6  *
7  * Andrew Miller <amiller@dappervision.com>
8  *
9  * This code is licensed to you under the terms of the Apache License, version
10  * 2.0, or, at your option, the terms of the GNU General Public License,
11  * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
12  * or the following URLs:
13  * http://www.apache.org/licenses/LICENSE-2.0
14  * http://www.gnu.org/licenses/gpl-2.0.txt
15  *
16  * If you redistribute this file in source form, modified or unmodified, you
17  * may:
18  * 1) Leave this header intact and distribute it under the same terms,
19  * accompanying it with the APACHE20 and GPL20 files, or
20  * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
21  * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
22  * In all cases you must keep the copyright notice intact and include a copy
23  * of the CONTRIB file.
24  *
25  * Binary distributions must follow the binary distribution requirements of
26  * either License.
27  */
28 
29 #include "libfreenect.h"
30 #include "libfreenect_sync.h"
31 #include <stdio.h>
32 #include <stdlib.h>
33 #include <math.h>
34 
35 #if defined(__APPLE__)
36 #include <GLUT/glut.h>
37 #else
38 #include <GL/glut.h>
39 #endif
40 
41 int window;
42 GLuint gl_rgb_tex;
43 int mx=-1,my=-1; // Prevous mouse coordinates
44 int rotangles[2] = {0}; // Panning angles
45 float zoom = 1; // zoom factor
46 int color = 1; // Use the RGB texture or just draw it as color
47 
48 // Do the projection from u,v,depth to X,Y,Z directly in an opengl matrix
49 // These numbers come from a combination of the ros kinect_node wiki, and
50 // nicolas burrus' posts.
52 {
53  float fx = 594.21f;
54  float fy = 591.04f;
55  float a = -0.0030711f;
56  float b = 3.3309495f;
57  float cx = 339.5f;
58  float cy = 242.7f;
59  GLfloat mat[16] = {
60  1/fx, 0, 0, 0,
61  0, -1/fy, 0, 0,
62  0, 0, 0, a,
63  -cx/fx, cy/fy, -1, b
64  };
65  glMultMatrixf(mat);
66 }
67 
68 
69 // This matrix comes from a combination of nicolas burrus's calibration post
70 // and some python code I haven't documented yet.
72 {
73  float mat[16] = {
74  5.34866271e+02, 3.89654806e+00, 0.00000000e+00, 1.74704200e-02,
75  -4.70724694e+00, -5.28843603e+02, 0.00000000e+00, -1.22753400e-02,
76  -3.19670762e+02, -2.60999685e+02, 0.00000000e+00, -9.99772000e-01,
77  -6.98445586e+00, 3.31139785e+00, 0.00000000e+00, 1.09167360e-02
78  };
79  glMultMatrixf(mat);
80 }
81 
82 void mouseMoved(int x, int y)
83 {
84  if (mx>=0 && my>=0) {
85  rotangles[0] += y-my;
86  rotangles[1] += x-mx;
87  }
88  mx = x;
89  my = y;
90 }
91 
92 void mousePress(int button, int state, int x, int y)
93 {
94  if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
95  mx = x;
96  my = y;
97  }
98  if (button == GLUT_LEFT_BUTTON && state == GLUT_UP) {
99  mx = -1;
100  my = -1;
101  }
102 }
103 
104 void no_kinect_quit(void)
105 {
106  printf("Error: Kinect not connected?\n");
107  exit(1);
108 }
109 
111 {
112  short *depth = 0;
113  char *rgb = 0;
114  uint32_t ts;
115  if (freenect_sync_get_depth((void**)&depth, &ts, 0, FREENECT_DEPTH_11BIT) < 0)
116  no_kinect_quit();
117  if (freenect_sync_get_video((void**)&rgb, &ts, 0, FREENECT_VIDEO_RGB) < 0)
118  no_kinect_quit();
119 
120  static unsigned int indices[480][640];
121  static short xyz[480][640][3];
122  int i,j;
123  for (i = 0; i < 480; i++) {
124  for (j = 0; j < 640; j++) {
125  xyz[i][j][0] = j;
126  xyz[i][j][1] = i;
127  xyz[i][j][2] = depth[i*640+j];
128  indices[i][j] = i*640+j;
129  }
130  }
131 
132  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
133  glLoadIdentity();
134 
135  glPushMatrix();
136  glScalef(zoom,zoom,1);
137  glTranslatef(0,0,-3.5);
138  glRotatef(rotangles[0], 1,0,0);
139  glRotatef(rotangles[1], 0,1,0);
140  glTranslatef(0,0,1.5);
141 
143 
144  // Set the projection from the XYZ to the texture image
145  glMatrixMode(GL_TEXTURE);
146  glLoadIdentity();
147  glScalef(1/640.0f,1/480.0f,1);
148  LoadRGBMatrix();
150  glMatrixMode(GL_MODELVIEW);
151 
152  glPointSize(1);
153 
154  glEnableClientState(GL_VERTEX_ARRAY);
155  glVertexPointer(3, GL_SHORT, 0, xyz);
156  glEnableClientState(GL_TEXTURE_COORD_ARRAY);
157  glTexCoordPointer(3, GL_SHORT, 0, xyz);
158 
159  if (color)
160  glEnable(GL_TEXTURE_2D);
161  glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
162  glTexImage2D(GL_TEXTURE_2D, 0, 3, 640, 480, 0, GL_RGB, GL_UNSIGNED_BYTE, rgb);
163 
164  glPointSize(2.0f);
165  glDrawElements(GL_POINTS, 640*480, GL_UNSIGNED_INT, indices);
166  glPopMatrix();
167  glDisable(GL_TEXTURE_2D);
168  glutSwapBuffers();
169 }
170 
171 void keyPressed(unsigned char key, int x, int y)
172 {
173  if (key == 27) {
175  glutDestroyWindow(window);
176  exit(0);
177  }
178  if (key == 'w')
179  zoom *= 1.1f;
180  if (key == 's')
181  zoom /= 1.1f;
182  if (key == 'c')
183  color = !color;
184 }
185 
186 void ReSizeGLScene(int Width, int Height)
187 {
188  glViewport(0,0,Width,Height);
189  glMatrixMode(GL_PROJECTION);
190  glLoadIdentity();
191  gluPerspective(60, 4/3., 0.3, 200);
192  glMatrixMode(GL_MODELVIEW);
193 }
194 
195 void InitGL(int Width, int Height)
196 {
197  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
198  glEnable(GL_DEPTH_TEST);
199  glGenTextures(1, &gl_rgb_tex);
200  glBindTexture(GL_TEXTURE_2D, gl_rgb_tex);
201  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
202  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
203  ReSizeGLScene(Width, Height);
204 }
205 
206 int main(int argc, char **argv)
207 {
208  glutInit(&argc, argv);
209 
210  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
211  glutInitWindowSize(640, 480);
212  glutInitWindowPosition(0, 0);
213 
214  window = glutCreateWindow("LibFreenect");
215 
216  glutDisplayFunc(&DrawGLScene);
217  glutIdleFunc(&DrawGLScene);
218  glutReshapeFunc(&ReSizeGLScene);
219  glutKeyboardFunc(&keyPressed);
220  glutMotionFunc(&mouseMoved);
221  glutMouseFunc(&mousePress);
222 
223  InitGL(640, 480);
224 
225  glutMainLoop();
226 
227  return 0;
228 }
229 
int freenect_sync_get_depth(void **depth, uint32_t *timestamp, int index, freenect_depth_format fmt)
int rotangles[2]
Definition: glpclview.c:44
void ReSizeGLScene(int Width, int Height)
Definition: glpclview.c:186
void LoadVertexMatrix()
Definition: glpclview.c:51
void mousePress(int button, int state, int x, int y)
Definition: glpclview.c:92
int color
Definition: glpclview.c:46
int mx
Definition: glpclview.c:43
void no_kinect_quit(void)
Definition: glpclview.c:104
void LoadRGBMatrix()
Definition: glpclview.c:71
void InitGL(int Width, int Height)
Definition: glpclview.c:195
void freenect_sync_stop(void)
float zoom
Definition: glpclview.c:45
GLuint gl_rgb_tex
Definition: glpclview.c:42
void DrawGLScene()
Definition: glpclview.c:110
int window
Definition: glpclview.c:41
void keyPressed(unsigned char key, int x, int y)
Definition: glpclview.c:171
unsigned int uint32_t
int main(int argc, char **argv)
Definition: glpclview.c:206
int freenect_sync_get_video(void **video, uint32_t *timestamp, int index, freenect_video_format fmt)
capture state
Definition: micview.c:53
void mouseMoved(int x, int y)
Definition: glpclview.c:82
int my
Definition: glpclview.c:43


libfreenect
Author(s): Hector Martin, Josh Blake, Kyle Machulis, OpenKinect community
autogenerated on Mon Jun 10 2019 13:46:42