urdf_filter.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2011, Nico Blodow <blodow@cs.tum.edu>
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Intelligent Autonomous Systems Group/
00014  *       Technische Universitaet Muenchen nor the names of its contributors 
00015  *       may be used to endorse or promote products derived from this software 
00016  *       without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include "realtime_urdf_filter/urdf_filter.h"
00032 
00033 #include <cv_bridge/cv_bridge.h>
00034 #include <image_transport/image_transport.h>
00035 #include <sensor_msgs/image_encodings.h>
00036 
00037 #define USE_OWN_CALIBRATION
00038 
00039 using namespace realtime_urdf_filter;
00040 
00041 // constructor. sets up ros and reads in parameters
00042 RealtimeURDFFilter::RealtimeURDFFilter (ros::NodeHandle &nh, int argc, char **argv)
00043   : nh_(nh)
00044   , image_transport_(nh)
00045   , fbo_initialized_(false)
00046   , depth_image_pbo_ (GL_INVALID_VALUE)
00047   , depth_texture_(GL_INVALID_VALUE)
00048   , width_(0)
00049   , height_(0)
00050   , camera_tx_(0)
00051   , camera_ty_(0)
00052   , far_plane_ (8)
00053   , near_plane_ (0.1)
00054   , argc_ (argc), argv_(argv)
00055 {
00056   // get fixed frame name
00057   XmlRpc::XmlRpcValue v;
00058   nh_.getParam ("fixed_frame", v);
00059   ROS_ASSERT (v.getType() == XmlRpc::XmlRpcValue::TypeString && "fixed_frame paramter!");
00060   fixed_frame_ = (std::string)v;
00061   ROS_INFO ("using fixed frame %s", fixed_frame_.c_str ());
00062 
00063   // get camera frame name 
00064   // we do not read this from ROS message, for being able to run this within openni (self filtered tracker..)
00065   nh_.getParam ("camera_frame", v);
00066   ROS_ASSERT (v.getType() == XmlRpc::XmlRpcValue::TypeString && "need a camera_frame paramter!");
00067   cam_frame_ = (std::string)v;
00068   ROS_INFO ("using camera frame %s", cam_frame_.c_str ());
00069 
00070   // read additional camera offset (TODO: make optional)
00071   nh_.getParam ("camera_offset", v);
00072   ROS_ASSERT (v.getType() == XmlRpc::XmlRpcValue::TypeStruct && "need a camera_offset paramter!");
00073   ROS_ASSERT (v.hasMember ("translation") && v.hasMember ("rotation") && "camera offset needs a translation and rotation parameter!");
00074 
00075   // translation
00076   XmlRpc::XmlRpcValue vec = v["translation"];
00077   ROS_ASSERT (vec.getType() == XmlRpc::XmlRpcValue::TypeArray && vec.size() == 3 && "camera_offset.translation parameter must be a 3-value array!");
00078   ROS_INFO ("using camera translational offset: %f %f %f",
00079       (double)(vec[0]),
00080       (double)(vec[1]),
00081       (double)(vec[2])
00082       );
00083   camera_offset_t_ = tf::Vector3((double)vec[0], (double)vec[1], (double)vec[2]);
00084 
00085   // rotation
00086   vec = v["rotation"];
00087   ROS_ASSERT (vec.getType() == XmlRpc::XmlRpcValue::TypeArray && vec.size() == 4 && "camera_offset.rotation parameter must be a 4-value array [x y z w]!");
00088   ROS_INFO ("using camera rotational offset: %f %f %f %f", (double)vec[0], (double)vec[1], (double)vec[2], (double)vec[3]);
00089   camera_offset_q_ = tf::Quaternion((double)vec[0], (double)vec[1], (double)vec[2], (double)vec[3]);
00090 
00091   // depth distance threshold (how far from the model are points still deleted?)
00092   nh_.getParam ("depth_distance_threshold", v);
00093   ROS_ASSERT (v.getType() == XmlRpc::XmlRpcValue::TypeDouble && "need a depth_distance_threshold paramter!");
00094   depth_distance_threshold_ = (double)v;
00095   ROS_INFO ("using depth distance threshold %f", depth_distance_threshold_);
00096 
00097   // depth distance threshold (how far from the model are points still deleted?)
00098   nh_.getParam ("show_gui", v);
00099   ROS_ASSERT (v.getType() == XmlRpc::XmlRpcValue::TypeBoolean && "need a show_gui paramter!");
00100   show_gui_ = (bool)v;
00101   ROS_INFO ("showing gui / visualization: %s", (show_gui_?"ON":"OFF"));
00102 
00103   // fitler replace value
00104   nh_.getParam ("filter_replace_value", v);
00105   ROS_ASSERT (v.getType() == XmlRpc::XmlRpcValue::TypeDouble && "need a filter_replace_value paramter!");
00106   filter_replace_value_ = (double)v;
00107   ROS_INFO ("using filter replace value %f", filter_replace_value_);
00108 
00109   // setup publishers 
00110   depth_sub_ = image_transport_.subscribeCamera("input_depth", 10,
00111       &RealtimeURDFFilter::filter_callback, this);
00112   depth_pub_ = image_transport_.advertiseCamera("output_depth", 10);
00113   depth_pub_raw_ = image_transport_.advertiseCamera("output_depth_raw", 10);
00114   mask_pub_ = image_transport_.advertiseCamera("output_mask", 10);
00115 }
00116 
00117 RealtimeURDFFilter::~RealtimeURDFFilter ()
00118 {
00119   delete masked_depth_;
00120   delete mask_;
00121 }
00122 
00123 // loads URDF models
00124 void RealtimeURDFFilter::loadModels ()
00125 {
00126   XmlRpc::XmlRpcValue v;
00127   nh_.getParam ("models", v);
00128   
00129   if (v.getType () == XmlRpc::XmlRpcValue::TypeArray)
00130   {
00131     for (int i = 0; i < v.size(); ++i)
00132     {
00133       XmlRpc::XmlRpcValue elem = v[i];
00134       ROS_ASSERT (elem.getType()  == XmlRpc::XmlRpcValue::TypeStruct);
00135 
00136       std::string description_param = elem["model"];
00137       std::string tf_prefix = elem["tf_prefix"];
00138 
00139       // read URDF model
00140       std::string content;
00141 
00142       if (!nh_.getParam(description_param, content))
00143       {
00144         std::string loc;
00145         if (nh_.searchParam(description_param, loc))
00146         {
00147           nh_.getParam(loc, content);
00148         }
00149         else
00150         {
00151           ROS_ERROR ("Parameter [%s] does not exist, and was not found by searchParam()",
00152               description_param.c_str());
00153           continue;
00154         }
00155       }
00156 
00157       if (content.empty())
00158       {
00159         ROS_ERROR ("URDF is empty");
00160         continue;
00161       }
00162 
00163       // finally, set the model description so we can later parse it.
00164       ROS_INFO ("Loading URDF model: %s", description_param.c_str ());
00165       renderers_.push_back (new URDFRenderer (content, tf_prefix, cam_frame_, fixed_frame_, tf_));
00166     }
00167   }
00168   else
00169   {
00170     ROS_ERROR ("models parameter must be an array!");
00171   }
00172 }
00173 
00174 // helper function to get current time
00175 double RealtimeURDFFilter::getTime ()
00176 {
00177   timeval current_time;
00178   gettimeofday (&current_time, NULL);
00179   return (current_time.tv_sec + 1e-6 * current_time.tv_usec);
00180 }
00181 
00182 void RealtimeURDFFilter::filter (
00183     unsigned char* buffer, double* projection_matrix, int width, int height)
00184 {
00185   static std::vector<double> timings;
00186   double begin = getTime();
00187   if (width_ != width || height_ != height) {
00188     if(width_ !=0 || height_!=0) {
00189       ROS_ERROR ("image size has changed (%ix%i) -> (%ix%i)", width_, height_, width, height);
00190     }
00191     width_ = width;
00192     height_ = height;
00193     this->initGL();
00194   }
00195   
00196   // Load models / construct renderers
00197   if(renderers_.empty()) {
00198     return;
00199   }
00200 
00201   if (mask_pub_.getNumSubscribers() > 0) {
00202     need_mask_ = true;
00203   } else {
00204     need_mask_ = false;
00205   }
00206 
00207   // get depth_image into OpenGL texture buffer
00208   int size_in_bytes = width_ * height_ * sizeof(float);
00209   textureBufferFromDepthBuffer(buffer, size_in_bytes);
00210 
00211   // render everything
00212   this->render(projection_matrix);
00213 
00214   // Timing
00215   static unsigned count = 0;
00216   static double last = getTime ();
00217 
00218   double now = getTime ();
00219   timings.push_back ((now - begin) * 1000.0);
00220 
00221   if (++count == 30 || (now - last) > 5) {
00222     double sum = 0.0;
00223     double max = 0.0;
00224     double min = FLT_MAX;
00225     for (std::vector<double>::iterator it = timings.begin(); it!=timings.end(); ++it)
00226     {
00227       min = std::min (min, *it);
00228       max = std::max (max, *it);
00229       sum += *it;
00230     }
00231 
00232     std::cout << "Average framerate: " 
00233       << std::setprecision(3) << double(count)/double(now - last) << " Hz " 
00234       << " (min: "<< min
00235       << ", max: " << max 
00236       << ", avg: " << sum / timings.size()
00237       << " ms)" << std::endl;
00238     count = 0;
00239     last = now;
00240     timings.clear();
00241   }
00242 }
00243 
00244 // callback function that gets ROS images and does everything
00245 void RealtimeURDFFilter::filter_callback
00246      (const sensor_msgs::ImageConstPtr& ros_depth_image,
00247       const sensor_msgs::CameraInfo::ConstPtr& camera_info)
00248 {
00249   // Debugging
00250   ROS_DEBUG_STREAM("Received image with camera info: "<<*camera_info);
00251 
00252   // convert to OpenCV cv::Mat
00253   cv_bridge::CvImageConstPtr orig_depth_img;
00254   try {
00255     orig_depth_img = cv_bridge::toCvShare (ros_depth_image, sensor_msgs::image_encodings::TYPE_32FC1);
00256   } catch (cv_bridge::Exception& e) {
00257     ROS_ERROR("cv_bridge Exception: %s", e.what());
00258     return;
00259   }
00260 
00261   // Convert the depth image into a char buffer
00262   cv::Mat1f depth_image = orig_depth_img->image;
00263   unsigned char *buffer = bufferFromDepthImage(depth_image);
00264 
00265   // Compute the projection matrix from the camera_info 
00266   double projection_matrix[16];
00267   getProjectionMatrix (camera_info, projection_matrix);
00268 
00269   // Filter the image
00270   this->filter(buffer, projection_matrix, depth_image.cols, depth_image.rows);
00271 
00272   // publish processed depth image and image mask
00273   if (depth_pub_.getNumSubscribers() > 0)
00274   {
00275     cv::Mat masked_depth_image (height_, width_, CV_32FC1, masked_depth_);
00276     cv_bridge::CvImage out_masked_depth;
00277     //out_masked_depth.header.frame_id = cam_frame_;
00278     //out_masked_depth.header.stamp = ros_depth_image->header.stamp;
00279     out_masked_depth.header = ros_depth_image->header;
00280     out_masked_depth.encoding = "32FC1";
00281     out_masked_depth.image = masked_depth_image;
00282     depth_pub_.publish (out_masked_depth.toImageMsg (), camera_info);
00283   }
00284 
00285   if (mask_pub_.getNumSubscribers() > 0)
00286   {
00287     cv::Mat mask_image (height_, width_, CV_8UC1, mask_);
00288 
00289     cv_bridge::CvImage out_mask;
00290     out_mask.header.frame_id = cam_frame_;
00291     out_mask.header.stamp = ros_depth_image->header.stamp;
00292     out_mask.encoding = "mono8";
00293     out_mask.image = mask_image;
00294     mask_pub_.publish (out_mask.toImageMsg (), camera_info);
00295   }
00296 }
00297 
00298 void RealtimeURDFFilter::textureBufferFromDepthBuffer(unsigned char* buffer, int size_in_bytes)
00299 {    
00300   ROS_DEBUG("Texture buffer from depth buffer...");
00301   // check if we already have a PBO 
00302   if (depth_image_pbo_ == GL_INVALID_VALUE) {
00303     ROS_DEBUG("Generating Pixel Buffer Object...");
00304     glGenBuffers(1, &depth_image_pbo_);
00305   }
00306   // upload buffer data to GPU
00307   glBindBuffer(GL_ARRAY_BUFFER, depth_image_pbo_);
00308   glBufferData(GL_ARRAY_BUFFER, size_in_bytes, buffer, GL_DYNAMIC_DRAW);
00309   glBindBuffer(GL_ARRAY_BUFFER, 0);
00310 
00311   // Check if we already have a texture buffer
00312   if (depth_texture_ == GL_INVALID_VALUE) {
00313     ROS_DEBUG("Generating Texture Object...");
00314     glGenTextures(1, &depth_texture_);
00315   }
00316   // assign PBO to Texture Buffer
00317   glBindTexture(GL_TEXTURE_BUFFER, depth_texture_);
00318   glTexBuffer(GL_TEXTURE_BUFFER, GL_R32F, depth_image_pbo_);
00319 }
00320 
00321 unsigned char* RealtimeURDFFilter::bufferFromDepthImage (cv::Mat1f depth_image)
00322 {
00323   // Host buffer to hold depth pixel data
00324   static unsigned char* buffer = NULL;
00325 
00326   // Try to get the pixel data from cv::Mat as one continuous buffer
00327   if (depth_image.isContinuous()) {
00328     buffer = depth_image.data;
00329   } else {
00330     // Get the size of each row in bytes
00331     int row_size = depth_image.cols * depth_image.elemSize();
00332 
00333     // Allocate the buffer
00334     if (buffer == NULL) {
00335       std::cout << "(re)allocating opengl depth buffer" << std::endl;
00336       buffer = (unsigned char*) malloc(row_size * depth_image.rows);
00337     }
00338 
00339     // Copy the image row by row
00340     for (int i = 0; i < depth_image.rows; i++) {
00341       memcpy(
00342           (void*)(buffer + i * row_size), 
00343           (void*) &depth_image.data[i],
00344           row_size);
00345     }
00346   }
00347 
00348   return buffer;
00349 }
00350 
00351 // set up OpenGL stuff
00352 void RealtimeURDFFilter::initGL ()
00353 {
00354   static bool gl_initialized = false;
00355 
00356   ROS_INFO("Initializing OpenGL subsystem...");
00357 
00358   if (!gl_initialized) {
00359     // Initialize GLUT
00360     glutInit (&argc_, argv_);
00361 
00362     //TODO: change this to use an offscreen pbuffer, so no window is necessary,
00363     //for now, we can just hide it (see below)
00364     
00365     // The debug window shows a 3x2 grid of images
00366     glutInitWindowSize (960, 480);
00367     glutInitDisplayMode ( GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH | GLUT_STENCIL);
00368     glutCreateWindow ("Realtime URDF Filter Debug Window");
00369 
00370     // Hide the GLUT window
00371     if (!show_gui_) {
00372       glutHideWindow();
00373     }
00374 
00375     gl_initialized = true;
00376   }
00377 
00378   // initialize OpenGL Extension Wrangler library
00379   GLenum err = glewInit();
00380   if (GLEW_OK != err) {
00381     throw std::runtime_error("ERROR: could not initialize GLEW!");
00382   }
00383 
00384   // Set up FBO 
00385   // FIXME: Replace this with more robust / specialized FBO
00386   this->initFrameBufferObject();
00387 
00388   // Load URDF models + meshes onto GPU
00389   this->loadModels();
00390 
00391   // Make sure we loaded something!
00392   if(renderers_.empty()) { 
00393     throw std::runtime_error("Could not load any models for filtering!");
00394   } else {
00395     ROS_INFO_STREAM("Loaded "<<renderers_.size()<<" models for filtering.");
00396   }
00397   
00398   // Alocate buffer for the masked depth image (float) 
00399   masked_depth_ = (GLfloat*) malloc(width_ * height_ * sizeof(GLfloat));
00400   // Alocate buffer for the mask (uchar) 
00401   mask_ = (GLubyte*) malloc(width_ * height_ * sizeof(GLubyte));
00402 }
00403 
00404 // set up FBO
00405 void RealtimeURDFFilter::initFrameBufferObject ()
00406 {
00407 
00408   fbo_ = new FramebufferObject("rgba=4x32t depth=24t stencil=8t");
00409   fbo_->initialize(width_, height_);
00410 
00411   fbo_initialized_ = true;
00412 
00413   GLenum err = glGetError();
00414   if(err != GL_NO_ERROR) {
00415     ROS_ERROR("OpenGL ERROR after FBO initialization: %s", gluErrorString(err));
00416   }
00417 
00418   GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
00419   if(status != GL_FRAMEBUFFER_COMPLETE) {
00420     ROS_ERROR("OpenGL FrameBuffer ERROR after FBO initialization: %i", status);
00421   }
00422 }
00423 
00424 // compute Projection matrix from CameraInfo message
00425 void RealtimeURDFFilter::getProjectionMatrix (
00426     const sensor_msgs::CameraInfo::ConstPtr& info, btScalar* glTf)
00427 {
00428   tf::Vector3 position;
00429   tf::Quaternion orientation;
00430 
00431 #ifdef USE_OWN_CALIBRATION
00432   float P[12];
00433   P[0] = 585.260; P[1] = 0.0;     P[2]  = 317.387; P[3]  = 0.0;
00434   P[4] = 0.0;     P[5] = 585.028; P[6]  = 239.264; P[7]  = 0.0;
00435   P[8] = 0.0;     P[9] = 0.0;     P[10] = 1.0;     P[11] = 0.0;
00436 
00437   double fx = P[0];
00438   double fy = P[5];
00439   double cx = P[2];
00440   double cy = P[6];
00441 #else
00442   double fx = info->P[0] * 0.5;
00443   double fy = info->P[5] * 0.5;
00444   double cx = info->P[2] * 0.5;
00445   double cy = (info->P[6]) * 0.5 - 48;
00446 
00447   // TODO: check if this does the right thing with respect to registered depth / camera info
00448   // Add the camera's translation relative to the left camera (from P[3]);
00449   camera_tx_ = -1 * (info->P[3] / fx);
00450   camera_ty_ = -1 * (info->P[7] / fy);
00451 
00452 #endif
00453 
00454   for (unsigned int i = 0; i < 16; ++i) {
00455     glTf[i] = 0.0;
00456   }
00457 
00458   // calculate the projection matrix
00459   // NOTE: this minus is there to flip the x-axis of the image.
00460   glTf[0]= -2.0 * fx / width_;
00461   glTf[5]= 2.0 * fy / height_;
00462 
00463   glTf[8]= 2.0 * (0.5 - cx / width_);
00464   glTf[9]= 2.0 * (cy / height_ - 0.5);
00465 
00466   glTf[10]= - (far_plane_ + near_plane_) / (far_plane_ - near_plane_);
00467   glTf[14]= -2.0 * far_plane_ * near_plane_ / (far_plane_ - near_plane_);
00468 
00469   glTf[11]= -1;
00470 }
00471 
00472 void RealtimeURDFFilter::render (const double* camera_projection_matrix)
00473 {   
00474   static const GLenum buffers[] = {
00475     GL_COLOR_ATTACHMENT0,
00476     GL_COLOR_ATTACHMENT1,
00477     GL_COLOR_ATTACHMENT2,
00478     GL_COLOR_ATTACHMENT3
00479   };
00480 
00481   GLenum err;
00482 
00483   // Check if the framebuffer object has been initialized
00484   if (!fbo_initialized_) {
00485     return;
00486   }
00487 
00488   // get transformation from camera to "fixed frame"
00489   tf::StampedTransform camera_transform;
00490   try {
00491     tf_.lookupTransform (cam_frame_, fixed_frame_, ros::Time (), camera_transform);
00492     ROS_DEBUG_STREAM("Camera to world translation "<<
00493         cam_frame_<<" -> "<<fixed_frame_<<
00494         " ["<<
00495         " "<<camera_transform.getOrigin().x()<<
00496         " "<<camera_transform.getOrigin().y()<<
00497         " "<<camera_transform.getOrigin().z()<<
00498         "]"
00499         );
00500   } catch (tf::TransformException ex) {
00501     ROS_ERROR("%s",ex.what());
00502     return;
00503   }
00504 
00505   err = glGetError();
00506   if(err != GL_NO_ERROR) {
00507     ROS_ERROR("OpenGL ERROR at beginning of rendering: %s", gluErrorString(err));
00508     return;
00509   }
00510 
00511   glPushAttrib(GL_ALL_ATTRIB_BITS);
00512   glEnable(GL_NORMALIZE);
00513 
00514   // Render into FBO
00515   fbo_->beginCapture();
00516 
00517   // Create shader programs
00518   static ShaderWrapper shader = ShaderWrapper::fromFiles(
00519       "package://realtime_urdf_filter/include/shaders/urdf_filter.vert", 
00520       "package://realtime_urdf_filter/include/shaders/urdf_filter.frag");
00521 
00522   err = glGetError();
00523   if(err != GL_NO_ERROR) {
00524     ROS_ERROR("OpenGL ERROR compiling shaders: %s", gluErrorString(err));
00525     return;
00526   }
00527   
00528   // Enable shader for this frame
00529   shader();
00530 
00531   // Specify the list of color buffers to draw into
00532   glDrawBuffers(sizeof(buffers) / sizeof(GLenum), buffers);
00533 
00534   //Cclear the buffers
00535   glClearColor(0.0, 0.0, 0.0, 1.0);
00536   glClearStencil(0x0);
00537   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
00538 
00539   glEnable(GL_DEPTH_TEST);
00540   glDisable(GL_TEXTURE_2D);
00541 
00542   fbo_->disableTextureTarget();
00543 
00544   // Setup camera projection
00545   glMatrixMode (GL_PROJECTION);
00546   glLoadIdentity();
00547 
00548   // Load camera projection matrix into OpenGL camera matrix
00549   glMultMatrixd(camera_projection_matrix);
00550 
00551   // Setup camera position
00552   glMatrixMode(GL_MODELVIEW);
00553   glLoadIdentity();
00554 
00555   // Kinect has x right, y down, z into image
00556   gluLookAt (0,0,0, 0,0,1, 0,1,0);
00557   
00558   // Draw background quad behind everything (just before the far plane)
00559   // Otherwise, the shader only sees kinect points where he rendered stuff
00560   glBegin(GL_QUADS);
00561     glVertex3f(-100.0, -100.0, far_plane_*0.99);
00562     glVertex3f( 100.0, -100.0, far_plane_*0.99);
00563     glVertex3f( 100.0,  100.0, far_plane_*0.99);
00564     glVertex3f(-100.0,  100.0, far_plane_*0.99);
00565   glEnd();
00566  
00567   // Transformation matrix
00568   btScalar glTf[16];
00569   
00570   // Apply user-defined camera offset transformation (launch file)
00571   tf::Transform transform (camera_offset_q_, camera_offset_t_);
00572   transform.inverse().getOpenGLMatrix(glTf);
00573   glMultMatrixd((GLdouble*)glTf);
00574 
00575   // Apply camera to "fixed frame" transform (world coordinates)
00576   tf::Vector3 right = tf::Transform(camera_transform.getRotation()) * tf::Vector3 (1,0,0);
00577   camera_transform.setOrigin(camera_transform.getOrigin() + (right * camera_tx_));
00578   tf::Vector3 down = tf::Transform(camera_transform.getRotation()) * tf::Vector3 (0,1,0);
00579   camera_transform.setOrigin(camera_transform.getOrigin() + (down * camera_ty_));
00580 
00581 
00582   camera_transform.getOpenGLMatrix(glTf);
00583   glMultMatrixd((GLdouble*)glTf);
00584   
00585   // Set up stencil buffer etc.
00586   // The background quad is not in the stencil buffer
00587   glEnable(GL_STENCIL_TEST);
00588   glStencilFunc(GL_ALWAYS, 0x1, 0x1);
00589   glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE);
00590 
00591   // make texture with depth image available in shader
00592   glActiveTexture (GL_TEXTURE0);
00593   GLuint depth_texture_id = 0;
00594   shader.SetUniformVal1i (std::string("depth_texture"), depth_texture_id);
00595   shader.SetUniformVal1i (std::string("width"), int(width_));
00596   shader.SetUniformVal1i (std::string("height"), int(height_));
00597   shader.SetUniformVal1f (std::string("z_far"), far_plane_);
00598   shader.SetUniformVal1f (std::string("z_near"), near_plane_);
00599   shader.SetUniformVal1f (std::string("max_diff"), float(depth_distance_threshold_));
00600   shader.SetUniformVal1f (std::string("replace_value"), float(filter_replace_value_));
00601   glBindTexture (GL_TEXTURE_BUFFER, depth_texture_);
00602 
00603   // render every renderable / urdf model
00604   std::vector<URDFRenderer*>::const_iterator r;
00605   for (r = renderers_.begin (); r != renderers_.end (); r++) {
00606     (*r)->render ();
00607   }
00608 
00609   // Disable shader
00610   glUseProgram((GLuint)NULL);
00611   
00612   fbo_->endCapture();
00613   glPopAttrib();
00614 
00615   // Use stencil buffer to draw a red / blue mask into color attachment 3
00616   if (need_mask_ || show_gui_) {
00617     glPushAttrib(GL_ALL_ATTRIB_BITS);
00618 
00619     fbo_->beginCapture();
00620       glDrawBuffer(GL_COLOR_ATTACHMENT3);
00621 
00622       glEnable(GL_STENCIL_TEST);
00623       glStencilFunc(GL_EQUAL, 0x1, 0x1);
00624       glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
00625 
00626       glDisable(GL_DEPTH_TEST);
00627       glDisable(GL_TEXTURE_2D);
00628       fbo_->disableTextureTarget();
00629 
00630       glMatrixMode(GL_PROJECTION);
00631       glPushMatrix();
00632         glLoadIdentity();
00633         gluOrtho2D(0.0, 1.0, 0.0, 1.0);
00634 
00635         glMatrixMode(GL_MODELVIEW);     
00636         glPushMatrix();
00637           glLoadIdentity();
00638 
00639           glColor3f(1.0, 0.0, 0.0);
00640 
00641           glBegin(GL_QUADS);
00642             glVertex2f(0.0, 0.0);
00643             glVertex2f(1.0, 0.0);
00644             glVertex2f(1.0, 1.0);
00645             glVertex2f(0.0, 1.0);
00646           glEnd();
00647 
00648         glPopMatrix();
00649         glMatrixMode(GL_PROJECTION);
00650       glPopMatrix();
00651 
00652       glStencilFunc(GL_EQUAL, 0x0, 0x1);
00653       glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
00654 
00655       glDisable(GL_DEPTH_TEST);
00656       glDisable(GL_TEXTURE_2D);
00657       fbo_->disableTextureTarget();
00658 
00659       glMatrixMode(GL_PROJECTION);
00660       glPushMatrix();
00661         glLoadIdentity();
00662         gluOrtho2D(0.0, 1.0, 0.0, 1.0);
00663 
00664         glMatrixMode(GL_MODELVIEW);     
00665         glPushMatrix();
00666           glLoadIdentity();
00667 
00668           glColor3f(0.0, 0.0, 1.0);
00669 
00670           glBegin(GL_QUADS);
00671             glVertex2f(0.0, 0.0);
00672             glVertex2f(1.0, 0.0);
00673             glVertex2f(1.0, 1.0);
00674             glVertex2f(0.0, 1.0);
00675           glEnd();
00676 
00677         glPopMatrix();
00678         glMatrixMode(GL_PROJECTION);
00679       glPopMatrix();
00680     fbo_->endCapture();
00681 
00682     glPopAttrib();
00683   }
00684 
00685   // Render all color buffer attachments into window
00686   if (show_gui_) {
00687     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00688 
00689     glMatrixMode(GL_PROJECTION);
00690     glPushMatrix();
00691       glLoadIdentity();
00692       gluOrtho2D(0.0, 1.0, 0.0, 1.0);
00693 
00694       glMatrixMode(GL_MODELVIEW);       
00695       glPushMatrix();
00696         glLoadIdentity();
00697 
00698         // draw color buffer 0
00699         fbo_->bind(0);
00700         glBegin(GL_QUADS);
00701           glTexCoord2f(0.0, fbo_->getHeight());
00702           glVertex2f(0.0, 0.5);
00703           glTexCoord2f(fbo_->getWidth(), fbo_->getHeight());
00704           glVertex2f(0.333, 0.5);
00705           glTexCoord2f(fbo_->getWidth(), 0.0);
00706           glVertex2f(0.333, 1.0);
00707           glTexCoord2f(0.0, 0.0);
00708           glVertex2f(0.0, 1.0);
00709         glEnd();
00710 
00711         // draw color buffer 1
00712         fbo_->bind(1);
00713         glBegin(GL_QUADS);
00714           glTexCoord2f(0.0, fbo_->getHeight());
00715           glVertex2f(0.0, 0.0);
00716           glTexCoord2f(fbo_->getWidth(), fbo_->getHeight());
00717           glVertex2f(0.333, 0.0);
00718           glTexCoord2f(fbo_->getWidth(), 0.0);
00719           glVertex2f(0.333, 0.5);
00720           glTexCoord2f(0.0, 0.0);
00721           glVertex2f(0.0, 0.5);
00722         glEnd();
00723 
00724         // draw color buffer 2
00725         fbo_->bind(2);
00726         glBegin(GL_QUADS);
00727           glTexCoord2f(0.0, fbo_->getHeight());
00728           glVertex2f(0.333, 0.5);
00729           glTexCoord2f(fbo_->getWidth(), fbo_->getHeight());
00730           glVertex2f(0.666, 0.5);
00731           glTexCoord2f(fbo_->getWidth(), 0.0);
00732           glVertex2f(0.666, 1.0);
00733           glTexCoord2f(0.0, 0.0);
00734           glVertex2f(0.333, 1.0);
00735         glEnd();
00736 
00737         // draw color buffer 3
00738         fbo_->bind(3);
00739         glBegin(GL_QUADS);
00740           glTexCoord2f(0.0, fbo_->getHeight());
00741           glVertex2f(0.333, 0.0);
00742           glTexCoord2f(fbo_->getWidth(), fbo_->getHeight());
00743           glVertex2f(0.666, 0.0);
00744           glTexCoord2f(fbo_->getWidth(), 0.0);
00745           glVertex2f(0.666, 0.5);
00746           glTexCoord2f(0.0, 0.0);
00747           glVertex2f(0.333, 0.5);
00748         glEnd();
00749 
00750         // draw depth buffer 
00751         fbo_->bindDepth();
00752         glBegin(GL_QUADS);
00753           glTexCoord2f(0.0, fbo_->getHeight());
00754           glVertex2f(0.666, 0.5);
00755           glTexCoord2f(fbo_->getWidth(), fbo_->getHeight());
00756           glVertex2f(1.0, 0.5);
00757           glTexCoord2f(fbo_->getWidth(), 0.0);
00758           glVertex2f(1.0, 1.0);
00759           glTexCoord2f(0.0, 0.0);
00760           glVertex2f(0.666, 1.0);
00761         glEnd();
00762 
00763       glPopMatrix();
00764       glMatrixMode(GL_PROJECTION);
00765     glPopMatrix();
00766   } 
00767 
00768   fbo_->bind(1);
00769   glGetTexImage (fbo_->getTextureTarget(), 0, GL_RED, GL_FLOAT, masked_depth_);
00770   if (need_mask_)
00771   {
00772     fbo_->bind(3);
00773     glGetTexImage (fbo_->getTextureTarget(), 0, GL_RED, GL_UNSIGNED_BYTE, mask_);
00774   }
00775 
00776   // Ok, finished with all OpenGL, let's swap!
00777   if (show_gui_) {
00778     glutSwapBuffers ();
00779     glutPostRedisplay();
00780     glutMainLoopEvent ();
00781   }
00782   // TODO: this necessary? glFlush ();
00783 }
00784 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


realtime_urdf_filter
Author(s): Nico Blodow
autogenerated on Thu May 23 2013 16:50:36