Point Cloud Library (PCL)  1.7.1
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 (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::CorrespondenceEstimationBase::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.");
117 
118  /** \brief Get a pointer to the input point cloud dataset target. */
120  "[pcl::registration::CorrespondenceEstimationBase::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.");
121 
122  /** \brief Provide a pointer to the input source
123  * (e.g., the point cloud that we want to align to the target)
124  *
125  * \param[in] cloud the input point cloud source
126  */
127  inline void
129  {
130  source_cloud_updated_ = true;
132  pcl::getFields (*cloud, input_fields_);
133  }
134 
135  /** \brief Get a pointer to the input point cloud dataset target. */
136  inline PointCloudSourceConstPtr const
138  {
139  return (input_ );
140  }
141 
142  /** \brief Provide a pointer to the input target
143  * (e.g., the point cloud that we want to align the input source to)
144  * \param[in] cloud the input point cloud target
145  */
146  inline void
148 
149  /** \brief Get a pointer to the input point cloud dataset target. */
150  inline PointCloudTargetConstPtr const
151  getInputTarget () { return (target_ ); }
152 
153  /** \brief Provide a pointer to the vector of indices that represent the
154  * input source point cloud.
155  * \param[in] indices a pointer to the vector of indices
156  */
157  inline void
158  setIndicesSource (const IndicesPtr &indices)
159  {
160  setIndices (indices);
161  }
162 
163  /** \brief Get a pointer to the vector of indices used for the source dataset. */
164  inline IndicesPtr const
165  getIndicesSource () { return (indices_); }
166 
167  /** \brief Provide a pointer to the vector of indices that represent the input target point cloud.
168  * \param[in] indices a pointer to the vector of indices
169  */
170  inline void
171  setIndicesTarget (const IndicesPtr &indices)
172  {
173  target_cloud_updated_ = true;
174  target_indices_ = indices;
175  }
176 
177  /** \brief Get a pointer to the vector of indices used for the target dataset. */
178  inline IndicesPtr const
180 
181  /** \brief Provide a pointer to the search object used to find correspondences in
182  * the target cloud.
183  * \param[in] tree a pointer to the spatial search object.
184  * \param[in] force_no_recompute If set to true, this tree will NEVER be
185  * recomputed, regardless of calls to setInputTarget. Only use if you are
186  * confident that the tree will be set correctly.
187  */
188  inline void
190  bool force_no_recompute = false)
191  {
192  tree_ = tree;
193  if (force_no_recompute)
194  {
195  force_no_recompute_ = true;
196  }
197  // Since we just set a new tree, we need to check for updates
198  target_cloud_updated_ = true;
199  }
200 
201  /** \brief Get a pointer to the search method used to find correspondences in the
202  * target cloud. */
203  inline KdTreePtr
205  {
206  return (tree_);
207  }
208 
209  /** \brief Provide a pointer to the search object used to find correspondences in
210  * the source cloud (usually used by reciprocal correspondence finding).
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 setInputSource. Only use if you are
214  * extremely confident that the tree will be set correctly.
215  */
216  inline void
218  bool force_no_recompute = false)
219  {
220  tree_reciprocal_ = tree;
221  if ( force_no_recompute )
222  {
224  }
225  // Since we just set a new tree, we need to check for updates
226  source_cloud_updated_ = true;
227  }
228 
229  /** \brief Get a pointer to the search method used to find correspondences in the
230  * source cloud. */
231  inline KdTreeReciprocalPtr
233  {
234  return (tree_reciprocal_);
235  }
236 
237  /** \brief Determine the correspondences between input and target cloud.
238  * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
239  * \param[in] max_distance maximum allowed distance between correspondences
240  */
241  virtual void
243  double max_distance = std::numeric_limits<double>::max ()) = 0;
244 
245  /** \brief Determine the reciprocal correspondences between input and target cloud.
246  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
247  * correspondence, and Tgt_i has Src_i as one.
248  *
249  * \param[out] correspondences the found correspondences (index of query and target point, distance)
250  * \param[in] max_distance maximum allowed distance between correspondences
251  */
252  virtual void
254  double max_distance = std::numeric_limits<double>::max ()) = 0;
255 
256  /** \brief Provide a boost shared pointer to the PointRepresentation to be used
257  * when searching for nearest neighbors.
258  *
259  * \param[in] point_representation the PointRepresentation to be used by the
260  * k-D tree for nearest neighbor search
261  */
262  inline void
264  {
265  point_representation_ = point_representation;
266  }
267 
268  protected:
269  /** \brief The correspondence estimation method name. */
270  std::string corr_name_;
271 
272  /** \brief A pointer to the spatial search object used for the target dataset. */
274 
275  /** \brief A pointer to the spatial search object used for the source dataset. */
277 
278 
279 
280  /** \brief The input point cloud dataset target. */
282 
283  /** \brief The target point cloud dataset indices. */
285 
286  /** \brief The point representation used (internal). */
288 
289  /** \brief The transformed input source point cloud dataset. */
291 
292  /** \brief The types of input point fields available. */
293  std::vector<pcl::PCLPointField> input_fields_;
294 
295  /** \brief Abstract class get name method. */
296  inline const std::string&
297  getClassName () const { return (corr_name_); }
298 
299  /** \brief Internal computation initalization. */
300  bool
301  initCompute ();
302 
303  /** \brief Internal computation initalization for reciprocal correspondences. */
304  bool
306 
307  /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
308  * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method
309  * is called. */
311  /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
312  * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method
313  * is called. */
315  /** \brief A flag which, if set, means the tree operating on the target cloud
316  * will never be recomputed*/
318 
319  /** \brief A flag which, if set, means the tree operating on the source cloud
320  * will never be recomputed*/
322 
323  };
324 
325  /** \brief @b CorrespondenceEstimation represents the base class for
326  * determining correspondences between target and query point
327  * sets/features.
328  *
329  * Code example:
330  *
331  * \code
332  * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target;
333  * // ... read or fill in source and target
334  * pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est;
335  * est.setInputSource (source);
336  * est.setInputTarget (target);
337  *
338  * pcl::Correspondences all_correspondences;
339  * // Determine all reciprocal correspondences
340  * est.determineReciprocalCorrespondences (all_correspondences);
341  * \endcode
342  *
343  * \author Radu B. Rusu, Michael Dixon, Dirk Holz
344  * \ingroup registration
345  */
346  template <typename PointSource, typename PointTarget, typename Scalar = float>
347  class CorrespondenceEstimation : public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>
348  {
349  public:
350  typedef boost::shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar> > Ptr;
351  typedef boost::shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar> > ConstPtr;
352 
367 
370 
374 
378 
380 
381  /** \brief Empty constructor. */
383  {
384  corr_name_ = "CorrespondenceEstimation";
385  }
386 
387  /** \brief Empty destructor */
389 
390  /** \brief Determine the correspondences between input and target cloud.
391  * \param[out] correspondences the found correspondences (index of query point, index of target point, distance)
392  * \param[in] max_distance maximum allowed distance between correspondences
393  */
394  virtual void
396  double max_distance = std::numeric_limits<double>::max ());
397 
398  /** \brief Determine the reciprocal correspondences between input and target cloud.
399  * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
400  * correspondence, and Tgt_i has Src_i as one.
401  *
402  * \param[out] correspondences the found correspondences (index of query and target point, distance)
403  * \param[in] max_distance maximum allowed distance between correspondences
404  */
405  virtual void
407  double max_distance = std::numeric_limits<double>::max ());
408  };
409  }
410 }
411 
412 #include <pcl/registration/impl/correspondence_estimation.hpp>
413 
414 #endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_H_ */