124 final_transformation_ =
guess;
127 if (
guess != Matrix4::Identity()) {
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())
152 if (
rej->requiresTargetNormals() && target_has_normals_)
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_;
177 if (correspondence_estimation_->requiresSourceNormals())
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())
195 if (
rej->requiresSourceNormals() && source_has_normals_)
198 rej->getCorrespondences(*correspondences_);
200 if (i < correspondence_rejectors_.size() - 1)
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(
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_);