mesh_filter_base.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Suat Gedikli */
00036 
00037 #include <moveit/mesh_filter/mesh_filter_base.h>
00038 #include <moveit/mesh_filter/gl_mesh.h>
00039 #include <moveit/mesh_filter/filter_job.h>
00040 
00041 #include <geometric_shapes/shapes.h>
00042 #include <geometric_shapes/shape_operations.h>
00043 #include <Eigen/Eigen>
00044 #include <stdexcept>
00045 #include <sstream>
00046 #include <sensor_msgs/image_encodings.h>
00047 
00048 #include <ros/console.h>
00049 
00050 // include SSE headers
00051 #ifdef HAVE_SSE_EXTENSIONS
00052 #include <xmmintrin.h>
00053 #endif
00054 
00055 mesh_filter::MeshFilterBase::MeshFilterBase(const TransformCallback& transform_callback,
00056                                             const SensorModel::Parameters& sensor_parameters,
00057                                             const std::string& render_vertex_shader,
00058                                             const std::string& render_fragment_shader,
00059                                             const std::string& filter_vertex_shader,
00060                                             const std::string& filter_fragment_shader)
00061   : sensor_parameters_(sensor_parameters.clone())
00062   , next_handle_(FirstLabel)  // 0 and 1 are reserved!
00063   , min_handle_(FirstLabel)
00064   , stop_(false)
00065   , transform_callback_(transform_callback)
00066   , padding_scale_(1.0)
00067   , padding_offset_(0.01)
00068   , shadow_threshold_(0.5)
00069 {
00070   filter_thread_ = boost::thread(boost::bind(&MeshFilterBase::run, this, render_vertex_shader, render_fragment_shader,
00071                                              filter_vertex_shader, filter_fragment_shader));
00072 }
00073 
00074 void mesh_filter::MeshFilterBase::initialize(const std::string& render_vertex_shader,
00075                                              const std::string& render_fragment_shader,
00076                                              const std::string& filter_vertex_shader,
00077                                              const std::string& filter_fragment_shader)
00078 {
00079   mesh_renderer_.reset(new GLRenderer(sensor_parameters_->getWidth(), sensor_parameters_->getHeight(),
00080                                       sensor_parameters_->getNearClippingPlaneDistance(),
00081                                       sensor_parameters_->getFarClippingPlaneDistance()));
00082   depth_filter_.reset(new GLRenderer(sensor_parameters_->getWidth(), sensor_parameters_->getHeight(),
00083                                      sensor_parameters_->getNearClippingPlaneDistance(),
00084                                      sensor_parameters_->getFarClippingPlaneDistance()));
00085 
00086   mesh_renderer_->setShadersFromString(render_vertex_shader, render_fragment_shader);
00087   depth_filter_->setShadersFromString(filter_vertex_shader, filter_fragment_shader);
00088 
00089   depth_filter_->begin();
00090 
00091   glGenTextures(1, &sensor_depth_texture_);
00092 
00093   glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(), "sensor"), 0);
00094   glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(), "depth"), 2);
00095   glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(), "label"), 4);
00096 
00097   shadow_threshold_location_ = glGetUniformLocation(depth_filter_->getProgramID(), "shadow_threshold");
00098 
00099   depth_filter_->end();
00100 
00101   canvas_ = glGenLists(1);
00102   glNewList(canvas_, GL_COMPILE);
00103   glBegin(GL_QUADS);
00104 
00105   glColor3f(1, 1, 1);
00106   glTexCoord2f(0, 0);
00107   glVertex3f(-1, -1, 1);
00108 
00109   glTexCoord2f(1, 0);
00110   glVertex3f(1, -1, 1);
00111 
00112   glTexCoord2f(1, 1);
00113   glVertex3f(1, 1, 1);
00114 
00115   glTexCoord2f(0, 1);
00116   glVertex3f(-1, 1, 1);
00117 
00118   glEnd();
00119   glEndList();
00120 }
00121 
00122 mesh_filter::MeshFilterBase::~MeshFilterBase()
00123 {
00124   {
00125     boost::unique_lock<boost::mutex> lock(jobs_mutex_);
00126     stop_ = true;
00127     while (!jobs_queue_.empty())
00128     {
00129       jobs_queue_.front()->cancel();
00130       jobs_queue_.pop();
00131     }
00132   }
00133   jobs_condition_.notify_one();
00134   filter_thread_.join();
00135 }
00136 
00137 void mesh_filter::MeshFilterBase::addJob(const JobPtr& job) const
00138 {
00139   {
00140     boost::unique_lock<boost::mutex> _(jobs_mutex_);
00141     jobs_queue_.push(job);
00142   }
00143   jobs_condition_.notify_one();
00144 }
00145 
00146 void mesh_filter::MeshFilterBase::deInitialize()
00147 {
00148   glDeleteLists(canvas_, 1);
00149   glDeleteTextures(1, &sensor_depth_texture_);
00150 
00151   meshes_.clear();
00152   mesh_renderer_.reset();
00153   depth_filter_.reset();
00154 }
00155 
00156 void mesh_filter::MeshFilterBase::setSize(unsigned int width, unsigned int height)
00157 {
00158   mesh_renderer_->setBufferSize(width, height);
00159   mesh_renderer_->setCameraParameters(width, width, width >> 1, height >> 1);
00160 
00161   depth_filter_->setBufferSize(width, height);
00162   depth_filter_->setCameraParameters(width, width, width >> 1, height >> 1);
00163 }
00164 
00165 void mesh_filter::MeshFilterBase::setTransformCallback(const TransformCallback& transform_callback)
00166 {
00167   boost::mutex::scoped_lock _(transform_callback_mutex_);
00168   transform_callback_ = transform_callback;
00169 }
00170 
00171 mesh_filter::MeshHandle mesh_filter::MeshFilterBase::addMesh(const shapes::Mesh& mesh)
00172 {
00173   boost::mutex::scoped_lock _(meshes_mutex_);
00174 
00175   JobPtr job(new FilterJob<void>(boost::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh)));
00176   addJob(job);
00177   job->wait();
00178   mesh_filter::MeshHandle ret = next_handle_;
00179   const std::size_t sz = min_handle_ + meshes_.size() + 1;
00180   for (std::size_t i = min_handle_; i < sz; ++i)
00181     if (meshes_.find(i) == meshes_.end())
00182     {
00183       next_handle_ = i;
00184       break;
00185     }
00186   min_handle_ = next_handle_;
00187   return ret;
00188 }
00189 
00190 void mesh_filter::MeshFilterBase::addMeshHelper(MeshHandle handle, const shapes::Mesh* cmesh)
00191 {
00192   meshes_[handle] = GLMeshPtr(new GLMesh(*cmesh, handle));
00193 }
00194 
00195 void mesh_filter::MeshFilterBase::removeMesh(MeshHandle handle)
00196 {
00197   boost::mutex::scoped_lock _(meshes_mutex_);
00198   FilterJob<bool>* remover = new FilterJob<bool>(boost::bind(&MeshFilterBase::removeMeshHelper, this, handle));
00199   JobPtr job(remover);
00200   addJob(job);
00201   job->wait();
00202 
00203   if (!remover->getResult())
00204     throw std::runtime_error("Could not remove mesh. Mesh not found!");
00205   min_handle_ = std::min(handle, min_handle_);
00206 }
00207 
00208 bool mesh_filter::MeshFilterBase::removeMeshHelper(MeshHandle handle)
00209 {
00210   std::size_t erased = meshes_.erase(handle);
00211   return (erased != 0);
00212 }
00213 
00214 void mesh_filter::MeshFilterBase::setShadowThreshold(float threshold)
00215 {
00216   shadow_threshold_ = threshold;
00217 }
00218 
00219 void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const
00220 {
00221   JobPtr job(
00222       new FilterJob<void>(boost::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels)));
00223   addJob(job);
00224   job->wait();
00225 }
00226 
00227 void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const
00228 {
00229   JobPtr job1(new FilterJob<void>(boost::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth)));
00230   JobPtr job2(new FilterJob<void>(
00231       boost::bind(&SensorModel::Parameters::transformModelDepthToMetricDepth, sensor_parameters_.get(), depth)));
00232   {
00233     boost::unique_lock<boost::mutex> lock(jobs_mutex_);
00234     jobs_queue_.push(job1);
00235     jobs_queue_.push(job2);
00236   }
00237   jobs_condition_.notify_one();
00238   job1->wait();
00239   job2->wait();
00240 }
00241 
00242 void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const
00243 {
00244   JobPtr job1(new FilterJob<void>(boost::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth)));
00245   JobPtr job2(new FilterJob<void>(
00246       boost::bind(&SensorModel::Parameters::transformFilteredDepthToMetricDepth, sensor_parameters_.get(), depth)));
00247   {
00248     boost::unique_lock<boost::mutex> lock(jobs_mutex_);
00249     jobs_queue_.push(job1);
00250     jobs_queue_.push(job2);
00251   }
00252   jobs_condition_.notify_one();
00253   job1->wait();
00254   job2->wait();
00255 }
00256 
00257 void mesh_filter::MeshFilterBase::getFilteredLabels(LabelType* labels) const
00258 {
00259   JobPtr job(
00260       new FilterJob<void>(boost::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels)));
00261   addJob(job);
00262   job->wait();
00263 }
00264 
00265 void mesh_filter::MeshFilterBase::run(const std::string& render_vertex_shader,
00266                                       const std::string& render_fragment_shader,
00267                                       const std::string& filter_vertex_shader,
00268                                       const std::string& filter_fragment_shader)
00269 {
00270   initialize(render_vertex_shader, render_fragment_shader, filter_vertex_shader, filter_fragment_shader);
00271 
00272   while (!stop_)
00273   {
00274     boost::unique_lock<boost::mutex> lock(jobs_mutex_);
00275     // check if we have new sensor data to be processed. If not, wait until we get notified.
00276     if (jobs_queue_.empty())
00277       jobs_condition_.wait(lock);
00278 
00279     if (!jobs_queue_.empty())
00280     {
00281       JobPtr job = jobs_queue_.front();
00282       jobs_queue_.pop();
00283       lock.unlock();
00284       job->execute();
00285       lock.lock();
00286     }
00287   }
00288   deInitialize();
00289 }
00290 
00291 void mesh_filter::MeshFilterBase::filter(const void* sensor_data, GLushort type, bool wait) const
00292 {
00293   if (type != GL_FLOAT && type != GL_UNSIGNED_SHORT)
00294   {
00295     std::stringstream msg;
00296     msg << "unknown type \"" << type << "\". Allowed values are GL_FLOAT or GL_UNSIGNED_SHORT.";
00297     throw std::runtime_error(msg.str());
00298   }
00299 
00300   JobPtr job(new FilterJob<void>(boost::bind(&MeshFilterBase::doFilter, this, sensor_data, type)));
00301   addJob(job);
00302   if (wait)
00303     job->wait();
00304 }
00305 
00306 void mesh_filter::MeshFilterBase::doFilter(const void* sensor_data, const int encoding) const
00307 {
00308   boost::mutex::scoped_lock _(transform_callback_mutex_);
00309 
00310   mesh_renderer_->begin();
00311   sensor_parameters_->setRenderParameters(*mesh_renderer_);
00312 
00313   glEnable(GL_TEXTURE_2D);
00314   glEnable(GL_DEPTH_TEST);
00315   glDepthFunc(GL_LESS);
00316   glEnable(GL_CULL_FACE);
00317   glCullFace(GL_FRONT);
00318   glDisable(GL_ALPHA_TEST);
00319   glDisable(GL_BLEND);
00320 
00321   GLuint padding_coefficients_id = glGetUniformLocation(mesh_renderer_->getProgramID(), "padding_coefficients");
00322   Eigen::Vector3f padding_coefficients =
00323       sensor_parameters_->getPaddingCoefficients() * padding_scale_ + Eigen::Vector3f(0, 0, padding_offset_);
00324   glUniform3f(padding_coefficients_id, padding_coefficients[0], padding_coefficients[1], padding_coefficients[2]);
00325 
00326   Eigen::Affine3d transform;
00327   for (std::map<MeshHandle, GLMeshPtr>::const_iterator meshIt = meshes_.begin(); meshIt != meshes_.end(); ++meshIt)
00328     if (transform_callback_(meshIt->first, transform))
00329       meshIt->second->render(transform);
00330 
00331   mesh_renderer_->end();
00332 
00333   // now filter the depth_map with the second rendering stage
00334   // depth_filter_.setBufferSize (width, height);
00335   // depth_filter_.setCameraParameters (fx, fy, cx, cy);
00336   depth_filter_->begin();
00337   sensor_parameters_->setFilterParameters(*depth_filter_);
00338   glEnable(GL_TEXTURE_2D);
00339   glEnable(GL_DEPTH_TEST);
00340   glDepthFunc(GL_ALWAYS);
00341   glDisable(GL_CULL_FACE);
00342   glDisable(GL_ALPHA_TEST);
00343   glDisable(GL_BLEND);
00344 
00345   //  glUniform1f (near_location_, depth_filter_.getNearClippingDistance ());
00346   //  glUniform1f (far_location_, depth_filter_.getFarClippingDistance ());
00347   glUniform1f(shadow_threshold_location_, shadow_threshold_);
00348 
00349   GLuint depth_texture = mesh_renderer_->getDepthTexture();
00350   GLuint color_texture = mesh_renderer_->getColorTexture();
00351 
00352   // bind sensor depth
00353   glActiveTexture(GL_TEXTURE0);
00354   glBindTexture(GL_TEXTURE_2D, sensor_depth_texture_);
00355 
00356   float scale =
00357       1.0 / (sensor_parameters_->getFarClippingPlaneDistance() - sensor_parameters_->getNearClippingPlaneDistance());
00358 
00359   if (encoding == GL_UNSIGNED_SHORT)
00360     // unsigned shorts shorts will be mapped to the range 0-1 during transfer. Afterwards we can apply another scale +
00361     // offset to
00362     // map the values between near and far clipping plane to 0 - 1. -> scale = (65535 * depth - near ) / (far - near)
00363     // we have: [0 - 65535] -> [0 - 1]
00364     // we want: [near - far] -> [0 - 1]
00365     glPixelTransferf(GL_DEPTH_SCALE, scale * 65.535);
00366   else
00367     glPixelTransferf(GL_DEPTH_SCALE, scale);
00368   glPixelTransferf(GL_DEPTH_BIAS, -scale * sensor_parameters_->getNearClippingPlaneDistance());
00369 
00370   glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, sensor_parameters_->getWidth(), sensor_parameters_->getHeight(), 0,
00371                GL_DEPTH_COMPONENT, encoding, sensor_data);
00372   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
00373   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
00374   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
00375   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
00376 
00377   // bind depth map
00378   glActiveTexture(GL_TEXTURE2);
00379   glBindTexture(GL_TEXTURE_2D, depth_texture);
00380 
00381   // bind labels
00382   glActiveTexture(GL_TEXTURE4);
00383   glBindTexture(GL_TEXTURE_2D, color_texture);
00384   glCallList(canvas_);
00385   depth_filter_->end();
00386 }
00387 
00388 void mesh_filter::MeshFilterBase::setPaddingOffset(float offset)
00389 {
00390   padding_offset_ = offset;
00391 }
00392 
00393 void mesh_filter::MeshFilterBase::setPaddingScale(float scale)
00394 {
00395   padding_scale_ = scale;
00396 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon Apr 23 2018 10:25:10