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> 48 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 54 Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
55 Eigen::Matrix4f tr = transform.template cast<float> ();
58 if (source_has_normals_)
60 Eigen::Vector3f nt, nt_t;
61 Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);
63 for (
size_t i = 0; i < input.size (); ++i)
65 const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
66 uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
67 memcpy (&pt[0], data_in + x_idx_offset_,
sizeof (
float));
68 memcpy (&pt[1], data_in + y_idx_offset_,
sizeof (
float));
69 memcpy (&pt[2], data_in + z_idx_offset_,
sizeof (
float));
71 if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
76 memcpy (data_out + x_idx_offset_, &pt_t[0],
sizeof (
float));
77 memcpy (data_out + y_idx_offset_, &pt_t[1],
sizeof (
float));
78 memcpy (data_out + z_idx_offset_, &pt_t[2],
sizeof (
float));
80 memcpy (&nt[0], data_in + nx_idx_offset_,
sizeof (
float));
81 memcpy (&nt[1], data_in + ny_idx_offset_,
sizeof (
float));
82 memcpy (&nt[2], data_in + nz_idx_offset_,
sizeof (
float));
84 if (!pcl_isfinite (nt[0]) || !pcl_isfinite (nt[1]) || !pcl_isfinite (nt[2]))
89 memcpy (data_out + nx_idx_offset_, &nt_t[0],
sizeof (
float));
90 memcpy (data_out + ny_idx_offset_, &nt_t[1],
sizeof (
float));
91 memcpy (data_out + nz_idx_offset_, &nt_t[2],
sizeof (
float));
96 for (
size_t i = 0; i < input.size (); ++i)
98 const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
99 uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
100 memcpy (&pt[0], data_in + x_idx_offset_,
sizeof (
float));
101 memcpy (&pt[1], data_in + y_idx_offset_,
sizeof (
float));
102 memcpy (&pt[2], data_in + z_idx_offset_,
sizeof (
float));
104 if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
109 memcpy (data_out + x_idx_offset_, &pt_t[0],
sizeof (
float));
110 memcpy (data_out + y_idx_offset_, &pt_t[1],
sizeof (
float));
111 memcpy (data_out + z_idx_offset_, &pt_t[2],
sizeof (
float));
118 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 129 final_transformation_ = guess;
132 if (guess != Matrix4::Identity ())
134 input_transformed->resize (input_->size ());
136 transformCloud (*input_, *input_transformed, guess);
139 *input_transformed = *input_;
141 transformation_ = Matrix4::Identity ();
144 determineRequiredBlobData ();
146 if (need_target_blob_)
150 correspondence_estimation_->setInputTarget (target_);
151 if (correspondence_estimation_->requiresTargetNormals ())
152 correspondence_estimation_->setTargetNormals (target_blob);
154 for (
size_t i = 0; i < correspondence_rejectors_.size (); ++i)
157 if (rej->requiresTargetPoints ())
158 rej->setTargetPoints (target_blob);
159 if (rej->requiresTargetNormals () && target_has_normals_)
160 rej->setTargetNormals (target_blob);
163 convergence_criteria_->setMaximumIterations (max_iterations_);
164 convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
165 convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
166 convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
173 if (need_source_blob_)
179 previous_transformation_ = transformation_;
182 correspondence_estimation_->setInputSource (input_transformed);
183 if (correspondence_estimation_->requiresSourceNormals ())
184 correspondence_estimation_->setSourceNormals (input_transformed_blob);
186 if (use_reciprocal_correspondence_)
187 correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
189 correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
193 for (
size_t i = 0; i < correspondence_rejectors_.size (); ++i)
196 PCL_DEBUG (
"Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
197 if (rej->requiresSourcePoints ())
198 rej->setSourcePoints (input_transformed_blob);
199 if (rej->requiresSourceNormals () && source_has_normals_)
200 rej->setSourceNormals (input_transformed_blob);
201 rej->setInputCorrespondences (temp_correspondences);
202 rej->getCorrespondences (*correspondences_);
204 if (i < correspondence_rejectors_.size () - 1)
205 *temp_correspondences = *correspondences_;
208 size_t cnt = correspondences_->size ();
210 if (static_cast<int> (cnt) < min_number_correspondences_)
212 PCL_ERROR (
"[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
219 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
222 transformCloud (*input_transformed, *input_transformed, transformation_);
225 final_transformation_ = transformation_ * final_transformation_;
233 converged_ = static_cast<bool> ((*convergence_criteria_));
238 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",
239 final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
240 final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
241 final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
242 final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
247 transformCloud (*input_, output, final_transformation_);
250 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 253 need_source_blob_ =
false;
254 need_target_blob_ =
false;
256 need_source_blob_ |= correspondence_estimation_->requiresSourceNormals ();
257 need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
259 if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
261 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
263 if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
265 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
268 for (
size_t i = 0; i < correspondence_rejectors_.size (); i++)
271 need_source_blob_ |= rej->requiresSourcePoints ();
272 need_source_blob_ |= rej->requiresSourceNormals ();
273 need_target_blob_ |= rej->requiresTargetPoints ();
274 need_target_blob_ |= rej->requiresTargetNormals ();
275 if (rej->requiresSourceNormals () && !source_has_normals_)
277 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
279 if (rej->requiresTargetNormals () && !target_has_normals_)
281 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
287 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
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=true)
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...
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Registration< PointSource, PointTarget, float >::PointCloudSource PointCloudSource
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< Correspondences > CorrespondencesPtr
Registration< PointSource, PointTarget, float >::Matrix4 Matrix4
virtual void computeTransformation(PointCloudSource &output, const Matrix4 &guess)
Rigid transformation computation method with initial guess.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> 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.
boost::shared_ptr< CorrespondenceRejector > Ptr
PointCloudSource::Ptr PointCloudSourcePtr