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 #ifndef MOVEIT_MESH_FILTER_MESH_FILTER_BASE_
38 #define MOVEIT_MESH_FILTER_MESH_FILTER_BASE_
39 
40 #include <map>
44 #include <boost/function.hpp>
45 #include <boost/thread/mutex.hpp>
46 #include <Eigen/Eigen>
47 #include <queue>
48 
49 // forward declarations
50 namespace shapes
51 {
52 class Mesh;
53 }
54 
55 namespace mesh_filter
56 {
58 MOVEIT_CLASS_FORWARD(GLMesh);
59 
60 typedef unsigned int MeshHandle;
61 typedef uint32_t LabelType;
62 
64 {
65  // inner types and typedefs
66 public:
67  typedef boost::function<bool(MeshHandle, Eigen::Affine3d&)> 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  {
73  Background = 0,
74  Shadow = 1,
75  NearClip = 2,
76  FarClip = 3,
77  FirstLabel = 16
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 
93  ~MeshFilterBase();
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 
242  MeshHandle next_handle_;
243 
246  MeshHandle min_handle_;
247 
249  boost::thread filter_thread_;
250 
252  mutable boost::condition_variable jobs_condition_;
253 
255  mutable boost::mutex jobs_mutex_;
256 
258  mutable std::queue<JobPtr> jobs_queue_;
259 
261  mutable boost::mutex meshes_mutex_;
262 
264  mutable boost::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 
285  TransformCallback transform_callback_;
286 
289 
292 
295 };
296 } // namespace mesh_filter
297 
298 #endif /* __MESH_FILTER_MESH_FILTER_BASE_H__ */
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
ROSCONSOLE_DECL void initialize()
GLRendererPtr mesh_renderer_
first pass renderer for rendering the mesh
float padding_scale_
padding scale
GLuint sensor_depth_texture_
handle depth texture from sensor data
GLuint canvas_
canvas element (screen-filling quad) for second pass
TransformCallback transform_callback_
callback function for retrieving the mesh transformations
SensorModel::ParametersPtr sensor_parameters_
the parameters of the used sensor model
MeshHandle min_handle_
Handle values below this are all taken (this variable is used for more efficient computation of next_...
MOVEIT_CLASS_FORWARD(Shape)
MeshHandle next_handle_
next handle to be used for next mesh that is added
float shadow_threshold_
threshold for shadowed pixels vs. filtered pixels
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
boost::thread filter_thread_
the filtering thread that also holds the OpenGL context
std::map< MeshHandle, GLMeshPtr > meshes_
storage for meshed to be filtered
boost::mutex transform_callback_mutex_
mutex for synchronization of setting/calling transform_callback_
GLuint shadow_threshold_location_
handle to GLSL location of shadow threshold
void run(ClassLoader *loader)
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