implicit_shape_model.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, 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, Inc. 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.
 *
 */

#ifndef PCL_IMPLICIT_SHAPE_MODEL_H_
#define PCL_IMPLICIT_SHAPE_MODEL_H_

#include <vector>
#include <fstream>
#include <limits>
#include <Eigen/src/Core/Matrix.h>
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl/point_representation.h>
#include <pcl/features/feature.h>
#include <pcl/features/spin_image.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/search/search.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>

namespace pcl
{
  struct ISMPeak
  {
    PCL_ADD_POINT4D;

    double density;

    int class_id;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  } EIGEN_ALIGN16;

  namespace features
  {
    template <typename PointT>
    class PCL_EXPORTS ISMVoteList
    {
      public:

        ISMVoteList ();

        virtual
        ~ISMVoteList ();

        void
        addVote (pcl::InterestPoint& in_vote, const PointT &vote_origin, int in_class);

        typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr
        getColoredCloud (typename pcl::PointCloud<PointT>::Ptr cloud = 0);

        void
        findStrongestPeaks (std::vector<ISMPeak, Eigen::aligned_allocator<ISMPeak> > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma);

        double
        getDensityAtPoint (const PointT &point, double sigma_dist);

        unsigned int
        getNumberOfVotes ();

      protected:

        void
        validateTree ();

        Eigen::Vector3f
        shiftMean (const Eigen::Vector3f& snapPt, const double in_dSigmaDist);

      protected:

        pcl::PointCloud<pcl::InterestPoint>::Ptr votes_;

        bool tree_is_valid_;

        typename pcl::PointCloud<PointT>::Ptr votes_origins_;

        std::vector<int> votes_class_;

        pcl::KdTreeFLANN<pcl::InterestPoint>::Ptr tree_;

        std::vector<int> k_ind_;

        std::vector<float> k_sqr_dist_;
    };
 
    struct PCL_EXPORTS ISMModel
    {
      ISMModel ();

      ISMModel (ISMModel const & copy);

      virtual
      ~ISMModel ();

      bool
      saveModelToFile (std::string& file_name);

      bool
      loadModelFromfile (std::string& file_name);

      void
      reset ();

      ISMModel & operator = (const ISMModel& other);

      std::vector<std::vector<float> > statistical_weights_;

      std::vector<float> learned_weights_;

      std::vector<unsigned int> classes_;

      std::vector<float> sigmas_;

      Eigen::MatrixXf directions_to_center_;

      Eigen::MatrixXf clusters_centers_;

      std::vector<std::vector<unsigned int> > clusters_;

      unsigned int number_of_classes_;

      unsigned int number_of_visual_words_;

      unsigned int number_of_clusters_;

      unsigned int descriptors_dimension_;

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
  }

  namespace ism
  {
    template <int FeatureSize, typename PointT, typename NormalT = pcl::Normal>
    class PCL_EXPORTS ImplicitShapeModelEstimation
    {
      public:

        typedef boost::shared_ptr<pcl::features::ISMModel> ISMModelPtr;

      protected:

        struct PCL_EXPORTS LocationInfo
        {
          LocationInfo (unsigned int model_num, const PointT& dir_to_center, const PointT& origin, const NormalT& normal) :
            model_num_ (model_num),
            dir_to_center_ (dir_to_center),
            point_ (origin),
            normal_ (normal) {};

          unsigned int model_num_;

          PointT dir_to_center_;

          PointT point_;

          NormalT normal_;
        };

        typedef struct PCL_EXPORTS TC
        {
          enum
          {
            COUNT = 1,
            EPS = 2
          };

          TC(int type, int max_count, float epsilon) :
            type_ (type),
            max_count_ (max_count),
            epsilon_ (epsilon) {};

          int type_;

          int max_count_;

          float epsilon_;
        } TermCriteria;

        struct PCL_EXPORTS VisualWordStat
        {
          VisualWordStat () :
            class_ (-1),
            learned_weight_ (0.0f),
            dir_to_center_ (0.0f, 0.0f, 0.0f) {};

          int class_;

          float learned_weight_;

          pcl::PointXYZ dir_to_center_;
        };

      public:

        ImplicitShapeModelEstimation ();

