Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * pclviewer.cpp - Really simple viewer for the OpenNI PCLs 00004 * 00005 * Created: Fri Apr 01 14:39:04 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "trackball.h" 00024 #include "skel_drawer.h" 00025 #include "transfer_thread.h" 00026 #include <plugins/openni/utils/skel_if_observer.h> 00027 #include <plugins/openni/utils/hand_if_observer.h> 00028 00029 #include <core/threading/thread.h> 00030 #include <utils/math/angle.h> 00031 #include <fvcams/net.h> 00032 #include <fvcams/shmem.h> 00033 #include <utils/system/argparser.h> 00034 #include <fvutils/ipc/shm_image.h> 00035 #include <fvutils/color/conversions.h> 00036 #include <fvutils/base/types.h> 00037 00038 #include <blackboard/remote.h> 00039 #include <interfaces/ObjectPositionInterface.h> 00040 00041 #include <GL/glut.h> 00042 #include <cstdio> 00043 00044 #define GL_WIN_SIZE_X 800 00045 #define GL_WIN_SIZE_Y 800 00046 00047 using namespace fawkes; 00048 using namespace fawkes::openni; 00049 using namespace firevision; 00050 00051 Camera *g_pcl_cam = NULL; 00052 Camera *g_image_cam = NULL; 00053 unsigned char *g_rgb_buf = NULL; 00054 const pcl_point_t *g_pcl_buf = NULL; 00055 const unsigned char *g_image_buf = NULL; 00056 00057 GLfloat g_scale; /* scaling factor */ 00058 GLdouble g_pan_x = 0.0; 00059 GLdouble g_pan_y = 0.0; 00060 GLdouble g_pan_z = 0.0; 00061 GLint g_mouse_state = -1; 00062 GLint g_mouse_button = -1; 00063 00064 BlackBoard *g_bb = NULL;; 00065 std::list<ObjectPositionInterface *> g_obj_ifs; 00066 00067 UserMap g_users; 00068 HandMap g_hands; 00069 00070 SkelIfObserver *g_obs = NULL; 00071 HandIfObserver *g_hands_obs = NULL; 00072 SkelGuiSkeletonDrawer3D *g_skel_drawer = NULL; 00073 00074 PclViewerTransferThread *g_transfer_thread = NULL; 00075 00076 void 00077 rotate_scale_matrix() 00078 { 00079 glRotatef(90., 0, 0, 1); 00080 glRotatef(45., 0, 1, 0); 00081 //glTranslatef(0, 0, -4); 00082 glScalef(3.0, 3.0, 3.0); 00083 } 00084 00085 00086 void 00087 draw_points() 00088 { 00089 if (! g_transfer_thread) { 00090 g_pcl_cam->capture(); 00091 if (g_image_cam) { 00092 g_image_cam->capture(); 00093 convert(g_image_cam->colorspace(), RGB, g_image_cam->buffer(), g_rgb_buf, 00094 g_image_cam->pixel_width(), g_image_cam->pixel_height()); 00095 g_image_cam->dispose_buffer(); 00096 } 00097 } else { 00098 if (g_image_cam) { 00099 g_transfer_thread->lock_for_read(); 00100 convert(g_image_cam->colorspace(), RGB, g_image_buf, g_rgb_buf, 00101 g_image_cam->pixel_width(), g_image_cam->pixel_height()); 00102 g_transfer_thread->unlock(); 00103 } 00104 } 00105 00106 rotate_scale_matrix(); 00107 glBegin(GL_POINTS); 00108 00109 const unsigned int width = g_pcl_cam->pixel_width(); 00110 const unsigned int height = g_pcl_cam->pixel_height(); 00111 00112 const pcl_point_t *pcl = g_pcl_buf; 00113 00114 if (g_transfer_thread) { 00115 g_transfer_thread->lock_for_read(); 00116 } 00117 00118 if (g_image_cam) { 00119 unsigned char *rgb = g_rgb_buf; 00120 //unsigned int num_values = 0, zero_values = 0; 00121 for (unsigned int h = 0; h < height; ++h) { 00122 for (unsigned int w = 0; w < width; ++w, rgb += 3, ++pcl) { 00123 //++num_values; 00124 register const pcl_point_t &p = *pcl; 00125 if ((p.x != 0) || (p.y != 0) || (p.z != 0)) { 00126 glColor3f(rgb[0] / 255.,rgb[1] / 255.,rgb[2] / 255.); 00127 glVertex3f(p.x, p.y, p.z); 00128 } 00129 } 00130 } 00131 } else { 00132 //unsigned int num_values = 0, zero_values = 0; 00133 for (unsigned int h = 0; h < height; ++h) { 00134 for (unsigned int w = 0; w < width; ++w, ++pcl) { 00135 //++num_values; 00136 register const pcl_point_t &p = *pcl; 00137 if ((p.x != 0) || (p.y != 0) || (p.z != 0)) { 00138 glVertex3f(p.x, p.y, p.z); 00139 } 00140 } 00141 } 00142 } 00143 00144 if (g_transfer_thread) { 00145 g_transfer_thread->unlock(); 00146 } 00147 00148 //printf("Zero values %u/%u\n", zero_values, num_values); 00149 glEnd(); 00150 00151 glPointSize(5); 00152 glBegin(GL_POINTS); 00153 glColor3f(1.0, 0, 0); 00154 glVertex3f(0, 0, 0); 00155 glColor3f(0.0, 0, 1.0); 00156 for (int i = -90; i <= 90; i += 10) { 00157 glVertex3f(cos(deg2rad(i)), sin(deg2rad(i)), 0); 00158 } 00159 00160 00161 glColor3f(1.0, 1.0, 1.0); 00162 glEnd(); 00163 glPointSize(1); 00164 00165 if (! g_transfer_thread) { 00166 g_pcl_cam->dispose_buffer(); 00167 } 00168 } 00169 00170 00171 void 00172 draw_objects() 00173 { 00174 glRotatef(90., 0, 0, 1); 00175 glRotatef(45., 0, 1, 0); 00176 //glTranslatef(0, 0, -4); 00177 glScalef(3.0, 3.0, 3.0); 00178 00179 glPointSize(10); 00180 glBegin(GL_POINTS); 00181 glColor3f(0, 1, 0); 00182 std::list<ObjectPositionInterface *>::iterator i; 00183 for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) { 00184 (*i)->read(); 00185 if ((*i)->is_visible()) { 00186 glVertex4f((*i)->relative_x(), 00187 (*i)->relative_y(), 00188 (*i)->relative_z(), 00189 1.0); 00190 } 00191 } 00192 glColor3f(1.0, 1.0, 1.0); 00193 glEnd(); 00194 glPointSize(1); 00195 } 00196 00197 00198 void 00199 mouse(int button, int state, int x, int y) 00200 { 00201 tbMouse(button, state, x, y); 00202 00203 g_mouse_state = state; 00204 g_mouse_button = button; 00205 00206 if (state == GLUT_DOWN && button == GLUT_LEFT_BUTTON) { 00207 GLdouble model[4*4]; 00208 GLdouble proj[4*4]; 00209 GLint view[4]; 00210 00211 glGetDoublev(GL_MODELVIEW_MATRIX, model); 00212 glGetDoublev(GL_PROJECTION_MATRIX, proj); 00213 glGetIntegerv(GL_VIEWPORT, view); 00214 gluProject((GLdouble)x, (GLdouble)y, 0.0, 00215 model, proj, view, 00216 &g_pan_x, &g_pan_y, &g_pan_z); 00217 gluUnProject((GLdouble)x, (GLdouble)y, g_pan_z, 00218 model, proj, view, 00219 &g_pan_x, &g_pan_y, &g_pan_z); 00220 g_pan_y = -g_pan_y; 00221 } 00222 00223 glutPostRedisplay(); 00224 } 00225 00226 00227 void 00228 motion(int x, int y) 00229 { 00230 tbMotion(x, y); 00231 00232 if (g_mouse_state == GLUT_DOWN && g_mouse_button == GLUT_LEFT_BUTTON) { 00233 GLdouble model[4*4]; 00234 GLdouble proj[4*4]; 00235 GLint view[4]; 00236 00237 glGetDoublev(GL_MODELVIEW_MATRIX, model); 00238 glGetDoublev(GL_PROJECTION_MATRIX, proj); 00239 glGetIntegerv(GL_VIEWPORT, view); 00240 gluProject((GLdouble)x, (GLdouble)y, 0.0, 00241 model, proj, view, 00242 &g_pan_x, &g_pan_y, &g_pan_z); 00243 gluUnProject((GLdouble)x, (GLdouble)y, g_pan_z, 00244 model, proj, view, 00245 &g_pan_x, &g_pan_y, &g_pan_z); 00246 g_pan_y = -g_pan_y; 00247 } 00248 00249 glutPostRedisplay(); 00250 } 00251 00252 00253 void 00254 reshape(int width, int height) 00255 { 00256 tbReshape(width, height); 00257 00258 glViewport(0, 0, width, height); 00259 00260 //glMatrixMode(GL_PROJECTION); 00261 //glLoadIdentity(); 00262 //gluPerspective(60.0, (GLfloat)height / (GLfloat)width, 1.0, 128.0); 00263 //glMatrixMode(GL_MODELVIEW); 00264 //glLoadIdentity(); 00265 //glTranslatef(0.0, 0.0, -3.0); 00266 } 00267 00268 00269 void 00270 display() 00271 { 00272 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 00273 00274 glPushMatrix(); 00275 glTranslatef(g_pan_x, g_pan_y, 0.0); 00276 tbMatrix(); 00277 00278 glPushMatrix(); 00279 draw_points(); 00280 glPopMatrix(); 00281 00282 if (g_bb) { 00283 try { 00284 if (! g_obj_ifs.empty()) { 00285 glPushMatrix(); 00286 draw_objects(); 00287 glPopMatrix(); 00288 } 00289 00290 if (g_skel_drawer) { 00291 glPushMatrix(); 00292 rotate_scale_matrix(); 00293 g_skel_drawer->draw(); 00294 glPopMatrix(); 00295 } 00296 00297 } catch (Exception &e) { 00298 printf("Interface read failed, closing"); 00299 std::list<ObjectPositionInterface *>::iterator i; 00300 for (i = g_obj_ifs.begin(); i != g_obj_ifs.end(); ++i) { 00301 g_bb->close(*i); 00302 } 00303 g_obj_ifs.clear(); 00304 delete g_bb; 00305 g_bb = NULL; 00306 } 00307 } 00308 00309 glPopMatrix(); 00310 glutSwapBuffers(); 00311 } 00312 00313 00314 void 00315 idle() 00316 { 00317 if (g_obs) g_obs->process_queue(); 00318 if (g_hands_obs) g_hands_obs->process_queue(); 00319 glutPostRedisplay(); 00320 } 00321 00322 00323 void 00324 init_gl() 00325 { 00326 tbInit(GLUT_MIDDLE_BUTTON); 00327 00328 // Enable animation, set idle function for reset 00329 tbAnimate(GL_TRUE, idle); 00330 00331 // Enable a single OpenGL light. 00332 //glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse); 00333 //glLightfv(GL_LIGHT0, GL_POSITION, light_position); 00334 //glEnable(GL_LIGHT0); 00335 //glEnable(GL_LIGHTING); 00336 00337 // Use depth buffering for hidden surface elimination 00338 glEnable(GL_DEPTH_TEST); 00339 00340 // Setup view 00341 glMatrixMode(GL_PROJECTION); 00342 gluPerspective( /* field of view in degree */ 60.0, 00343 /* aspect ratio */ 1.0, 00344 /* Z near */ 0.1, /* Z far */ 100.0); 00345 glMatrixMode(GL_MODELVIEW); 00346 gluLookAt(0.0, 0.0, 5.0, // eye is at (0,0,5) 00347 0.0, 0.0, 0.0, // center is at (0,0,0) 00348 0.0, 1.0, 0.0); // up is in positive Y direction 00349 } 00350 00351 void 00352 init(ArgumentParser &argp) 00353 { 00354 std::string host = "localhost"; 00355 unsigned short int port = 1910; 00356 if ( argp.has_arg("r") ) { 00357 argp.parse_hostport("r", host, port); 00358 00359 printf("Connecting to %s:%u\n", host.c_str(), port); 00360 try { 00361 g_bb = new RemoteBlackBoard(host.c_str(), port); 00362 00363 const std::vector< const char * > &items = argp.items(); 00364 for (unsigned int i = 0; i < items.size(); ++i) { 00365 ObjectPositionInterface * obj_if = 00366 g_bb->open_for_reading<ObjectPositionInterface>(items[i]); 00367 g_obj_ifs.push_back(obj_if); 00368 } 00369 } catch (Exception &e) { 00370 e.print_trace(); 00371 exit(-1); 00372 } 00373 00374 g_obs = new SkelIfObserver(g_bb, g_users); 00375 g_hands_obs = new HandIfObserver(g_bb, g_hands); 00376 g_skel_drawer = new SkelGuiSkeletonDrawer3D(g_users, g_hands); 00377 } 00378 00379 std::string fvhost = host; 00380 unsigned short int fvport = 2208; 00381 00382 if (argp.has_arg("n")) { 00383 argp.parse_hostport("n", fvhost, fvport); 00384 g_pcl_cam = new NetworkCamera(fvhost.c_str(), fvport, "openni-pointcloud-xyz"); 00385 g_pcl_cam->open(); 00386 g_pcl_cam->start(); 00387 00388 g_transfer_thread = new PclViewerTransferThread(); 00389 g_transfer_thread->add_camera("openni-pointcloud-xyz", g_pcl_cam); 00390 00391 g_pcl_buf = (const pcl_point_t *)g_transfer_thread->buffer("openni-pointcloud-xyz"); 00392 00393 if (argp.has_arg("R")) { 00394 g_image_cam = new NetworkCamera(fvhost.c_str(), fvport, "openni-image-rgb", 00395 argp.has_arg("j")); 00396 g_image_cam->open(); 00397 g_image_cam->start(); 00398 g_rgb_buf = malloc_buffer(RGB, g_image_cam->pixel_width(), 00399 g_image_cam->pixel_height()); 00400 g_transfer_thread->add_camera("openni-image-rgb", g_image_cam); 00401 g_image_buf = g_transfer_thread->buffer("openni-image-rgb"); 00402 } 00403 00404 g_transfer_thread->start(); 00405 00406 } else { 00407 g_pcl_cam = new SharedMemoryCamera("openni-pointcloud-xyz"); 00408 g_pcl_cam->open(); 00409 g_pcl_cam->start(); 00410 g_pcl_buf = (const pcl_point_t *)g_pcl_cam->buffer(); 00411 if (argp.has_arg("R")) { 00412 g_image_cam = new SharedMemoryCamera("openni-image-rgb"); 00413 g_image_cam->open(); 00414 g_image_cam->start(); 00415 g_image_buf = g_image_cam->buffer(); 00416 g_rgb_buf = malloc_buffer(RGB, g_image_cam->pixel_width(), 00417 g_image_cam->pixel_height()); 00418 } 00419 } 00420 } 00421 00422 00423 int 00424 main(int argc, char **argv) 00425 { 00426 ArgumentParser argp(argc, argv, "hr:n::sRj"); 00427 Thread::init_main(); 00428 00429 glutInit(&argc, argv); 00430 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); 00431 glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y); 00432 glutCreateWindow("Fawkes OpenNI PCL Viewer"); 00433 glutReshapeFunc(reshape); 00434 glutMouseFunc(mouse); 00435 glutMotionFunc(motion); 00436 glutDisplayFunc(display); 00437 glutIdleFunc(idle); 00438 init_gl(); 00439 init(argp); 00440 glutMainLoop(); 00441 return 0; 00442 }