correspondence_estimation.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.
 *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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.
 *
 * $Id$
 *
 */

#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_
#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_

#include <string>

#include <pcl/pcl_base.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h>
#include <pcl/pcl_macros.h>

#include <pcl/registration/correspondence_types.h>

namespace pcl
{
  namespace registration
  {
    template <typename PointSource, typename PointTarget, typename Scalar = float>
    class CorrespondenceEstimationBase: public PCLBase<PointSource>
    {
      public:
        typedef boost::shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > Ptr;
        typedef boost::shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > ConstPtr;

        // using PCLBase<PointSource>::initCompute;
        using PCLBase<PointSource>::deinitCompute;
        using PCLBase<PointSource>::input_;
        using PCLBase<PointSource>::indices_;
        using PCLBase<PointSource>::setIndices;

        typedef pcl::search::KdTree<PointTarget> KdTree;
        typedef typename KdTree::Ptr KdTreePtr;

        typedef pcl::search::KdTree<PointSource> KdTreeReciprocal;
        typedef typename KdTree::Ptr KdTreeReciprocalPtr;

        typedef pcl::PointCloud<PointSource> PointCloudSource;
        typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
        typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;

        typedef pcl::PointCloud<PointTarget> PointCloudTarget;
        typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
        typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;

        typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr;

        CorrespondenceEstimationBase () 
          : corr_name_ ("CorrespondenceEstimationBase")
          , tree_ (new pcl::search::KdTree<PointTarget>)
          , tree_reciprocal_ (new pcl::search::KdTree<PointSource>)
          , target_ ()
          , target_indices_ ()
          , point_representation_ ()
          , input_transformed_ ()
          , input_fields_ ()
          , target_cloud_updated_ (true)
          , source_cloud_updated_ (true)
          , force_no_recompute_ (false)
          , force_no_recompute_reciprocal_ (false)
        {
        }
      
        virtual ~CorrespondenceEstimationBase () {}

        PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::CorrespondenceEstimationBase::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.");

        PCL_DEPRECATED (PointCloudSourceConstPtr const getInputCloud (), 
            "[pcl::registration::CorrespondenceEstimationBase::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.");

        inline void 
        setInputSource (const PointCloudSourceConstPtr &cloud)
        {
          source_cloud_updated_ = true;
          PCLBase<PointSource>::setInputCloud (cloud);
          pcl::getFields (*cloud, input_fields_);
        }

        inline PointCloudSourceConstPtr const 
        getInputSource () 
        { 
          return (input_ ); 
        }

        inline void 
        setInputTarget (const PointCloudTargetConstPtr &cloud);

        inline PointCloudTargetConstPtr const 
        getInputTarget () { return (target_ ); }

        inline void
        setIndicesSource (const IndicesPtr &indices)
        {
          setIndices (indices);
        }

        inline IndicesPtr const 
        getIndicesSource () { return (indices_); }

        inline void
        setIndicesTarget (const IndicesPtr &indices)
        {
          target_cloud_updated_ = true;
          target_indices_ = indices;
        }

        inline IndicesPtr const 
        getIndicesTarget () { return (target_indices_); }

        inline void
        setSearchMethodTarget (const KdTreePtr &tree, 
                               bool force_no_recompute = false) 
        { 
          tree_ = tree; 
          if (force_no_recompute)
          {
            force_no_recompute_ = true;
          }
          // Since we just set a new tree, we need to check for updates
          target_cloud_updated_ = true;
        }

        inline KdTreePtr
        getSearchMethodTarget () const
        {
          return (tree_);
        }

        inline void
        setSearchMethodSource (const KdTreeReciprocalPtr &tree, 
                               bool force_no_recompute = false) 
        { 
          tree_reciprocal_ = tree; 
          if ( force_no_recompute )
          {
            force_no_recompute_reciprocal_ = true;
          }
          // Since we just set a new tree, we need to check for updates
          source_cloud_updated_ = true;
        }

        inline KdTreeReciprocalPtr
        getSearchMethodSource () const
        {
          return (tree_reciprocal_);
        }

        virtual void 
        determineCorrespondences (pcl::Correspondences &correspondences,
                                  double max_distance = std::numeric_limits<double>::max ()) = 0;

        virtual void 
        determineReciprocalCorrespondences (pcl::Correspondences &correspondences,
                                            double max_distance = std::numeric_limits<double>::max ()) = 0;

        inline void
        setPointRepresentation (const PointRepresentationConstPtr &point_representation)
        {
          point_representation_ = point_representation;
        }

      protected:
        std::string corr_name_;

        KdTreePtr tree_;

        KdTreeReciprocalPtr tree_reciprocal_;


        
        PointCloudTargetConstPtr target_;

        IndicesPtr target_indices_;

        PointRepresentationConstPtr point_representation_;

        PointCloudTargetPtr input_transformed_;

        std::vector<pcl::PCLPointField> input_fields_;

        inline const std::string& 
        getClassName () const { return (corr_name_); }

        bool
        initCompute ();
        
        bool
        initComputeReciprocal ();

        bool target_cloud_updated_;
        bool source_cloud_updated_;
        bool force_no_recompute_;
        
        bool force_no_recompute_reciprocal_;

     };

    template <typename PointSource, typename PointTarget, typename Scalar = float>
    class CorrespondenceEstimation : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>
    {
      public:
        typedef boost::shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar> > Ptr;
        typedef boost::shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar> > ConstPtr;

        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_reciprocal_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::indices_;
        using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_fields_;
        using PCLBase<PointSource>::deinitCompute;

        typedef pcl::search::KdTree<PointTarget> KdTree;
        typedef typename pcl::search::KdTree<PointTarget>::Ptr KdTreePtr;

        typedef pcl::PointCloud<PointSource> PointCloudSource;
        typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
        typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;

        typedef pcl::PointCloud<PointTarget> PointCloudTarget;
        typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
        typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;

        typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr;

        CorrespondenceEstimation () 
        {
          corr_name_  = "CorrespondenceEstimation";
        }
      
        virtual ~CorrespondenceEstimation () {}

        virtual void 
        determineCorrespondences (pcl::Correspondences &correspondences,
                                  double max_distance = std::numeric_limits<double>::max ());

        virtual void 
        determineReciprocalCorrespondences (pcl::Correspondences &correspondences,
                                            double max_distance = std::numeric_limits<double>::max ());
     };
  }
}

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

#endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_ */