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_ESTIMATION_H_ 00042 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_ 00043 00044 #include <string> 00045 00046 #include <pcl/pcl_base.h> 00047 #include <pcl/common/transforms.h> 00048 #include <pcl/search/kdtree.h> 00049 #include <pcl/pcl_macros.h> 00050 00051 #include <pcl/registration/correspondence_types.h> 00052 00053 namespace pcl 00054 { 00055 namespace registration 00056 { 00057 /** \brief Abstract @b CorrespondenceEstimationBase class. 00058 * All correspondence estimation methods should inherit from this. 00059 * \author Radu B. Rusu 00060 * \ingroup registration 00061 */ 00062 template <typename PointSource, typename PointTarget, typename Scalar = float> 00063 class CorrespondenceEstimationBase: public PCLBase<PointSource> 00064 { 00065 public: 00066 typedef boost::shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > Ptr; 00067 typedef boost::shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > ConstPtr; 00068 00069 // using PCLBase<PointSource>::initCompute; 00070 using PCLBase<PointSource>::deinitCompute; 00071 using PCLBase<PointSource>::input_; 00072 using PCLBase<PointSource>::indices_; 00073 using PCLBase<PointSource>::setIndices; 00074 00075 typedef pcl::search::KdTree<PointTarget> KdTree; 00076 typedef typename KdTree::Ptr KdTreePtr; 00077 00078 typedef pcl::search::KdTree<PointSource> KdTreeReciprocal; 00079 typedef typename KdTree::Ptr KdTreeReciprocalPtr; 00080 00081 typedef pcl::PointCloud<PointSource> PointCloudSource; 00082 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00083 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00084 00085 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00086 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00087 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00088 00089 typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr; 00090 00091 /** \brief Empty constructor. */ 00092 CorrespondenceEstimationBase () 00093 : corr_name_ ("CorrespondenceEstimationBase") 00094 , tree_ (new pcl::search::KdTree<PointTarget>) 00095 , tree_reciprocal_ (new pcl::search::KdTree<PointSource>) 00096 , target_ () 00097 , target_indices_ () 00098 , point_representation_ () 00099 , input_transformed_ () 00100 , input_fields_ () 00101 , target_cloud_updated_ (true) 00102 , source_cloud_updated_ (true) 00103 , force_no_recompute_ (false) 00104 , force_no_recompute_reciprocal_ (false) 00105 { 00106 } 00107 00108 /** \brief Empty destructor */ 00109 virtual ~CorrespondenceEstimationBase () {} 00110 00111 /** \brief Provide a pointer to the input source 00112 * (e.g., the point cloud that we want to align to the target) 00113 * 00114 * \param[in] cloud the input point cloud source 00115 */ 00116 PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::CorrespondenceEstimationBase::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); 00117 00118 /** \brief Get a pointer to the input point cloud dataset target. */ 00119 PCL_DEPRECATED (PointCloudSourceConstPtr const getInputCloud (), 00120 "[pcl::registration::CorrespondenceEstimationBase::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead."); 00121 00122 /** \brief Provide a pointer to the input source 00123 * (e.g., the point cloud that we want to align to the target) 00124 * 00125 * \param[in] cloud the input point cloud source 00126 */ 00127 inline void 00128 setInputSource (const PointCloudSourceConstPtr &cloud) 00129 { 00130 source_cloud_updated_ = true; 00131 PCLBase<PointSource>::setInputCloud (cloud); 00132 pcl::getFields (*cloud, input_fields_); 00133 } 00134 00135 /** \brief Get a pointer to the input point cloud dataset target. */ 00136 inline PointCloudSourceConstPtr const 00137 getInputSource () 00138 { 00139 return (input_ ); 00140 } 00141 00142 /** \brief Provide a pointer to the input target 00143 * (e.g., the point cloud that we want to align the input source to) 00144 * \param[in] cloud the input point cloud target 00145 */ 00146 inline void 00147 setInputTarget (const PointCloudTargetConstPtr &cloud); 00148 00149 /** \brief Get a pointer to the input point cloud dataset target. */ 00150 inline PointCloudTargetConstPtr const 00151 getInputTarget () { return (target_ ); } 00152 00153 /** \brief Provide a pointer to the vector of indices that represent the 00154 * input source point cloud. 00155 * \param[in] indices a pointer to the vector of indices 00156 */ 00157 inline void 00158 setIndicesSource (const IndicesPtr &indices) 00159 { 00160 setIndices (indices); 00161 } 00162 00163 /** \brief Get a pointer to the vector of indices used for the source dataset. */ 00164 inline IndicesPtr const 00165 getIndicesSource () { return (indices_); } 00166 00167 /** \brief Provide a pointer to the vector of indices that represent the input target point cloud. 00168 * \param[in] indices a pointer to the vector of indices 00169 */ 00170 inline void 00171 setIndicesTarget (const IndicesPtr &indices) 00172 { 00173 target_cloud_updated_ = true; 00174 target_indices_ = indices; 00175 } 00176 00177 /** \brief Get a pointer to the vector of indices used for the target dataset. */ 00178 inline IndicesPtr const 00179 getIndicesTarget () { return (target_indices_); } 00180 00181 /** \brief Provide a pointer to the search object used to find correspondences in 00182 * the target cloud. 00183 * \param[in] tree a pointer to the spatial search object. 00184 * \param[in] force_no_recompute If set to true, this tree will NEVER be 00185 * recomputed, regardless of calls to setInputTarget. Only use if you are 00186 * confident that the tree will be set correctly. 00187 */ 00188 inline void 00189 setSearchMethodTarget (const KdTreePtr &tree, 00190 bool force_no_recompute = false) 00191 { 00192 tree_ = tree; 00193 if (force_no_recompute) 00194 { 00195 force_no_recompute_ = true; 00196 } 00197 // Since we just set a new tree, we need to check for updates 00198 target_cloud_updated_ = true; 00199 } 00200 00201 /** \brief Get a pointer to the search method used to find correspondences in the 00202 * target cloud. */ 00203 inline KdTreePtr 00204 getSearchMethodTarget () const 00205 { 00206 return (tree_); 00207 } 00208 00209 /** \brief Provide a pointer to the search object used to find correspondences in 00210 * the source cloud (usually used by reciprocal correspondence finding). 00211 * \param[in] tree a pointer to the spatial search object. 00212 * \param[in] force_no_recompute If set to true, this tree will NEVER be 00213 * recomputed, regardless of calls to setInputSource. Only use if you are 00214 * extremely confident that the tree will be set correctly. 00215 */ 00216 inline void 00217 setSearchMethodSource (const KdTreeReciprocalPtr &tree, 00218 bool force_no_recompute = false) 00219 { 00220 tree_reciprocal_ = tree; 00221 if ( force_no_recompute ) 00222 { 00223 force_no_recompute_reciprocal_ = true; 00224 } 00225 // Since we just set a new tree, we need to check for updates 00226 source_cloud_updated_ = true; 00227 } 00228 00229 /** \brief Get a pointer to the search method used to find correspondences in the 00230 * source cloud. */ 00231 inline KdTreeReciprocalPtr 00232 getSearchMethodSource () const 00233 { 00234 return (tree_reciprocal_); 00235 } 00236 00237 /** \brief Determine the correspondences between input and target cloud. 00238 * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) 00239 * \param[in] max_distance maximum allowed distance between correspondences 00240 */ 00241 virtual void 00242 determineCorrespondences (pcl::Correspondences &correspondences, 00243 double max_distance = std::numeric_limits<double>::max ()) = 0; 00244 00245 /** \brief Determine the reciprocal correspondences between input and target cloud. 00246 * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 00247 * correspondence, and Tgt_i has Src_i as one. 00248 * 00249 * \param[out] correspondences the found correspondences (index of query and target point, distance) 00250 * \param[in] max_distance maximum allowed distance between correspondences 00251 */ 00252 virtual void 00253 determineReciprocalCorrespondences (pcl::Correspondences &correspondences, 00254 double max_distance = std::numeric_limits<double>::max ()) = 0; 00255 00256 /** \brief Provide a boost shared pointer to the PointRepresentation to be used 00257 * when searching for nearest neighbors. 00258 * 00259 * \param[in] point_representation the PointRepresentation to be used by the 00260 * k-D tree for nearest neighbor search 00261 */ 00262 inline void 00263 setPointRepresentation (const PointRepresentationConstPtr &point_representation) 00264 { 00265 point_representation_ = point_representation; 00266 } 00267 00268 protected: 00269 /** \brief The correspondence estimation method name. */ 00270 std::string corr_name_; 00271 00272 /** \brief A pointer to the spatial search object used for the target dataset. */ 00273 KdTreePtr tree_; 00274 00275 /** \brief A pointer to the spatial search object used for the source dataset. */ 00276 KdTreeReciprocalPtr tree_reciprocal_; 00277 00278 00279 00280 /** \brief The input point cloud dataset target. */ 00281 PointCloudTargetConstPtr target_; 00282 00283 /** \brief The target point cloud dataset indices. */ 00284 IndicesPtr target_indices_; 00285 00286 /** \brief The point representation used (internal). */ 00287 PointRepresentationConstPtr point_representation_; 00288 00289 /** \brief The transformed input source point cloud dataset. */ 00290 PointCloudTargetPtr input_transformed_; 00291 00292 /** \brief The types of input point fields available. */ 00293 std::vector<pcl::PCLPointField> input_fields_; 00294 00295 /** \brief Abstract class get name method. */ 00296 inline const std::string& 00297 getClassName () const { return (corr_name_); } 00298 00299 /** \brief Internal computation initalization. */ 00300 bool 00301 initCompute (); 00302 00303 /** \brief Internal computation initalization for reciprocal correspondences. */ 00304 bool 00305 initComputeReciprocal (); 00306 00307 /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. 00308 * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method 00309 * is called. */ 00310 bool target_cloud_updated_; 00311 /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again. 00312 * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method 00313 * is called. */ 00314 bool source_cloud_updated_; 00315 /** \brief A flag which, if set, means the tree operating on the target cloud 00316 * will never be recomputed*/ 00317 bool force_no_recompute_; 00318 00319 /** \brief A flag which, if set, means the tree operating on the source cloud 00320 * will never be recomputed*/ 00321 bool force_no_recompute_reciprocal_; 00322 00323 }; 00324 00325 /** \brief @b CorrespondenceEstimation represents the base class for 00326 * determining correspondences between target and query point 00327 * sets/features. 00328 * 00329 * Code example: 00330 * 00331 * \code 00332 * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target; 00333 * // ... read or fill in source and target 00334 * pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est; 00335 * est.setInputSource (source); 00336 * est.setInputTarget (target); 00337 * 00338 * pcl::Correspondences all_correspondences; 00339 * // Determine all reciprocal correspondences 00340 * est.determineReciprocalCorrespondences (all_correspondences); 00341 * \endcode 00342 * 00343 * \author Radu B. Rusu, Michael Dixon, Dirk Holz 00344 * \ingroup registration 00345 */ 00346 template <typename PointSource, typename PointTarget, typename Scalar = float> 00347 class CorrespondenceEstimation : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> 00348 { 00349 public: 00350 typedef boost::shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar> > Ptr; 00351 typedef boost::shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar> > ConstPtr; 00352 00353 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::point_representation_; 00354 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_transformed_; 00355 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_; 00356 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_reciprocal_; 00357 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_; 00358 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_; 00359 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_; 00360 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName; 00361 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute; 00362 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal; 00363 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_; 00364 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::indices_; 00365 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_fields_; 00366 using PCLBase<PointSource>::deinitCompute; 00367 00368 typedef pcl::search::KdTree<PointTarget> KdTree; 00369 typedef typename pcl::search::KdTree<PointTarget>::Ptr KdTreePtr; 00370 00371 typedef pcl::PointCloud<PointSource> PointCloudSource; 00372 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00373 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00374 00375 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00376 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00377 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00378 00379 typedef typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr; 00380 00381 /** \brief Empty constructor. */ 00382 CorrespondenceEstimation () 00383 { 00384 corr_name_ = "CorrespondenceEstimation"; 00385 } 00386 00387 /** \brief Empty destructor */ 00388 virtual ~CorrespondenceEstimation () {} 00389 00390 /** \brief Determine the correspondences between input and target cloud. 00391 * \param[out] correspondences the found correspondences (index of query point, index of target point, distance) 00392 * \param[in] max_distance maximum allowed distance between correspondences 00393 */ 00394 virtual void 00395 determineCorrespondences (pcl::Correspondences &correspondences, 00396 double max_distance = std::numeric_limits<double>::max ()); 00397 00398 /** \brief Determine the reciprocal correspondences between input and target cloud. 00399 * A correspondence is considered reciprocal if both Src_i has Tgt_i as a 00400 * correspondence, and Tgt_i has Src_i as one. 00401 * 00402 * \param[out] correspondences the found correspondences (index of query and target point, distance) 00403 * \param[in] max_distance maximum allowed distance between correspondences 00404 */ 00405 virtual void 00406 determineReciprocalCorrespondences (pcl::Correspondences &correspondences, 00407 double max_distance = std::numeric_limits<double>::max ()); 00408 }; 00409 } 00410 } 00411 00412 #include <pcl/registration/impl/correspondence_estimation.hpp> 00413 00414 #endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_ */