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) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_ 00042 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_ 00043 00044 #include <pcl/registration/correspondence_types.h> 00045 #include <pcl/registration/correspondence_sorting.h> 00046 #include <pcl/console/print.h> 00047 #include <pcl/common/transforms.h> 00048 #include <pcl/point_cloud.h> 00049 #include <pcl/search/kdtree.h> 00050 00051 namespace pcl 00052 { 00053 namespace registration 00054 { 00055 /** @b CorrespondenceRejector represents the base class for correspondence rejection methods 00056 * \author Dirk Holz 00057 * \ingroup registration 00058 */ 00059 class CorrespondenceRejector 00060 { 00061 public: 00062 typedef boost::shared_ptr<CorrespondenceRejector> Ptr; 00063 typedef boost::shared_ptr<const CorrespondenceRejector> ConstPtr; 00064 00065 /** \brief Empty constructor. */ 00066 CorrespondenceRejector () 00067 : rejection_name_ () 00068 , input_correspondences_ () 00069 {} 00070 00071 /** \brief Empty destructor. */ 00072 virtual ~CorrespondenceRejector () {} 00073 00074 /** \brief Provide a pointer to the vector of the input correspondences. 00075 * \param[in] correspondences the const boost shared pointer to a correspondence vector 00076 */ 00077 virtual inline void 00078 setInputCorrespondences (const CorrespondencesConstPtr &correspondences) 00079 { 00080 input_correspondences_ = correspondences; 00081 }; 00082 00083 /** \brief Get a pointer to the vector of the input correspondences. 00084 * \return correspondences the const boost shared pointer to a correspondence vector 00085 */ 00086 inline CorrespondencesConstPtr 00087 getInputCorrespondences () { return input_correspondences_; }; 00088 00089 /** \brief Run correspondence rejection 00090 * \param[out] correspondences Vector of correspondences that have not been rejected. 00091 */ 00092 inline void 00093 getCorrespondences (pcl::Correspondences &correspondences) 00094 { 00095 if (!input_correspondences_ || (input_correspondences_->empty ())) 00096 return; 00097 00098 applyRejection (correspondences); 00099 } 00100 00101 /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. 00102 * Pure virtual. Compared to \a getCorrespondences this function is 00103 * stateless, i.e., input correspondences do not need to be provided beforehand, 00104 * but are directly provided in the function call. 00105 * \param[in] original_correspondences the set of initial correspondences given 00106 * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences 00107 */ 00108 virtual inline void 00109 getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, 00110 pcl::Correspondences& remaining_correspondences) = 0; 00111 00112 /** \brief Determine the indices of query points of 00113 * correspondences that have been rejected, i.e., the difference 00114 * between the input correspondences (set via \a setInputCorrespondences) 00115 * and the given correspondence vector. 00116 * \param[in] correspondences Vector of correspondences after rejection 00117 * \param[out] indices Vector of query point indices of those correspondences 00118 * that have been rejected. 00119 */ 00120 inline void 00121 getRejectedQueryIndices (const pcl::Correspondences &correspondences, 00122 std::vector<int>& indices) 00123 { 00124 if (!input_correspondences_ || input_correspondences_->empty ()) 00125 { 00126 PCL_WARN ("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences not set (lookup of rejected correspondences _not_ possible).\n", getClassName ().c_str ()); 00127 return; 00128 } 00129 00130 pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices); 00131 } 00132 00133 /** \brief Get a string representation of the name of this class. */ 00134 inline const std::string& 00135 getClassName () const { return (rejection_name_); } 00136 00137 protected: 00138 00139 /** \brief The name of the rejection method. */ 00140 std::string rejection_name_; 00141 00142 /** \brief The input correspondences. */ 00143 CorrespondencesConstPtr input_correspondences_; 00144 00145 /** \brief Abstract rejection method. */ 00146 virtual void 00147 applyRejection (Correspondences &correspondences) = 0; 00148 }; 00149 00150 /** @b DataContainerInterface provides a generic interface for computing correspondence scores between correspondent 00151 * points in the input and target clouds 00152 * \ingroup registration 00153 */ 00154 class DataContainerInterface 00155 { 00156 public: 00157 virtual ~DataContainerInterface () {} 00158 virtual double getCorrespondenceScore (int index) = 0; 00159 virtual double getCorrespondenceScore (const pcl::Correspondence &) = 0; 00160 }; 00161 00162 /** @b DataContainer is a container for the input and target point clouds and implements the interface 00163 * to compute correspondence scores between correspondent points in the input and target clouds 00164 * \ingroup registration 00165 */ 00166 template <typename PointT, typename NormalT = pcl::PointNormal> 00167 class DataContainer : public DataContainerInterface 00168 { 00169 typedef pcl::PointCloud<PointT> PointCloud; 00170 typedef typename PointCloud::Ptr PointCloudPtr; 00171 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00172 00173 typedef typename pcl::search::KdTree<PointT>::Ptr KdTreePtr; 00174 00175 typedef pcl::PointCloud<NormalT> Normals; 00176 typedef typename Normals::Ptr NormalsPtr; 00177 typedef typename Normals::ConstPtr NormalsConstPtr; 00178 00179 public: 00180 00181 /** \brief Empty constructor. */ 00182 DataContainer (bool needs_normals = false) 00183 : input_ () 00184 , input_transformed_ () 00185 , target_ () 00186 , input_normals_ () 00187 , input_normals_transformed_ () 00188 , target_normals_ () 00189 , tree_ (new pcl::search::KdTree<PointT>) 00190 , class_name_ ("DataContainer") 00191 , needs_normals_ (needs_normals) 00192 , target_cloud_updated_ (true) 00193 , force_no_recompute_ (false) 00194 { 00195 } 00196 00197 /** \brief Empty destructor */ 00198 virtual ~DataContainer () {} 00199 00200 /** \brief Provide a source point cloud dataset (must contain XYZ 00201 * data!), used to compute the correspondence distance. 00202 * \param[in] cloud a cloud containing XYZ data 00203 */ 00204 PCL_DEPRECATED (void setInputCloud (const PointCloudConstPtr &cloud), "[pcl::registration::DataContainer::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); 00205 00206 /** \brief Get a pointer to the input point cloud dataset target. */ 00207 PCL_DEPRECATED (PointCloudConstPtr const getInputCloud (), "[pcl::registration::DataContainer::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead."); 00208 00209 /** \brief Provide a source point cloud dataset (must contain XYZ 00210 * data!), used to compute the correspondence distance. 00211 * \param[in] cloud a cloud containing XYZ data 00212 */ 00213 inline void 00214 setInputSource (const PointCloudConstPtr &cloud) 00215 { 00216 input_ = cloud; 00217 } 00218 00219 /** \brief Get a pointer to the input point cloud dataset target. */ 00220 inline PointCloudConstPtr const 00221 getInputSource () { return (input_); } 00222 00223 /** \brief Provide a target point cloud dataset (must contain XYZ 00224 * data!), used to compute the correspondence distance. 00225 * \param[in] target a cloud containing XYZ data 00226 */ 00227 inline void 00228 setInputTarget (const PointCloudConstPtr &target) 00229 { 00230 target_ = target; 00231 target_cloud_updated_ = true; 00232 } 00233 00234 /** \brief Get a pointer to the input point cloud dataset target. */ 00235 inline PointCloudConstPtr const 00236 getInputTarget () { return (target_); } 00237 00238 /** \brief Provide a pointer to the search object used to find correspondences in 00239 * the target cloud. 00240 * \param[in] tree a pointer to the spatial search object. 00241 * \param[in] force_no_recompute If set to true, this tree will NEVER be 00242 * recomputed, regardless of calls to setInputTarget. Only use if you are 00243 * confident that the tree will be set correctly. 00244 */ 00245 inline void 00246 setSearchMethodTarget (const KdTreePtr &tree, 00247 bool force_no_recompute = false) 00248 { 00249 tree_ = tree; 00250 if (force_no_recompute) 00251 { 00252 force_no_recompute_ = true; 00253 } 00254 target_cloud_updated_ = true; 00255 } 00256 00257 /** \brief Set the normals computed on the input point cloud 00258 * \param[in] normals the normals computed for the input cloud 00259 */ 00260 inline void 00261 setInputNormals (const NormalsConstPtr &normals) { input_normals_ = normals; } 00262 00263 /** \brief Get the normals computed on the input point cloud */ 00264 inline NormalsConstPtr 00265 getInputNormals () { return (input_normals_); } 00266 00267 /** \brief Set the normals computed on the target point cloud 00268 * \param[in] normals the normals computed for the input cloud 00269 */ 00270 inline void 00271 setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; } 00272 00273 /** \brief Get the normals computed on the target point cloud */ 00274 inline NormalsConstPtr 00275 getTargetNormals () { return (target_normals_); } 00276 00277 /** \brief Get the correspondence score for a point in the input cloud 00278 * \param[in] index index of the point in the input cloud 00279 */ 00280 inline double 00281 getCorrespondenceScore (int index) 00282 { 00283 if ( target_cloud_updated_ && !force_no_recompute_ ) 00284 { 00285 tree_->setInputCloud (target_); 00286 } 00287 std::vector<int> indices (1); 00288 std::vector<float> distances (1); 00289 if (tree_->nearestKSearch (input_->points[index], 1, indices, distances)) 00290 return (distances[0]); 00291 else 00292 return (std::numeric_limits<double>::max ()); 00293 } 00294 00295 /** \brief Get the correspondence score for a given pair of correspondent points 00296 * \param[in] corr Correspondent points 00297 */ 00298 inline double 00299 getCorrespondenceScore (const pcl::Correspondence &corr) 00300 { 00301 // Get the source and the target feature from the list 00302 const PointT &src = input_->points[corr.index_query]; 00303 const PointT &tgt = target_->points[corr.index_match]; 00304 00305 return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ()); 00306 } 00307 00308 /** \brief Get the correspondence score for a given pair of correspondent points based on 00309 * the angle betweeen the normals. The normmals for the in put and target clouds must be 00310 * set before using this function 00311 * \param[in] corr Correspondent points 00312 */ 00313 inline double 00314 getCorrespondenceScoreFromNormals (const pcl::Correspondence &corr) 00315 { 00316 //assert ( (input_normals_->points.size () != 0) && (target_normals_->points.size () != 0) && "Normals are not set for the input and target point clouds"); 00317 assert (input_normals_ && target_normals_ && "Normals are not set for the input and target point clouds"); 00318 const NormalT &src = input_normals_->points[corr.index_query]; 00319 const NormalT &tgt = target_normals_->points[corr.index_match]; 00320 return (double ((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) + (src.normal[2] * tgt.normal[2]))); 00321 } 00322 00323 private: 00324 /** \brief The input point cloud dataset */ 00325 PointCloudConstPtr input_; 00326 00327 /** \brief The input transformed point cloud dataset */ 00328 PointCloudPtr input_transformed_; 00329 00330 /** \brief The target point cloud datase. */ 00331 PointCloudConstPtr target_; 00332 00333 /** \brief Normals to the input point cloud */ 00334 NormalsConstPtr input_normals_; 00335 00336 /** \brief Normals to the input point cloud */ 00337 NormalsPtr input_normals_transformed_; 00338 00339 /** \brief Normals to the target point cloud */ 00340 NormalsConstPtr target_normals_; 00341 00342 /** \brief A pointer to the spatial search object. */ 00343 KdTreePtr tree_; 00344 00345 /** \brief The name of the rejection method. */ 00346 std::string class_name_; 00347 00348 /** \brief Should the current data container use normals? */ 00349 bool needs_normals_; 00350 00351 /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. 00352 * This way, we avoid rebuilding the kd-tree */ 00353 bool target_cloud_updated_; 00354 00355 /** \brief A flag which, if set, means the tree operating on the target cloud 00356 * will never be recomputed*/ 00357 bool force_no_recompute_; 00358 00359 00360 00361 /** \brief Get a string representation of the name of this class. */ 00362 inline const std::string& 00363 getClassName () const { return (class_name_); } 00364 }; 00365 } 00366 } 00367 00368 #include <pcl/registration/impl/correspondence_rejection.hpp> 00369 00370 #endif /* PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_ */ 00371