Point Cloud Library (PCL)  1.7.0
correspondence_rejection.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_REJECTION_H_
42 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_
43 
44 #include <pcl/registration/correspondence_types.h>
45 #include <pcl/registration/correspondence_sorting.h>
46 #include <pcl/console/print.h>
47 #include <pcl/common/transforms.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/search/kdtree.h>
50 
51 namespace pcl
52 {
53  namespace registration
54  {
55  /** @b CorrespondenceRejector represents the base class for correspondence rejection methods
56  * \author Dirk Holz
57  * \ingroup registration
58  */
60  {
61  public:
62  typedef boost::shared_ptr<CorrespondenceRejector> Ptr;
63  typedef boost::shared_ptr<const CorrespondenceRejector> ConstPtr;
64 
65  /** \brief Empty constructor. */
67  : rejection_name_ ()
69  {}
70 
71  /** \brief Empty destructor. */
73 
74  /** \brief Provide a pointer to the vector of the input correspondences.
75  * \param[in] correspondences the const boost shared pointer to a correspondence vector
76  */
77  virtual inline void
79  {
80  input_correspondences_ = correspondences;
81  };
82 
83  /** \brief Get a pointer to the vector of the input correspondences.
84  * \return correspondences the const boost shared pointer to a correspondence vector
85  */
88 
89  /** \brief Run correspondence rejection
90  * \param[out] correspondences Vector of correspondences that have not been rejected.
91  */
92  inline void
94  {
96  return;
97 
98  applyRejection (correspondences);
99  }
100 
101  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
102  * Pure virtual. Compared to \a getCorrespondences this function is
103  * stateless, i.e., input correspondences do not need to be provided beforehand,
104  * but are directly provided in the function call.
105  * \param[in] original_correspondences the set of initial correspondences given
106  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
107  */
108  virtual inline void
109  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
110  pcl::Correspondences& remaining_correspondences) = 0;
111 
112  /** \brief Determine the indices of query points of
113  * correspondences that have been rejected, i.e., the difference
114  * between the input correspondences (set via \a setInputCorrespondences)
115  * and the given correspondence vector.
116  * \param[in] correspondences Vector of correspondences after rejection
117  * \param[out] indices Vector of query point indices of those correspondences
118  * that have been rejected.
119  */
120  inline void
122  std::vector<int>& indices)
123  {
125  {
126  PCL_WARN ("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences not set (lookup of rejected correspondences _not_ possible).\n", getClassName ().c_str ());
127  return;
128  }
129 
130  pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices);
131  }
132 
133  /** \brief Get a string representation of the name of this class. */
134  inline const std::string&
135  getClassName () const { return (rejection_name_); }
136 
137  protected:
138 
139  /** \brief The name of the rejection method. */
140  std::string rejection_name_;
141 
142  /** \brief The input correspondences. */
144 
145  /** \brief Abstract rejection method. */
146  virtual void
147  applyRejection (Correspondences &correspondences) = 0;
148  };
149 
150  /** @b DataContainerInterface provides a generic interface for computing correspondence scores between correspondent
151  * points in the input and target clouds
152  * \ingroup registration
153  */
155  {
156  public:
158  virtual double getCorrespondenceScore (int index) = 0;
159  virtual double getCorrespondenceScore (const pcl::Correspondence &) = 0;
160  };
161 
162  /** @b DataContainer is a container for the input and target point clouds and implements the interface
163  * to compute correspondence scores between correspondent points in the input and target clouds
164  * \ingroup registration
165  */
166  template <typename PointT, typename NormalT = pcl::PointNormal>
168  {
170  typedef typename PointCloud::Ptr PointCloudPtr;
171  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
172 
173  typedef typename pcl::search::KdTree<PointT>::Ptr KdTreePtr;
174 
176  typedef typename Normals::Ptr NormalsPtr;
177  typedef typename Normals::ConstPtr NormalsConstPtr;
178 
179  public:
180 
181  /** \brief Empty constructor. */
182  DataContainer (bool needs_normals = false)
183  : input_ ()
184  , input_transformed_ ()
185  , target_ ()
186  , input_normals_ ()
187  , input_normals_transformed_ ()
188  , target_normals_ ()
189  , tree_ (new pcl::search::KdTree<PointT>)
190  , class_name_ ("DataContainer")
191  , needs_normals_ (needs_normals)
192  , target_cloud_updated_ (true)
193  , force_no_recompute_ (false)
194  {
195  }
196 
197  /** \brief Empty destructor */
198  virtual ~DataContainer () {}
199 
200  /** \brief Provide a source point cloud dataset (must contain XYZ
201  * data!), used to compute the correspondence distance.
202  * \param[in] cloud a cloud containing XYZ data
203  */
204  PCL_DEPRECATED (void setInputCloud (const PointCloudConstPtr &cloud), "[pcl::registration::DataContainer::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.");
205 
206  /** \brief Get a pointer to the input point cloud dataset target. */
207  PCL_DEPRECATED (PointCloudConstPtr const getInputCloud (), "[pcl::registration::DataContainer::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.");
208 
209  /** \brief Provide a source point cloud dataset (must contain XYZ
210  * data!), used to compute the correspondence distance.
211  * \param[in] cloud a cloud containing XYZ data
212  */
213  inline void
214  setInputSource (const PointCloudConstPtr &cloud)
215  {
216  input_ = cloud;
217  }
218 
219  /** \brief Get a pointer to the input point cloud dataset target. */
220  inline PointCloudConstPtr const
221  getInputSource () { return (input_); }
222 
223  /** \brief Provide a target point cloud dataset (must contain XYZ
224  * data!), used to compute the correspondence distance.
225  * \param[in] target a cloud containing XYZ data
226  */
227  inline void
228  setInputTarget (const PointCloudConstPtr &target)
229  {
230  target_ = target;
231  target_cloud_updated_ = true;
232  }
233 
234  /** \brief Get a pointer to the input point cloud dataset target. */
235  inline PointCloudConstPtr const
236  getInputTarget () { return (target_); }
237 
238  /** \brief Provide a pointer to the search object used to find correspondences in
239  * the target cloud.
240  * \param[in] tree a pointer to the spatial search object.
241  * \param[in] force_no_recompute If set to true, this tree will NEVER be
242  * recomputed, regardless of calls to setInputTarget. Only use if you are
243  * confident that the tree will be set correctly.
244  */
245  inline void
246  setSearchMethodTarget (const KdTreePtr &tree,
247  bool force_no_recompute = false)
248  {
249  tree_ = tree;
250  if (force_no_recompute)
251  {
252  force_no_recompute_ = true;
253  }
254  target_cloud_updated_ = true;
255  }
256 
257  /** \brief Set the normals computed on the input point cloud
258  * \param[in] normals the normals computed for the input cloud
259  */
260  inline void
261  setInputNormals (const NormalsConstPtr &normals) { input_normals_ = normals; }
262 
263  /** \brief Get the normals computed on the input point cloud */
264  inline NormalsConstPtr
265  getInputNormals () { return (input_normals_); }
266 
267  /** \brief Set the normals computed on the target point cloud
268  * \param[in] normals the normals computed for the input cloud
269  */
270  inline void
271  setTargetNormals (const NormalsConstPtr &normals) { target_normals_ = normals; }
272 
273  /** \brief Get the normals computed on the target point cloud */
274  inline NormalsConstPtr
275  getTargetNormals () { return (target_normals_); }
276 
277  /** \brief Get the correspondence score for a point in the input cloud
278  * \param[in] index index of the point in the input cloud
279  */
280  inline double
282  {
283  if ( target_cloud_updated_ && !force_no_recompute_ )
284  {
285  tree_->setInputCloud (target_);
286  }
287  std::vector<int> indices (1);
288  std::vector<float> distances (1);
289  if (tree_->nearestKSearch (input_->points[index], 1, indices, distances))
290  return (distances[0]);
291  else
292  return (std::numeric_limits<double>::max ());
293  }
294 
295  /** \brief Get the correspondence score for a given pair of correspondent points
296  * \param[in] corr Correspondent points
297  */
298  inline double
300  {
301  // Get the source and the target feature from the list
302  const PointT &src = input_->points[corr.index_query];
303  const PointT &tgt = target_->points[corr.index_match];
304 
305  return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ());
306  }
307 
308  /** \brief Get the correspondence score for a given pair of correspondent points based on
309  * the angle betweeen the normals. The normmals for the in put and target clouds must be
310  * set before using this function
311  * \param[in] corr Correspondent points
312  */
313  inline double
315  {
316  //assert ( (input_normals_->points.size () != 0) && (target_normals_->points.size () != 0) && "Normals are not set for the input and target point clouds");
317  assert (input_normals_ && target_normals_ && "Normals are not set for the input and target point clouds");
318  const NormalT &src = input_normals_->points[corr.index_query];
319  const NormalT &tgt = target_normals_->points[corr.index_match];
320  return (double ((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) + (src.normal[2] * tgt.normal[2])));
321  }
322 
323  private:
324  /** \brief The input point cloud dataset */
325  PointCloudConstPtr input_;
326 
327  /** \brief The input transformed point cloud dataset */
328  PointCloudPtr input_transformed_;
329 
330  /** \brief The target point cloud datase. */
331  PointCloudConstPtr target_;
332 
333  /** \brief Normals to the input point cloud */
334  NormalsConstPtr input_normals_;
335 
336  /** \brief Normals to the input point cloud */
337  NormalsPtr input_normals_transformed_;
338 
339  /** \brief Normals to the target point cloud */
340  NormalsConstPtr target_normals_;
341 
342  /** \brief A pointer to the spatial search object. */
343  KdTreePtr tree_;
344 
345  /** \brief The name of the rejection method. */
346  std::string class_name_;
347 
348  /** \brief Should the current data container use normals? */
349  bool needs_normals_;
350 
351  /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
352  * This way, we avoid rebuilding the kd-tree */
353  bool target_cloud_updated_;
354 
355  /** \brief A flag which, if set, means the tree operating on the target cloud
356  * will never be recomputed*/
357  bool force_no_recompute_;
358 
359 
360 
361  /** \brief Get a string representation of the name of this class. */
362  inline const std::string&
363  getClassName () const { return (class_name_); }
364  };
365  }
366 }
367 
368 #include <pcl/registration/impl/correspondence_rejection.hpp>
369 
370 #endif /* PCL_REGISTRATION_CORRESPONDENCE_REJECTION_H_ */
371