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 if (transformation_rotation_epsilon_ > 0)
167 convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_);
169 convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
176 if (need_source_blob_)
182 previous_transformation_ = transformation_;
185 correspondence_estimation_->setInputSource (input_transformed);
186 if (correspondence_estimation_->requiresSourceNormals ())
187 correspondence_estimation_->setSourceNormals (input_transformed_blob);
189 if (use_reciprocal_correspondence_)
190 correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
192 correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
196 for (
size_t i = 0; i < correspondence_rejectors_.size (); ++i)
199 PCL_DEBUG (
"Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
200 if (rej->requiresSourcePoints ())
201 rej->setSourcePoints (input_transformed_blob);
202 if (rej->requiresSourceNormals () && source_has_normals_)
203 rej->setSourceNormals (input_transformed_blob);
204 rej->setInputCorrespondences (temp_correspondences);
205 rej->getCorrespondences (*correspondences_);
207 if (i < correspondence_rejectors_.size () - 1)
208 *temp_correspondences = *correspondences_;
211 size_t cnt = correspondences_->size ();
213 if (
static_cast<int> (cnt) < min_number_correspondences_)
215 PCL_ERROR (
"[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
222 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
225 transformCloud (*input_transformed, *input_transformed, transformation_);
228 final_transformation_ = transformation_ * final_transformation_;
236 converged_ =
static_cast<bool> ((*convergence_criteria_));
241 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",
242 final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
243 final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
244 final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
245 final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
250 transformCloud (*input_, output, final_transformation_);
253 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
256 need_source_blob_ =
false;
257 need_target_blob_ =
false;
259 need_source_blob_ |= correspondence_estimation_->requiresSourceNormals ();
260 need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
262 if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
264 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
266 if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
268 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
271 for (
size_t i = 0; i < correspondence_rejectors_.size (); i++)
274 need_source_blob_ |= rej->requiresSourcePoints ();
275 need_source_blob_ |= rej->requiresSourceNormals ();
276 need_target_blob_ |= rej->requiresTargetPoints ();
277 need_target_blob_ |= rej->requiresTargetNormals ();
278 if (rej->requiresSourceNormals () && !source_has_normals_)
280 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
282 if (rej->requiresTargetNormals () && !target_has_normals_)
284 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
290 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void