Fawkes API  Fawkes Development Version
pclviewer.cpp
1 
2 /***************************************************************************
3  * pclviewer.cpp - Really simple viewer for the OpenNI PCLs
4  *
5  * Created: Fri Apr 01 14:39:04 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "trackball.h"
24 #include "skel_drawer.h"
25 #include "transfer_thread.h"
26 #include <plugins/openni/utils/skel_if_observer.h>
27 #include <plugins/openni/utils/hand_if_observer.h>
28 
29 #include <core/threading/thread.h>
30 #include <utils/math/angle.h>
31 #include <fvcams/net.h>
32 #include <fvcams/shmem.h>
33 #include <utils/system/argparser.h>
34 #include <fvutils/ipc/shm_image.h>
35 #include <fvutils/color/conversions.h>
36 #include <fvutils/base/types.h>
37 
38 #include <blackboard/remote.h>
39 #include <interfaces/ObjectPositionInterface.h>
40 
41 #include <GL/glut.h>
42 #include <cstdio>
43 
44 #define GL_WIN_SIZE_X 800
45 #define GL_WIN_SIZE_Y 800
46 
47 using namespace fawkes;
48 using namespace fawkes::openni;
49 using namespace firevision;
50 
51 Camera *g_pcl_cam = NULL;
52 Camera *g_image_cam = NULL;
53 unsigned char *g_rgb_buf = NULL;
54 const pcl_point_t *g_pcl_buf = NULL;
55 const unsigned char *g_image_buf = NULL;
56 
57 GLfloat g_scale; /* scaling factor */
58 GLdouble g_pan_x = 0.0;
59 GLdouble g_pan_y = 0.0;
60 GLdouble g_pan_z = 0.0;
61 GLint g_mouse_state = -1;
62 GLint g_mouse_button = -1;
63 
64 BlackBoard *g_bb = NULL;;
65 std::list<ObjectPositionInterface *> g_obj_ifs;
66 
67 UserMap g_users;
68 HandMap g_hands;
69 
70 SkelIfObserver *g_obs = NULL;
71 HandIfObserver *g_hands_obs = NULL;
72 SkelGuiSkeletonDrawer3D *g_skel_drawer = NULL;
73 
74 PclViewerTransferThread *g_transfer_thread = NULL;
75 
76 void
77 rotate_scale_matrix()
78 {
79  glRotatef(90., 0, 0, 1);
80  glRotatef(45., 0, 1, 0);
81  //glTranslatef(0, 0, -4);
82  glScalef(3.0, 3.0, 3.0);
83 }
84 
85 
86 void
87 draw_points()
88 {
89  if (! g_transfer_thread) {
90  g_pcl_cam->capture();
91  if (g_image_cam) {
92  g_image_cam->capture();
93  convert(g_image_cam->colorspace(), RGB, g_image_cam->buffer(), g_rgb_buf,
94  g_image_cam->pixel_width(), g_image_cam->pixel_height());
95  g_image_cam->dispose_buffer();
96  }
97  } else {
98  if (g_image_cam) {
99  g_transfer_thread->lock_for_read();
100  convert(g_image_cam->colorspace(), RGB, g_image_buf, g_rgb_buf,
101  g_image_cam->pixel_width(), g_image_cam->pixel_height());
102  g_transfer_thread->unlock();
103  }
104  }
105 
106  rotate_scale_matrix();
107  glBegin(GL_POINTS);
108 
109  const unsigned int width = g_pcl_cam->pixel_width();
110  const unsigned int height = g_pcl_cam->pixel_height();
111 
112  const pcl_point_t *pcl = g_pcl_buf;
113 
114  if (g_transfer_thread) {
115  g_transfer_thread->lock_for_read();
116  }
117 
118  if (g_image_cam) {
119  unsigned char *rgb = g_rgb_buf;
120  //unsigned int num_values = 0, zero_values = 0;
121  for (unsigned int h = 0; h < height; ++h) {
122  for (unsigned int w = 0; w < width; ++w, rgb += 3, ++pcl) {
123  //++num_values;
124  register const pcl_point_t &p = *pcl;
125  if ((p.x != 0) || (p.y != 0) || (p.z != 0)) {
126  glColor3f(rgb[0] / 255.,rgb[1] / 255.,rgb[2] / 255.);
127  glVertex3f(p.x, p.y, p.z);
128  }
129  }
130  }
131  } else {
132  //unsigned int num_values = 0, zero_values = 0;
133  for (unsigned int h = 0; h < height; ++h) {
134  for (unsigned int w = 0; w < width; ++w, ++pcl) {
135  //++num_values;
136  register const pcl_point_t &p = *pcl;
137  if ((p.x != 0) || (p.y != 0) || (p.z != 0)) {
138  glVertex3f(p.x, p.y, p.z);
139  }
140  }
141  }
142  }
143 
144  if (g_transfer_thread) {
145  g_transfer_thread->unlock();
146  }
147 
148  //printf("Zero values %u/%u\n", zero_values, num_values);
149  glEnd();
150 
151  glPointSize(5);
152  glBegin(GL_POINTS);
153  glColor3f(1.0, 0, 0);
154  glVertex3f(0, 0, 0);
155  glColor3f(0.0, 0, 1.0);
156  for (int i = -90; i <= 90; i += 10) {
157  glVertex3f(cos(deg2rad(i)), sin(deg2rad(i)), 0);
158  }
159 
160 
161  glColor3f(1.0, 1.0, 1.0);
162  glEnd();
163  glPointSize(1);
164 
165  if (! g_transfer_thread) {
166  g_pcl_cam->dispose_buffer();
167  }
168 }
169 
170 
171 void
172 draw_objects()
173 {
174  glRotatef(90., 0, 0, 1);
175  glRotatef(45., 0, 1, 0);
176  //glTranslatef(0, 0, -4);
177  glScalef(3.0, 3.0, 3.0);
178 
179  glPointSize(10);
180  glBegin(GL_POINTS);
181  glColor3f(0, 1, 0);
182  std::list<ObjectPositionInterface *>::iterator i;
183  for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) {
184  (*i)->read();
185  if ((*i)->is_visible()) {
186  glVertex4f((*i)->relative_x(),
187  (*i)->relative_y(),
188  (*i)->relative_z(),
189  1.0);
190  }
191  }
192  glColor3f(1.0, 1.0, 1.0);
193  glEnd();
194  glPointSize(1);
195 }
196 
197 
198 void
199 mouse(int button, int state, int x, int y)
200 {
201  tbMouse(button, state, x, y);
202 
203  g_mouse_state = state;
204  g_mouse_button = button;
205 
206  if (state == GLUT_DOWN && button == GLUT_LEFT_BUTTON) {
207  GLdouble model[4*4];
208  GLdouble proj[4*4];
209  GLint view[4];
210 
211  glGetDoublev(GL_MODELVIEW_MATRIX, model);
212  glGetDoublev(GL_PROJECTION_MATRIX, proj);
213  glGetIntegerv(GL_VIEWPORT, view);
214  gluProject((GLdouble)x, (GLdouble)y, 0.0,
215  model, proj, view,
216  &g_pan_x, &g_pan_y, &g_pan_z);
217  gluUnProject((GLdouble)x, (GLdouble)y, g_pan_z,
218  model, proj, view,
219  &g_pan_x, &g_pan_y, &g_pan_z);
220  g_pan_y = -g_pan_y;
221  }
222 
223  glutPostRedisplay();
224 }
225 
226 
227 void
228 motion(int x, int y)
229 {
230  tbMotion(x, y);
231 
232  if (g_mouse_state == GLUT_DOWN && g_mouse_button == GLUT_LEFT_BUTTON) {
233  GLdouble model[4*4];
234  GLdouble proj[4*4];
235  GLint view[4];
236 
237  glGetDoublev(GL_MODELVIEW_MATRIX, model);
238  glGetDoublev(GL_PROJECTION_MATRIX, proj);
239  glGetIntegerv(GL_VIEWPORT, view);
240  gluProject((GLdouble)x, (GLdouble)y, 0.0,
241  model, proj, view,
242  &g_pan_x, &g_pan_y, &g_pan_z);
243  gluUnProject((GLdouble)x, (GLdouble)y, g_pan_z,
244  model, proj, view,
245  &g_pan_x, &g_pan_y, &g_pan_z);
246  g_pan_y = -g_pan_y;
247  }
248 
249  glutPostRedisplay();
250 }
251 
252 
253 void
254 reshape(int width, int height)
255 {
256  tbReshape(width, height);
257 
258  glViewport(0, 0, width, height);
259 
260  //glMatrixMode(GL_PROJECTION);
261  //glLoadIdentity();
262  //gluPerspective(60.0, (GLfloat)height / (GLfloat)width, 1.0, 128.0);
263  //glMatrixMode(GL_MODELVIEW);
264  //glLoadIdentity();
265  //glTranslatef(0.0, 0.0, -3.0);
266 }
267 
268 
269 void
270 display()
271 {
272  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
273 
274  glPushMatrix();
275  glTranslatef(g_pan_x, g_pan_y, 0.0);
276  tbMatrix();
277 
278  glPushMatrix();
279  draw_points();
280  glPopMatrix();
281 
282  if (g_bb) {
283  try {
284  if (! g_obj_ifs.empty()) {
285  glPushMatrix();
286  draw_objects();
287  glPopMatrix();
288  }
289 
290  if (g_skel_drawer) {
291  glPushMatrix();
292  rotate_scale_matrix();
293  g_skel_drawer->draw();
294  glPopMatrix();
295  }
296 
297  } catch (Exception &e) {
298  printf("Interface read failed, closing");
299  std::list<ObjectPositionInterface *>::iterator i;
300  for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) {
301  g_bb->close(*i);
302  }
303  g_obj_ifs.clear();
304  delete g_bb;
305  g_bb = NULL;
306  }
307  }
308 
309  glPopMatrix();
310  glutSwapBuffers();
311 }
312 
313 
314 void
315 idle()
316 {
317  if (g_obs) g_obs->process_queue();
318  if (g_hands_obs) g_hands_obs->process_queue();
319  glutPostRedisplay();
320 }
321 
322 
323 void
324 init_gl()
325 {
326  tbInit(GLUT_MIDDLE_BUTTON);
327 
328  // Enable animation, set idle function for reset
329  tbAnimate(GL_TRUE, idle);
330 
331  // Enable a single OpenGL light.
332  //glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
333  //glLightfv(GL_LIGHT0, GL_POSITION, light_position);
334  //glEnable(GL_LIGHT0);
335  //glEnable(GL_LIGHTING);
336 
337  // Use depth buffering for hidden surface elimination
338  glEnable(GL_DEPTH_TEST);
339 
340  // Setup view
341  glMatrixMode(GL_PROJECTION);
342  gluPerspective( /* field of view in degree */ 60.0,
343  /* aspect ratio */ 1.0,
344  /* Z near */ 0.1, /* Z far */ 100.0);
345  glMatrixMode(GL_MODELVIEW);
346  gluLookAt(0.0, 0.0, 5.0, // eye is at (0,0,5)
347  0.0, 0.0, 0.0, // center is at (0,0,0)
348  0.0, 1.0, 0.0); // up is in positive Y direction
349 }
350 
351 void
352 init(ArgumentParser &argp)
353 {
354  std::string host = "localhost";
355  unsigned short int port = 1910;
356  if ( argp.has_arg("r") ) {
357  argp.parse_hostport("r", host, port);
358 
359  printf("Connecting to %s:%u\n", host.c_str(), port);
360  try {
361  g_bb = new RemoteBlackBoard(host.c_str(), port);
362 
363  const std::vector< const char * > &items = argp.items();
364  for (unsigned int i = 0; i < items.size(); ++i) {
365  ObjectPositionInterface * obj_if =
366  g_bb->open_for_reading<ObjectPositionInterface>(items[i]);
367  g_obj_ifs.push_back(obj_if);
368  }
369  } catch (Exception &e) {
370  e.print_trace();
371  exit(-1);
372  }
373 
374  g_obs = new SkelIfObserver(g_bb, g_users);
375  g_hands_obs = new HandIfObserver(g_bb, g_hands);
376  g_skel_drawer = new SkelGuiSkeletonDrawer3D(g_users, g_hands);
377  }
378 
379  std::string fvhost = host;
380  unsigned short int fvport = 2208;
381 
382  if (argp.has_arg("n")) {
383  argp.parse_hostport("n", fvhost, fvport);
384  g_pcl_cam = new NetworkCamera(fvhost.c_str(), fvport, "openni-pointcloud-xyz");
385  g_pcl_cam->open();
386  g_pcl_cam->start();
387 
388  g_transfer_thread = new PclViewerTransferThread();
389  g_transfer_thread->add_camera("openni-pointcloud-xyz", g_pcl_cam);
390 
391  g_pcl_buf = (const pcl_point_t *)g_transfer_thread->buffer("openni-pointcloud-xyz");
392 
393  if (argp.has_arg("R")) {
394  g_image_cam = new NetworkCamera(fvhost.c_str(), fvport, "openni-image-rgb",
395  argp.has_arg("j"));
396  g_image_cam->open();
397  g_image_cam->start();
398  g_rgb_buf = malloc_buffer(RGB, g_image_cam->pixel_width(),
399  g_image_cam->pixel_height());
400  g_transfer_thread->add_camera("openni-image-rgb", g_image_cam);
401  g_image_buf = g_transfer_thread->buffer("openni-image-rgb");
402  }
403 
404  g_transfer_thread->start();
405 
406  } else {
407  g_pcl_cam = new SharedMemoryCamera("openni-pointcloud-xyz");
408  g_pcl_cam->open();
409  g_pcl_cam->start();
410  g_pcl_buf = (const pcl_point_t *)g_pcl_cam->buffer();
411  if (argp.has_arg("R")) {
412  g_image_cam = new SharedMemoryCamera("openni-image-rgb");
413  g_image_cam->open();
414  g_image_cam->start();
415  g_image_buf = g_image_cam->buffer();
416  g_rgb_buf = malloc_buffer(RGB, g_image_cam->pixel_width(),
417  g_image_cam->pixel_height());
418  }
419  }
420 }
421 
422 
423 int
424 main(int argc, char **argv)
425 {
426  ArgumentParser argp(argc, argv, "hr:n::sRj");
428 
429  glutInit(&argc, argv);
430  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
431  glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
432  glutCreateWindow("Fawkes OpenNI PCL Viewer");
433  glutReshapeFunc(reshape);
434  glutMouseFunc(mouse);
435  glutMotionFunc(motion);
436  glutDisplayFunc(display);
437  glutIdleFunc(idle);
438  init_gl();
439  init(argp);
440  glutMainLoop();
441  return 0;
442 }
void add_camera(std::string name, firevision::Camera *cam)
Add a camera from which to pull images.
bool parse_hostport(const char *argn, char **host, unsigned short int *port)
Parse host:port string.
Definition: argparser.cpp:231
Camera interface for image aquiring devices in FireVision.
Definition: camera.h:35
const std::vector< const char *> & items() const
Get non-option items.
Definition: argparser.cpp:462
float y
Y value.
Definition: types.h:102
ObjectPositionInterface Fawkes BlackBoard Interface.
Definition: pointcloud.h:30
Fawkes library namespace.
void process_queue()
Process internal queue.
virtual unsigned int pixel_width()=0
Width of image in pixels.
Draw body skeleton using OpenGL (3D).
Definition: skel_drawer.h:32
Parse command line arguments.
Definition: argparser.h:66
void draw()
Draw skeletons.
static void init_main()
Initialize Thread wrapper instance for main thread.
Definition: thread.cpp:1271
virtual colorspace_t colorspace()=0
Colorspace of returned image.
float x
X value.
Definition: types.h:101
void process_queue()
Process internal queue.
Skeleton interface observer.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void capture()=0
Capture an image.
Shared memory camera.
Definition: shmem.h:38
Hand interface observer.
virtual void open()=0
Open the camera.
const unsigned char * buffer(std::string name)
Get buffer for specified camera.
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
PCL viewer transfer thread.
Remote BlackBoard.
Definition: remote.h:48
virtual unsigned char * buffer()=0
Get access to current image buffer.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
Structure defining a point in a CARTESIAN_3D_FLOAT buffer.
Definition: types.h:100
void lock_for_read()
Lock for reading.
virtual unsigned int pixel_height()=0
Height of image in pixels.
float z
Z value.
Definition: types.h:103
bool has_arg(const char *argn)
Check if argument has been supplied.
Definition: argparser.cpp:169
The BlackBoard abstract class.
Definition: blackboard.h:48
virtual void start()=0
Start image transfer from the camera.
Network camera.
Definition: net.h:42
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:511
virtual void dispose_buffer()=0
Dispose current buffer.
virtual void close(Interface *interface)=0
Close interface.