00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
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
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
00064
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
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
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
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
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
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
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
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
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
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
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
00175 double RealtimeURDFFilter::getTime ()
00176 {
00177 timeval current_time;
00178 gettimeofday (¤t_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
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
00208 int size_in_bytes = width_ * height_ * sizeof(float);
00209 textureBufferFromDepthBuffer(buffer, size_in_bytes);
00210
00211
00212 this->render(projection_matrix);
00213
00214
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
00245 void RealtimeURDFFilter::filter_callback
00246 (const sensor_msgs::ImageConstPtr& ros_depth_image,
00247 const sensor_msgs::CameraInfo::ConstPtr& camera_info)
00248 {
00249
00250 ROS_DEBUG_STREAM("Received image with camera info: "<<*camera_info);
00251
00252
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
00262 cv::Mat1f depth_image = orig_depth_img->image;
00263 unsigned char *buffer = bufferFromDepthImage(depth_image);
00264
00265
00266 double projection_matrix[16];
00267 getProjectionMatrix (camera_info, projection_matrix);
00268
00269
00270 this->filter(buffer, projection_matrix, depth_image.cols, depth_image.rows);
00271
00272
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
00278
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
00302 if (depth_image_pbo_ == GL_INVALID_VALUE) {
00303 ROS_DEBUG("Generating Pixel Buffer Object...");
00304 glGenBuffers(1, &depth_image_pbo_);
00305 }
00306
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
00312 if (depth_texture_ == GL_INVALID_VALUE) {
00313 ROS_DEBUG("Generating Texture Object...");
00314 glGenTextures(1, &depth_texture_);
00315 }
00316
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
00324 static unsigned char* buffer = NULL;
00325
00326
00327 if (depth_image.isContinuous()) {
00328 buffer = depth_image.data;
00329 } else {
00330
00331 int row_size = depth_image.cols * depth_image.elemSize();
00332
00333
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
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
00352 void RealtimeURDFFilter::initGL ()
00353 {
00354 static bool gl_initialized = false;
00355
00356 ROS_INFO("Initializing OpenGL subsystem...");
00357
00358 if (!gl_initialized) {
00359
00360 glutInit (&argc_, argv_);
00361
00362
00363
00364
00365
00366 glutInitWindowSize (960, 480);
00367 glutInitDisplayMode ( GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH | GLUT_STENCIL);
00368 glutCreateWindow ("Realtime URDF Filter Debug Window");
00369
00370
00371 if (!show_gui_) {
00372 glutHideWindow();
00373 }
00374
00375 gl_initialized = true;
00376 }
00377
00378
00379 GLenum err = glewInit();
00380 if (GLEW_OK != err) {
00381 throw std::runtime_error("ERROR: could not initialize GLEW!");
00382 }
00383
00384
00385
00386 this->initFrameBufferObject();
00387
00388
00389 this->loadModels();
00390
00391
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
00399 masked_depth_ = (GLfloat*) malloc(width_ * height_ * sizeof(GLfloat));
00400
00401 mask_ = (GLubyte*) malloc(width_ * height_ * sizeof(GLubyte));
00402 }
00403
00404
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
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
00448
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
00459
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
00484 if (!fbo_initialized_) {
00485 return;
00486 }
00487
00488
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
00515 fbo_->beginCapture();
00516
00517
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
00529 shader();
00530
00531
00532 glDrawBuffers(sizeof(buffers) / sizeof(GLenum), buffers);
00533
00534
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
00545 glMatrixMode (GL_PROJECTION);
00546 glLoadIdentity();
00547
00548
00549 glMultMatrixd(camera_projection_matrix);
00550
00551
00552 glMatrixMode(GL_MODELVIEW);
00553 glLoadIdentity();
00554
00555
00556 gluLookAt (0,0,0, 0,0,1, 0,1,0);
00557
00558
00559
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
00568 btScalar glTf[16];
00569
00570
00571 tf::Transform transform (camera_offset_q_, camera_offset_t_);
00572 transform.inverse().getOpenGLMatrix(glTf);
00573 glMultMatrixd((GLdouble*)glTf);
00574
00575
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
00586
00587 glEnable(GL_STENCIL_TEST);
00588 glStencilFunc(GL_ALWAYS, 0x1, 0x1);
00589 glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE);
00590
00591
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
00604 std::vector<URDFRenderer*>::const_iterator r;
00605 for (r = renderers_.begin (); r != renderers_.end (); r++) {
00606 (*r)->render ();
00607 }
00608
00609
00610 glUseProgram((GLuint)NULL);
00611
00612 fbo_->endCapture();
00613 glPopAttrib();
00614
00615
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
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
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
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
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
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
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
00777 if (show_gui_) {
00778 glutSwapBuffers ();
00779 glutPostRedisplay();
00780 glutMainLoopEvent ();
00781 }
00782
00783 }
00784