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