correspondence_rejection_surface_normal2.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  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_REJECTION_SURFACE_NORMAL2_H_  // NOLINT
#define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SURFACE_NORMAL2_H_  // NOLINT
 
#include <pcl/registration/correspondence_rejection.h>
#include <pcl/point_cloud.h>
 
namespace pcl {
namespace registration {
// TODO(rsoussan): This fixes a bug in PCL for setSourceNormals, where PCL hardcodes the normal type to pcl::Normal
// when it should use NormalT.  Switch back to PCL implementation when this is fixed. Additionally this should be
// either fixed in setTargetNormals or requiresTargetNormals should return false (ideally the second, and
// requires*Points should also be false).
template <typename PointWithNormalT>
class PCL_EXPORTS CorrespondenceRejectorSurfaceNormal2 : public CorrespondenceRejector {
  using CorrespondenceRejector::getClassName;
  using CorrespondenceRejector::input_correspondences_;
  using CorrespondenceRejector::rejection_name_;
 
 public:
  typedef boost::shared_ptr<CorrespondenceRejectorSurfaceNormal2> Ptr;
  typedef boost::shared_ptr<const CorrespondenceRejectorSurfaceNormal2> ConstPtr;
 
  CorrespondenceRejectorSurfaceNormal2() : threshold_(1.0), data_container_() {
    rejection_name_ = "CorrespondenceRejectorSurfaceNormal2";
  }
 
  // void
  // getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
  //                            pcl::Correspondences& remaining_correspondences);
 
  inline void setThreshold(double threshold) { threshold_ = threshold; }
 
  inline double getThreshold() const { return threshold_; }
 
  void getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
                                   pcl::Correspondences& remaining_correspondences) {
    if (!data_container_) {
      PCL_ERROR("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer object is not initialized!\n",
                getClassName().c_str());
      return;
    }
 
    unsigned int number_valid_correspondences = 0;
    remaining_correspondences.resize(original_correspondences.size());
 
    // Test each correspondence
    for (size_t i = 0; i < original_correspondences.size(); ++i) {
      // TODO(rsoussan): Abs val?
      if (std::abs(boost::static_pointer_cast<DataContainer<PointWithNormalT, PointWithNormalT> >(data_container_)
                     ->getCorrespondenceScoreFromNormals(original_correspondences[i])) > threshold_) {
        remaining_correspondences[number_valid_correspondences++] = original_correspondences[i];
      }
    }
    remaining_correspondences.resize(number_valid_correspondences);
  }
 
  inline void initializeDataContainer() {
    data_container_.reset(new DataContainer<PointWithNormalT, PointWithNormalT>);
  }
 
