Point Cloud Library (PCL)  1.8.1-dev
correspondence_estimation.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_
42 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_
43 
44 #include <string>
45 
46 #include <pcl/pcl_base.h>
47 #include <pcl/common/transforms.h>
48 #include <pcl/search/kdtree.h>
49 #include <pcl/pcl_macros.h>
50 
51 #include <pcl/registration/correspondence_types.h>
52 
53 namespace pcl
54 {
55  namespace registration
56  {
57  /** \brief Abstract @b CorrespondenceEstimationBase class.
58  * All correspondence estimation methods should inherit from this.
59  * \author Radu B. Rusu
60  * \ingroup registration
61  */
62  template <typename PointSource, typename PointTarget, typename Scalar = float>
63  class CorrespondenceEstimationBase: public PCLBase<PointSource>
64  {
65  public:
66  typedef boost::shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > Ptr;
67  typedef boost::shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > ConstPtr;
68 
69  // using PCLBase<PointSource>::initCompute;
74 
76  typedef typename KdTree::Ptr KdTreePtr;
77 
79  typedef typename KdTree::Ptr KdTreeReciprocalPtr;
80 
84 
88 
90 
91  /** \brief Empty constructor. */
93  : corr_name_ ("CorrespondenceEstimationBase")
94  , tree_ (new pcl::search::KdTree<PointTarget>)
95  , tree_reciprocal_ (new pcl::search::KdTree<PointSource>)
96  , target_ ()
97  , target_indices_ ()
100  , input_fields_ ()
101  , target_cloud_updated_ (true)
102  , source_cloud_updated_ (true)
103  , force_no_recompute_ (false)
105  {
106  }
107 
108  /** \brief Empty destructor */
110 
111  /** \brief Provide a pointer to the input source
112  * (e.g., the point cloud that we want to align to the target)
113  *
114  * \param[in] cloud the input point cloud source
115  */
116  PCL_DEPRECATED ("[pcl::registration::CorrespondenceEstimationBase::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.")
117  void
119 
120  /** \brief Get a pointer to the input point cloud dataset target. */
121  PCL_DEPRECATED ("[pcl::registration::CorrespondenceEstimationBase::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.")
123  getInputCloud ();
124 
125  /** \brief Provide a pointer to the input source
126  * (e.g., the point cloud that we want to align to the target)
127  *
128  * \param[in] cloud the input point cloud source
129  */
130  inline void
132  {
133  source_cloud_updated_ = true;
135  pcl::getFields (*cloud, input_fields_);
136  }
137 
138  /** \brief Get a pointer to the input point cloud dataset target. */
139  inline PointCloudSourceConstPtr const
141  {
142  return (input_ );
143  }
144 
145  /** \brief Provide a pointer to the input target
146  * (e.g., the point cloud that we want to align the input source to)
147  * \param[in] cloud the input point cloud target
148  */
149  inline void
151 
152  /** \brief Get a pointer to the input point cloud dataset target. */
153  inline PointCloudTargetConstPtr const
154  getInputTarget () { return (target_ ); }
155 
156 
157  /** \brief See if this rejector requires source normals */
158  virtual bool
160  { return (false); }
161 
162  /** \brief Abstract method for setting the source normals */
163  virtual void
165  {
166  PCL_WARN ("[pcl::registration::%s::setSourceNormals] This class does not require input source normals", getClassName ().c_str ());
167  }
168 
169  /** \brief See if this rejector requires target normals */
170  virtual bool
172  { return (false); }
173 
174  /** \brief Abstract method for setting the target normals */
175  virtual void
177  {
178  PCL_WARN ("[pcl::registration::%s::setTargetNormals] This class does not require input target normals", getClassName ().c_str ());
179  }
180 
181  /** \brief Provide a pointer to the vector of indices that represent the
182  * input source point cloud.
183  * \param[in] indices a pointer to the vector of indices
184  */
185  inline void
186  setIndicesSource (const IndicesPtr &indices)
187  {
188  setIndices (indices);
189  }
190 
191  /** \brief Get a pointer to the vector of indices used for the source dataset. */
192  inline IndicesPtr const
193  getIndicesSource () { return (indices_); }
194 
195  /** \brief Provide a pointer to the vector of indices that represent the input target point cloud.
196  * \param[in] indices a pointer to the vector of indices
197  */
198  inline void
199  setIndicesTarget (const IndicesPtr &indices)
200  {
201  target_cloud_updated_ = true;
202  target_indices_ = indices;
203  }
204 
205  /** \brief Get a pointer to the vector of indices used for the target dataset. */
206  inline IndicesPtr const
208 
209  /** \brief Provide a pointer to the search object used to find correspondences in
210  * the target cloud.
211  * \param[in] tree a pointer to the spatial search object.
212  * \param[in] force_no_recompute If set to true, this tree will NEVER be
213  * recomputed, regardless of calls to setInputTarget. Only use if you are
214  * confident that the tree will be set correctly.
215  */
216  inline void
218  bool force_no_recompute = false)
219  {
220  tree_ = tree;
221  if (force_no_recompute)
222  {
223  force_no_recompute_ = true;
224  }
225  // Since we just set a new tree, we need to check for updates
226  target_cloud_updated_ = true;
227  }
228 
229  /** \brief Get a pointer to the search method used to find correspondences in the
230  * target cloud. */
231  inline KdTreePtr
233  {
234  return (tree_);
235  }
236 
237  /** \brief Provide a pointer to the search object used to find correspondences in
238  * the source cloud (usually used by reciprocal correspondence finding).
239  * \param[in] tree a pointer to the spatial search object.
240  * \param[in] force_no_recompute If set to true, this tree will NEVER be
241  * recomputed, regardless of calls to setInputSource. Only use if you are
242  * extremely confident that the tree will be set correctly.
243  */
244  inline void
246  bool force_no_recompute = false)
247  {
248  tree_reciprocal_ = tree;
249  if ( force_no_recompute )
250  {
252  }
253  // Since we just set a new tree, we need to check for updates
254  source_cloud_updated_ = true;
255  }
256 
257  /** \brief Get a pointer to the search method used to find correspondences in the
258  * source cloud. */
259  inline KdTreeReciprocalPtr
261  {
262  return (tree_reciprocal_);
263  }
264 
265  /** \brief Determine the correspondences between input and target cloud.
266  * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
267  * \param[in] max_distance maximum allowed distance between correspondences
268  */
269  virtual void
271  double max_distance = std::numeric_limits<double>::max ()) = 0;
272 
273  /** \brief Determine the reciprocal correspondences between input and target cloud.
274  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
275  * correspondence, and Tgt_i has Src_i as one.
276  *
277  * \param[out] correspondences the found correspondences (index of query and target point, distance)
278  * \param[in] max_distance maximum allowed distance between correspondences
279  */
280  virtual void
282  double max_distance = std::numeric_limits<double>::max ()) = 0;
283 
284  /** \brief Provide a boost shared pointer to the PointRepresentation to be used
285  * when searching for nearest neighbors.
286  *
287  * \param[in] point_representation the PointRepresentation to be used by the
288  * k-D tree for nearest neighbor search
289  */
290  inline void
292  {
293  point_representation_ = point_representation;
294  }
295 
296  /** \brief Clone and cast to CorrespondenceEstimationBase */
297  virtual boost::shared_ptr< CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > clone () const = 0;
298 
299  protected:
300  /** \brief The correspondence estimation method name. */
301  std::string corr_name_;
302 
303  /** \brief A pointer to the spatial search object used for the target dataset. */
305 
306  /** \brief A pointer to the spatial search object used for the source dataset. */
308 
309 
310 
311  /** \brief The input point cloud dataset target. */
313 
314  /** \brief The target point cloud dataset indices. */
316 
317  /** \brief The point representation used (internal). */
319 
320  /** \brief The transformed input source point cloud dataset. */
322 
323  /** \brief The types of input point fields available. */
324  std::vector<pcl::PCLPointField> input_fields_;
325 
326  /** \brief Abstract class get name method. */
327  inline const std::string&
328  getClassName () const { return (corr_name_); }
329 
330  /** \brief Internal computation initalization. */
331  bool
332  initCompute ();
333 
334  /** \brief Internal computation initalization for reciprocal correspondences. */
335  bool
337 
338  /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
339  * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method
340  * is called. */
342  /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
343  * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method
344  * is called. */
346  /** \brief A flag which, if set, means the tree operating on the target cloud
347  * will never be recomputed*/
349 
350  /** \brief A flag which, if set, means the tree operating on the source cloud
351  * will never be recomputed*/
353 
354  };
355 
356  /** \brief @b CorrespondenceEstimation represents the base class for
357  * determining correspondences between target and query point
358  * sets/features.
359  *
360  * Code example:
361  *
362  * \code
363  * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target;
364  * // ... read or fill in source and target
365  * pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est;
366  * est.setInputSource (source);
367  * est.setInputTarget (target);
368  *
369  * pcl::Correspondences all_correspondences;
370  * // Determine all reciprocal correspondences
371  * est.determineReciprocalCorrespondences (all_correspondences);
372  * \endcode
373  *
374  * \author Radu B. Rusu, Michael Dixon, Dirk Holz
375  * \ingroup registration
376  */
377  template <typename PointSource, typename PointTarget, typename Scalar = float>
378  class CorrespondenceEstimation : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>
379  {
380  public:
381  typedef boost::shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar> > Ptr;
382  typedef boost::shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar> > ConstPtr;
383 
398 
401 
405 
409 
411 
412  /** \brief Empty constructor. */
414  {
415  corr_name_ = "CorrespondenceEstimation";
416  }
417 
418  /** \brief Empty destructor */
420 
421  /** \brief Determine the correspondences between input and target cloud.
422  * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
423  * \param[in] max_distance maximum allowed distance between correspondences
424  */
425  virtual void
427  double max_distance = std::numeric_limits<double>::max ());
428 
429  /** \brief Determine the reciprocal correspondences between input and target cloud.
430  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
431  * correspondence, and Tgt_i has Src_i as one.
432  *
433  * \param[out] correspondences the found correspondences (index of query and target point, distance)
434  * \param[in] max_distance maximum allowed distance between correspondences
435  */
436  virtual void
438  double max_distance = std::numeric_limits<double>::max ());
439 
440 
441  /** \brief Clone and cast to CorrespondenceEstimationBase */
442  virtual boost::shared_ptr< CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> >
443  clone () const
444  {
446  return (copy);
447  }
448  };
449  }
450 }
451 
452 #include <pcl/registration/impl/correspondence_estimation.hpp>
453 
454 #endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_ */
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
const std::string & getClassName() const
Abstract class get name method.
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
bool force_no_recompute_
A flag which, if set, means the tree operating on the target cloud will never be recomputed.
boost::shared_ptr< CorrespondenceEstimation< PointSource, PointTarget, Scalar > > Ptr
void setSearchMethodSource(const KdTreeReciprocalPtr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the source cloud (usually used...
IndicesPtr const getIndicesTarget()
Get a pointer to the vector of indices used for the target dataset.
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
bool source_cloud_updated_
Variable that stores whether we have a new source cloud, meaning we need to pre-process it again...
PointCloudTargetConstPtr target_
The input point cloud dataset target.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
std::string corr_name_
The correspondence estimation method name.
virtual void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the correspondences between input and target cloud.
pcl::search::KdTree< PointTarget >::Ptr KdTreePtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:153
PointCloudSourceConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) ...
PointRepresentationConstPtr point_representation_
The point representation used (internal).
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
boost::shared_ptr< KdTree< PointT > > Ptr
Definition: kdtree.h:71
KdTreePtr getSearchMethodTarget() const
Get a pointer to the search method used to find correspondences in the target cloud.
IndicesPtr const getIndicesSource()
Get a pointer to the vector of indices used for the source dataset.
std::vector< pcl::PCLPointField > input_fields_
The types of input point fields available.
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
void setPointRepresentation(const PointRepresentationConstPtr &point_representation)
Provide a boost shared pointer to the PointRepresentation to be used when searching for nearest neigh...
bool target_cloud_updated_
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again...
PointCloudSourceConstPtr const getInputCloud()
Get a pointer to the input point cloud dataset target.
IndicesPtr target_indices_
The target point cloud dataset indices.
bool initCompute()
Internal computation initalization.
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:79
virtual void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())=0
Determine the correspondences between input and target cloud.
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) ...
PointCloudTargetPtr input_transformed_
The transformed input source point cloud dataset.
boost::shared_ptr< const CorrespondenceEstimation< PointSource, PointTarget, Scalar > > ConstPtr
PointCloudTargetConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
KdTreePtr tree_
A pointer to the spatial search object used for the target dataset.
PCL base class.
Definition: pcl_base.h:68
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
virtual boost::shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > clone() const =0
Clone and cast to CorrespondenceEstimationBase.
bool initComputeReciprocal()
Internal computation initalization for reciprocal correspondences.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())=0
Determine the reciprocal correspondences between input and target cloud.
void setIndicesTarget(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represent the input target point cloud.
boost::shared_ptr< const PointRepresentation > PointRepresentationConstPtr
Definition: kdtree.h:68
virtual boost::shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > clone() const
Clone and cast to CorrespondenceEstimationBase.
bool force_no_recompute_reciprocal_
A flag which, if set, means the tree operating on the source cloud will never be recomputed.
void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
void setSearchMethodTarget(const KdTreePtr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the target cloud...
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the reciprocal correspondences between input and target cloud.
CorrespondenceEstimation represents the base class for determining correspondences between target and...
KdTreeReciprocalPtr getSearchMethodSource() const
Get a pointer to the search method used to find correspondences in the source cloud.
KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
void setIndicesSource(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represent the input source point cloud.
boost::shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
void getFields(const pcl::PointCloud< PointT > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
Definition: io.hpp:79
boost::shared_ptr< const PointRepresentation< PointTarget > > PointRepresentationConstPtr
Definition: kdtree.h:84
boost::shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object used for the source dataset.
Abstract CorrespondenceEstimationBase class.