42 #include <pcl/registration/correspondence_rejection.h>
43 #include <pcl/conversions.h>
44 #include <pcl/memory.h>
45 #include <pcl/point_cloud.h>
48 namespace registration {
66 using Ptr = shared_ptr<CorrespondenceRejectorSurfaceNormal>;
67 using ConstPtr = shared_ptr<const CorrespondenceRejectorSurfaceNormal>;
72 rejection_name_ =
"CorrespondenceRejectorSurfaceNormal";
91 threshold_ = threshold;
104 template <
typename Po
intT,
typename NormalT>
114 template <
typename Po
intT>
118 if (!data_container_) {
120 "[pcl::registration::%s::setInputCloud] Initialize the data container object "
121 "by calling intializeDataContainer () before using this function.\n",
122 getClassName().c_str());
129 template <
typename Po
intT>
133 if (!data_container_) {
135 "[pcl::registration::%s::getInputSource] Initialize the data container "
136 "object by calling intializeDataContainer () before using this function.\n",
137 getClassName().c_str());
147 template <
typename Po
intT>
151 if (!data_container_) {
153 "[pcl::registration::%s::setInputTarget] Initialize the data container "
154 "object by calling intializeDataContainer () before using this function.\n",
155 getClassName().c_str());
168 template <
typename Po
intT>
171 bool force_no_recompute =
false)
174 ->setSearchMethodTarget(tree, force_no_recompute);
178 template <
typename Po
intT>
182 if (!data_container_) {
184 "[pcl::registration::%s::getInputTarget] Initialize the data container "
185 "object by calling intializeDataContainer () before using this function.\n",
186 getClassName().c_str());
196 template <
typename Po
intT,
typename NormalT>
200 if (!data_container_) {
202 "[pcl::registration::%s::setInputNormals] Initialize the data container "
203 "object by calling intializeDataContainer () before using this function.\n",
204 getClassName().c_str());
208 ->setInputNormals(normals);
212 template <
typename NormalT>
216 if (!data_container_) {
218 "[pcl::registration::%s::getInputNormals] Initialize the data container "
219 "object by calling intializeDataContainer () before using this function.\n",
220 getClassName().c_str());
224 ->getInputNormals());
230 template <
typename Po
intT,
typename NormalT>
234 if (!data_container_) {
236 "[pcl::registration::%s::setTargetNormals] Initialize the data container "
237 "object by calling intializeDataContainer () before using this function.\n",
238 getClassName().c_str());
242 ->setTargetNormals(normals);
246 template <
typename NormalT>
250 if (!data_container_) {
252 "[pcl::registration::%s::getTargetNormals] Initialize the data container "
253 "object by calling intializeDataContainer () before using this function.\n",
254 getClassName().c_str());
258 ->getTargetNormals());
272 if (!data_container_)
273 initializeDataContainer<PointXYZ, Normal>();
276 setInputSource<PointXYZ>(cloud);
290 if (!data_container_)
291 initializeDataContainer<PointXYZ, Normal>();
294 setInputTarget<PointXYZ>(cloud);
308 if (!data_container_)
309 initializeDataContainer<PointXYZ, Normal>();
312 setInputNormals<PointXYZ, Normal>(cloud);
326 if (!data_container_)
327 initializeDataContainer<PointXYZ, Normal>();
330 setTargetNormals<PointXYZ, Normal>(cloud);
340 getRemainingCorrespondences(*input_correspondences_, correspondences);
345 double threshold_{1.0};
shared_ptr< KdTree< PointT, Tree > > Ptr
CorrespondenceRejectorSurfaceNormal implements a simple correspondence rejection method based on the ...
shared_ptr< const CorrespondenceRejector > ConstPtr
DataContainer is a container for the input and target point clouds and implements the interface to co...
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...
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< CorrespondenceRejector > Ptr
bool requiresTargetNormals() const override
See if this rejector requires target normals.
CorrespondenceRejector represents the base class for correspondence rejection methods ...
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
bool requiresSourceNormals() const override
See if this rejector requires source normals.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source normals.
void setThreshold(double threshold)
Set the thresholding angle between the normals for correspondence rejection.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
pcl::PointCloud< NormalT >::Ptr getInputNormals() const
Get the normals computed on the input point cloud.
bool requiresSourcePoints() const override
See if this rejector requires source points.
pcl::PointCloud< PointT >::ConstPtr getInputSource() const
Get the target input point cloud.
void setTargetNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the target point cloud.
const std::string & getClassName() const
Get a string representation of the name of this class.
DataContainerInterface::Ptr DataContainerPtr
void setInputNormals(const typename pcl::PointCloud< NormalT >::ConstPtr &normals)
Set the normals computed on the input point cloud.
double getThreshold() const
Get the thresholding angle between the normals for correspondence rejection.
void setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target normals.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PointCloud< NormalT >::Ptr getTargetNormals() const
Get the normals computed on the target point cloud.
DataContainerPtr data_container_
A pointer to the DataContainer object containing the input and target point clouds.
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 object using a field_map...
shared_ptr< DataContainerInterface > Ptr
CorrespondenceRejectorSurfaceNormal()
Empty constructor.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
shared_ptr< const PointCloud< PointT > > ConstPtr
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
pcl::PointCloud< PointT >::ConstPtr getInputTarget() const
Get the target input point cloud.
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 initializeDataContainer()
Initialize the data container object for the point type and the normal type.
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...
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.