mesh_filter_base.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Suat Gedikli */
36 
40 
43 #include <Eigen/Eigen>
44 #include <stdexcept>
45 #include <sstream>
47 
48 #include <ros/console.h>
49 
50 // include SSE headers
51 #ifdef HAVE_SSE_EXTENSIONS
52 #include <xmmintrin.h>
53 #endif
54 
56  const SensorModel::Parameters& sensor_parameters,
57  const std::string& render_vertex_shader,
58  const std::string& render_fragment_shader,
59  const std::string& filter_vertex_shader,
60  const std::string& filter_fragment_shader)
61  : sensor_parameters_(sensor_parameters.clone())
62  , next_handle_(FirstLabel) // 0 and 1 are reserved!
63  , min_handle_(FirstLabel)
64  , stop_(false)
65  , transform_callback_(transform_callback)
66  , padding_scale_(1.0)
67  , padding_offset_(0.01)
68  , shadow_threshold_(0.5)
69 {
70  filter_thread_ = boost::thread(boost::bind(&MeshFilterBase::run, this, render_vertex_shader, render_fragment_shader,
71  filter_vertex_shader, filter_fragment_shader));
72 }
73 
74 void mesh_filter::MeshFilterBase::initialize(const std::string& render_vertex_shader,
75  const std::string& render_fragment_shader,
76  const std::string& filter_vertex_shader,
77  const std::string& filter_fragment_shader)
78 {
79  mesh_renderer_.reset(new GLRenderer(sensor_parameters_->getWidth(), sensor_parameters_->getHeight(),
80  sensor_parameters_->getNearClippingPlaneDistance(),
81  sensor_parameters_->getFarClippingPlaneDistance()));
82  depth_filter_.reset(new GLRenderer(sensor_parameters_->getWidth(), sensor_parameters_->getHeight(),
83  sensor_parameters_->getNearClippingPlaneDistance(),
84  sensor_parameters_->getFarClippingPlaneDistance()));
85 
86  mesh_renderer_->setShadersFromString(render_vertex_shader, render_fragment_shader);
87  depth_filter_->setShadersFromString(filter_vertex_shader, filter_fragment_shader);
88 
89  depth_filter_->begin();
90 
91  glGenTextures(1, &sensor_depth_texture_);
92 
93  glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(), "sensor"), 0);
94  glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(), "depth"), 2);
95  glUniform1i(glGetUniformLocation(depth_filter_->getProgramID(), "label"), 4);
96 
97  shadow_threshold_location_ = glGetUniformLocation(depth_filter_->getProgramID(), "shadow_threshold");
98 
99  depth_filter_->end();
100 
101  canvas_ = glGenLists(1);
102  glNewList(canvas_, GL_COMPILE);
103  glBegin(GL_QUADS);
104 
105  glColor3f(1, 1, 1);
106  glTexCoord2f(0, 0);
107  glVertex3f(-1, -1, 1);
108 
109  glTexCoord2f(1, 0);
110  glVertex3f(1, -1, 1);
111 
112  glTexCoord2f(1, 1);
113  glVertex3f(1, 1, 1);
114 
115  glTexCoord2f(0, 1);
116  glVertex3f(-1, 1, 1);
117 
118  glEnd();
119  glEndList();
120 }
121 
123 {
124  {
125  boost::unique_lock<boost::mutex> lock(jobs_mutex_);
126  stop_ = true;
127  while (!jobs_queue_.empty())
128  {
129  jobs_queue_.front()->cancel();
130  jobs_queue_.pop();
131  }
132  }
133  jobs_condition_.notify_one();
134  filter_thread_.join();
135 }
136 
137 void mesh_filter::MeshFilterBase::addJob(const JobPtr& job) const
138 {
139  {
140  boost::unique_lock<boost::mutex> _(jobs_mutex_);
141  jobs_queue_.push(job);
142  }
143  jobs_condition_.notify_one();
144 }
145 
147 {
148  glDeleteLists(canvas_, 1);
149  glDeleteTextures(1, &sensor_depth_texture_);
150 
151  meshes_.clear();
152  mesh_renderer_.reset();
153  depth_filter_.reset();
154 }
155 
156 void mesh_filter::MeshFilterBase::setSize(unsigned int width, unsigned int height)
157 {
158  mesh_renderer_->setBufferSize(width, height);
159  mesh_renderer_->setCameraParameters(width, width, width >> 1, height >> 1);
160 
161  depth_filter_->setBufferSize(width, height);
162  depth_filter_->setCameraParameters(width, width, width >> 1, height >> 1);
163 }
164 
166 {
167  boost::mutex::scoped_lock _(transform_callback_mutex_);
168  transform_callback_ = transform_callback;
169 }
170 
172 {
173  boost::mutex::scoped_lock _(meshes_mutex_);
174 
175  JobPtr job(new FilterJob<void>(boost::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh)));
176  addJob(job);
177  job->wait();
179  const std::size_t sz = min_handle_ + meshes_.size() + 1;
180  for (std::size_t i = min_handle_; i < sz; ++i)
181  if (meshes_.find(i) == meshes_.end())
182  {
183  next_handle_ = i;
184  break;
185  }
187  return ret;
188 }
189 
191 {
192  meshes_[handle] = GLMeshPtr(new GLMesh(*cmesh, handle));
193 }
194 
196 {
197  boost::mutex::scoped_lock _(meshes_mutex_);
198  FilterJob<bool>* remover = new FilterJob<bool>(boost::bind(&MeshFilterBase::removeMeshHelper, this, handle));
199  JobPtr job(remover);
200  addJob(job);
201  job->wait();
202 
203  if (!remover->getResult())
204  throw std::runtime_error("Could not remove mesh. Mesh not found!");
205  min_handle_ = std::min(handle, min_handle_);
206 }
207 
209 {
210  std::size_t erased = meshes_.erase(handle);
211  return (erased != 0);
212 }
213 
215 {
216  shadow_threshold_ = threshold;
217 }
218 
220 {
221  JobPtr job(
222  new FilterJob<void>(boost::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels)));
223  addJob(job);
224  job->wait();
225 }
226 
228 {
229  JobPtr job1(new FilterJob<void>(boost::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth)));
230  JobPtr job2(new FilterJob<void>(
232  {
233  boost::unique_lock<boost::mutex> lock(jobs_mutex_);
234  jobs_queue_.push(job1);
235  jobs_queue_.push(job2);
236  }
237  jobs_condition_.notify_one();
238  job1->wait();
239  job2->wait();
240 }
241 
243 {
244  JobPtr job1(new FilterJob<void>(boost::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth)));
245  JobPtr job2(new FilterJob<void>(
247  {
248  boost::unique_lock<boost::mutex> lock(jobs_mutex_);
249  jobs_queue_.push(job1);
250  jobs_queue_.push(job2);
251  }
252  jobs_condition_.notify_one();
253  job1->wait();
254  job2->wait();
255 }
256 
258 {
259  JobPtr job(
260  new FilterJob<void>(boost::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels)));
261  addJob(job);
262  job->wait();
263 }
264 
265 void mesh_filter::MeshFilterBase::run(const std::string& render_vertex_shader,
266  const std::string& render_fragment_shader,
267  const std::string& filter_vertex_shader,
268  const std::string& filter_fragment_shader)
269 {
270  initialize(render_vertex_shader, render_fragment_shader, filter_vertex_shader, filter_fragment_shader);
271 
272  while (!stop_)
273  {
274  boost::unique_lock<boost::mutex> lock(jobs_mutex_);
275  // check if we have new sensor data to be processed. If not, wait until we get notified.
276  if (jobs_queue_.empty())
277  jobs_condition_.wait(lock);
278 
279  if (!jobs_queue_.empty())
280  {
281  JobPtr job = jobs_queue_.front();
282  jobs_queue_.pop();
283  lock.unlock();
284  job->execute();
285  lock.lock();
286  }
287  }
288  deInitialize();
289 }
290 
291 void mesh_filter::MeshFilterBase::filter(const void* sensor_data, GLushort type, bool wait) const
292 {
293  if (type != GL_FLOAT && type != GL_UNSIGNED_SHORT)
294  {
295  std::stringstream msg;
296  msg << "unknown type \"" << type << "\". Allowed values are GL_FLOAT or GL_UNSIGNED_SHORT.";
297  throw std::runtime_error(msg.str());
298  }
299 
300  JobPtr job(new FilterJob<void>(boost::bind(&MeshFilterBase::doFilter, this, sensor_data, type)));
301  addJob(job);
302  if (wait)
303  job->wait();
304 }
305 
306 void mesh_filter::MeshFilterBase::doFilter(const void* sensor_data, const int encoding) const
307 {
308  boost::mutex::scoped_lock _(transform_callback_mutex_);
309 
310  mesh_renderer_->begin();
311  sensor_parameters_->setRenderParameters(*mesh_renderer_);
312 
313  glEnable(GL_TEXTURE_2D);
314  glEnable(GL_DEPTH_TEST);
315  glDepthFunc(GL_LESS);
316  glEnable(GL_CULL_FACE);
317  glCullFace(GL_FRONT);
318  glDisable(GL_ALPHA_TEST);
319  glDisable(GL_BLEND);
320 
321  GLuint padding_coefficients_id = glGetUniformLocation(mesh_renderer_->getProgramID(), "padding_coefficients");
322  Eigen::Vector3f padding_coefficients =
323  sensor_parameters_->getPaddingCoefficients() * padding_scale_ + Eigen::Vector3f(0, 0, padding_offset_);
324  glUniform3f(padding_coefficients_id, padding_coefficients[0], padding_coefficients[1], padding_coefficients[2]);
325 
326  Eigen::Affine3d transform;
327  for (std::map<MeshHandle, GLMeshPtr>::const_iterator meshIt = meshes_.begin(); meshIt != meshes_.end(); ++meshIt)
328  if (transform_callback_(meshIt->first, transform))
329  meshIt->second->render(transform);
330 
331  mesh_renderer_->end();
332 
333  // now filter the depth_map with the second rendering stage
334  // depth_filter_.setBufferSize (width, height);
335  // depth_filter_.setCameraParameters (fx, fy, cx, cy);
336  depth_filter_->begin();
337  sensor_parameters_->setFilterParameters(*depth_filter_);
338  glEnable(GL_TEXTURE_2D);
339  glEnable(GL_DEPTH_TEST);
340  glDepthFunc(GL_ALWAYS);
341  glDisable(GL_CULL_FACE);
342  glDisable(GL_ALPHA_TEST);
343  glDisable(GL_BLEND);
344 
345  // glUniform1f (near_location_, depth_filter_.getNearClippingDistance ());
346  // glUniform1f (far_location_, depth_filter_.getFarClippingDistance ());
348 
349  GLuint depth_texture = mesh_renderer_->getDepthTexture();
350  GLuint color_texture = mesh_renderer_->getColorTexture();
351 
352  // bind sensor depth
353  glActiveTexture(GL_TEXTURE0);
354  glBindTexture(GL_TEXTURE_2D, sensor_depth_texture_);
355 
356  float scale =
357  1.0 / (sensor_parameters_->getFarClippingPlaneDistance() - sensor_parameters_->getNearClippingPlaneDistance());
358 
359  if (encoding == GL_UNSIGNED_SHORT)
360  // unsigned shorts shorts will be mapped to the range 0-1 during transfer. Afterwards we can apply another scale +
361  // offset to
362  // map the values between near and far clipping plane to 0 - 1. -> scale = (65535 * depth - near ) / (far - near)
363  // we have: [0 - 65535] -> [0 - 1]
364  // we want: [near - far] -> [0 - 1]
365  glPixelTransferf(GL_DEPTH_SCALE, scale * 65.535);
366  else
367  glPixelTransferf(GL_DEPTH_SCALE, scale);
368  glPixelTransferf(GL_DEPTH_BIAS, -scale * sensor_parameters_->getNearClippingPlaneDistance());
369 
370  glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, sensor_parameters_->getWidth(), sensor_parameters_->getHeight(), 0,
371  GL_DEPTH_COMPONENT, encoding, sensor_data);
372  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
373  glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
374  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
375  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
376 
377  // bind depth map
378  glActiveTexture(GL_TEXTURE2);
379  glBindTexture(GL_TEXTURE_2D, depth_texture);
380 
381  // bind labels
382  glActiveTexture(GL_TEXTURE4);
383  glBindTexture(GL_TEXTURE_2D, color_texture);
384  glCallList(canvas_);
385  depth_filter_->end();
386 }
387 
389 {
390  padding_offset_ = offset;
391 }
392 
394 {
395  padding_scale_ = scale;
396 }
void addJob(const JobPtr &job) const
add a Job for the main thread that needs to be executed there
void getDepthBuffer(float *buffer) const
retrieves the depth buffer from OpenGL
bool removeMeshHelper(MeshHandle handle)
used within a Job to allow the main thread removing meshes
void setPaddingScale(float scale)
set the scale component of padding used to multiply with sensor-specific padding coefficients to get ...
float padding_offset_
padding offset
boost::mutex meshes_mutex_
mutex for synchronization of updating filtered meshes
boost::mutex jobs_mutex_
mutex required for synchronization of condition states
void setTransformCallback(const TransformCallback &transform_callback)
set the callback for retrieving transformations for each mesh.
GLRendererPtr mesh_renderer_
first pass renderer for rendering the mesh
void getFilteredDepth(float *depth) const
retrieves the filtered depth values
float padding_scale_
padding scale
GLuint sensor_depth_texture_
handle depth texture from sensor data
MeshHandle addMesh(const shapes::Mesh &mesh)
adds a mesh to the filter object.
GLMesh represents a mesh from geometric_shapes for rendering in GL frame buffers. ...
Definition: gl_mesh.h:60
GLuint canvas_
canvas element (screen-filling quad) for second pass
MeshFilterBase(const TransformCallback &transform_callback, const SensorModel::Parameters &sensor_parameters, const std::string &render_vertex_shader="", const std::string &render_fragment_shader="", const std::string &filter_vertex_shader="", const std::string &filter_fragment_shader="")
Constructor.
TransformCallback transform_callback_
callback function for retrieving the mesh transformations
SensorModel::ParametersPtr sensor_parameters_
the parameters of the used sensor model
Abstracts the OpenGL frame buffer objects, and provides an interface to render meshes, and retrieve the color and depth ap from opengl.
Definition: gl_renderer.h:58
MeshHandle min_handle_
Handle values below this are all taken (this variable is used for more efficient computation of next_...
void run(const std::string &render_vertex_shader, const std::string &render_fragment_shader, const std::string &filter_vertex_shader, const std::string &filter_fragment_shader)
filtering thread
void removeMesh(MeshHandle mesh_handle)
removes a mesh given by its handle
MeshHandle next_handle_
next handle to be used for next mesh that is added
float shadow_threshold_
threshold for shadowed pixels vs. filtered pixels
void initialize(const std::string &render_vertex_shader, const std::string &render_fragment_shader, const std::string &filter_vertex_shader, const std::string &filter_fragment_shader)
initializes OpenGL related things as well as renderers
void getFilteredLabels(LabelType *labels) const
retrieves the labels of the input data
Abstract Interface defining Sensor Parameters.
Definition: sensor_model.h:61
bool stop_
indicates whether the filtering loop should stop
boost::condition_variable jobs_condition_
condition variable to notify the filtering thread if a new image arrived
virtual void transformModelDepthToMetricDepth(float *depth) const
transforms depth values from rendered model to metric depth values
void getModelDepth(float *depth) const
retrieves the depth values of the rendered model
void wait() const
Definition: filter_job.h:70
boost::thread filter_thread_
the filtering thread that also holds the OpenGL context
void getModelLabels(LabelType *labels) const
retrieves the labels of the rendered model
void doFilter(const void *sensor_data, const int encoding) const
the filter method that does the magic
const ReturnType & getResult() const
Definition: filter_job.h:116
virtual void transformFilteredDepthToMetricDepth(float *depth) const
transforms depth values from filtered depth to metric depth values
std::map< MeshHandle, GLMeshPtr > meshes_
storage for meshed to be filtered
boost::mutex transform_callback_mutex_
mutex for synchronization of setting/calling transform_callback_
void setPaddingOffset(float offset)
set the offset component of padding. This value is added to the scaled sensor-specific constant compo...
void setSize(unsigned int width, unsigned int height)
sets the size of the fram buffers
void addMeshHelper(MeshHandle handle, const shapes::Mesh *cmesh)
used within a Job to allow the main thread adding meshes
GLuint shadow_threshold_location_
handle to GLSL location of shadow threshold
void setShadowThreshold(float threshold)
set the shadow threshold. points that are further away than the rendered model are filtered out...
void getColorBuffer(unsigned char *buffer) const
retrieves the color buffer from OpenGL
void filter(const void *sensor_data, GLushort type, bool wait=false) const
label/remove pixels from input depth-image
std::queue< JobPtr > jobs_queue_
OpenGL job queue that need to be processed by the worker thread.
boost::function< bool(MeshHandle, Eigen::Affine3d &)> TransformCallback
unsigned int MeshHandle
uint32_t LabelType
GLRendererPtr depth_filter_
second pass renderer for filtering the results of first pass


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23