        virtual
        ~ImplicitShapeModelEstimation ();

        std::vector<typename pcl::PointCloud<PointT>::Ptr>
                getTrainingClouds ();

        void
                setTrainingClouds (const std::vector< typename pcl::PointCloud<PointT>::Ptr >& training_clouds);

        std::vector<unsigned int>
        getTrainingClasses ();

        void
        setTrainingClasses (const std::vector<unsigned int>& training_classes);

        std::vector<typename pcl::PointCloud<NormalT>::Ptr>
        getTrainingNormals ();

        void
        setTrainingNormals (const std::vector< typename pcl::PointCloud<NormalT>::Ptr >& training_normals);

        float
        getSamplingSize ();

        void
        setSamplingSize (float sampling_size);

        boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > >
        getFeatureEstimator ();

        void
        setFeatureEstimator (boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > > feature);

        unsigned int
        getNumberOfClusters ();

        void
        setNumberOfClusters (unsigned int num_of_clusters);

        std::vector<float>
        getSigmaDists ();

        void
        setSigmaDists (const std::vector<float>& training_sigmas);

        bool
        getNVotState ();

        void
        setNVotState (bool state);

        bool
        trainISM (ISMModelPtr& trained_model);

        boost::shared_ptr<pcl::features::ISMVoteList<PointT> >
        findObjects (ISMModelPtr model, typename pcl::PointCloud<PointT>::Ptr in_cloud, typename pcl::PointCloud<Normal>::Ptr in_normals, int in_class_of_interest);

      protected:

        bool
        extractDescriptors (std::vector<pcl::Histogram<FeatureSize> >& histograms,
                            std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations);

        bool
        clusterDescriptors (std::vector< pcl::Histogram<FeatureSize> >& histograms, Eigen::MatrixXi& labels, Eigen::MatrixXf& clusters_centers);

        void
        calculateSigmas (std::vector<float>& sigmas);

        void
        calculateWeights (const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
                          const Eigen::MatrixXi &labels,
                          std::vector<float>& sigmas,
                          std::vector<std::vector<unsigned int> >& clusters,
                          std::vector<std::vector<float> >& statistical_weights,
                          std::vector<float>& learned_weights);

        void
        simplifyCloud (typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
                       typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
                       typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
                       typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud);

        void
        shiftCloud (typename pcl::PointCloud<PointT>::Ptr in_cloud, Eigen::Vector3f shift_point);

        Eigen::Matrix3f
        alignYCoordWithNormal (const NormalT& in_normal);

        void
        applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform);

        void
        estimateFeatures (typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
                          typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
                          typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud);

        double
        computeKMeansClustering (const Eigen::MatrixXf& points_to_cluster,
                                 int number_of_clusters,
                                 Eigen::MatrixXi& io_labels,
                                 TermCriteria criteria,
                                 int attempts,
                                 int flags,
                                 Eigen::MatrixXf& cluster_centers);

        void
        generateCentersPP (const Eigen::MatrixXf& data,
                           Eigen::MatrixXf& out_centers,
                           int number_of_clusters,
                           int trials);

        void
        generateRandomCenter (const std::vector<Eigen::Vector2f>& boxes, Eigen::VectorXf& center);

        float
        computeDistance (Eigen::VectorXf& vec_1, Eigen::VectorXf& vec_2);

        ImplicitShapeModelEstimation&
        operator= (const ImplicitShapeModelEstimation&);

      protected:

        std::vector<typename pcl::PointCloud<PointT>::Ptr> training_clouds_;

        std::vector<unsigned int> training_classes_;

        std::vector<typename pcl::PointCloud<NormalT>::Ptr> training_normals_;

        std::vector<float> training_sigmas_;

        float sampling_size_;

        boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > > feature_estimator_;

        unsigned int number_of_clusters_;

        bool n_vot_ON_;

        static const int PP_CENTERS = 2;

        static const int USE_INITIAL_LABELS = 1;
    };
  }
}

POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ISMPeak,
  (float, x, x)
  (float, y, y)
  (float, z, z)
  (float, density, ism_density)
  (float, class_id, ism_class_id)
)

#endif  //#ifndef PCL_IMPLICIT_SHAPE_MODEL_H_