44 #include <pcl/registration/correspondence_estimation.h>
45 #include <pcl/registration/default_convergence_criteria.h>
46 #include <pcl/registration/registration.h>
47 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
48 #include <pcl/registration/transformation_estimation_svd.h>
49 #include <pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h>
50 #include <pcl/memory.h>
51 #include <pcl/pcl_config.h>
97 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar =
float>
113 using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
115 shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
129 transformation_rotation_epsilon_;
151 TransformationEstimationSVD<PointSource, PointTarget, Scalar>());
202 const auto fields = pcl::getFields<PointSource>();
204 for (
const auto& field : fields) {
205 if (field.name ==
"x")
207 else if (field.name ==
"y")
209 else if (field.name ==
"z")
211 else if (field.name ==
"normal_x") {
212 source_has_normals_ =
true;
215 else if (field.name ==
"normal_y") {
216 source_has_normals_ =
true;
219 else if (field.name ==
"normal_z") {
220 source_has_normals_ =
true;
235 const auto fields = pcl::getFields<PointSource>();
237 for (
const auto& field : fields) {
238 if (field.name ==
"normal_x" || field.name ==
"normal_y" ||
239 field.name ==
"normal_z") {
240 target_has_normals_ =
true;
329 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar =
float>
344 transformation_estimation_;
346 correspondence_rejectors_;
349 shared_ptr<IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>>;
356 reg_name_ =
"IterativeClosestPointWithNormals";
380 symmetric_transformation_estimation->setEnforceSameDirectionNormals(
410 auto symmetric_transformation_estimation = dynamic_pointer_cast<
415 if (symmetric_transformation_estimation)
416 symmetric_transformation_estimation->setEnforceSameDirectionNormals(
438 const Matrix4& transform)
override;
450 #include <pcl/registration/impl/icp.hpp>
452 #if !defined(PCL_NO_PRECOMPILE) && !defined(PCL_REGISTRATION_ICP_CPP_)
456 #endif // PCL_NO_PRECOMPILE
~IterativeClosestPoint() override=default
Empty destructor.
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< PointXYZRGBA, PointXYZRGBA, 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.
IterativeClosestPoint & operator=(const IterativeClosestPoint &)=delete
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< PointXYZRGBA, PointXYZRGBA, float >::Matrix4 Matrix4
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.
shared_ptr< T > make_shared(Args &&...args)
Returns a pcl::shared_ptr compliant with type T's allocation policy.
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
shared_ptr< DefaultConvergenceCriteria< Scalar >> Ptr
shared_ptr< IterativeClosestPoint< PointXYZRGBA, PointXYZRGBA, float >> Ptr
virtual void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
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) ...
shared_ptr< const IterativeClosestPoint< PointXYZRGBA, PointXYZRGBA, float >> ConstPtr
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...
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
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, Scalar >::PointCloudSource PointCloudSource
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
Registration represents the base registration class for general purpose, ICP-like methods...
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
~IterativeClosestPointWithNormals() override=default
Empty destructor.
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 the input source t...
void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform) override
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.
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_
Abstract CorrespondenceEstimationBase class.