Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
correspondence_rejection_surface_normal.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40#pragma once
41
42#include <pcl/registration/correspondence_rejection.h>
43#include <pcl/conversions.h> // for fromPCLPointCloud2
44#include <pcl/memory.h> // for static_pointer_cast
45#include <pcl/point_cloud.h>
46
47namespace pcl {
48namespace registration {
49/**
50 * @b CorrespondenceRejectorSurfaceNormal implements a simple correspondence
51 * rejection method based on the angle between the normals at correspondent points.
52 *
53 * \note If \ref setInputCloud and \ref setInputTarget are given, then the
54 * distances between correspondences will be estimated using the given XYZ
55 * data, and not read from the set of input correspondences.
56 *
57 * \author Aravindhan K Krishnan (original code from libpointmatcher:
58 * https://github.com/ethz-asl/libpointmatcher) \ingroup registration
59 */
61 using CorrespondenceRejector::getClassName;
62 using CorrespondenceRejector::input_correspondences_;
63 using CorrespondenceRejector::rejection_name_;
64
65public:
66 using Ptr = shared_ptr<CorrespondenceRejectorSurfaceNormal>;
67 using ConstPtr = shared_ptr<const CorrespondenceRejectorSurfaceNormal>;
68
69 /** \brief Empty constructor. Sets the threshold to 1.0. */
71 {
72 rejection_name_ = "CorrespondenceRejectorSurfaceNormal";
73 }
74
75 /** \brief Get a list of valid correspondences after rejection from the original set
76 * of correspondences. \param[in] original_correspondences the set of initial
77 * correspondences given \param[out] remaining_correspondences the resultant filtered
78 * set of remaining correspondences
79 */
80 void
81 getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
82 pcl::Correspondences& remaining_correspondences) override;
83
84 /** \brief Set the thresholding angle between the normals for correspondence
85 * rejection. \param[in] threshold cosine of the thresholding angle between the
86 * normals for rejection
87 */
88 inline void
89 setThreshold(double threshold)
90 {
91 threshold_ = threshold;
92 };
93
94 /** \brief Get the thresholding angle between the normals for correspondence
95 * rejection. */
96 inline double
98 {
99 return threshold_;
100 };
101
102 /** \brief Initialize the data container object for the point type and the normal
103 * type. */
104 template <typename PointT, typename NormalT>
105 inline void
107 {
108 data_container_.reset(new DataContainer<PointT, NormalT>);
109 }
110
111 /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to
112 * compute the correspondence distance. \param[in] input a cloud containing XYZ data
113 */
114 template <typename PointT>
115 inline void
117 {
118 if (!data_container_) {
119 PCL_ERROR(
120 "[pcl::registration::%s::setInputCloud] Initialize the data container object "
121 "by calling intializeDataContainer () before using this function.\n",
122 getClassName().c_str());
123 return;
124 }
125 static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputSource(input);
126 }
127
128 /** \brief Get the target input point cloud */
129 template <typename PointT>
132 {
133 if (!data_container_) {
134 PCL_ERROR(
135 "[pcl::registration::%s::getInputSource] Initialize the data container "
136 "object by calling intializeDataContainer () before using this function.\n",
137 getClassName().c_str());
138 return;
139 }
140 return (
141 static_pointer_cast<DataContainer<PointT>>(data_container_)->getInputSource());
142 }
143
144 /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to
145 * compute the correspondence distance. \param[in] target a cloud containing XYZ data
146 */
147 template <typename PointT>
148 inline void
150 {
151 if (!data_container_) {
152 PCL_ERROR(
153 "[pcl::registration::%s::setInputTarget] Initialize the data container "
154 "object by calling intializeDataContainer () before using this function.\n",
155 getClassName().c_str());
156 return;
157 }
158 static_pointer_cast<DataContainer<PointT>>(data_container_)->setInputTarget(target);
159 }
160
161 /** \brief Provide a pointer to the search object used to find correspondences in
162 * the target cloud.
163 * \param[in] tree a pointer to the spatial search object.
164 * \param[in] force_no_recompute If set to true, this tree will NEVER be
165 * recomputed, regardless of calls to setInputTarget. Only use if you are
166 * confident that the tree will be set correctly.
167 */
168 template <typename PointT>
169 inline void
171 bool force_no_recompute = false)
172 {
173 static_pointer_cast<DataContainer<PointT>>(data_container_)
174 ->setSearchMethodTarget(tree, force_no_recompute);
175 }
176
177 /** \brief Get the target input point cloud */
178 template <typename PointT>
181 {
182 if (!data_container_) {
183 PCL_ERROR(
184 "[pcl::registration::%s::getInputTarget] Initialize the data container "
185 "object by calling intializeDataContainer () before using this function.\n",
186 getClassName().c_str());
187 return;
188 }
189 return (
190 static_pointer_cast<DataContainer<PointT>>(data_container_)->getInputTarget());
191 }
192
193 /** \brief Set the normals computed on the input point cloud
194 * \param[in] normals the normals computed for the input cloud
195 */
196 template <typename PointT, typename NormalT>
197 inline void
199 {
200 if (!data_container_) {
201 PCL_ERROR(
202 "[pcl::registration::%s::setInputNormals] Initialize the data container "
203 "object by calling intializeDataContainer () before using this function.\n",
204 getClassName().c_str());
205 return;
206 }
207 static_pointer_cast<DataContainer<PointT, NormalT>>(data_container_)
208 ->setInputNormals(normals);
209 }
210
211 /** \brief Get the normals computed on the input point cloud */
212 template <typename NormalT>
213 inline typename pcl::PointCloud<NormalT>::Ptr
215 {
216 if (!data_container_) {
217 PCL_ERROR(
218 "[pcl::registration::%s::getInputNormals] Initialize the data container "
219 "object by calling intializeDataContainer () before using this function.\n",
220 getClassName().c_str());
221 return;
222 }
223 return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT>>(data_container_)
224 ->getInputNormals());
225 }
226
227 /** \brief Set the normals computed on the target point cloud
228 * \param[in] normals the normals computed for the input cloud
229 */
230 template <typename PointT, typename NormalT>
231 inline void
233 {
234 if (!data_container_) {
235 PCL_ERROR(
236 "[pcl::registration::%s::setTargetNormals] Initialize the data container "
237 "object by calling intializeDataContainer () before using this function.\n",
238 getClassName().c_str());
239 return;
240 }
241 static_pointer_cast<DataContainer<PointT, NormalT>>(data_container_)
242 ->setTargetNormals(normals);
243 }
244
245 /** \brief Get the normals computed on the target point cloud */
246 template <typename NormalT>
247 inline typename pcl::PointCloud<NormalT>::Ptr
249 {
250 if (!data_container_) {
251 PCL_ERROR(
252 "[pcl::registration::%s::getTargetNormals] Initialize the data container "
253 "object by calling intializeDataContainer () before using this function.\n",
254 getClassName().c_str());
255 return;
256 }
257 return (static_pointer_cast<DataContainer<pcl::PointXYZ, NormalT>>(data_container_)
258 ->getTargetNormals());
259 }
260
261 /** \brief See if this rejector requires source points */
262 bool
263 requiresSourcePoints() const override
264 {
265 return (true);
266 }
267
268 /** \brief Blob method for setting the source cloud */
269 void
271 {
272 if (!data_container_)
273 initializeDataContainer<PointXYZ, Normal>();
275 fromPCLPointCloud2(*cloud2, *cloud);
276 setInputSource<PointXYZ>(cloud);
277 }
278
279 /** \brief See if this rejector requires a target cloud */
280 bool
281 requiresTargetPoints() const override
282 {
283 return (true);
284 }
285
286 /** \brief Method for setting the target cloud */
287 void
289 {
290 if (!data_container_)
291 initializeDataContainer<PointXYZ, Normal>();
293 fromPCLPointCloud2(*cloud2, *cloud);
294 setInputTarget<PointXYZ>(cloud);
295 }
296
297 /** \brief See if this rejector requires source normals */
298 bool
299 requiresSourceNormals() const override
300 {
301 return (true);
302 }
303
304 /** \brief Blob method for setting the source normals */
305 void
307 {
308 if (!data_container_)
309 initializeDataContainer<PointXYZ, Normal>();
311 fromPCLPointCloud2(*cloud2, *cloud);
312 setInputNormals<PointXYZ, Normal>(cloud);
313 }
314
315 /** \brief See if this rejector requires target normals*/
316 bool
317 requiresTargetNormals() const override
318 {
319 return (true);
320 }
321
322 /** \brief Method for setting the target normals */
323 void
325 {
326 if (!data_container_)
327 initializeDataContainer<PointXYZ, Normal>();
329 fromPCLPointCloud2(*cloud2, *cloud);
330 setTargetNormals<PointXYZ, Normal>(cloud);
331 }
332
333protected:
334 /** \brief Apply the rejection algorithm.
335 * \param[out] correspondences the set of resultant correspondences.
336 */
337 inline void
338 applyRejection(pcl::Correspondences& correspondences) override
339 {
340 getRemainingCorrespondences(*input_correspondences_, correspondences);
341 }
342
343 /** \brief The median distance threshold between two correspondent points in source
344 * <-> target. */
345 double threshold_{1.0};
346
348 /** \brief A pointer to the DataContainer object containing the input and target point
349 * clouds */
351};
352} // namespace registration
353} // namespace pcl
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
CorrespondenceRejectorSurfaceNormal implements a simple correspondence rejection method based on the ...
void setInputTarget(const typename pcl::PointCloud< PointT >::ConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
double getThreshold() const
Get the thresholding angle between the normals for correspondence rejection.
void initializeDataContainer()
Initialize the data container object for the point type and the normal type.
void setTargetNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the target point cloud.
void setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target normals.
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
bool requiresTargetNormals() const override
See if this rejector requires target normals.
void setInputSource(const typename pcl::PointCloud< PointT >::ConstPtr &input)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setInputNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the input point cloud.
void setThreshold(double threshold)
Set the thresholding angle between the normals for correspondence rejection.
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
bool requiresSourcePoints() const override
See if this rejector requires source points.
void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source normals.
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
void setSearchMethodTarget(const typename pcl::search::KdTree< PointT >::Ptr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the target cloud.
pcl::PointCloud< NormalT >::Ptr getInputNormals() const
Get the normals computed on the input point cloud.
shared_ptr< const CorrespondenceRejectorSurfaceNormal > ConstPtr
pcl::PointCloud< PointT >::ConstPtr getInputTarget() const
Get the target input point cloud.
pcl::PointCloud< NormalT >::Ptr getTargetNormals() const
Get the normals computed on the target point cloud.
bool requiresSourceNormals() const override
See if this rejector requires source normals.
pcl::PointCloud< PointT >::ConstPtr getInputSource() const
Get the target input point cloud.
DataContainer is a container for the input and target point clouds and implements the interface to co...
shared_ptr< DataContainerInterface > Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition kdtree.h:75
Defines functions, macros and traits for allocating and using memory.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr