41 #ifndef PCL_REGISTRATION_IMPL_ICP_HPP_
42 #define PCL_REGISTRATION_IMPL_ICP_HPP_
44 #include <pcl/correspondence.h>
48 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
53 Eigen::Vector4f pt(0.0f, 0.0f, 0.0f, 1.0f), pt_t;
54 Eigen::Matrix4f tr = transform.template cast<float>();
58 if (source_has_normals_) {
59 Eigen::Vector3f nt, nt_t;
60 Eigen::Matrix3f rot = tr.block<3, 3>(0, 0);
62 for (std::size_t i = 0; i < input.size(); ++i) {
63 const std::uint8_t* data_in =
reinterpret_cast<const std::uint8_t*
>(&input[i]);
64 std::uint8_t* data_out =
reinterpret_cast<std::uint8_t*
>(&output[i]);
65 memcpy(&pt[0], data_in + x_idx_offset_,
sizeof(
float));
66 memcpy(&pt[1], data_in + y_idx_offset_,
sizeof(
float));
67 memcpy(&pt[2], data_in + z_idx_offset_,
sizeof(
float));
69 if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
74 memcpy(data_out + x_idx_offset_, &pt_t[0],
sizeof(
float));
75 memcpy(data_out + y_idx_offset_, &pt_t[1],
sizeof(
float));
76 memcpy(data_out + z_idx_offset_, &pt_t[2],
sizeof(
float));
78 memcpy(&nt[0], data_in + nx_idx_offset_,
sizeof(
float));
79 memcpy(&nt[1], data_in + ny_idx_offset_,
sizeof(
float));
80 memcpy(&nt[2], data_in + nz_idx_offset_,
sizeof(
float));
82 if (!std::isfinite(nt[0]) || !std::isfinite(nt[1]) || !std::isfinite(nt[2]))
87 memcpy(data_out + nx_idx_offset_, &nt_t[0],
sizeof(
float));
88 memcpy(data_out + ny_idx_offset_, &nt_t[1],
sizeof(
float));
89 memcpy(data_out + nz_idx_offset_, &nt_t[2],
sizeof(
float));
93 for (std::size_t i = 0; i < input.size(); ++i) {
94 const std::uint8_t* data_in =
reinterpret_cast<const std::uint8_t*
>(&input[i]);
95 std::uint8_t* data_out =
reinterpret_cast<std::uint8_t*
>(&output[i]);
96 memcpy(&pt[0], data_in + x_idx_offset_,
sizeof(
float));
97 memcpy(&pt[1], data_in + y_idx_offset_,
sizeof(
float));
98 memcpy(&pt[2], data_in + z_idx_offset_,
sizeof(
float));
100 if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
105 memcpy(data_out + x_idx_offset_, &pt_t[0],
sizeof(
float));
106 memcpy(data_out + y_idx_offset_, &pt_t[1],
sizeof(
float));
107 memcpy(data_out + z_idx_offset_, &pt_t[2],
sizeof(
float));
112 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
124 final_transformation_ = guess;
127 if (guess != Matrix4::Identity()) {
128 input_transformed->resize(input_->size());
130 transformCloud(*input_, *input_transformed, guess);
133 *input_transformed = *input_;
135 transformation_ = Matrix4::Identity();
138 determineRequiredBlobData();
140 if (need_target_blob_)
144 correspondence_estimation_->setInputTarget(target_);
145 if (correspondence_estimation_->requiresTargetNormals())
146 correspondence_estimation_->setTargetNormals(target_blob);
148 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
150 if (rej->requiresTargetPoints())
151 rej->setTargetPoints(target_blob);
152 if (rej->requiresTargetNormals() && target_has_normals_)
153 rej->setTargetNormals(target_blob);
156 convergence_criteria_->setMaximumIterations(max_iterations_);
157 convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_);
158 convergence_criteria_->setTranslationThreshold(transformation_epsilon_);
159 if (transformation_rotation_epsilon_ > 0)
160 convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);
162 convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
168 if (need_source_blob_) {
173 previous_transformation_ = transformation_;
176 correspondence_estimation_->setInputSource(input_transformed);
177 if (correspondence_estimation_->requiresSourceNormals())
178 correspondence_estimation_->setSourceNormals(input_transformed_blob);
180 if (use_reciprocal_correspondence_)
181 correspondence_estimation_->determineReciprocalCorrespondences(
182 *correspondences_, corr_dist_threshold_);
184 correspondence_estimation_->determineCorrespondences(*correspondences_,
185 corr_dist_threshold_);
189 for (std::size_t i = 0; i < correspondence_rejectors_.size(); ++i) {
191 PCL_DEBUG(
"Applying a correspondence rejector method: %s.\n",
192 rej->getClassName().c_str());
193 if (rej->requiresSourcePoints())
194 rej->setSourcePoints(input_transformed_blob);
195 if (rej->requiresSourceNormals() && source_has_normals_)
196 rej->setSourceNormals(input_transformed_blob);
197 rej->setInputCorrespondences(temp_correspondences);
198 rej->getCorrespondences(*correspondences_);
200 if (i < correspondence_rejectors_.size() - 1)
201 *temp_correspondences = *correspondences_;
204 std::size_t cnt = correspondences_->size();
206 if (
static_cast<int>(cnt) < min_number_correspondences_) {
207 PCL_ERROR(
"[pcl::%s::computeTransformation] Not enough correspondences found. "
208 "Relax your threshold parameters.\n",
209 getClassName().c_str());
210 convergence_criteria_->setConvergenceState(
212 Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
218 transformation_estimation_->estimateRigidTransformation(
219 *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_));
234 }
while (convergence_criteria_->getConvergenceState() ==
236 Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
239 PCL_DEBUG(
"Transformation "
240 "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%"
241 "5f\t%5f\t%5f\t%5f\n",
242 final_transformation_(0, 0),
243 final_transformation_(0, 1),
244 final_transformation_(0, 2),
245 final_transformation_(0, 3),
246 final_transformation_(1, 0),
247 final_transformation_(1, 1),
248 final_transformation_(1, 2),
249 final_transformation_(1, 3),
250 final_transformation_(2, 0),
251 final_transformation_(2, 1),
252 final_transformation_(2, 2),
253 final_transformation_(2, 3),
254 final_transformation_(3, 0),
255 final_transformation_(3, 1),
256 final_transformation_(3, 2),
257 final_transformation_(3, 3));
262 transformCloud(*input_, output, final_transformation_);
265 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
269 need_source_blob_ =
false;
270 need_target_blob_ =
false;
272 need_source_blob_ |= correspondence_estimation_->requiresSourceNormals();
273 need_target_blob_ |= correspondence_estimation_->requiresTargetNormals();
275 if (correspondence_estimation_->requiresSourceNormals() && !source_has_normals_) {
276 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects source normals, "
277 "but we can't provide them.\n",
278 getClassName().c_str());
280 if (correspondence_estimation_->requiresTargetNormals() && !target_has_normals_) {
281 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Estimator expects target normals, "
282 "but we can't provide them.\n",
283 getClassName().c_str());
286 for (std::size_t i = 0; i < correspondence_rejectors_.size(); i++) {
288 need_source_blob_ |= rej->requiresSourcePoints();
289 need_source_blob_ |= rej->requiresSourceNormals();
290 need_target_blob_ |= rej->requiresTargetPoints();
291 need_target_blob_ |= rej->requiresTargetNormals();
292 if (rej->requiresSourceNormals() && !source_has_normals_) {
293 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects source "
294 "normals, but we can't provide them.\n",
295 getClassName().c_str(),
296 rej->getClassName().c_str());
298 if (rej->requiresTargetNormals() && !target_has_normals_) {
299 PCL_WARN(
"[pcl::%s::determineRequiredBlobData] Rejector %s expects target "
300 "normals, but we can't provide them.\n",
301 getClassName().c_str(),
302 rej->getClassName().c_str());
307 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>