moveit2
The MoveIt Motion Planning Framework for ROS 2.
Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
mesh_filter::MeshFilterBase Class Reference

#include <mesh_filter_base.h>

Inheritance diagram for mesh_filter::MeshFilterBase:
Inheritance graph
[legend]

Public Types

enum  {
  BACKGROUND = 0 , SHADOW = 1 , NEAR_CLIP = 2 , FAR_CLIP = 3 ,
  FIRST_LABEL = 16
}
 
typedef std::function< bool(MeshHandle, Eigen::Isometry3d &)> TransformCallback
 

Public Member Functions

 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. More...
 
 ~MeshFilterBase ()
 Destructor. More...
 
MeshHandle addMesh (const shapes::Mesh &mesh)
 adds a mesh to the filter object. More...
 
void removeMesh (MeshHandle mesh_handle)
 removes a mesh given by its handle More...
 
void filter (const void *sensor_data, GLushort type, bool wait=false) const
 label/remove pixels from input depth-image More...
 
void getFilteredLabels (LabelType *labels) const
 retrieves the labels of the input data More...
 
void getFilteredDepth (float *depth) const
 retrieves the filtered depth values More...
 
void getModelLabels (LabelType *labels) const
 retrieves the labels of the rendered model More...
 
void getModelDepth (float *depth) const
 retrieves the depth values of the rendered model More...
 
void setShadowThreshold (float threshold)
 set the shadow threshold. points that are further away than the rendered model are filtered out. Except they are further away than this threshold. Then these points are kept, but its label is set to 1 indicating that it is in the shadow of the model More...
 
void setTransformCallback (const TransformCallback &transform_callback)
 set the callback for retrieving transformations for each mesh. More...
 
void setPaddingScale (float scale)
 set the scale component of padding used to multiply with sensor-specific padding coefficients to get final coefficients. More...
 
void setPaddingOffset (float offset)
 set the offset component of padding. This value is added to the scaled sensor-specific constant component. More...
 

Protected Member Functions

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 More...
 
void deInitialize ()
 cleaning up More...
 
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 More...
 
void doFilter (const void *sensor_data, const int encoding) const
 the filter method that does the magic More...
 
void addMeshHelper (MeshHandle handle, const shapes::Mesh &cmesh)
 used within a Job to allow the main thread adding meshes More...
 
bool removeMeshHelper (MeshHandle handle)
 used within a Job to allow the main thread removing meshes More...
 
void addJob (const JobPtr &job) const
 add a Job for the main thread that needs to be executed there More...
 
void setSize (unsigned int width, unsigned int height)
 sets the size of the fram buffers More...
 

Protected Attributes

std::map< MeshHandle, GLMeshPtr > meshes_
 storage for meshed to be filtered More...
 
SensorModel::ParametersPtr sensor_parameters_
 the parameters of the used sensor model More...
 
MeshHandle next_handle_
 next handle to be used for next mesh that is added More...
 
MeshHandle min_handle_
 Handle values below this are all taken (this variable is used for more efficient computation of next_label_) More...
 
std::thread filter_thread_
 the filtering thread that also holds the OpenGL context More...
 
std::condition_variable jobs_condition_
 condition variable to notify the filtering thread if a new image arrived More...
 
std::mutex jobs_mutex_
 mutex required for synchronization of condition states More...
 
std::queue< JobPtr > jobs_queue_
 OpenGL job queue that need to be processed by the worker thread. More...
 
std::mutex meshes_mutex_
 mutex for synchronization of updating filtered meshes More...
 
std::mutex transform_callback_mutex_
 mutex for synchronization of setting/calling transform_callback_ More...
 
bool stop_
 indicates whether the filtering loop should stop More...
 
GLRendererPtr mesh_renderer_
 first pass renderer for rendering the mesh More...
 
GLRendererPtr depth_filter_
 second pass renderer for filtering the results of first pass More...
 
GLuint canvas_
 canvas element (screen-filling quad) for second pass More...
 
