Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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#pragma once
42
43#include <pcl/common/io.h> // for getFields
44#include <pcl/registration/correspondence_types.h>
45#include <pcl/search/kdtree.h>
46#include <pcl/memory.h>
47#include <pcl/pcl_base.h>
48#include <pcl/pcl_macros.h>
49
50#include <string>
51
52namespace pcl {
53namespace registration {
54/** \brief Abstract @b CorrespondenceEstimationBase class.
55 * All correspondence estimation methods should inherit from this.
56 * \author Radu B. Rusu
57 * \ingroup registration
58 */
59template <typename PointSource, typename PointTarget, typename Scalar = float>
60class CorrespondenceEstimationBase : public PCLBase<PointSource> {
61public:
62 using Ptr =
63 shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
64 using ConstPtr =
65 shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
66
67 // using PCLBase<PointSource>::initCompute;
68 using PCLBase<PointSource>::deinitCompute;
69 using PCLBase<PointSource>::input_;
70 using PCLBase<PointSource>::indices_;
71 using PCLBase<PointSource>::setIndices;
72
74 using KdTreePtr = typename KdTree::Ptr;
76
80
84
88
92
93 /** \brief Empty constructor. */
95 : corr_name_("CorrespondenceEstimationBase")
96 , tree_(new pcl::search::KdTree<PointTarget>)
97 , tree_reciprocal_(new pcl::search::KdTree<PointSource>)
98 , target_()
101 {}
102
103 /** \brief Empty destructor */
104 ~CorrespondenceEstimationBase() override = default;
105
106 /** \brief Provide a pointer to the input source
107 * (e.g., the point cloud that we want to align to the target)
108 *
109 * \param[in] cloud the input point cloud source
110 */
111 inline void
113 {
116 input_fields_ = pcl::getFields<PointSource>();
117 }
118
119 /** \brief Get a pointer to the input point cloud dataset target. */
120 inline PointCloudSourceConstPtr const
122 {
123 return (input_);
124 }
125
126 /** \brief Provide a pointer to the input target
127 * (e.g., the point cloud that we want to align the input source to)
128 * \param[in] cloud the input point cloud target
129 */
130 inline void
132
133 /** \brief Get a pointer to the input point cloud dataset target. */
134 inline PointCloudTargetConstPtr const
136 {
137 return (target_);
138 }
139
140 /** \brief Set the number of threads to use.
141 * \param nr_threads the number of hardware threads to use (0 sets the value back to
142 * automatic)
143 */
144 void
145 setNumberOfThreads(unsigned int nr_threads)
146 {
147#ifdef _OPENMP
148 num_threads_ = nr_threads != 0 ? nr_threads : omp_get_num_procs();
149#else
150 if (nr_threads != 1) {
151 PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1\n");
152 }
153 num_threads_ = 1;
154#endif
155 }
156
157 /** \brief See if this rejector requires source normals */
158 virtual bool
160 {
161 return (false);
162 }
163
164 /** \brief Abstract method for setting the source normals */
165 virtual void
167 {
168 PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
169 "input source normals\n",
170 getClassName().c_str());
171 }
172
173 /** \brief See if this rejector requires target normals */
174 virtual bool
176 {
177 return (false);
178 }
179
180 /** \brief Abstract method for setting the target normals */
181 virtual void
183 {
184 PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
185 "input target normals\n",
186 getClassName().c_str());
187 }
188
189 /** \brief Provide a pointer to the vector of indices that represent the
190 * input source point cloud.
191 * \param[in] indices a pointer to the vector of indices
192 */
193 inline void
195 {
196 setIndices(indices);
197 }
198
199 /** \brief Get a pointer to the vector of indices used for the source dataset. */
200 inline IndicesPtr const
202 {
203 return (indices_);
204 }
205
206 /** \brief Provide a pointer to the vector of indices that represent the input target
207 * point cloud. \param[in] indices a pointer to the vector of indices
208 */
209 inline void
211 {
213 target_indices_ = indices;
214 }
215
216 /** \brief Get a pointer to the vector of indices used for the target dataset. */
217 inline IndicesPtr const
219 {
220 return (target_indices_);
221 }
222
223 /** \brief Provide a pointer to the search object used to find correspondences in
224 * the target cloud.
225 * \param[in] tree a pointer to the spatial search object.
226 * \param[in] force_no_recompute If set to true, this tree will NEVER be
227 * recomputed, regardless of calls to setInputTarget. Only use if you are
228 * confident that the tree will be set correctly.
229 */
230 inline void
231 setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
232 {
233 tree_ = tree;
234 force_no_recompute_ = force_no_recompute;
235 // Since we just set a new tree, we need to check for updates
237 }
238
239 /** \brief Get a pointer to the search method used to find correspondences in the
240 * target cloud. */
241 inline KdTreePtr
243 {
244 return (tree_);
245 }
246
247 /** \brief Provide a pointer to the search object used to find correspondences in
248 * the source cloud (usually used by reciprocal correspondence finding).
249 * \param[in] tree a pointer to the spatial search object.
250 * \param[in] force_no_recompute If set to true, this tree will NEVER be
251 * recomputed, regardless of calls to setInputSource. Only use if you are
252 * extremely confident that the tree will be set correctly.
253 */
254 inline void
256 bool force_no_recompute = false)
257 {
258 tree_reciprocal_ = tree;
259 force_no_recompute_reciprocal_ = force_no_recompute;
260 // Since we just set a new tree, we need to check for updates
262 }
263
264 /** \brief Get a pointer to the search method used to find correspondences in the
265 * source cloud. */
268 {
269 return (tree_reciprocal_);
270 }
271
272 /** \brief Determine the correspondences between input and target cloud.
273 * \param[out] correspondences the found correspondences (index of query point, index
274 * of target point, distance) \param[in] max_distance maximum allowed distance between
275 * correspondences
276 */
277 virtual void
279 pcl::Correspondences& correspondences,
280 double max_distance = std::numeric_limits<double>::max()) = 0;
281
282 /** \brief Determine the reciprocal correspondences between input and target cloud.
283 * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
284 * correspondence, and Tgt_i has Src_i as one.
285 *
286 * \param[out] correspondences the found correspondences (index of query and target
287 * point, distance) \param[in] max_distance maximum allowed distance between
288 * correspondences
289 */
290 virtual void
292 pcl::Correspondences& correspondences,
293 double max_distance = std::numeric_limits<double>::max()) = 0;
294
295 /** \brief Provide a boost shared pointer to the PointRepresentation for target cloud
296 * to be used when searching for nearest neighbors.
297 *
298 * \param[in] point_representation the PointRepresentation to be used by the
299 * k-D tree for nearest neighbor search
300 */
301 inline void
303 {
304 point_representation_ = point_representation;
305 }
306
307 /** \brief Provide a boost shared pointer to the PointRepresentation for source cloud
308 * to be used when searching for nearest neighbors.
309 *
310 * \param[in] point_representation the PointRepresentation to be used by the
311 * k-D tree for nearest neighbor search
312 */
313 inline void
315 const PointRepresentationReciprocalConstPtr& point_representation_reciprocal)
316 {
317 point_representation_reciprocal_ = point_representation_reciprocal;
318 }
319
320 /** \brief Clone and cast to CorrespondenceEstimationBase */
322 clone() const = 0;
323
324protected:
325 /** \brief The correspondence estimation method name. */
326 std::string corr_name_;
327
328 /** \brief A pointer to the spatial search object used for the target dataset. */
330
331 /** \brief A pointer to the spatial search object used for the source dataset. */
333
334 /** \brief The input point cloud dataset target. */
336
337 /** \brief The target point cloud dataset indices. */
339
340 /** \brief The target point representation used (internal). */
342
343 /** \brief The source point representation used (internal). */
345
346 /** \brief The transformed input source point cloud dataset. */
348
349 /** \brief The types of input point fields available. */
350 std::vector<pcl::PCLPointField> input_fields_;
351
352 /** \brief Abstract class get name method. */
353 inline const std::string&
355 {
356 return (corr_name_);
357 }
358
359 /** \brief Internal computation initialization. */
360 bool
361 initCompute();
362
363 /** \brief Internal computation initialization for reciprocal correspondences. */
364 bool
366
367 /** \brief Variable that stores whether we have a new target cloud, meaning we need to
368 * pre-process it again. This way, we avoid rebuilding the kd-tree for the target
369 * cloud every time the determineCorrespondences () method is called. */
371 /** \brief Variable that stores whether we have a new source cloud, meaning we need to
372 * pre-process it again. This way, we avoid rebuilding the reciprocal kd-tree for the
373 * source cloud every time the determineCorrespondences () method is called. */
375 /** \brief A flag which, if set, means the tree operating on the target cloud
376 * will never be recomputed*/
378
379 /** \brief A flag which, if set, means the tree operating on the source cloud
380 * will never be recomputed*/
382
383 unsigned int num_threads_{1};
384};
385
386/** \brief @b CorrespondenceEstimation represents a simple class for
387 * determining correspondences between target and query point
388 * sets/features, using nearest neighbor search.
389 *
390 * Code example:
391 *
392 * \code
393 * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target;
394 * // ... read or fill in source and target
395 * pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est;
396 * est.setInputSource (source);
397 * est.setInputTarget (target);
398 *
399 * pcl::Correspondences all_correspondences;
400 * // Determine all reciprocal correspondences
401 * est.determineReciprocalCorrespondences (all_correspondences);
402 * \endcode
403 *
404 * \author Radu B. Rusu, Michael Dixon, Dirk Holz
405 * \ingroup registration
406 */
407template <typename PointSource, typename PointTarget, typename Scalar = float>
409: public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
410public:
411 using Ptr = shared_ptr<CorrespondenceEstimation<PointSource, PointTarget, Scalar>>;
412 using ConstPtr =
413 shared_ptr<const CorrespondenceEstimation<PointSource, PointTarget, Scalar>>;
414
415 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
417 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
419 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::tree_;
420 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
422 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
423 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::corr_name_;
424 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
425 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
426 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
427 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
429 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_;
430 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::indices_;
431 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::input_fields_;
432 using PCLBase<PointSource>::deinitCompute;
433
434 using KdTree =
436 using KdTreePtr = typename CorrespondenceEstimationBase<PointSource,
437 PointTarget,
438 Scalar>::KdTreePtr;
439 using KdTreeConstPtr = typename CorrespondenceEstimationBase<PointSource,
440 PointTarget,
441 Scalar>::KdTreeConstPtr;
448
452
456
460
463 typename KdTreeReciprocal::PointRepresentationConstPtr;
464
465 /** \brief Empty constructor. */
466 CorrespondenceEstimation() { corr_name_ = "CorrespondenceEstimation"; }
467
468 /** \brief Empty destructor */
469 ~CorrespondenceEstimation() override = default;
470
471 /** \brief Determine the correspondences between input and target cloud.
472 * \param[out] correspondences the found correspondences (index of query point, index
473 * of target point, distance) \param[in] max_distance maximum allowed distance between
474 * correspondences
475 */
476 void
478 pcl::Correspondences& correspondences,
479 double max_distance = std::numeric_limits<double>::max()) override;
480
481 /** \brief Determine the reciprocal correspondences between input and target cloud.
482 * A correspondence is considered reciprocal if both Src_i has Tgt_i as a
483 * correspondence, and Tgt_i has Src_i as one.
484 *
485 * \param[out] correspondences the found correspondences (index of query and target
486 * point, distance) \param[in] max_distance maximum allowed distance between
487 * correspondences
488 */
489 void
491 pcl::Correspondences& correspondences,
492 double max_distance = std::numeric_limits<double>::max()) override;
493
494 /** \brief Clone and cast to CorrespondenceEstimationBase */
496 clone() const override
497 {
499 return (copy);
500 }
501
502protected:
503 using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::num_threads_;
504};
505} // namespace registration
506} // namespace pcl
507
508#include <pcl/registration/impl/correspondence_estimation.hpp>
typename PointRepresentation::ConstPtr PointRepresentationConstPtr
Definition kdtree.h:66
PCL base class.
Definition pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition pcl_base.hpp:175
shared_ptr< PointCloud< PointSource > > Ptr
shared_ptr< const PointCloud< PointSource > > ConstPtr
Abstract CorrespondenceEstimationBase class.
void setPointRepresentationReciprocal(const PointRepresentationReciprocalConstPtr &point_representation_reciprocal)
Provide a boost shared pointer to the PointRepresentation for source cloud to be used when searching ...
typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
PointCloudTargetPtr input_transformed_
The transformed input source point cloud dataset.
void setNumberOfThreads(unsigned int nr_threads)
Set the number of threads to use.
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...
PointCloudTargetConstPtr target_
The input point cloud dataset target.
PointCloudTargetConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
bool initCompute()
Internal computation initialization.
virtual CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const =0
Clone and cast to CorrespondenceEstimationBase.
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())=0
Determine the reciprocal correspondences between input and target cloud.
IndicesPtr const getIndicesTarget()
Get a pointer to the vector of indices used for the target dataset.
void setPointRepresentation(const PointRepresentationConstPtr &point_representation)
Provide a boost shared pointer to the PointRepresentation for target cloud to be used when searching ...
bool source_cloud_updated_
Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
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 setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
PointRepresentationReciprocalConstPtr point_representation_reciprocal_
The source point representation used (internal).
typename KdTreeReciprocal::PointRepresentationConstPtr PointRepresentationReciprocalConstPtr
KdTreeReciprocalPtr getSearchMethodSource() const
Get a pointer to the search method used to find correspondences in the source cloud.
PointRepresentationConstPtr point_representation_
The target point representation used (internal).
IndicesPtr const getIndicesSource()
Get a pointer to the vector of indices used for the source dataset.
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)
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
std::vector< pcl::PCLPointField > input_fields_
The types of input point fields available.
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
std::string corr_name_
The correspondence estimation method name.
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...
const std::string & getClassName() const
Abstract class get name method.
bool target_cloud_updated_
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
void setIndicesSource(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represent the input source point cloud.
KdTreePtr tree_
A pointer to the spatial search object used for the target dataset.
PointCloudSourceConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object used for the source dataset.
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
typename KdTreeReciprocal::ConstPtr KdTreeReciprocalConstPtr
virtual void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())=0
Determine the correspondences between input and target cloud.
~CorrespondenceEstimationBase() override=default
Empty destructor.
bool initComputeReciprocal()
Internal computation initialization for reciprocal correspondences.
bool force_no_recompute_reciprocal_
A flag which, if set, means the tree operating on the source cloud will never be recomputed.
IndicesPtr target_indices_
The target point cloud dataset indices.
void setIndicesTarget(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represent the input target point cloud.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
bool force_no_recompute_
A flag which, if set, means the tree operating on the target cloud will never be recomputed.
KdTreePtr getSearchMethodTarget() const
Get a pointer to the search method used to find correspondences in the target cloud.
CorrespondenceEstimation represents a simple class for determining correspondences between target and...
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeReciprocalPtr KdTreeReciprocalPtr
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the correspondences between input and target cloud.
typename KdTreeReciprocal::PointRepresentationConstPtr PointRepresentationReciprocalConstPtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeReciprocalConstPtr KdTreeReciprocalConstPtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTree KdTree
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeConstPtr KdTreeConstPtr
shared_ptr< CorrespondenceEstimation< PointSource, PointTarget, Scalar > > Ptr
void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the reciprocal correspondences between input and target cloud.
shared_ptr< const CorrespondenceEstimation< PointSource, PointTarget, Scalar > > ConstPtr
typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
~CorrespondenceEstimation() override=default
Empty destructor.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreePtr KdTreePtr
CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const override
Clone and cast to CorrespondenceEstimationBase.
typename CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::KdTreeReciprocal KdTreeReciprocal
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition kdtree.h:75
shared_ptr< const KdTree< PointT, Tree > > ConstPtr
Definition kdtree.h:76
typename PointRepresentation< PointT >::ConstPtr PointRepresentationConstPtr
Definition kdtree.h:80
Defines functions, macros and traits for allocating and using memory.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
Defines all the PCL and non-PCL macros used.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr