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 
A point structure representing normal coordinates and the surface curvature estimate.
DataContainer is a container for the input and target point clouds and implements the interface to co...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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...
int index_match
Index of the matching (target) point.
DataContainerInterface provides a generic interface for computing correspondence scores between corre...
boost::shared_ptr< const CorrespondenceRejector > ConstPtr
virtual void setInputCorrespondences(const CorrespondencesConstPtr &correspondences)
Provide a pointer to the vector of the input correspondences.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
CorrespondenceRejector represents the base class for correspondence rejection methods ...
double getCorrespondenceScoreFromNormals(const pcl::Correspondence &corr)
Get the correspondence score for a given pair of correspondent points based on the angle betweeen the...
virtual void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences)=0
Get a list of valid correspondences after rejection from the original set of correspondences.
void getCorrespondences(pcl::Correspondences &correspondences)
Run correspondence rejection.
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
DataContainer(bool needs_normals=false)
Empty constructor.
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
virtual double getCorrespondenceScore(int index)=0
const std::string & getClassName() const
Get a string representation of the name of this class.
int index_query
Index of the query (source) point.
CorrespondencesConstPtr getInputCorrespondences()
Get a pointer to the vector of the input correspondences.
void setInputNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
boost::shared_ptr< const Correspondences > CorrespondencesConstPtr
PCL_DEPRECATED(void setInputCloud(const PointCloudConstPtr &cloud),"[pcl::registration::DataContainer::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.")
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
boost::shared_ptr< KdTree< PointT > > Ptr
Definition: kdtree.h:79
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void getRejectedQueryIndices(const pcl::Correspondences &correspondences_before, const pcl::Correspondences &correspondences_after, std::vector< int > &indices, bool presorting_required=true)
Get the query points of correspondences that are present in one correspondence vector but not in the ...
NormalsConstPtr getInputNormals()
Get the normals computed on the input point cloud.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
void setInputTarget(const PointCloudConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
double getCorrespondenceScore(const pcl::Correspondence &corr)
Get the correspondence score for a given pair of correspondent points.
std::string rejection_name_
The name of the rejection method.
void getRejectedQueryIndices(const pcl::Correspondences &correspondences, std::vector< int > &indices)
Determine the indices of query points of correspondences that have been rejected, i...
virtual void applyRejection(Correspondences &correspondences)=0
Abstract rejection method.
void setTargetNormals(const NormalsConstPtr &normals)
Set the normals computed on the target point cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
double getCorrespondenceScore(int index)
Get the correspondence score for a point in the input cloud.
boost::shared_ptr< CorrespondenceRejector > Ptr
NormalsConstPtr getTargetNormals()
Get the normals computed on the target point cloud.
virtual ~DataContainer()
Empty destructor.
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:55
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.