correspondence_rejection_features.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-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_REGISTRATION_CORRESPONDENCE_REJECTION_FEATURES_H_
#define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_FEATURES_H_

#include <boost/function.hpp>
#include <boost/unordered_map.hpp>
#include <pcl/registration/correspondence_rejection.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>

namespace pcl
{
  namespace registration
  {
    class CorrespondenceRejectorFeatures: public CorrespondenceRejector
    {
      using CorrespondenceRejector::input_correspondences_;
      using CorrespondenceRejector::rejection_name_;
      using CorrespondenceRejector::getClassName;

      public:
        CorrespondenceRejectorFeatures () : max_distance_ (std::numeric_limits<float>::max ())
        {
          rejection_name_ = "CorrespondenceRejectorFeatures";
        }

        void 
        getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 
                                     pcl::Correspondences& remaining_correspondences);

        template <typename FeatureT> inline void 
        setSourceFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature, 
                          const std::string &key);

        template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
        getSourceFeature (const std::string &key);

        template <typename FeatureT> inline void 
        setTargetFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature, 
                          const std::string &key);

        template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr 
        getTargetFeature (const std::string &key);

        template <typename FeatureT> inline void 
        setDistanceThreshold (double thresh, const std::string &key);

        inline bool 
        hasValidFeatures ();

        template <typename FeatureT> inline void
        setFeatureRepresentation (const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
                                  const std::string &key);

      protected:

        inline void 
        applyRejection (pcl::Correspondences &correspondences)
        {
          getRemainingCorrespondences (*input_correspondences_, correspondences);
        }

        float max_distance_;

        class FeatureContainerInterface
        {
          public:
            virtual bool isValid () = 0;
            virtual double getCorrespondenceScore (int index) = 0;
            virtual bool isCorrespondenceValid (int index) = 0;
        };

        typedef boost::unordered_map<std::string, boost::shared_ptr<FeatureContainerInterface> > FeaturesMap;

        FeaturesMap features_map_;

        template <typename FeatureT>
        class FeatureContainer : public pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface
        {
          public:
            typedef typename pcl::PointCloud<FeatureT>::ConstPtr FeatureCloudConstPtr;
            typedef boost::function<int (const pcl::PointCloud<FeatureT> &, int, std::vector<int> &, 
                                          std::vector<float> &)> SearchMethod;
            
            typedef typename pcl::PointRepresentation<FeatureT>::ConstPtr PointRepresentationConstPtr;

            FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
            {
            }

            inline void 
            setSourceFeature (const FeatureCloudConstPtr &source_features)
            {
              source_features_ = source_features;
            }
            
            inline FeatureCloudConstPtr 
            getSourceFeature ()
            {
              return (source_features_);
            }
            
            inline void 
            setTargetFeature (const FeatureCloudConstPtr &target_features)
            {
              target_features_ = target_features;
            }
            
            inline FeatureCloudConstPtr 
            getTargetFeature ()
            {
              return (target_features_);
            }
            
            inline void 
            setDistanceThreshold (double thresh)
            {
              thresh_ = thresh;
            }

            virtual inline bool 
            isValid ()
            {
              if (!source_features_ || !target_features_)
                return (false);
              else
                return (source_features_->points.size () > 0 && 
                        target_features_->points.size () > 0);
            }

            inline void
            setFeatureRepresentation (const PointRepresentationConstPtr &fr)
            {
              feature_representation_ = fr;
            }

            virtual inline double
            getCorrespondenceScore (int index)
            {
              // If no feature representation was given, reset to the default implementation for FeatureT
              if (!feature_representation_)
                feature_representation_.reset (new DefaultFeatureRepresentation<FeatureT>);

              // Get the source and the target feature from the list
              const FeatureT &feat_src = source_features_->points[index];
              const FeatureT &feat_tgt = target_features_->points[index];

              // Check if the representations are valid
              if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
              {
                PCL_ERROR ("[pcl::registration::CorrespondenceRejectorFeatures::getCorrespondenceScore] Invalid feature representation given!\n");
                return (std::numeric_limits<double>::max ());
              }

              // Set the internal feature point representation of choice
              Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
              feature_representation_->vectorize (FeatureT (feat_src), feat_src_ptr);
              Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
              feature_representation_->vectorize (FeatureT (feat_tgt), feat_tgt_ptr);

              // Compute the L2 norm
              return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
            }

            virtual inline bool
            isCorrespondenceValid (int index)
            {
              if (getCorrespondenceScore (index) < thresh_ * thresh_)
                return (true);
              else
                return (false);
            }
             
          private:
            FeatureCloudConstPtr source_features_, target_features_;
            SearchMethod search_method_;

            double thresh_;

            PointRepresentationConstPtr feature_representation_;
        };
    };
  }
}

#include <pcl/registration/impl/correspondence_rejection_features.hpp>

#endif /* PCL_REGISTRATION_CORRESPONDENCE_REJECTION_FEATURES_H_ */