Point Cloud Library (PCL)  1.10.1
correspondence_estimation.hpp
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 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
42 
43 #include <pcl/common/io.h>
44 #include <pcl/common/copy_point.h>
45 
46 ///////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointSource, typename PointTarget, typename Scalar> void
49  const PointCloudTargetConstPtr &cloud)
50 {
51  if (cloud->points.empty ())
52  {
53  PCL_ERROR ("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
54  return;
55  }
56  target_ = cloud;
57 
58  // Set the internal point representation of choice
59  if (point_representation_)
60  tree_->setPointRepresentation (point_representation_);
61 
62  target_cloud_updated_ = true;
63 }
64 
65 ///////////////////////////////////////////////////////////////////////////////////////////
66 template <typename PointSource, typename PointTarget, typename Scalar> bool
68 {
69  if (!target_)
70  {
71  PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
72  return (false);
73  }
74 
75  // Only update target kd-tree if a new target cloud was set
76  if (target_cloud_updated_ && !force_no_recompute_)
77  {
78  // If the target indices have been given via setIndicesTarget
79  if (target_indices_)
80  tree_->setInputCloud (target_, target_indices_);
81  else
82  tree_->setInputCloud (target_);
83 
84  target_cloud_updated_ = false;
85  }
86 
88 }
89 
90 ///////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointSource, typename PointTarget, typename Scalar> bool
93 {
94  // Only update source kd-tree if a new target cloud was set
95  if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
96  {
97  if (point_representation_)
98  tree_reciprocal_->setPointRepresentation (point_representation_);
99  // If the target indices have been given via setIndicesTarget
100  if (indices_)
101  tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource());
102  else
103  tree_reciprocal_->setInputCloud (getInputSource());
104 
105  source_cloud_updated_ = false;
106  }
107 
108  return (true);
109 }
110 
111 ///////////////////////////////////////////////////////////////////////////////////////////
112 template <typename PointSource, typename PointTarget, typename Scalar> void
114  pcl::Correspondences &correspondences, double max_distance)
115 {
116  if (!initCompute ())
117  return;
118 
119  double max_dist_sqr = max_distance * max_distance;
120 
121  correspondences.resize (indices_->size ());
122 
123  std::vector<int> index (1);
124  std::vector<float> distance (1);
125  pcl::Correspondence corr;
126  unsigned int nr_valid_correspondences = 0;
127 
128  // Check if the template types are the same. If true, avoid a copy.
129  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
130  if (isSamePointType<PointSource, PointTarget> ())
131  {
132  // Iterate over the input set of source indices
133  for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
134  {
135  tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
136  if (distance[0] > max_dist_sqr)
137  continue;
138 
139  corr.index_query = *idx;
140  corr.index_match = index[0];
141  corr.distance = distance[0];
142  correspondences[nr_valid_correspondences++] = corr;
143  }
144  }
145  else
146  {
147  PointTarget pt;
148 
149  // Iterate over the input set of source indices
150  for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
151  {
152  // Copy the source data to a target PointTarget format so we can search in the tree
153  copyPoint (input_->points[*idx], pt);
154 
155  tree_->nearestKSearch (pt, 1, index, distance);
156  if (distance[0] > max_dist_sqr)
157  continue;
158 
159  corr.index_query = *idx;
160  corr.index_match = index[0];
161  corr.distance = distance[0];
162  correspondences[nr_valid_correspondences++] = corr;
163  }
164  }
165  correspondences.resize (nr_valid_correspondences);
166  deinitCompute ();
167 }
168 
169 ///////////////////////////////////////////////////////////////////////////////////////////
170 template <typename PointSource, typename PointTarget, typename Scalar> void
172  pcl::Correspondences &correspondences, double max_distance)
173 {
174  if (!initCompute ())
175  return;
176 
177  // setup tree for reciprocal search
178  // Set the internal point representation of choice
179  if (!initComputeReciprocal())
180  return;
181  double max_dist_sqr = max_distance * max_distance;
182 
183  correspondences.resize (indices_->size());
184  std::vector<int> index (1);
185  std::vector<float> distance (1);
186  std::vector<int> index_reciprocal (1);
187  std::vector<float> distance_reciprocal (1);
188  pcl::Correspondence corr;
189  unsigned int nr_valid_correspondences = 0;
190  int target_idx = 0;
191 
192  // Check if the template types are the same. If true, avoid a copy.
193  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
194  if (isSamePointType<PointSource, PointTarget> ())
195  {
196  // Iterate over the input set of source indices
197  for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
198  {
199  tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
200  if (distance[0] > max_dist_sqr)
201  continue;
202 
203  target_idx = index[0];
204 
205  tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
206  if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
207  continue;
208 
209  corr.index_query = *idx;
210  corr.index_match = index[0];
211  corr.distance = distance[0];
212  correspondences[nr_valid_correspondences++] = corr;
213  }
214  }
215  else
216  {
217  PointTarget pt_src;
218  PointSource pt_tgt;
219 
220  // Iterate over the input set of source indices
221  for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
222  {
223  // Copy the source data to a target PointTarget format so we can search in the tree
224  copyPoint (input_->points[*idx], pt_src);
225 
226  tree_->nearestKSearch (pt_src, 1, index, distance);
227  if (distance[0] > max_dist_sqr)
228  continue;
229 
230  target_idx = index[0];
231 
232  // Copy the target data to a target PointSource format so we can search in the tree_reciprocal
233  copyPoint (target_->points[target_idx], pt_tgt);
234 
235  tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
236  if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
237  continue;
238 
239  corr.index_query = *idx;
240  corr.index_match = index[0];
241  corr.distance = distance[0];
242  correspondences[nr_valid_correspondences++] = corr;
243  }
244  }
245  correspondences.resize (nr_valid_correspondences);
246  deinitCompute ();
247 }
248 
249 //#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation<T,U>;
250 
251 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */
pcl::Correspondence::index_match
int index_match
Index of the matching (target) point.
Definition: correspondence.h:64
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::Correspondence::distance
float distance
Definition: correspondence.h:68
pcl::registration::CorrespondenceEstimationBase::setInputTarget
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...
Definition: correspondence_estimation.hpp:48
pcl::registration::CorrespondenceEstimationBase::initCompute
bool initCompute()
Internal computation initialization.
Definition: correspondence_estimation.hpp:67
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:69
pcl::registration::CorrespondenceEstimationBase< PointSource, PointTarget, float >::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: correspondence_estimation.h:86
pcl::registration::CorrespondenceEstimationBase::initComputeReciprocal
bool initComputeReciprocal()
Internal computation initialization for reciprocal correspondences.
Definition: correspondence_estimation.hpp:92
pcl::copyPoint
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:138
pcl::registration::CorrespondenceEstimation::determineCorrespondences
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the correspondences between input and target cloud.
Definition: correspondence_estimation.hpp:113
pcl::Correspondence::index_query
int index_query
Index of the query (source) point.
Definition: correspondence.h:62
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:88
pcl::registration::CorrespondenceEstimation::determineReciprocalCorrespondences
void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the reciprocal correspondences between input and target cloud.
Definition: correspondence_estimation.hpp:171
pcl::Correspondence
Correspondence represents a match between two entities (e.g., points, descriptors,...
Definition: correspondence.h:59