43 #include <pcl/registration/correspondence_types.h> 44 #include <pcl/registration/correspondence_sorting.h> 45 #include <pcl/console/print.h> 46 #include <pcl/common/transforms.h> 47 #include <pcl/point_cloud.h> 48 #include <pcl/search/kdtree.h> 52 namespace registration
61 using Ptr = shared_ptr<CorrespondenceRejector>;
62 using ConstPtr = shared_ptr<const CorrespondenceRejector>;
119 std::vector<int>& indices)
123 PCL_WARN (
"[pcl::registration::%s::getRejectedQueryIndices] Input correspondences not set (lookup of rejected correspondences _not_ possible).\n",
getClassName ().c_str ());
131 inline const std::string&
144 PCL_WARN (
"[pcl::registration::%s::setSourcePoints] This class does not require an input source cloud",
getClassName ().c_str ());
156 PCL_WARN (
"[pcl::registration::%s::setSourceNormals] This class does not require input source normals",
getClassName ().c_str ());
167 PCL_WARN (
"[pcl::registration::%s::setTargetPoints] This class does not require an input target cloud",
getClassName ().c_str ());
179 PCL_WARN (
"[pcl::registration::%s::setTargetNormals] This class does not require input target normals",
getClassName ().c_str ());
202 using Ptr = shared_ptr<DataContainerInterface>;
203 using ConstPtr = shared_ptr<const DataContainerInterface>;
215 template <
typename Po
intT,
typename NormalT = pcl::Po
intNormal>
233 , input_transformed_ ()
236 , input_normals_transformed_ ()
239 , class_name_ (
"DataContainer")
240 , needs_normals_ (needs_normals)
241 , target_cloud_updated_ (true)
242 , force_no_recompute_ (false)
260 inline PointCloudConstPtr
const 271 target_cloud_updated_ =
true;
275 inline PointCloudConstPtr
const 287 bool force_no_recompute =
false)
290 if (force_no_recompute)
292 force_no_recompute_ =
true;
294 target_cloud_updated_ =
true;
304 inline NormalsConstPtr
314 inline NormalsConstPtr
323 if ( target_cloud_updated_ && !force_no_recompute_ )
325 tree_->setInputCloud (target_);
327 std::vector<int> indices (1);
328 std::vector<float> distances (1);
329 if (tree_->nearestKSearch ((*input_)[index], 1, indices, distances))
330 return (distances[0]);
331 return (std::numeric_limits<double>::max ());
344 return ((src.getVector4fMap () - tgt.getVector4fMap ()).squaredNorm ());
356 assert (input_normals_ && target_normals_ &&
"Normals are not set for the input and target point clouds");
359 return (
double ((src.normal[0] * tgt.normal[0]) + (src.normal[1] * tgt.normal[1]) + (src.normal[2] * tgt.normal[2])));
364 PointCloudConstPtr input_;
367 PointCloudPtr input_transformed_;
370 PointCloudConstPtr target_;
373 NormalsConstPtr input_normals_;
376 NormalsPtr input_normals_transformed_;
379 NormalsConstPtr target_normals_;
385 std::string class_name_;
392 bool target_cloud_updated_;
396 bool force_no_recompute_;
401 inline const std::string&
402 getClassName ()
const {
return (class_name_); }
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< KdTree< PointT, Tree > > Ptr
double getCorrespondenceScoreFromNormals(const pcl::Correspondence &corr) override
Get the correspondence score for a given pair of correspondent points based on the angle between the ...
shared_ptr< const CorrespondenceRejector > ConstPtr
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
DataContainer is a container for the input and target point clouds and implements the interface to co...
shared_ptr< const Correspondences > CorrespondencesConstPtr
shared_ptr< PointCloud< PointT > > Ptr
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...
DataContainerInterface provides a generic interface for computing correspondence scores between corre...
shared_ptr< CorrespondenceRejector > Ptr
double getCorrespondenceScore(int index) override
Get the correspondence score for a point in the input cloud.
virtual void setInputCorrespondences(const CorrespondencesConstPtr &correspondences)
Provide a pointer to the vector of the input correspondences.
shared_ptr< const DataContainerInterface > ConstPtr
CorrespondenceRejector represents the base class for correspondence rejection methods ...
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
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.
double getCorrespondenceScore(const pcl::Correspondence &corr) override
Get the correspondence score for a given pair of correspondent points.
virtual bool requiresSourcePoints() const
See if this rejector requires source points.
void getCorrespondences(pcl::Correspondences &correspondences)
Run correspondence rejection.
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
const std::string & getClassName() const
Get a string representation of the name of this class.
DataContainer(bool needs_normals=false)
Empty constructor.
~DataContainer()
Empty destructor.
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
virtual double getCorrespondenceScore(int index)=0
virtual void setTargetPoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target cloud.
CorrespondencesConstPtr getInputCorrespondences()
Get a pointer to the vector of the input correspondences.
void setInputNormals(const NormalsConstPtr &normals)
Set 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...
virtual ~DataContainerInterface()=default
index_t index_match
Index of the matching (target) point.
CorrespondenceRejector()
Empty constructor.
virtual ~CorrespondenceRejector()
Empty destructor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
NormalsConstPtr getInputNormals()
Get the normals computed on the input point cloud.
virtual void setSourcePoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source cloud.
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 ...
shared_ptr< DataContainerInterface > Ptr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
void setInputTarget(const PointCloudConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
shared_ptr< const PointCloud< PointT > > ConstPtr
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
std::string rejection_name_
The name of the rejection method.
void getRejectedQueryIndices(const pcl::Correspondences &correspondences, std::vector< int > &indices)
Determine the indices of query points of correspondences that have been rejected, i...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
virtual void applyRejection(Correspondences &correspondences)=0
Abstract rejection method.
void setTargetNormals(const NormalsConstPtr &normals)
Set the normals computed on the target point cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual bool requiresTargetPoints() const
See if this rejector requires a target cloud.
index_t index_query
Index of the query (source) point.
NormalsConstPtr getTargetNormals()
Get the normals computed on the target point cloud.
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
KdTree represents the base spatial locator class for kd-tree implementations.
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
virtual double getCorrespondenceScoreFromNormals(const pcl::Correspondence &)=0