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
46using namespace fawkes;
47using namespace fawkes::openni;
48using namespace firevision;
49
50Camera * g_pcl_cam = NULL;
51Camera * g_image_cam = NULL;
52unsigned char * g_rgb_buf = NULL;
53const pcl_point_t * g_pcl_buf = NULL;
54const unsigned char *g_image_buf = NULL;
55
56GLfloat g_scale; /* scaling factor */
57GLdouble g_pan_x = 0.0;
58GLdouble g_pan_y = 0.0;
59GLdouble g_pan_z = 0.0;
60GLint g_mouse_state = -1;
61GLint g_mouse_button = -1;
62
63BlackBoard *g_bb = NULL;
64;
65std::list<ObjectPositionInterface *> g_obj_ifs;
66
67UserMap g_users;
68HandMap g_hands;
69
70SkelIfObserver * g_obs = NULL;
71HandIfObserver * g_hands_obs = NULL;
72SkelGuiSkeletonDrawer3D *g_skel_drawer = NULL;
73
74PclViewerTransferThread *g_transfer_thread = NULL;
75
76void
77rotate_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
85void
86draw_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
176void
177draw_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
199void
200mouse(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
224void
225motion(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
246void
247reshape(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
261void
262display()
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
305void
306idle()
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
315void
316init_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
350void
351init(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
417int
418main(int argc, char **argv)
419{
420 ArgumentParser argp(argc, argv, "hr:n::sRj");
421 Thread::init_main();
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.
const unsigned char * buffer(std::string name)
Get buffer for specified camera.
void add_camera(std::string name, firevision::Camera *cam)
Add a camera from which to pull images.
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
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