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>
49 template <
typename Po
intSource,
typename Po
intTarget>
50 template <
typename Po
intT>
57 if (k_correspondences_ >
int(cloud->
size())) {
58 PCL_ERROR(
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
59 "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
66 std::vector<int> nn_indecies;
67 nn_indecies.reserve(k_correspondences_);
68 std::vector<float> nn_dist_sq;
69 nn_dist_sq.reserve(k_correspondences_);
72 if (cloud_covariances.size() < cloud->
size())
73 cloud_covariances.resize(cloud->
size());
75 MatricesVector::iterator matrices_iterator = cloud_covariances.begin();
76 for (
auto points_iterator = cloud->
begin(); points_iterator != cloud->
end();
77 ++points_iterator, ++matrices_iterator) {
78 const PointT& query_point = *points_iterator;
79 Eigen::Matrix3d& cov = *matrices_iterator;
85 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
88 for (
int j = 0; j < k_correspondences_; j++) {
89 const PointT& pt = (*cloud)[nn_indecies[j]];
95 cov(0, 0) += pt.x * pt.x;
97 cov(1, 0) += pt.y * pt.x;
98 cov(1, 1) += pt.y * pt.y;
100 cov(2, 0) += pt.z * pt.x;
101 cov(2, 1) += pt.z * pt.y;
102 cov(2, 2) += pt.z * pt.z;
105 mean /=
static_cast<double>(k_correspondences_);
107 for (
int k = 0; k < 3; k++)
108 for (
int l = 0; l <= k; l++) {
109 cov(k, l) /=
static_cast<double>(k_correspondences_);
110 cov(k, l) -= mean[k] * mean[l];
111 cov(l, k) = cov(k, l);
115 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
117 Eigen::Matrix3d U = svd.matrixU();
120 for (
int k = 0; k < 3; k++) {
121 Eigen::Vector3d col = U.col(k);
125 cov += v * col * col.transpose();
130 template <
typename Po
intSource,
typename Po
intTarget>
135 Eigen::Matrix3d dR_dPhi;
136 Eigen::Matrix3d dR_dTheta;
137 Eigen::Matrix3d dR_dPsi;
139 double phi = x[3], theta = x[4], psi = x[5];
141 double cphi = std::cos(phi), sphi = sin(phi);
142 double ctheta = std::cos(theta), stheta = sin(theta);
143 double cpsi = std::cos(psi), spsi = sin(psi);
149 dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
150 dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
151 dR_dPhi(2, 1) = cphi * ctheta;
153 dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
154 dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
155 dR_dPhi(2, 2) = -ctheta * sphi;
157 dR_dTheta(0, 0) = -cpsi * stheta;
158 dR_dTheta(1, 0) = -spsi * stheta;
159 dR_dTheta(2, 0) = -ctheta;
161 dR_dTheta(0, 1) = cpsi * ctheta * sphi;
162 dR_dTheta(1, 1) = ctheta * sphi * spsi;
163 dR_dTheta(2, 1) = -sphi * stheta;
165 dR_dTheta(0, 2) = cphi * cpsi * ctheta;
166 dR_dTheta(1, 2) = cphi * ctheta * spsi;
167 dR_dTheta(2, 2) = -cphi * stheta;
169 dR_dPsi(0, 0) = -ctheta * spsi;
170 dR_dPsi(1, 0) = cpsi * ctheta;
173 dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
174 dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
177 dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
178 dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
181 g[3] = matricesInnerProd(dR_dPhi, R);
182 g[4] = matricesInnerProd(dR_dTheta, R);
183 g[5] = matricesInnerProd(dR_dPsi, R);
186 template <
typename Po
intSource,
typename Po
intTarget>
190 const std::vector<int>& indices_src,
192 const std::vector<int>& indices_tgt,
193 Eigen::Matrix4f& transformation_matrix)
195 if (indices_src.size() < 4)
199 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
200 "at least 4 points to estimate a transform! Source and target have "
201 << indices_src.size() <<
" points!");
207 x[0] = transformation_matrix(0, 3);
208 x[1] = transformation_matrix(1, 3);
209 x[2] = transformation_matrix(2, 3);
212 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
213 x[4] = asin(-transformation_matrix(2, 0));
214 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
217 tmp_src_ = &cloud_src;
218 tmp_tgt_ = &cloud_tgt;
219 tmp_idx_src_ = &indices_src;
220 tmp_idx_tgt_ = &indices_tgt;
232 int inner_iterations_ = 0;
244 inner_iterations_ == max_inner_iterations_) {
245 PCL_DEBUG(
"[pcl::registration::TransformationEstimationBFGS::"
246 "estimateRigidTransformation]");
247 PCL_DEBUG(
"BFGS solver finished with exit code %i \n", result);
248 transformation_matrix.setIdentity();
249 applyState(transformation_matrix, x);
254 "[pcl::" << getClassName()
255 <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
256 "solver didn't converge!");
259 template <
typename Po
intSource,
typename Po
intTarget>
264 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
265 gicp_->applyState(transformation_matrix, x);
267 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
268 for (
int i = 0; i < m; ++i) {
271 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
274 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
275 Eigen::Vector4f pp(transformation_matrix * p_src);
278 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
279 Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
282 f += double(res.transpose() * temp);
287 template <
typename Po
intSource,
typename Po
intTarget>
292 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
293 gicp_->applyState(transformation_matrix, x);
297 Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
298 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
299 for (
int i = 0; i < m; ++i) {
302 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
305 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
307 Eigen::Vector4f pp(transformation_matrix * p_src);
309 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
311 Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
317 pp = gicp_->base_transformation_ * p_src;
318 Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
319 R += p_src3 * temp.transpose();
321 g.head<3>() *= 2.0 / m;
323 gicp_->computeRDerivative(x, R, g);
326 template <
typename Po
intSource,
typename Po
intTarget>
331 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
332 gicp_->applyState(transformation_matrix, x);
335 Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
336 const int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
337 for (
int i = 0; i < m; ++i) {
340 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
343 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
344 Eigen::Vector4f pp(transformation_matrix * p_src);
346 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
348 Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
350 f += double(res.transpose() * temp);
355 pp = gicp_->base_transformation_ * p_src;
356 Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
358 R += p_src3 * temp.transpose();
361 g.head<3>() *=
double(2.0 / m);
363 gicp_->computeRDerivative(x, R, g);
366 template <
typename Po
intSource,
typename Po
intTarget>
371 auto translation_epsilon = gicp_->translation_gradient_tolerance_;
372 auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
374 if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
378 auto translation_grad = g.head<3>().norm();
381 auto rotation_grad = g.tail<3>().norm();
383 if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
389 template <
typename Po
intSource,
typename Po
intTarget>
398 const std::size_t N = indices_->size();
400 mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
402 if ((!target_covariances_) || (target_covariances_->empty())) {
404 computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
407 if ((!input_covariances_) || (input_covariances_->empty())) {
409 computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
412 base_transformation_ = Eigen::Matrix4f::Identity();
415 double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
416 std::vector<int> nn_indices(1);
417 std::vector<float> nn_dists(1);
421 while (!converged_) {
423 std::vector<int> source_indices(indices_->size());
424 std::vector<int> target_indices(indices_->size());
427 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
428 for (std::size_t i = 0; i < 4; i++)
429 for (std::size_t j = 0; j < 4; j++)
430 for (std::size_t k = 0; k < 4; k++)
431 transform_R(i, j) += double(transformation_(i, k)) * double(guess(k, j));
433 Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
435 for (std::size_t i = 0; i < N; i++) {
436 PointSource query = output[i];
437 query.getVector4fMap() = transformation_ * query.getVector4fMap();
439 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
440 PCL_ERROR(
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
441 "in the target dataset for point %d in the source!\n",
442 getClassName().c_str(),
449 if (nn_dists[0] < dist_threshold) {
450 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
451 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
452 Eigen::Matrix3d& M = mahalanobis_[i];
456 Eigen::Matrix3d temp = M * R.transpose();
460 source_indices[cnt] =
static_cast<int>(i);
461 target_indices[cnt] = nn_indices[0];
466 source_indices.resize(cnt);
467 target_indices.resize(cnt);
469 previous_transformation_ = transformation_;
472 rigid_transformation_estimation_(
473 output, source_indices, *target_, target_indices, transformation_);
476 for (
int k = 0; k < 4; k++) {
477 for (
int l = 0; l < 4; l++) {
480 ratio = 1. / rotation_epsilon_;
482 ratio = 1. / transformation_epsilon_;
484 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
490 PCL_DEBUG(
"[pcl::%s::computeTransformation] Optimization issue %s\n",
491 getClassName().c_str(),
497 if (nr_iterations_ >= max_iterations_ || delta < 1) {
499 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence reached. Number of "
500 "iterations: %d out of %d. Transformation difference: %f\n",
501 getClassName().c_str(),
504 (transformation_ - previous_transformation_).array().abs().sum());
505 previous_transformation_ = transformation_;
508 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence failed\n",
509 getClassName().c_str());
511 final_transformation_ = previous_transformation_ * guess;
517 template <
typename Po
intSource,
typename Po
intTarget>
520 Eigen::Matrix4f& t,
const Vector6d& x)
const
524 R = Eigen::AngleAxisf(
static_cast<float>(x[5]), Eigen::Vector3f::UnitZ()) *
525 Eigen::AngleAxisf(
static_cast<float>(x[4]), Eigen::Vector3f::UnitY()) *
526 Eigen::AngleAxisf(
static_cast<float>(x[3]), Eigen::Vector3f::UnitX());
527 t.topLeftCorner<3, 3>().matrix() = R * t.topLeftCorner<3, 3>().matrix();
528 Eigen::Vector4f T(
static_cast<float>(x[0]),
529 static_cast<float>(x[1]),
530 static_cast<float>(x[2]),
537 #endif // PCL_REGISTRATION_IMPL_GICP_HPP_