41 #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
42 #define PCL_REGISTRATION_IMPL_GICP_HPP_
44 #include <pcl/registration/boost.h>
45 #include <pcl/registration/exceptions.h>
51 template <
typename Po
intSource,
typename Po
intTarget>
52 template<
typename Po
intT>
void
57 if (k_correspondences_ >
int (cloud->
size ()))
59 PCL_ERROR (
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->
size (), k_correspondences_);
64 std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
65 std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
68 if(cloud_covariances.size () < cloud->
size ())
69 cloud_covariances.resize (cloud->
size ());
71 MatricesVector::iterator matrices_iterator = cloud_covariances.begin ();
72 for(
auto points_iterator = cloud->
begin ();
73 points_iterator != cloud->
end ();
74 ++points_iterator, ++matrices_iterator)
76 const PointT &query_point = *points_iterator;
77 Eigen::Matrix3d &cov = *matrices_iterator;
83 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
86 for(
int j = 0; j < k_correspondences_; j++) {
87 const PointT &pt = (*cloud)[nn_indecies[j]];
93 cov(0,0) += pt.x*pt.x;
95 cov(1,0) += pt.y*pt.x;
96 cov(1,1) += pt.y*pt.y;
98 cov(2,0) += pt.z*pt.x;
99 cov(2,1) += pt.z*pt.y;
100 cov(2,2) += pt.z*pt.z;
103 mean /=
static_cast<double> (k_correspondences_);
105 for (
int k = 0; k < 3; k++)
106 for (
int l = 0; l <= k; l++)
108 cov(k,l) /=
static_cast<double> (k_correspondences_);
109 cov(k,l) -= mean[k]*mean[l];
114 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
116 Eigen::Matrix3d U = svd.matrixU ();
118 for(
int k = 0; k < 3; k++) {
119 Eigen::Vector3d col = U.col(k);
123 cov+= v * col * col.transpose();
129 template <
typename Po
intSource,
typename Po
intTarget>
void
132 Eigen::Matrix3d dR_dPhi;
133 Eigen::Matrix3d dR_dTheta;
134 Eigen::Matrix3d dR_dPsi;
136 double phi = x[3], theta = x[4], psi = x[5];
138 double cphi = std::cos(phi), sphi = sin(phi);
139 double ctheta = std::cos(theta), stheta = sin(theta);
140 double cpsi = std::cos(psi), spsi = sin(psi);
146 dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
147 dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
148 dR_dPhi(2,1) = cphi*ctheta;
150 dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
151 dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
152 dR_dPhi(2,2) = -ctheta*sphi;
154 dR_dTheta(0,0) = -cpsi*stheta;
155 dR_dTheta(1,0) = -spsi*stheta;
156 dR_dTheta(2,0) = -ctheta;
158 dR_dTheta(0,1) = cpsi*ctheta*sphi;
159 dR_dTheta(1,1) = ctheta*sphi*spsi;
160 dR_dTheta(2,1) = -sphi*stheta;
162 dR_dTheta(0,2) = cphi*cpsi*ctheta;
163 dR_dTheta(1,2) = cphi*ctheta*spsi;
164 dR_dTheta(2,2) = -cphi*stheta;
166 dR_dPsi(0,0) = -ctheta*spsi;
167 dR_dPsi(1,0) = cpsi*ctheta;
170 dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
171 dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
174 dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
175 dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
178 g[3] = matricesInnerProd(dR_dPhi, R);
179 g[4] = matricesInnerProd(dR_dTheta, R);
180 g[5] = matricesInnerProd(dR_dPsi, R);
184 template <
typename Po
intSource,
typename Po
intTarget>
void
186 const std::vector<int> &indices_src,
188 const std::vector<int> &indices_tgt,
189 Eigen::Matrix4f &transformation_matrix)
191 if (indices_src.size () < 4)
194 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () <<
" points!");
200 x[0] = transformation_matrix (0,3);
201 x[1] = transformation_matrix (1,3);
202 x[2] = transformation_matrix (2,3);
205 x[3] = std::atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
206 x[4] = asin (-transformation_matrix (2,0));
207 x[5] = std::atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
210 tmp_src_ = &cloud_src;
211 tmp_tgt_ = &cloud_tgt;
212 tmp_idx_src_ = &indices_src;
213 tmp_idx_tgt_ = &indices_tgt;
225 int inner_iterations_ = 0;
240 PCL_DEBUG (
"[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
241 PCL_DEBUG (
"BFGS solver finished with exit code %i \n", result);
242 transformation_matrix.setIdentity();
243 applyState(transformation_matrix, x);
247 "[pcl::" << getClassName () <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
251 template <
typename Po
intSource,
typename Po
intTarget>
inline double
258 for (
int i = 0; i < m; ++i)
264 Eigen::Vector4f pp (transformation_matrix * p_src);
267 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
270 f+= double(res.transpose() * temp);
276 template <
typename Po
intSource,
typename Po
intTarget>
inline void
279 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
280 gicp_->applyState(transformation_matrix, x);
284 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
285 int m =
static_cast<int> (gicp_->tmp_idx_src_->size ());
286 for (
int i = 0; i < m; ++i)
289 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
291 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
293 Eigen::Vector4f pp (transformation_matrix * p_src);
295 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
297 Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
302 pp = gicp_->base_transformation_ * p_src;
303 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
304 R+= p_src3 * temp.transpose();
306 g.head<3> ()*= 2.0/m;
308 gicp_->computeRDerivative(x, R, g);
312 template <
typename Po
intSource,
typename Po
intTarget>
inline void
315 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
316 gicp_->applyState(transformation_matrix, x);
319 Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
320 const int m =
static_cast<int> (gicp_->tmp_idx_src_->size ());
321 for (
int i = 0; i < m; ++i)
324 Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
326 Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
327 Eigen::Vector4f pp (transformation_matrix * p_src);
329 Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
331 Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
333 f+= double(res.transpose() * temp);
337 pp = gicp_->base_transformation_ * p_src;
338 Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
340 R+= p_src3 * temp.transpose();
343 g.head<3> ()*=
double(2.0/m);
345 gicp_->computeRDerivative(x, R, g);
351 auto translation_epsilon = gicp_->translation_gradient_tolerance_;
352 auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
354 if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
358 auto translation_grad = g.head<3>().norm();
361 auto rotation_grad = g.tail<3>().norm();
363 if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
369 template <
typename Po
intSource,
typename Po
intTarget>
inline void
377 const std::size_t N =
indices_->size ();
397 std::vector<int> nn_indices (1);
398 std::vector<float> nn_dists (1);
405 std::vector<int> source_indices (
indices_->size ());
406 std::vector<int> target_indices (
indices_->size ());
409 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
410 for(std::size_t i = 0; i < 4; i++)
411 for(std::size_t j = 0; j < 4; j++)
412 for(std::size_t k = 0; k < 4; k++)
415 Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
417 for (std::size_t i = 0; i < N; i++)
419 PointSource query = output[i];
424 PCL_ERROR (
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n",
getClassName ().c_str (), (*
indices_)[i]);
429 if (nn_dists[0] < dist_threshold)
431 Eigen::Matrix3d &C1 = (*input_covariances_)[i];
432 Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
437 Eigen::Matrix3d temp = M * R.transpose();
441 source_indices[cnt] =
static_cast<int> (i);
442 target_indices[cnt] = nn_indices[0];
447 source_indices.resize(cnt); target_indices.resize(cnt);
456 for(
int k = 0; k < 4; k++) {
457 for(
int l = 0; l < 4; l++) {
471 PCL_DEBUG (
"[pcl::%s::computeTransformation] Optimization issue %s\n",
getClassName ().c_str (), e.what ());
479 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
484 PCL_DEBUG (
"[pcl::%s::computeTransformation] Convergence failed\n",
getClassName ().c_str ());
493 template <
typename Po
intSource,
typename Po
intTarget>
void
498 R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
499 * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
500 * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
501 t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
502 Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
508 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
Eigen::Matrix4f base_transformation_
base transformation
shared_ptr< KdTree< PointT, Tree > > Ptr
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const std::vector< int > &indices_src, const PointCloudTarget &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
BFGSSpace::Status testGradient()
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
A base class for all pcl exceptions which inherits from std::runtime_error.
std::function< void(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &src_indices, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &tgt_indices, Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_
void df(const Vector6d &x, Vector6d &df) override
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
const GeneralizedIterativeClosestPoint * gicp_
IndicesPtr indices_
A pointer to the vector of point indices to use.
void fdf(const Vector6d &x, double &f, Vector6d &df) override
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
BFGSSpace::Status checkGradient(const Vector6d &g) override
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, MatricesVector &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
Eigen::Matrix< double, 6, 1 > Vector6d
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
KdTreePtr tree_
A pointer to the spatial search object.
const std::string & getClassName() const
Abstract class get name method.
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Matrix4 transformation_
The transformation matrix estimated by the registration method.
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
An exception that is thrown when the number of correspondents is not equal to the minimum required...
double rotation_epsilon_
The epsilon constant for rotation error.
An exception that is thrown when the non linear solver didn't converge.
MatricesVectorPtr input_covariances_
Input cloud points covariances.
bool converged_
Holds internal convergence state, given user parameters.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
shared_ptr< const PointCloud< PointT > > ConstPtr
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
PointCloudConstPtr input_
The input point cloud dataset.
A point structure representing Euclidean xyz coordinates, and the RGB color.
double operator()(const Vector6d &x) override
BFGSSpace::Status minimizeOneStep(FVectorType &x)
BFGSSpace::Status minimizeInit(FVectorType &x)
optimization functor structure
MatricesVectorPtr target_covariances_
Target cloud points covariances.
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear...