44 #include <pcl/memory.h>
45 #include <pcl/sample_consensus/ransac.h>
46 #include <pcl/sample_consensus/sac_model_registration.h>
47 #include <pcl/registration/registration.h>
48 #include <pcl/registration/transformation_estimation_svd.h>
49 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
50 #include <pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h>
51 #include <pcl/registration/correspondence_estimation.h>
52 #include <pcl/registration/default_convergence_criteria.h>
95 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar =
float>
110 using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
111 using ConstPtr = shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
183 const auto fields = pcl::getFields<PointSource> ();
185 for (
const auto &field : fields)
190 else if (field.name ==
"normal_x")
192 source_has_normals_ =
true;
195 else if (field.name ==
"normal_y")
197 source_has_normals_ =
true;
200 else if (field.name ==
"normal_z")
202 source_has_normals_ =
true;
217 const auto fields = pcl::getFields<PointSource> ();
219 for (
const auto &field : fields)
221 if (field.name ==
"normal_x" || field.name ==
"normal_y" || field.name ==
"normal_z")
223 target_has_normals_ =
true;
310 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar =
float>
322 using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
323 using ConstPtr = shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> >;
328 reg_name_ =
"IterativeClosestPointWithNormals";
347 auto symmetric_transformation_estimation = pcl::make_shared<pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> > ();
373 if (symmetric_transformation_estimation)
405 #include <pcl/registration/impl/icp.hpp>
bool enforce_same_direction_normals_
Whether or not to negate source and/or target normals such that they point in the same direction in t...
typename Registration< PointSource, PointTarget, float >::PointCloudTarget PointCloudTarget
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
IterativeClosestPointWithNormals is a special case of IterativeClosestPoint, that uses a transformati...
bool target_has_normals_
Internal check whether target dataset has normals or not.
typename Registration< PointSource, PointTarget, float >::Matrix4 Matrix4
shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, float > > ConstPtr
virtual 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) ...
PointIndices::Ptr PointIndicesPtr
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
std::size_t nx_idx_offset_
Normal fields offset.
bool getUseReciprocalCorrespondences() const
Obtain whether reciprocal correspondence are used or not.
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
virtual void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Eigen::Matrix< Scalar, 4, 4 > Matrix4
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
bool getEnforceSameDirectionNormals() const
Obtain whether source or target normals are negated on a per-point basis such that they point in the ...
virtual 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...
shared_ptr< ::pcl::PointIndices > Ptr
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
void setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
Set whether or not to negate source or target normals on a per-point basis such that they point in th...
Matrix4 transformation_
The transformation matrix estimated by the registration method.
IterativeClosestPointWithNormals()
Empty constructor.
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) ...
typename Registration< PointSource, PointTarget, float >::PointCloudSource PointCloudSource
shared_ptr< IterativeClosestPoint< PointSource, PointTarget, float > > Ptr
virtual ~IterativeClosestPointWithNormals()
Empty destructor.
~IterativeClosestPoint()
Empty destructor.
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
shared_ptr< DefaultConvergenceCriteria< Scalar > > Ptr
Registration represents the base registration class for general purpose, ICP-like methods...
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
std::size_t y_idx_offset_
bool source_has_normals_
Internal check whether source dataset has normals or not.
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target) ...
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
shared_ptr< const ::pcl::PointIndices > ConstPtr
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr getConvergeCriteria()
Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class...
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr convergence_criteria_
std::string reg_name_
The registration method name.
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
bool need_source_blob_
Checks for whether estimators and rejectors need various data.
bool use_reciprocal_correspondence_
The correspondence type used for correspondence estimation.
void setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
Set whether to use reciprocal correspondence or not.
IterativeClosestPoint()
Empty constructor.
bool use_symmetric_objective_
Type of objective function (asymmetric vs.
bool getUseSymmetricObjective() const
Obtain whether a symmetric objective is used or not.
CorrespondenceEstimation represents the base class for determining correspondences between target and...
typename PointCloudTarget::Ptr PointCloudTargetPtr
std::size_t z_idx_offset_
PointIndices::ConstPtr PointIndicesConstPtr
std::size_t x_idx_offset_
XYZ fields offset.
typename PointCloudSource::Ptr PointCloudSourcePtr
std::size_t nz_idx_offset_
void setUseSymmetricObjective(bool use_symmetric_objective)
Set whether to use a symmetric objective function or not.
std::size_t ny_idx_offset_