GLuint sensor_depth_texture_
 handle depth texture from sensor data More...
 
GLuint shadow_threshold_location_
 handle to GLSL location of shadow threshold More...
 
TransformCallback transform_callback_
 callback function for retrieving the mesh transformations More...
 
float padding_scale_
 padding scale More...
 
float padding_offset_
 padding offset More...
 
float shadow_threshold_
 threshold for shadowed pixels vs. filtered pixels More...
 

Detailed Description

Definition at line 63 of file mesh_filter_base.h.

Member Typedef Documentation

◆ TransformCallback

typedef std::function<bool(MeshHandle, Eigen::Isometry3d&)> mesh_filter::MeshFilterBase::TransformCallback

Definition at line 67 of file mesh_filter_base.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
Enumerator
BACKGROUND 
SHADOW 
NEAR_CLIP 
FAR_CLIP 
FIRST_LABEL 

Definition at line 71 of file mesh_filter_base.h.

Constructor & Destructor Documentation

◆ MeshFilterBase()

mesh_filter::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.

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[in]transform_callbackCallback function that is called for each mesh to obtain the current transformation.
Note
the callback expects the mesh handle but no time stamp. Its the users responsibility to return the correct transformation.

Definition at line 54 of file mesh_filter_base.cpp.

Here is the call graph for this function:

◆ ~MeshFilterBase()

mesh_filter::MeshFilterBase::~MeshFilterBase ( )

Destructor.

Definition at line 123 of file mesh_filter_base.cpp.

Member Function Documentation

◆ addJob()

void mesh_filter::MeshFilterBase::addJob ( const JobPtr &  job) const
protected

add a Job for the main thread that needs to be executed there

Parameters
[in]jobthe job object that has the function o be executed

Definition at line 138 of file mesh_filter_base.cpp.

◆ addMesh()

mesh_filter::MeshHandle mesh_filter::MeshFilterBase::addMesh ( const shapes::Mesh &  mesh)

adds a mesh to the filter object.

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[in]meshthe mesh to be added
Returns
handle to the mesh. This handle is used in the transform callback function to identify the mesh and retrieve the correct transformation.

Definition at line 172 of file mesh_filter_base.cpp.

◆ addMeshHelper()

void mesh_filter::MeshFilterBase::addMeshHelper ( MeshHandle  handle,
const shapes::Mesh &  cmesh 
)
protected

used within a Job to allow the main thread adding meshes

Parameters
[in]handlethe handle of the mesh that is predetermined and passed
[in]cmeshthe mesh to be added to the corresponding handle

Definition at line 191 of file mesh_filter_base.cpp.

◆ deInitialize()

void mesh_filter::MeshFilterBase::deInitialize ( )
protected

cleaning up

Definition at line 147 of file mesh_filter_base.cpp.

◆ doFilter()

void mesh_filter::MeshFilterBase::doFilter ( const void *  sensor_data,
const int  encoding 
) const
protected

the filter method that does the magic

Parameters
[in]sensor_datapointer to the buffer containing the depth readings
[in]encodingthe representation of the depth readings in the buffer

Definition at line 308 of file mesh_filter_base.cpp.

◆ filter()

void mesh_filter::MeshFilterBase::filter ( const void *  sensor_data,
GLushort  type,
bool  wait = false 
) const

label/remove pixels from input depth-image

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[in]sensor_datapointer to the input depth image from sensor readings.
Todo:
what is type?

Definition at line 293 of file mesh_filter_base.cpp.

◆ getFilteredDepth()

void mesh_filter::MeshFilterBase::getFilteredDepth ( float *  depth) const

retrieves the filtered depth values

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[out]depthpointer to buffer to be filled with depth values.

Definition at line 244 of file mesh_filter_base.cpp.

◆ getFilteredLabels()

void mesh_filter::MeshFilterBase::getFilteredLabels ( LabelType labels) const

