41 #ifndef PCL_REGISTRATION_IMPL_ICP_HPP_
42 #define PCL_REGISTRATION_IMPL_ICP_HPP_
44 #include <pcl/registration/boost.h>
45 #include <pcl/correspondence.h>
51 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
57 Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
58 Eigen::Matrix4f tr = transform.template cast<float> ();
61 if (source_has_normals_)
63 Eigen::Vector3f nt, nt_t;
64 Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);
66 for (std::size_t i = 0; i < input.size (); ++i)
68 const std::uint8_t* data_in =
reinterpret_cast<const std::uint8_t*
> (&input[i]);
69 std::uint8_t* data_out =
reinterpret_cast<std::uint8_t*
> (&output[i]);
70 memcpy (&pt[0], data_in + x_idx_offset_,
sizeof (
float));
71 memcpy (&pt[1], data_in + y_idx_offset_,
sizeof (
float));
72 memcpy (&pt[2], data_in + z_idx_offset_,
sizeof (
float));
74 if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
79 memcpy (data_out + x_idx_offset_, &pt_t[0],
sizeof (
float));
80 memcpy (data_out + y_idx_offset_, &pt_t[1],
sizeof (
float));
81 memcpy (data_out + z_idx_offset_, &pt_t[2],
sizeof (
float));
83 memcpy (&nt[0], data_in + nx_idx_offset_,
sizeof (
float));
84 memcpy (&nt[1], data_in + ny_idx_offset_,
sizeof (
float));
85 memcpy (&nt[2], data_in + nz_idx_offset_,
sizeof (
float));
87 if (!std::isfinite (nt[0]) || !std::isfinite (nt[1]) || !std::isfinite (nt[2]))
92 memcpy (data_out + nx_idx_offset_, &nt_t[0],
sizeof (
float));
93 memcpy (data_out + ny_idx_offset_, &nt_t[1],
sizeof (
float));
94 memcpy (data_out + nz_idx_offset_, &nt_t[2],
sizeof (
float));
99 for (std::size_t i = 0; i < input.size (); ++i)
101 const std::uint8_t* data_in =
reinterpret_cast<const std::uint8_t*
> (&input[i]);
102 std::uint8_t* data_out =
reinterpret_cast<std::uint8_t*
> (&output[i]);
103 memcpy (&pt[0], data_in + x_idx_offset_,
sizeof (
float));
104 memcpy (&pt[1], data_in + y_idx_offset_,
sizeof (
float));
105 memcpy (&pt[2], data_in + z_idx_offset_,
sizeof (
float));
107 if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
112 memcpy (data_out + x_idx_offset_, &pt_t[0],
sizeof (
float));
113 memcpy (data_out + y_idx_offset_, &pt_t[1],
sizeof (
float));
114 memcpy (data_out + z_idx_offset_, &pt_t[2],
sizeof (
float));
120 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
131 final_transformation_ = guess;
134 if (guess != Matrix4::Identity ())
136 input_transformed->resize (input_->size ());
138 transformCloud (*input_, *input_transformed, guess);
141 *input_transformed = *input_;
143 transformation_ = Matrix4::Identity ();
146 determineRequiredBlobData ();
148 if (need_target_blob_)
152 correspondence_estimation_->setInputTarget (target_);
153 if (correspondence_estimation_->requiresTargetNormals ())
154 correspondence_estimation_->setTargetNormals (target_blob);
156 for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
159 if (rej->requiresTargetPoints ())
160 rej->setTargetPoints (target_blob);
161 if (rej->requiresTargetNormals () && target_has_normals_)
162 rej->setTargetNormals (target_blob);
165 convergence_criteria_->setMaximumIterations (max_iterations_);
166 convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
167 convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
168 if (transformation_rotation_epsilon_ > 0)
169 convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_);
171 convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
178 if (need_source_blob_)
184 previous_transformation_ = transformation_;
187 correspondence_estimation_->setInputSource (input_transformed);
188 if (correspondence_estimation_->requiresSourceNormals ())
189 correspondence_estimation_->setSourceNormals (input_transformed_blob);
191 if (use_reciprocal_correspondence_)
192 correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
194 correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
198 for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i)
201 PCL_DEBUG (
"Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
202 if (rej->requiresSourcePoints ())
203 rej->setSourcePoints (input_transformed_blob);
204 if (rej->requiresSourceNormals () && source_has_normals_)
205 rej->setSourceNormals (input_transformed_blob);
206 rej->setInputCorrespondences (temp_correspondences);
207 rej->getCorrespondences (*correspondences_);
209 if (i < correspondence_rejectors_.size () - 1)
210 *temp_correspondences = *correspondences_;
213 std::size_t cnt = correspondences_->size ();
215 if (static_cast<int> (cnt) < min_number_correspondences_)
217 PCL_ERROR (
"[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
224 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
227 transformCloud (*input_transformed, *input_transformed, transformation_);
230 final_transformation_ = transformation_ * final_transformation_;
238 converged_ =
static_cast<bool> ((*convergence_criteria_));
243 PCL_DEBUG (
"Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
244 final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
245 final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
246 final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
247 final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
252 transformCloud (*input_, output, final_transformation_);
256 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
259 need_source_blob_ =
false;
260 need_target_blob_ =
false;
262 need_source_blob_ |= correspondence_estimation_->requiresSourceNormals ();
263 need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
265 if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
267 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
269 if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
271 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
274 for (std::size_t i = 0; i < correspondence_rejectors_.size (); i++)
277 need_source_blob_ |= rej->requiresSourcePoints ();
278 need_source_blob_ |= rej->requiresSourceNormals ();
279 need_target_blob_ |= rej->requiresTargetPoints ();
280 need_target_blob_ |= rej->requiresTargetNormals ();
281 if (rej->requiresSourceNormals () && !source_has_normals_)
283 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
285 if (rej->requiresTargetNormals () && !target_has_normals_)
287 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
293 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
shared_ptr< CorrespondenceRejector > Ptr
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Transform a point cloud and rotate its normals using an Eigen transform.
virtual void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob.
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
shared_ptr< Correspondences > CorrespondencesPtr
typename PointCloudSource::Ptr PointCloudSourcePtr