mesh_filter_base.h
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 
37 #pragma once
38 
39 #include <map>
43 #include <Eigen/Geometry> // for Isometry3d
44 #include <queue>
45 #include <thread>
46 #include <condition_variable>
47 #include <mutex>
48 
49 // forward declarations
50 namespace shapes
51 {
52 class Mesh;
53 }
54 
55 namespace mesh_filter
56 {
57 MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc
58 MOVEIT_CLASS_FORWARD(GLMesh); // Defines GLMeshPtr, ConstPtr, WeakPtr... etc
59 
60 typedef unsigned int MeshHandle;
61 typedef uint32_t LabelType;
62 
64 {
65  // inner types and typedefs
66 public:
67  typedef std::function<bool(MeshHandle, Eigen::Isometry3d&)> TransformCallback;
68  // \todo @suat: to avoid a few comparisons, it would be much nicer if background = 14 and shadow = 15 (near/far clip
69  // can be anything below that)
70  // this would allow me to do a single comparison instead of 3, in the code i write
71  enum
72  {
74  SHADOW = 1,
75  NEAR_CLIP = 2,
76  FAR_CLIP = 3,
78  };
79 
80 public:
88  MeshFilterBase(const TransformCallback& transform_callback, const SensorModel::Parameters& sensor_parameters,
89  const std::string& render_vertex_shader = "", const std::string& render_fragment_shader = "",
90  const std::string& filter_vertex_shader = "", const std::string& filter_fragment_shader = "");
91 
94 
102  MeshHandle addMesh(const shapes::Mesh& mesh);
103 
109  void removeMesh(MeshHandle mesh_handle);
110 
117  void filter(const void* sensor_data, GLushort type, bool wait = false) const;
118 
127  void getFilteredLabels(LabelType* labels) const;
128 
134  void getFilteredDepth(float* depth) const;
135 
145  void getModelLabels(LabelType* labels) const;
146 
152  void getModelDepth(float* depth) const;
153 
161  void setShadowThreshold(float threshold);
162 
168  void setTransformCallback(const TransformCallback& transform_callback);
169 
175  void setPaddingScale(float scale);
176 
181  void setPaddingOffset(float offset);
182 
183 protected:
187  void initialize(const std::string& render_vertex_shader, const std::string& render_fragment_shader,
188  const std::string& filter_vertex_shader, const std::string& filter_fragment_shader);
189 
193  void deInitialize();
194 
198  void run(const std::string& render_vertex_shader, const std::string& render_fragment_shader,
199  const std::string& filter_vertex_shader, const std::string& filter_fragment_shader);
200 
206  void doFilter(const void* sensor_data, const int encoding) const;
207 
213  void addMeshHelper(MeshHandle handle, const shapes::Mesh& cmesh);
214 
219  bool removeMeshHelper(MeshHandle handle);
220 
225  void addJob(const JobPtr& job) const;
226 
233  void setSize(unsigned int width, unsigned int height);
234 
236  std::map<MeshHandle, GLMeshPtr> meshes_;
237 
239  SensorModel::ParametersPtr sensor_parameters_;
240 
243 
247 
249  std::thread filter_thread_;
250 
252  mutable std::condition_variable jobs_condition_;
253 
255  mutable std::mutex jobs_mutex_;
256 
258  mutable std::queue<JobPtr> jobs_queue_;
259 
261  mutable std::mutex meshes_mutex_;
262 
264  mutable std::mutex transform_callback_mutex_;
265 
267  bool stop_;
268 
270  GLRendererPtr mesh_renderer_;
271 
273  GLRendererPtr depth_filter_;
274 
276  GLuint canvas_;
277 
280 
283 
286 
289 
292 
295 };
296 } // namespace mesh_filter
mesh_filter::MeshFilterBase::setPaddingScale
void setPaddingScale(float scale)
set the scale component of padding used to multiply with sensor-specific padding coefficients to get ...
Definition: mesh_filter_base.cpp:395
mesh_filter::MeshFilterBase::sensor_parameters_
SensorModel::ParametersPtr sensor_parameters_
the parameters of the used sensor model
Definition: mesh_filter_base.h:239
shapes
mesh_filter::MeshFilterBase::jobs_mutex_
std::mutex jobs_mutex_
mutex required for synchronization of condition states
Definition: mesh_filter_base.h:255
mesh_filter::MeshFilterBase::shadow_threshold_
float shadow_threshold_
threshold for shadowed pixels vs. filtered pixels
Definition: mesh_filter_base.h:294
sensor_model.h
mesh_filter::MeshFilterBase::~MeshFilterBase
~MeshFilterBase()
Desctructor.
Definition: mesh_filter_base.cpp:125
mesh_filter::MeshFilterBase::SHADOW
@ SHADOW
Definition: mesh_filter_base.h:74
mesh_filter::MeshFilterBase
Definition: mesh_filter_base.h:63
mesh_filter::MeshFilterBase::meshes_mutex_
std::mutex meshes_mutex_
mutex for synchronization of updating filtered meshes
Definition: mesh_filter_base.h:261
mesh_filter::MeshFilterBase::getModelDepth
void getModelDepth(float *depth) const
retrieves the depth values of the rendered model
Definition: mesh_filter_base.cpp:230
mesh_filter::MeshFilterBase::sensor_depth_texture_
GLuint sensor_depth_texture_
handle depth texture from sensor data
Definition: mesh_filter_base.h:279
mesh_filter::MeshFilterBase::next_handle_
MeshHandle next_handle_
next handle to be used for next mesh that is added
Definition: mesh_filter_base.h:242
mesh_filter::MeshFilterBase::deInitialize
void deInitialize()
cleaning up
Definition: mesh_filter_base.cpp:149
shapes::Mesh
mesh_filter::MeshFilterBase::getFilteredLabels
void getFilteredLabels(LabelType *labels) const
retrieves the labels of the input data
Definition: mesh_filter_base.cpp:260
mesh_filter::MeshFilterBase::min_handle_
MeshHandle min_handle_
Handle values below this are all taken (this variable is used for more efficient computation of next_...
Definition: mesh_filter_base.h:246
mesh_filter::MeshFilterBase::setTransformCallback
void setTransformCallback(const TransformCallback &transform_callback)
set the callback for retrieving transformations for each mesh.
Definition: mesh_filter_base.cpp:168
mesh_filter::MeshFilterBase::getFilteredDepth
void getFilteredDepth(float *depth) const
retrieves the filtered depth values
Definition: mesh_filter_base.cpp:245
mesh_filter::MeshFilterBase::addMeshHelper
void addMeshHelper(MeshHandle handle, const shapes::Mesh &cmesh)
used within a Job to allow the main thread adding meshes
Definition: mesh_filter_base.cpp:193
mesh_filter::MeshFilterBase::getModelLabels
void getModelLabels(LabelType *labels) const
retrieves the labels of the rendered model
Definition: mesh_filter_base.cpp:222
mesh_filter::MeshFilterBase::run
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
Definition: mesh_filter_base.cpp:267
mesh_filter::MeshFilterBase::doFilter
void doFilter(const void *sensor_data, const int encoding) const
the filter method that does the magic
Definition: mesh_filter_base.cpp:308
mesh_filter::LabelType
uint32_t LabelType
Definition: mesh_filter_base.h:61
mesh_filter::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(Job)
mesh_filter::MeshFilterBase::addJob
void addJob(const JobPtr &job) const
add a Job for the main thread that needs to be executed there
Definition: mesh_filter_base.cpp:140
mesh_filter::MeshFilterBase::jobs_queue_
std::queue< JobPtr > jobs_queue_
OpenGL job queue that need to be processed by the worker thread.
Definition: mesh_filter_base.h:258
mesh_filter::MeshFilterBase::initialize
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
Definition: mesh_filter_base.cpp:77
mesh_filter::SensorModel::Parameters
Abstract Interface defining Sensor Parameters.
Definition: sensor_model.h:92
mesh_filter::MeshFilterBase::NEAR_CLIP
@ NEAR_CLIP
Definition: mesh_filter_base.h:75
mesh_filter::MeshFilterBase::filter
void filter(const void *sensor_data, GLushort type, bool wait=false) const
label/remove pixels from input depth-image
Definition: mesh_filter_base.cpp:293
mesh_filter::MeshFilterBase::canvas_
GLuint canvas_
canvas element (screen-filling quad) for second pass
Definition: mesh_filter_base.h:276
mesh_filter::MeshFilterBase::transform_callback_mutex_
std::mutex transform_callback_mutex_
mutex for synchronization of setting/calling transform_callback_
Definition: mesh_filter_base.h:264
mesh_filter::MeshFilterBase::setShadowThreshold
void setShadowThreshold(float threshold)
set the shadow threshold. points that are further away than the rendered model are filtered out....
Definition: mesh_filter_base.cpp:217
mesh_filter::MeshFilterBase::padding_offset_
float padding_offset_
padding offset
Definition: mesh_filter_base.h:291
gl_renderer.h
mesh_filter::MeshFilterBase::mesh_renderer_
GLRendererPtr mesh_renderer_
first pass renderer for rendering the mesh
Definition: mesh_filter_base.h:270
mesh_filter::MeshFilterBase::setPaddingOffset
void setPaddingOffset(float offset)
set the offset component of padding. This value is added to the scaled sensor-specific constant compo...
Definition: mesh_filter_base.cpp:390
mesh_filter::MeshFilterBase::depth_filter_
GLRendererPtr depth_filter_
second pass renderer for filtering the results of first pass
Definition: mesh_filter_base.h:273
mesh_filter::MeshFilterBase::MeshFilterBase
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.
Definition: mesh_filter_base.cpp:56
mesh_filter::MeshFilterBase::TransformCallback
std::function< bool(MeshHandle, Eigen::Isometry3d &)> TransformCallback
Definition: mesh_filter_base.h:67
mesh_filter::MeshHandle
unsigned int MeshHandle
Definition: mesh_filter_base.h:60
mesh_filter::MeshFilterBase::jobs_condition_
std::condition_variable jobs_condition_
condition variable to notify the filtering thread if a new image arrived
Definition: mesh_filter_base.h:252
mesh_filter::MeshFilterBase::filter_thread_
std::thread filter_thread_
the filtering thread that also holds the OpenGL context
Definition: mesh_filter_base.h:249
class_forward.h
mesh_filter::MeshFilterBase::padding_scale_
float padding_scale_
padding scale
Definition: mesh_filter_base.h:288
mesh_filter::MeshFilterBase::FAR_CLIP
@ FAR_CLIP
Definition: mesh_filter_base.h:76
mesh_filter::MeshFilterBase::shadow_threshold_location_
GLuint shadow_threshold_location_
handle to GLSL location of shadow threshold
Definition: mesh_filter_base.h:282
mesh_filter::MeshFilterBase::meshes_
std::map< MeshHandle, GLMeshPtr > meshes_
storage for meshed to be filtered
Definition: mesh_filter_base.h:236
mesh_filter::MeshFilterBase::BACKGROUND
@ BACKGROUND
Definition: mesh_filter_base.h:73
mesh_filter::MeshFilterBase::removeMeshHelper
bool removeMeshHelper(MeshHandle handle)
used within a Job to allow the main thread removing meshes
Definition: mesh_filter_base.cpp:211
mesh_filter::MeshFilterBase::stop_
bool stop_
indicates whether the filtering loop should stop
Definition: mesh_filter_base.h:267
mesh_filter::MeshFilterBase::addMesh
MeshHandle addMesh(const shapes::Mesh &mesh)
adds a mesh to the filter object.
Definition: mesh_filter_base.cpp:174
mesh_filter::MeshFilterBase::FIRST_LABEL
@ FIRST_LABEL
Definition: mesh_filter_base.h:77
mesh_filter::MeshFilterBase::removeMesh
void removeMesh(MeshHandle mesh_handle)
removes a mesh given by its handle
Definition: mesh_filter_base.cpp:198
mesh_filter::MeshFilterBase::setSize
void setSize(unsigned int width, unsigned int height)
sets the size of the fram buffers
Definition: mesh_filter_base.cpp:159
mesh_filter::MeshFilterBase::transform_callback_
TransformCallback transform_callback_
callback function for retrieving the mesh transformations
Definition: mesh_filter_base.h:285
mesh_filter
Definition: depth_self_filter_nodelet.h:47


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon May 27 2024 02:27:57