retrieves the labels of the input data

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[out]labelspointer to buffer to be filled with labels
Note
labels are corresponding 1-1 to the mesh handles. 0 and 1 are reserved indicating either background (0) or shadow (1) The upper 8bit of a label is filled with the user given flag (see addMesh)

Definition at line 259 of file mesh_filter_base.cpp.

◆ getModelDepth()

void mesh_filter::MeshFilterBase::getModelDepth ( float *  depth) const

retrieves the depth values of the rendered model

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[out]depthpointer to buffer to be filled with depth values.

Definition at line 228 of file mesh_filter_base.cpp.

◆ getModelLabels()

void mesh_filter::MeshFilterBase::getModelLabels ( LabelType labels) const

retrieves the labels of the rendered model

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[out]labelspointer to buffer to be filled with labels
Note
labels are corresponding 1-1 to the mesh handles. 0 and 1 are reserved indicating either background (0) or shadow (1) The upper 8bit of a label is filled with the user given flag (see addMesh)
Todo:
How is this data different from the filtered labels?

Definition at line 220 of file mesh_filter_base.cpp.

◆ initialize()

void mesh_filter::MeshFilterBase::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 
)
protected

initializes OpenGL related things as well as renderers

Definition at line 75 of file mesh_filter_base.cpp.

◆ removeMesh()

void mesh_filter::MeshFilterBase::removeMesh ( MeshHandle  mesh_handle)

removes a mesh given by its handle

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[in]mesh_handlethe handle of the mesh to be removed.

Definition at line 196 of file mesh_filter_base.cpp.

Here is the call graph for this function:

◆ removeMeshHelper()

bool mesh_filter::MeshFilterBase::removeMeshHelper ( MeshHandle  handle)
protected

used within a Job to allow the main thread removing meshes

Parameters
[in]handlethe handle of the mesh to be removed

Definition at line 209 of file mesh_filter_base.cpp.

◆ run()

void mesh_filter::MeshFilterBase::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 
)
protected

filtering thread

Definition at line 267 of file mesh_filter_base.cpp.

Here is the caller graph for this function:

◆ setPaddingOffset()

void mesh_filter::MeshFilterBase::setPaddingOffset ( float  offset)

set the offset component of padding. This value is added to the scaled sensor-specific constant component.

Parameters
[in]offsetthe offset value

Definition at line 390 of file mesh_filter_base.cpp.

◆ setPaddingScale()

void mesh_filter::MeshFilterBase::setPaddingScale ( float  scale)

set the scale component of padding used to multiply with sensor-specific padding coefficients to get final coefficients.

Parameters
[in]scalethe scale value

Definition at line 395 of file mesh_filter_base.cpp.

◆ setShadowThreshold()

void mesh_filter::MeshFilterBase::setShadowThreshold ( float  threshold)

set the shadow threshold. points that are further away than the rendered model are filtered out. Except they are further away than this threshold. Then these points are kept, but its label is set to 1 indicating that it is in the shadow of the model

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[in]thresholdshadow threshold in meters

Definition at line 215 of file mesh_filter_base.cpp.

◆ setSize()

void mesh_filter::MeshFilterBase::setSize ( unsigned int  width,
unsigned int  height 
)
protected

sets the size of the fram buffers

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[in]widthwidth of frame buffers in pixels
[in]heightheight of frame buffers in pixels

Definition at line 157 of file mesh_filter_base.cpp.

◆ setTransformCallback()

void mesh_filter::MeshFilterBase::setTransformCallback ( const TransformCallback transform_callback)

set the callback for retrieving transformations for each mesh.

Author
Suat Gedikli (gedik.nosp@m.li@w.nosp@m.illow.nosp@m.gara.nosp@m.ge.co.nosp@m.m)
Parameters
[in]transform_callbackthe callback

Definition at line 166 of file mesh_filter_base.cpp.

Member Data Documentation

◆ canvas_

GLuint mesh_filter::MeshFilterBase::canvas_
protected

canvas element (screen-filling quad) for second pass

Definition at line 276 of file mesh_filter_base.h.

◆ depth_filter_