  inline void setInputCloud(const typename pcl::PointCloud<PointWithNormalT>::ConstPtr& input) {
    PCL_WARN("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n",
             getClassName().c_str());
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer "
        "() before using this function.\n",
        getClassName().c_str());
      return;
    }
    boost::static_pointer_cast<DataContainer<PointWithNormalT> >(data_container_)->setInputSource(input);
  }
 
  inline void setInputSource(const typename pcl::PointCloud<PointWithNormalT>::ConstPtr& input) {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer "
        "() before using this function.\n",
        getClassName().c_str());
      return;
    }
    boost::static_pointer_cast<DataContainer<PointWithNormalT> >(data_container_)->setInputSource(input);
  }
 
  inline typename pcl::PointCloud<PointWithNormalT>::ConstPtr getInputSource() const {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::getInputSource] Initialize the data container object by calling "
        "intializeDataContainer () before using this function.\n",
        getClassName().c_str());
      return;
    }
    return (boost::static_pointer_cast<DataContainer<PointWithNormalT> >(data_container_)->getInputSource());
  }
 
  inline void setInputTarget(const typename pcl::PointCloud<PointWithNormalT>::ConstPtr& target) {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::setInputTarget] Initialize the data container object by calling "
        "intializeDataContainer () before using this function.\n",
        getClassName().c_str());
      return;
    }
    boost::static_pointer_cast<DataContainer<PointWithNormalT> >(data_container_)->setInputTarget(target);
  }
 
  inline void setSearchMethodTarget(const boost::shared_ptr<pcl::search::KdTree<PointWithNormalT> >& tree,
                                    bool force_no_recompute = false) {
    boost::static_pointer_cast<DataContainer<PointWithNormalT> >(data_container_)
      ->setSearchMethodTarget(tree, force_no_recompute);
  }
 
  inline typename pcl::PointCloud<PointWithNormalT>::ConstPtr getInputTarget() const {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::getInputTarget] Initialize the data container object by calling "
        "intializeDataContainer () before using this function.\n",
        getClassName().c_str());
      return;
    }
    return (boost::static_pointer_cast<DataContainer<PointWithNormalT> >(data_container_)->getInputTarget());
  }
 
  inline void setInputNormals(const typename pcl::PointCloud<PointWithNormalT>::ConstPtr& normals) {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::setInputNormals] Initialize the data container object by calling "
        "intializeDataContainer () before using this function.\n",
        getClassName().c_str());
      return;
    }
    boost::static_pointer_cast<DataContainer<PointWithNormalT, PointWithNormalT> >(data_container_)
      ->setInputNormals(normals);
  }
 
  inline typename pcl::PointCloud<PointWithNormalT>::Ptr getInputNormals() const {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::getInputNormals] Initialize the data container object by calling "
        "intializeDataContainer () before using this function.\n",
        getClassName().c_str());
      return;
    }
    return (boost::static_pointer_cast<DataContainer<PointWithNormalT, PointWithNormalT> >(data_container_)
              ->getInputNormals());
  }
 
  inline void setTargetNormals(const typename pcl::PointCloud<PointWithNormalT>::ConstPtr& normals) {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling "
        "intializeDataContainer () before using this function.\n",
        getClassName().c_str());
      return;
    }
    boost::static_pointer_cast<DataContainer<PointWithNormalT, PointWithNormalT> >(data_container_)
      ->setTargetNormals(normals);
  }
 
  inline typename pcl::PointCloud<PointWithNormalT>::Ptr getTargetNormals() const {
    if (!data_container_) {
      PCL_ERROR(
        "[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling "
        "intializeDataContainer () before using this function.\n",
        getClassName().c_str());
      return;
    }
    return (boost::static_pointer_cast<DataContainer<PointWithNormalT, PointWithNormalT> >(data_container_)
              ->getTargetNormals());
  }
 
  bool requiresSourcePoints() const { return (false); }
 
  void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) {
    if (!data_container_) initializeDataContainer();
    typename PointCloud<PointWithNormalT>::Ptr cloud(new PointCloud<PointWithNormalT>);
    fromPCLPointCloud2(*cloud2, *cloud);
    setInputSource(cloud);
  }
 
  bool requiresTargetPoints() const { return (false); }
 
  void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) {
    if (!data_container_) initializeDataContainer();
    typename PointCloud<PointWithNormalT>::Ptr cloud(new PointCloud<PointWithNormalT>);
    fromPCLPointCloud2(*cloud2, *cloud);
    setInputTarget(cloud);
  }
 
  bool requiresSourceNormals() const { return (true); }
 
  void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) {
    if (!data_container_) initializeDataContainer();
    typename PointCloud<PointWithNormalT>::Ptr cloud(new PointCloud<PointWithNormalT>);
    fromPCLPointCloud2(*cloud2, *cloud);
    setInputNormals(cloud);
  }
 
  bool requiresTargetNormals() const { return (false); }
 
  void setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) {
    if (!data_container_) initializeDataContainer();
    typename PointCloud<PointWithNormalT>::Ptr cloud(new PointCloud<PointWithNormalT>);
    fromPCLPointCloud2(*cloud2, *cloud);
    setTargetNormals(cloud);
  }
 
 protected:
  inline void applyRejection(pcl::Correspondences& correspondences) {
    getRemainingCorrespondences(*input_correspondences_, correspondences);
  }
 
  double threshold_;
 
  typedef boost::shared_ptr<DataContainerInterface> DataContainerPtr;
  DataContainerPtr data_container_;
};
}  // namespace registration
}  // namespace pcl
 
#endif  // NOLINT