Fawkes API  Fawkes Development Version
pclviewer.cpp
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 }