mesh_filter_base.h

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 */

#ifndef MOVEIT_MESH_FILTER_MESH_FILTER_BASE_
#define MOVEIT_MESH_FILTER_MESH_FILTER_BASE_

#include <map>
#include <moveit/macros/class_forward.h>
#include <moveit/mesh_filter/gl_renderer.h>
#include <moveit/mesh_filter/sensor_model.h>
#include <boost/function.hpp>
#include <boost/thread/mutex.hpp>
#include <Eigen/Eigen>
#include <queue>

// forward declarations
namespace shapes
{
class Mesh;
}

namespace mesh_filter
{
MOVEIT_CLASS_FORWARD(Job);
MOVEIT_CLASS_FORWARD(GLMesh);

typedef unsigned int MeshHandle;
typedef uint32_t LabelType;

class MeshFilterBase
{
  // inner types and typedefs
public:
  typedef boost::function<bool(MeshHandle, Eigen::Affine3d&)> 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,
    NearClip = 2,
    FarClip = 3,
    FirstLabel = 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_;

  boost::thread filter_thread_;

  mutable boost::condition_variable jobs_condition_;

  mutable boost::mutex jobs_mutex_;

  mutable std::queue<JobPtr> jobs_queue_;

  mutable boost::mutex meshes_mutex_;

  mutable boost::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

#endif /* __MESH_FILTER_MESH_FILTER_BASE_H__ */