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

class Job;
class 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 boost::shared_ptr<Job> &job) const;

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

    std::map<MeshHandle, boost::shared_ptr<GLMesh> > meshes_;

    boost::shared_ptr<SensorModel::Parameters> 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<boost::shared_ptr<Job> > jobs_queue_;

    mutable boost::mutex meshes_mutex_;

    mutable boost::mutex transform_callback_mutex_;

    bool stop_;

    boost::shared_ptr<GLRenderer> mesh_renderer_;

    boost::shared_ptr<GLRenderer> 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__ */