moveit2
The MoveIt Motion Planning Framework for ROS 2.
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,
77  FIRST_LABEL = 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 
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(double* depth) const;
135 
145  void getModelLabels(LabelType* labels) const;
146 
152  void getModelDepth(double* 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
GLMesh represents a mesh from geometric_shapes for rendering in GL frame buffers.
Definition: gl_mesh.h:59
void setTransformCallback(const TransformCallback &transform_callback)
set the callback for retrieving transformations for each mesh.
std::mutex transform_callback_mutex_
mutex for synchronization of setting/calling transform_callback_
void doFilter(const void *sensor_data, const int encoding) const
the filter method that does the magic
void getModelLabels(LabelType *labels) const
retrieves the labels of the rendered model
std::map< MeshHandle, GLMeshPtr > meshes_
storage for meshed to be filtered
void filter(const void *sensor_data, GLushort type, bool wait=false) const
label/remove pixels from input depth-image
SensorModel::ParametersPtr sensor_parameters_
the parameters of the used sensor model
GLuint sensor_depth_texture_
handle depth texture from sensor data
GLuint shadow_threshold_location_
handle to GLSL location of shadow threshold
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.
void getFilteredDepth(double *depth) const
retrieves the filtered depth values
bool removeMeshHelper(MeshHandle handle)
used within a Job to allow the main thread removing meshes
std::queue< JobPtr > jobs_queue_
OpenGL job queue that need to be processed by the worker thread.
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
bool stop_
indicates whether the filtering loop should stop
void addMeshHelper(MeshHandle handle, const shapes::Mesh &cmesh)
used within a Job to allow the main thread adding meshes
float padding_offset_
padding offset
GLRendererPtr depth_filter_
second pass renderer for filtering the results of first pass
void removeMesh(MeshHandle mesh_handle)
removes a mesh given by its handle
GLRendererPtr mesh_renderer_
first pass renderer for rendering the mesh
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 addJob(const JobPtr &job) const
add a Job for the main thread that needs to be executed there
MeshHandle next_handle_
next handle to be used for next mesh that is added
void setShadowThreshold(float threshold)
set the shadow threshold. points that are further away than the rendered model are filtered out....
float padding_scale_
padding scale
void getModelDepth(double *depth) const
retrieves the depth values of the rendered model
MeshHandle min_handle_
Handle values below this are all taken (this variable is used for more efficient computation of next_...
MeshHandle addMesh(const shapes::Mesh &mesh)
adds a mesh to the filter object.
TransformCallback transform_callback_
callback function for retrieving the mesh transformations
std::function< bool(MeshHandle, Eigen::Isometry3d &)> TransformCallback
void setPaddingOffset(float offset)
set the offset component of padding. This value is added to the scaled sensor-specific constant compo...
void getFilteredLabels(LabelType *labels) const
retrieves the labels of the input data
std::thread filter_thread_
the filtering thread that also holds the OpenGL context
void setPaddingScale(float scale)
set the scale component of padding used to multiply with sensor-specific padding coefficients to get ...
void setSize(unsigned int width, unsigned int height)
sets the size of the frame buffers
std::mutex meshes_mutex_
mutex for synchronization of updating filtered meshes
std::condition_variable jobs_condition_
condition variable to notify the filtering thread if a new image arrived
std::mutex jobs_mutex_
mutex required for synchronization of condition states
float shadow_threshold_
threshold for shadowed pixels vs. filtered pixels
GLuint canvas_
canvas element (screen-filling quad) for second pass
Abstract Interface defining Sensor Parameters.
Definition: sensor_model.h:61
unsigned int MeshHandle
MOVEIT_CLASS_FORWARD(Job)
uint32_t LabelType
Definition: world.h:49