Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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#pragma once
42
43#include <pcl/console/print.h>
44#include <pcl/registration/correspondence_sorting.h>
45#include <pcl/registration/correspondence_types.h>
46#include <pcl/search/kdtree.h>
47#include <pcl/point_cloud.h>
48
49namespace pcl {
50namespace registration {
51/** @b CorrespondenceRejector represents the base class for correspondence rejection
52 * methods \author Dirk Holz \ingroup registration
53 */
55public:
56 using Ptr = shared_ptr<CorrespondenceRejector>;
57 using ConstPtr = shared_ptr<const CorrespondenceRejector>;
58
59 /** \brief Empty constructor. */
61
62 /** \brief Empty destructor. */
63 virtual ~CorrespondenceRejector() = default;
64
65 /** \brief Provide a pointer to the vector of the input correspondences.
66 * \param[in] correspondences the const shared pointer to a correspondence vector
67 */
68 virtual inline void
70 {
71 input_correspondences_ = correspondences;
72 };
73
74 /** \brief Get a pointer to the vector of the input correspondences.
75 * \return correspondences the const shared pointer to a correspondence vector
76 */
82
83 /** \brief Run correspondence rejection
84 * \param[out] correspondences Vector of correspondences that have not been rejected.
85 */
86 inline void
88 {
90 return;
91
92 applyRejection(correspondences);
93 }
94
95 /** \brief Get a list of valid correspondences after rejection from the original set
96 * of correspondences. Pure virtual. Compared to \a getCorrespondences this function
97 * is stateless, i.e., input correspondences do not need to be provided beforehand,
98 * but are directly provided in the function call.
99 * \param[in] original_correspondences the set of initial correspondences given
100 * \param[out] remaining_correspondences the resultant filtered set of remaining
101 * correspondences
102 */
103 virtual inline void
104 getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
105 pcl::Correspondences& remaining_correspondences) = 0;
106
107 /** \brief Determine the indices of query points of
108 * correspondences that have been rejected, i.e., the difference
109 * between the input correspondences (set via \a setInputCorrespondences)
110 * and the given correspondence vector.
111 * \param[in] correspondences Vector of correspondences after rejection
112 * \param[out] indices Vector of query point indices of those correspondences
113 * that have been rejected.
114 */
115 inline void
117 pcl::Indices& indices)
118 {
120 PCL_WARN("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences "
121 "not set (lookup of rejected correspondences _not_ possible).\n",
122 getClassName().c_str());
123 return;
124 }
125
126 pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices);
127 }
128
129 /** \brief Get a string representation of the name of this class. */
130 inline const std::string&
132 {
133 return (rejection_name_);
134 }
135
136 /** \brief See if this rejector requires source points */
137 virtual bool
139 {
140 return (false);
141 }
142
143 /** \brief Abstract method for setting the source cloud */
144 virtual void
146 {
147 PCL_WARN("[pcl::registration::%s::setSourcePoints] This class does not require an "
148 "input source cloud\n",
149 getClassName().c_str());
150 }
151
152 /** \brief See if this rejector requires source normals */
153 virtual bool
155 {
156 return (false);
157 }
158
159 /** \brief Abstract method for setting the source normals */
160 virtual void
162 {
163 PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
164 "input source normals\n",
165 getClassName().c_str());
166 }
167 /** \brief See if this rejector requires a target cloud */
168 virtual bool
170 {
171 return (false);
172 }
173
174 /** \brief Abstract method for setting the target cloud */
175 virtual void
177 {
178 PCL_WARN("[pcl::registration::%s::setTargetPoints] This class does not require an "
179 "input target cloud\n",
180 getClassName().c_str());
181 }
182
183 /** \brief See if this rejector requires target normals */
184 virtual bool
186 {
187 return (false);
188 }
189
190 /** \brief Abstract method for setting the target normals */
191 virtual void
193 {
194 PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
195 "input target normals\n",
196 getClassName().c_str());
197 }
198
199protected:
200 /** \brief The name of the rejection method. */
201 std::string rejection_name_;
202
203 /** \brief The input correspondences. */
205
206 /** \brief Abstract rejection method. */
207 virtual void
208 applyRejection(Correspondences& correspondences) = 0;
209};
210
211/** @b DataContainerInterface provides a generic interface for computing correspondence
212 * scores between correspondent points in the input and target clouds
213 * \ingroup registration
214 */
216public:
217 using Ptr = shared_ptr<DataContainerInterface>;
218 using ConstPtr = shared_ptr<const DataContainerInterface>;
219
220 virtual ~DataContainerInterface() = default;
221 virtual double
223 virtual double
225 virtual double
227};
228
229/** @b DataContainer is a container for the input and target point clouds and implements
230 * the interface to compute correspondence scores between correspondent points in the
231 * input and target clouds \ingroup registration
232 */
233template <typename PointT, typename NormalT = pcl::PointNormal>
235 using PointCloud = pcl::PointCloud<PointT>;
236 using PointCloudPtr = typename PointCloud::Ptr;
237 using PointCloudConstPtr = typename PointCloud::ConstPtr;
238
239 using KdTreePtr = typename pcl::search::KdTree<PointT>::Ptr;
240
242 using NormalsPtr = typename Normals::Ptr;
243 using NormalsConstPtr = typename Normals::ConstPtr;
244
245public:
246 /** \brief Empty constructor. */
247 DataContainer(bool needs_normals = false)
248 : input_()
249 , input_transformed_()
250 , target_()
251 , input_normals_()
252 , input_normals_transformed_()
253 , target_normals_()
254 , tree_(new pcl::search::KdTree<PointT>)
255 , class_name_("DataContainer")
256 , needs_normals_(needs_normals)
257 , target_cloud_updated_(true)
258 , force_no_recompute_(false)
259 {}
260
261 /** \brief Empty destructor */
262 ~DataContainer() override = default;
263
264 /** \brief Provide a source point cloud dataset (must contain XYZ
265 * data!), used to compute the correspondence distance.
266 * \param[in] cloud a cloud containing XYZ data
267 */
268 inline void
269 setInputSource(const PointCloudConstPtr& cloud)
270 {
271 input_ = cloud;
272 }
273
274 /** \brief Get a pointer to the input point cloud dataset target. */
275 inline PointCloudConstPtr const
277 {
278 return (input_);
279 }
280
281 /** \brief Provide a target point cloud dataset (must contain XYZ
282 * data!), used to compute the correspondence distance.
283 * \param[in] target a cloud containing XYZ data
284 */
285 inline void
286 setInputTarget(const PointCloudConstPtr& target)
287 {
288 target_ = target;
289 target_cloud_updated_ = true;
290 }
291
292 /** \brief Get a pointer to the input point cloud dataset target. */
293 inline PointCloudConstPtr const
295 {
296 return (target_);
297 }
298
299 /** \brief Provide a pointer to the search object used to find correspondences in
300 * the target cloud.
301 * \param[in] tree a pointer to the spatial search object.
302 * \param[in] force_no_recompute If set to true, this tree will NEVER be
303 * recomputed, regardless of calls to setInputTarget. Only use if you are
304 * confident that the tree will be set correctly.
305 */
306 inline void
307 setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
308 {
309 tree_ = tree;
310 force_no_recompute_ = force_no_recompute;
311 target_cloud_updated_ = true;
312 }
313
314 /** \brief Set the normals computed on the input point cloud
315 * \param[in] normals the normals computed for the input cloud
316 */
317 inline void
318 setInputNormals(const NormalsConstPtr& normals)
319 {
320 input_normals_ = normals;
321 }
322
323 /** \brief Get the normals computed on the input point cloud */
324 inline NormalsConstPtr
326 {
327 return (input_normals_);
328 }
329
330 /** \brief Set the normals computed on the target point cloud
331 * \param[in] normals the normals computed for the input cloud
332 */
333 inline void
334 setTargetNormals(const NormalsConstPtr& normals)
335 {
336 target_normals_ = normals;
337 }
338
339 /** \brief Get the normals computed on the target point cloud */
340 inline NormalsConstPtr
342 {
343 return (target_normals_);
344 }
345
346 /** \brief Get the correspondence score for a point in the input cloud
347 * \param[in] index index of the point in the input cloud
348 */
349 inline double
350 getCorrespondenceScore(int index) override
351 {
352 if (target_cloud_updated_ && !force_no_recompute_) {
353 tree_->setInputCloud(target_);
354 }
355 pcl::Indices indices(1);
356 std::vector<float> distances(1);
357 if (tree_->nearestKSearch((*input_)[index], 1, indices, distances))
358 return (distances[0]);
359 return (std::numeric_limits<double>::max());
360 }
361
362 /** \brief Get the correspondence score for a given pair of correspondent points
363 * \param[in] corr Correspondent points
364 */
365 inline double
367 {
368 // Get the source and the target feature from the list
369 const PointT& src = (*input_)[corr.index_query];
370 const PointT& tgt = (*target_)[corr.index_match];
371
372 return ((src.getVector4fMap() - tgt.getVector4fMap()).squaredNorm());
373 }
374
375 /** \brief Get the correspondence score for a given pair of correspondent points based
376 * on the angle between the normals. The normmals for the in put and target clouds
377 * must be set before using this function \param[in] corr Correspondent points
378 */
379 inline double
381 {
382 // assert ( (input_normals_->size () != 0) && (target_normals_->size () != 0) &&
383 // "Normals are not set for the input and target point clouds");
384 assert(input_normals_ && target_normals_ &&
385 "Normals are not set for the input and target point clouds");
386 const NormalT& src = (*input_normals_)[corr.index_query];
387 const NormalT& tgt = (*target_normals_)[corr.index_match];
388 return (static_cast<double>((src.normal[0] * tgt.normal[0]) +
389 (src.normal[1] * tgt.normal[1]) +
390 (src.normal[2] * tgt.normal[2])));
391 }
392
393private:
394 /** \brief The input point cloud dataset */
395 PointCloudConstPtr input_;
396
397 /** \brief The input transformed point cloud dataset */
398 PointCloudPtr input_transformed_;
399
400 /** \brief The target point cloud dataset. */
401 PointCloudConstPtr target_;
402
403 /** \brief Normals to the input point cloud */
404 NormalsConstPtr input_normals_;
405
406 /** \brief Normals to the input point cloud */
407 NormalsPtr input_normals_transformed_;
408
409 /** \brief Normals to the target point cloud */
410 NormalsConstPtr target_normals_;
411
412 /** \brief A pointer to the spatial search object. */
413 KdTreePtr tree_;
414
415 /** \brief The name of the rejection method. */
416 std::string class_name_;
417
418 /** \brief Should the current data container use normals? */
419 bool needs_normals_;
420
421 /** \brief Variable that stores whether we have a new target cloud, meaning we need to
422 * pre-process it again. This way, we avoid rebuilding the kd-tree */
423 bool target_cloud_updated_;
424
425 /** \brief A flag which, if set, means the tree operating on the target cloud
426 * will never be recomputed*/
427 bool force_no_recompute_;
428
429 /** \brief Get a string representation of the name of this class. */
430 inline const std::string&
431 getClassName() const
432 {
433 return (class_name_);
434 }
435};
436} // namespace registration
437} // namespace pcl
KdTree represents the base spatial locator class for kd-tree implementations.
Definition kdtree.h:55
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
CorrespondenceRejector represents the base class for correspondence rejection methods
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
CorrespondenceRejector()=default
Empty constructor.
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
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.
virtual void setSourcePoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source cloud.
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
shared_ptr< const CorrespondenceRejector > ConstPtr
virtual void applyRejection(Correspondences &correspondences)=0
Abstract rejection method.
void getCorrespondences(pcl::Correspondences &correspondences)
Run correspondence rejection.
virtual bool requiresSourcePoints() const
See if this rejector requires source points.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
void getRejectedQueryIndices(const pcl::Correspondences &correspondences, pcl::Indices &indices)
Determine the indices of query points of correspondences that have been rejected, i....
shared_ptr< CorrespondenceRejector > Ptr
virtual bool requiresTargetPoints() const
See if this rejector requires a target cloud.
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
std::string rejection_name_
The name of the rejection method.
virtual ~CorrespondenceRejector()=default
Empty destructor.
const std::string & getClassName() const
Get a string representation of the name of this class.
CorrespondencesConstPtr getInputCorrespondences()
Get a pointer to the vector of the input correspondences.
virtual void setTargetPoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target cloud.
virtual void setInputCorrespondences(const CorrespondencesConstPtr &correspondences)
Provide a pointer to the vector of the input correspondences.
DataContainer is a container for the input and target point clouds and implements the interface to co...
NormalsConstPtr getTargetNormals()
Get the normals computed on the target point cloud.
void setInputTarget(const PointCloudConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
double getCorrespondenceScore(int index) override
Get the correspondence score for a point in the input cloud.
double getCorrespondenceScore(const pcl::Correspondence &corr) override
Get the correspondence score for a given pair of correspondent points.
NormalsConstPtr getInputNormals()
Get the normals computed on the input point cloud.
void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setTargetNormals(const NormalsConstPtr &normals)
Set the normals computed on the target point cloud.
double getCorrespondenceScoreFromNormals(const pcl::Correspondence &corr) override
Get the correspondence score for a given pair of correspondent points based on the angle between the ...
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.
DataContainer(bool needs_normals=false)
Empty constructor.
void setInputNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
~DataContainer() override=default
Empty destructor.
DataContainerInterface provides a generic interface for computing correspondence scores between corre...
virtual double getCorrespondenceScore(int index)=0
shared_ptr< const DataContainerInterface > ConstPtr
shared_ptr< DataContainerInterface > Ptr
virtual double getCorrespondenceScore(const pcl::Correspondence &)=0
virtual double getCorrespondenceScoreFromNormals(const pcl::Correspondence &)=0
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition kdtree.h:75
shared_ptr< const Correspondences > CorrespondencesConstPtr
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
void getRejectedQueryIndices(const pcl::Correspondences &correspondences_before, const pcl::Correspondences &correspondences_after, Indices &indices, bool presorting_required=true)
Get the query points of correspondences that are present in one correspondence vector but not in the ...
Correspondence represents a match between two entities (e.g., points, descriptors,...
index_t index_query
Index of the query (source) point.
index_t index_match
Index of the matching (target) point.
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.