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 correspondence_estimation_->setInputTarget (target_);
153 convergence_criteria_->setMaximumIterations (max_iterations_);
154 convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
155 convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
156 convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
162 previous_transformation_ = transformation_;
165 correspondence_estimation_->setInputSource (input_transformed);
167 if (use_reciprocal_correspondence_)
168 correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
170 correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
174 for (
size_t i = 0; i < correspondence_rejectors_.size (); ++i)
176 PCL_DEBUG (
"Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ());
181 correspondence_rejectors_[i]->setInputCorrespondences (temp_correspondences);
182 correspondence_rejectors_[i]->getCorrespondences (*correspondences_);
184 if (i < correspondence_rejectors_.size () - 1)
185 *temp_correspondences = *correspondences_;
188 size_t cnt = correspondences_->size ();
190 if (cnt < min_number_correspondences_)
192 PCL_ERROR (
"[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
199 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
202 transformCloud (*input_transformed, *input_transformed, transformation_);
205 final_transformation_ = transformation_ * final_transformation_;
213 converged_ =
static_cast<bool> ((*convergence_criteria_));
218 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",
219 final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
220 final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
221 final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
222 final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
227 transformCloud (*input_, output, final_transformation_);
231 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
PointCloudSource::Ptr PointCloudSourcePtr
boost::shared_ptr< Correspondences > CorrespondencesPtr
virtual void computeTransformation(PointCloudSource &output, const Matrix4 &guess)
Rigid transformation computation method with initial guess.
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
void transformPointCloudWithNormals(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Transform a point cloud and rotate its normals using an Eigen transform.
Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...