mesh_filter_base.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Suat Gedikli */
 
#pragma once
 
#include <cstdint>
#include <map>
#include <moveit/macros/class_forward.hpp>
#include <moveit/mesh_filter/gl_renderer.hpp>
#include <moveit/mesh_filter/sensor_model.hpp>
#include <Eigen/Geometry>  // for Isometry3d
#include <queue>
#include <thread>
#include <condition_variable>
#include <mutex>
 
// forward declarations
namespace shapes
{
class Mesh;
}
 
namespace mesh_filter
{
MOVEIT_CLASS_FORWARD(Job);     // Defines JobPtr, ConstPtr, WeakPtr... etc
MOVEIT_CLASS_FORWARD(GLMesh);  // Defines GLMeshPtr, ConstPtr, WeakPtr... etc
 
typedef unsigned int MeshHandle;
typedef uint32_t LabelType;
 
class MeshFilterBase
{
  // inner types and typedefs
public:
  typedef std::function<bool(MeshHandle, Eigen::Isometry3d&)> TransformCallback;
  // \todo @suat: to avoid a few comparisons, it would be much nicer if background = 14 and shadow = 15 (near/far clip
  // can be anything below that)
  // this would allow me to do a single comparison instead of 3, in the code i write
  enum
  {
    BACKGROUND = 0,
    SHADOW = 1,
    NEAR_CLIP = 2,
    FAR_CLIP = 3,
    FIRST_LABEL = 16
  };
 
public:
  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 = "");

  ~MeshFilterBase();

  MeshHandle addMesh(const shapes::Mesh& mesh);

  void removeMesh(MeshHandle mesh_handle);

  void filter(const void* sensor_data, GLushort type, bool wait = false) const;

  void getFilteredLabels(LabelType* labels) const;

  void getFilteredDepth(float* depth) const;

  void getModelLabels(LabelType* labels) const;

  void getModelDepth(float* depth) const;

  void setShadowThreshold(float threshold);

  void setTransformCallback(const TransformCallback& transform_callback);

  void setPaddingScale(float scale);

  void setPaddingOffset(float offset);
 
protected:
  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);

  void deInitialize();

  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);

  void doFilter(const void* sensor_data, const int encoding) const;

  void addMeshHelper(MeshHandle handle, const shapes::Mesh& cmesh);

  bool removeMeshHelper(MeshHandle handle);

  void addJob(const JobPtr& job) const;

  void setSize(unsigned int width, unsigned int height);

  std::map<MeshHandle, GLMeshPtr> meshes_;

  SensorModel::ParametersPtr sensor_parameters_;

  MeshHandle next_handle_;

  MeshHandle min_handle_;

  std::thread filter_thread_;

  mutable std::condition_variable jobs_condition_;

  mutable std::mutex jobs_mutex_;

  mutable std::queue<JobPtr> jobs_queue_;

  mutable std::mutex meshes_mutex_;

  mutable std::mutex transform_callback_mutex_;

  bool stop_;

  GLRendererPtr mesh_renderer_;

  GLRendererPtr depth_filter_;

  GLuint canvas_;

  GLuint sensor_depth_texture_;

  GLuint shadow_threshold_location_;

  TransformCallback transform_callback_;

  float padding_scale_;

  float padding_offset_;

  float shadow_threshold_;
};
}  // namespace mesh_filter