Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_H_ 00040 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SURFACE_NORMAL_H_ 00041 00042 #include <pcl/registration/correspondence_rejection.h> 00043 #include <pcl/point_cloud.h> 00044 00045 namespace pcl 00046 { 00047 namespace registration 00048 { 00049 /** 00050 * @b CorrespondenceRejectorSurfaceNormal implements a simple correspondence 00051 * rejection method based on the angle between the normals at correspondent points. 00052 * 00053 * \note If \ref setInputCloud and \ref setInputTarget are given, then the 00054 * distances between correspondences will be estimated using the given XYZ 00055 * data, and not read from the set of input correspondences. 00056 * 00057 * \author Aravindhan K Krishnan (original code from libpointmatcher: https://github.com/ethz-asl/libpointmatcher) 00058 * \ingroup registration 00059 */ 00060 class PCL_EXPORTS CorrespondenceRejectorSurfaceNormal: public CorrespondenceRejector 00061 { 00062 using CorrespondenceRejector::input_correspondences_; 00063 using CorrespondenceRejector::rejection_name_; 00064 using CorrespondenceRejector::getClassName; 00065 00066 public: 00067 typedef boost::shared_ptr<CorrespondenceRejectorSurfaceNormal> Ptr; 00068 typedef boost::shared_ptr<const CorrespondenceRejectorSurfaceNormal> ConstPtr; 00069 00070 /** \brief Empty constructor. Sets the threshold to 1.0. */ 00071 CorrespondenceRejectorSurfaceNormal () 00072 : threshold_ (1.0) 00073 , data_container_ () 00074 { 00075 rejection_name_ = "CorrespondenceRejectorSurfaceNormal"; 00076 } 00077 00078 /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. 00079 * \param[in] original_correspondences the set of initial correspondences given 00080 * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences 00081 */ 00082 void 00083 getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 00084 pcl::Correspondences& remaining_correspondences); 00085 00086 /** \brief Set the thresholding angle between the normals for correspondence rejection. 00087 * \param[in] threshold cosine of the thresholding angle between the normals for rejection 00088 */ 00089 inline void 00090 setThreshold (double threshold) { threshold_ = threshold; }; 00091 00092 /** \brief Get the thresholding angle between the normals for correspondence rejection. */ 00093 inline double 00094 getThreshold () const { return threshold_; }; 00095 00096 /** \brief Initialize the data container object for the point type and the normal type. */ 00097 template <typename PointT, typename NormalT> inline void 00098 initializeDataContainer () 00099 { 00100 data_container_.reset (new DataContainer<PointT, NormalT>); 00101 } 00102 00103 /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. 00104 * \param[in] cloud a cloud containing XYZ data 00105 */ 00106 template <typename PointT> inline void 00107 setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr &input) 00108 { 00109 PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); 00110 if (!data_container_) 00111 { 00112 PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00113 return; 00114 } 00115 boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input); 00116 } 00117 00118 /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. 00119 * \param[in] cloud a cloud containing XYZ data 00120 */ 00121 template <typename PointT> inline void 00122 setInputSource (const typename pcl::PointCloud<PointT>::ConstPtr &input) 00123 { 00124 if (!data_container_) 00125 { 00126 PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00127 return; 00128 } 00129 boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputSource (input); 00130 } 00131 00132 /** \brief Get the target input point cloud */ 00133 template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr 00134 getInputSource () const 00135 { 00136 if (!data_container_) 00137 { 00138 PCL_ERROR ("[pcl::registration::%s::getInputSource] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00139 return; 00140 } 00141 return (boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputSource ()); 00142 } 00143 00144 /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. 00145 * \param[in] target a cloud containing XYZ data 00146 */ 00147 template <typename PointT> inline void 00148 setInputTarget (const typename pcl::PointCloud<PointT>::ConstPtr &target) 00149 { 00150 if (!data_container_) 00151 { 00152 PCL_ERROR ("[pcl::registration::%s::setInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00153 return; 00154 } 00155 boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->setInputTarget (target); 00156 } 00157 00158 /** \brief Provide a pointer to the search object used to find correspondences in 00159 * the target cloud. 00160 * \param[in] tree a pointer to the spatial search object. 00161 * \param[in] force_no_recompute If set to true, this tree will NEVER be 00162 * recomputed, regardless of calls to setInputTarget. Only use if you are 00163 * confident that the tree will be set correctly. 00164 */ 00165 template <typename PointT> inline void 00166 setSearchMethodTarget (const boost::shared_ptr<pcl::search::KdTree<PointT> > &tree, 00167 bool force_no_recompute = false) 00168 { 00169 boost::static_pointer_cast< DataContainer<PointT> > 00170 (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); 00171 } 00172 00173 /** \brief Get the target input point cloud */ 00174 template <typename PointT> inline typename pcl::PointCloud<PointT>::ConstPtr 00175 getInputTarget () const 00176 { 00177 if (!data_container_) 00178 { 00179 PCL_ERROR ("[pcl::registration::%s::getInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00180 return; 00181 } 00182 return (boost::static_pointer_cast<DataContainer<PointT> > (data_container_)->getInputTarget ()); 00183 } 00184 00185 /** \brief Set the normals computed on the input point cloud 00186 * \param[in] normals the normals computed for the input cloud 00187 */ 00188 template <typename PointT, typename NormalT> inline void 00189 setInputNormals (const typename pcl::PointCloud<NormalT>::ConstPtr &normals) 00190 { 00191 if (!data_container_) 00192 { 00193 PCL_ERROR ("[pcl::registration::%s::setInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00194 return; 00195 } 00196 boost::static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setInputNormals (normals); 00197 } 00198 00199 /** \brief Get the normals computed on the input point cloud */ 00200 template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr 00201 getInputNormals () const 00202 { 00203 if (!data_container_) 00204 { 00205 PCL_ERROR ("[pcl::registration::%s::getInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00206 return; 00207 } 00208 return (boost::static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getInputNormals ()); 00209 } 00210 00211 /** \brief Set the normals computed on the target point cloud 00212 * \param[in] normals the normals computed for the input cloud 00213 */ 00214 template <typename PointT, typename NormalT> inline void 00215 setTargetNormals (const typename pcl::PointCloud<NormalT>::ConstPtr &normals) 00216 { 00217 if (!data_container_) 00218 { 00219 PCL_ERROR ("[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00220 return; 00221 } 00222 boost::static_pointer_cast<DataContainer<PointT, NormalT> > (data_container_)->setTargetNormals (normals); 00223 } 00224 00225 /** \brief Get the normals computed on the target point cloud */ 00226 template <typename NormalT> inline typename pcl::PointCloud<NormalT>::Ptr 00227 getTargetNormals () const 00228 { 00229 if (!data_container_) 00230 { 00231 PCL_ERROR ("[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); 00232 return; 00233 } 00234 return (boost::static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT> > (data_container_)->getTargetNormals ()); 00235 } 00236 00237 protected: 00238 00239 /** \brief Apply the rejection algorithm. 00240 * \param[out] correspondences the set of resultant correspondences. 00241 */ 00242 inline void 00243 applyRejection (pcl::Correspondences &correspondences) 00244 { 00245 getRemainingCorrespondences (*input_correspondences_, correspondences); 00246 } 00247 00248 /** \brief The median distance threshold between two correspondent points in source <-> target. */ 00249 double threshold_; 00250 00251 typedef boost::shared_ptr<DataContainerInterface> DataContainerPtr; 00252 /** \brief A pointer to the DataContainer object containing the input and target point clouds */ 00253 DataContainerPtr data_container_; 00254 }; 00255 } 00256 } 00257 00258 #include <pcl/registration/impl/correspondence_rejection_surface_normal.hpp> 00259 00260 #endif