GLRendererPtr mesh_filter::MeshFilterBase::depth_filter_
protected

second pass renderer for filtering the results of first pass

Definition at line 273 of file mesh_filter_base.h.

◆ filter_thread_

std::thread mesh_filter::MeshFilterBase::filter_thread_
protected

the filtering thread that also holds the OpenGL context

Definition at line 249 of file mesh_filter_base.h.

◆ jobs_condition_

std::condition_variable mesh_filter::MeshFilterBase::jobs_condition_
mutableprotected

condition variable to notify the filtering thread if a new image arrived

Definition at line 252 of file mesh_filter_base.h.

◆ jobs_mutex_

std::mutex mesh_filter::MeshFilterBase::jobs_mutex_
mutableprotected

mutex required for synchronization of condition states

Definition at line 255 of file mesh_filter_base.h.

◆ jobs_queue_

std::queue<JobPtr> mesh_filter::MeshFilterBase::jobs_queue_
mutableprotected

OpenGL job queue that need to be processed by the worker thread.

Definition at line 258 of file mesh_filter_base.h.

◆ mesh_renderer_

GLRendererPtr mesh_filter::MeshFilterBase::mesh_renderer_
protected

first pass renderer for rendering the mesh

Definition at line 270 of file mesh_filter_base.h.

◆ meshes_

std::map<MeshHandle, GLMeshPtr> mesh_filter::MeshFilterBase::meshes_
protected

storage for meshed to be filtered

Definition at line 236 of file mesh_filter_base.h.

◆ meshes_mutex_

std::mutex mesh_filter::MeshFilterBase::meshes_mutex_
mutableprotected

mutex for synchronization of updating filtered meshes

Definition at line 261 of file mesh_filter_base.h.

◆ min_handle_

MeshHandle mesh_filter::MeshFilterBase::min_handle_
protected

Handle values below this are all taken (this variable is used for more efficient computation of next_label_)

Definition at line 246 of file mesh_filter_base.h.

◆ next_handle_

MeshHandle mesh_filter::MeshFilterBase::next_handle_
protected

next handle to be used for next mesh that is added

Definition at line 242 of file mesh_filter_base.h.

◆ padding_offset_

float mesh_filter::MeshFilterBase::padding_offset_
protected

padding offset

Definition at line 291 of file mesh_filter_base.h.

◆ padding_scale_

float mesh_filter::MeshFilterBase::padding_scale_
protected

padding scale

Definition at line 288 of file mesh_filter_base.h.

◆ sensor_depth_texture_

GLuint mesh_filter::MeshFilterBase::sensor_depth_texture_
protected

handle depth texture from sensor data

Definition at line 279 of file mesh_filter_base.h.

◆ sensor_parameters_

SensorModel::ParametersPtr mesh_filter::MeshFilterBase::sensor_parameters_
protected

the parameters of the used sensor model

Definition at line 239 of file mesh_filter_base.h.

◆ shadow_threshold_

float mesh_filter::MeshFilterBase::shadow_threshold_
protected

threshold for shadowed pixels vs. filtered pixels

Definition at line 294 of file mesh_filter_base.h.

◆ shadow_threshold_location_

GLuint mesh_filter::MeshFilterBase::shadow_threshold_location_
protected

handle to GLSL location of shadow threshold

Definition at line 282 of file mesh_filter_base.h.

◆ stop_

bool mesh_filter::MeshFilterBase::stop_
protected

indicates whether the filtering loop should stop

Definition at line 267 of file mesh_filter_base.h.

◆ transform_callback_

TransformCallback mesh_filter::MeshFilterBase::transform_callback_
protected

callback function for retrieving the mesh transformations

Definition at line 285 of file mesh_filter_base.h.

◆ transform_callback_mutex_

std::mutex mesh_filter::MeshFilterBase::transform_callback_mutex_
mutableprotected

mutex for synchronization of setting/calling transform_callback_

Definition at line 264 of file mesh_filter_base.h.


The documentation for this class was generated from the following files: