41 #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
42 #define PCL_REGISTRATION_IMPL_GICP_HPP_
44 #include <pcl/registration/exceptions.h>
48 template <
typename Po
intSource,
typename Po
intTarget>
49 template <
typename Po
intT>
56 if (k_correspondences_ >
int(cloud->
size())) {
57 PCL_ERROR(
"[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
58 "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
66 nn_indecies.reserve(k_correspondences_);
67 std::vector<float> nn_dist_sq;
68 nn_dist_sq.reserve(k_correspondences_);
71 if (cloud_covariances.size() < cloud->
size())
72 cloud_covariances.resize(cloud->
size());
74 MatricesVector::iterator matrices_iterator = cloud_covariances.begin();
75 for (
auto points_iterator = cloud->
begin(); points_iterator != cloud->
end();
76 ++points_iterator, ++matrices_iterator) {
77 const PointT& query_point = *points_iterator;
78 Eigen::Matrix3d& cov = *matrices_iterator;
84 kdtree->
nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
87 for (
int j = 0; j < k_correspondences_; j++) {
88 const PointT& pt = (*cloud)[nn_indecies[j]];
94 cov(0, 0) += pt.x * pt.x;
96 cov(1, 0) += pt.y * pt.x;
97 cov(1, 1) += pt.y * pt.y;
99 cov(2, 0) += pt.z * pt.x;
100 cov(2, 1) += pt.z * pt.y;
101 cov(2, 2) += pt.z * pt.z;
104 mean /=
static_cast<double>(k_correspondences_);
106 for (
int k = 0; k < 3; k++)
107 for (
int l = 0; l <= k; l++) {
108 cov(k, l) /=
static_cast<double>(k_correspondences_);
109 cov(k, l) -= mean[k] * mean[l];
110 cov(l, k) = cov(k, l);
114 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
116 Eigen::Matrix3d U = svd.matrixU();
119 for (
int k = 0; k < 3; k++) {
120 Eigen::Vector3d col = U.col(k);
124 cov += v * col * col.transpose();
129 template <
typename Po
intSource,
typename Po
intTarget>
134 Eigen::Matrix3d dR_dPhi;
135 Eigen::Matrix3d dR_dTheta;
136 Eigen::Matrix3d dR_dPsi;
138 double phi = x[3], theta = x[4], psi = x[5];
140 double cphi = std::cos(phi), sphi = sin(phi);
141 double ctheta = std::cos(theta), stheta = sin(theta);
142 double cpsi = std::cos(psi), spsi = sin(psi);
148 dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
149 dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
150 dR_dPhi(2, 1) = cphi * ctheta;
152 dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
153 dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
154 dR_dPhi(2, 2) = -ctheta * sphi;
156 dR_dTheta(0, 0) = -cpsi * stheta;
157 dR_dTheta(1, 0) = -spsi * stheta;
158 dR_dTheta(2, 0) = -ctheta;
160 dR_dTheta(0, 1) = cpsi * ctheta * sphi;
161 dR_dTheta(1, 1) = ctheta * sphi * spsi;
162 dR_dTheta(2, 1) = -sphi * stheta;
164 dR_dTheta(0, 2) = cphi * cpsi * ctheta;
165 dR_dTheta(1, 2) = cphi * ctheta * spsi;
166 dR_dTheta(2, 2) = -cphi * stheta;
168 dR_dPsi(0, 0) = -ctheta * spsi;
169 dR_dPsi(1, 0) = cpsi * ctheta;
172 dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
173 dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
176 dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
177 dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
180 g[3] = matricesInnerProd(dR_dPhi, R);
181 g[4] = matricesInnerProd(dR_dTheta, R);
182 g[5] = matricesInnerProd(dR_dPsi, R);
185 template <
typename Po
intSource,
typename Po
intTarget>
192 Eigen::Matrix4f& transformation_matrix)
194 if (indices_src.size() < 4)
198 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
199 "at least 4 points to estimate a transform! Source and target have "
200 << indices_src.size() <<
" points!");
204 Vector6d x = Vector6d::Zero();
206 x[0] = transformation_matrix(0, 3);
207 x[1] = transformation_matrix(1, 3);
208 x[2] = transformation_matrix(2, 3);
211 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
212 x[4] = asin(-transformation_matrix(2, 0));
213 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
216 tmp_src_ = &cloud_src;
217 tmp_tgt_ = &cloud_tgt;
218 tmp_idx_src_ = &indices_src;
219 tmp_idx_tgt_ = &indices_tgt;
222 OptimizationFunctorWithIndices functor(
this);
224 bfgs.parameters.sigma = 0.01;
225 bfgs.parameters.rho = 0.01;
226 bfgs.parameters.tau1 = 9;
227 bfgs.parameters.tau2 = 0.05;
228 bfgs.parameters.tau3 = 0.5;
229 bfgs.parameters.order = 3;
231 int inner_iterations_ = 0;
232 int result = bfgs.minimizeInit(x);
236 result = bfgs.minimizeOneStep(x);
240 result = bfgs.testGradient();
243 inner_iterations_ == max_inner_iterations_) {
244 PCL_DEBUG(
"[pcl::registration::TransformationEstimationBFGS::"
245 "estimateRigidTransformation]");
246 PCL_DEBUG(
"BFGS solver finished with exit code %i \n", result);
247 transformation_matrix.setIdentity();
248 applyState(transformation_matrix, x);
252 SolverDidntConvergeException,
253 "[pcl::" << getClassName()
254 <<
"::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
255 "solver didn't converge!");
258 template <
typename Po
intSource,
typename Po
intTarget>
263 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
264 gicp_->applyState(transformation_matrix, x);
266 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
267 for (
int i = 0; i < m; ++i) {
270 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
273 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
274 Eigen::Vector4f pp(transformation_matrix * p_src);
277 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
278 Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
281 f += double(res.transpose() * temp);
286 template <
typename Po
intSource,
typename Po
intTarget>
291 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
292 gicp_->applyState(transformation_matrix, x);
296 Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
297 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
298 for (
int i = 0; i < m; ++i) {
301 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
304 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
306 Eigen::Vector4f pp(transformation_matrix * p_src);
308 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
310 Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
316 pp = gicp_->base_transformation_ * p_src;
317 Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
318 R += p_src3 * temp.transpose();
320 g.head<3>() *= 2.0 / m;
322 gicp_->computeRDerivative(x, R, g);
325 template <
typename Po
intSource,
typename Po
intTarget>
330 Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
331 gicp_->applyState(transformation_matrix, x);
334 Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
335 const int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
336 for (
int i = 0; i < m; ++i) {
339 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
342 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
343 Eigen::Vector4f pp(transformation_matrix * p_src);
345 Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
347 Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
349 f += double(res.transpose() * temp);
354 pp = gicp_->base_transformation_ * p_src;
355 Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
357 R += p_src3 * temp.transpose();
360 g.head<3>() *=
double(2.0 / m);
362 gicp_->computeRDerivative(x, R, g);
365 template <
typename Po
intSource,
typename Po
intTarget>
370 auto translation_epsilon = gicp_->translation_gradient_tolerance_;
371 auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
373 if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
377 auto translation_grad = g.head<3>().norm();
380 auto rotation_grad = g.tail<3>().norm();
382 if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
388 template <
typename Po
intSource,
typename Po
intTarget>
397 const std::size_t N = indices_->size();
399 mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
401 if ((!target_covariances_) || (target_covariances_->empty())) {
403 computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
406 if ((!input_covariances_) || (input_covariances_->empty())) {
407 input_covariances_.reset(
new MatricesVector);
408 computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
411 base_transformation_ = Eigen::Matrix4f::Identity();
414 double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
416 std::vector<float> nn_dists(1);
420 while (!converged_) {
426 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
427 for (std::size_t i = 0; i < 4; i++)
428 for (std::size_t j = 0; j < 4; j++)
429 for (std::size_t k = 0; k < 4; k++)
430 transform_R(i, j) += double(transformation_(i, k)) * double(guess(k, j));
432 Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
434 for (std::size_t i = 0; i < N; i++) {
435 PointSource query = output[i];
436 query.getVector4fMap() = transformation_ * query.getVector4fMap();
438 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
439 PCL_ERROR(
"[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
440 "in the target dataset for point %d in the source!\n",
441 getClassName().c_str(),
448 if (nn_dists[0] < dist_threshold) {
449 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
450 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
451 Eigen::Matrix3d& M = mahalanobis_[i];
455 Eigen::Matrix3d temp = M * R.transpose();
459 source_indices[cnt] =
static_cast<int>(i);
460 target_indices[cnt] = nn_indices[0];
465 source_indices.resize(cnt);
466 target_indices.resize(cnt);
468 previous_transformation_ = transformation_;
471 rigid_transformation_estimation_(
472 output, source_indices, *target_, target_indices, transformation_);
475 for (
int k = 0; k < 4; k++) {
476 for (
int l = 0; l < 4; l++) {
479 ratio = 1. / rotation_epsilon_;
481 ratio = 1. / transformation_epsilon_;
483 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
488 }
catch (PCLException& e) {
489 PCL_DEBUG(
"[pcl::%s::computeTransformation] Optimization issue %s\n",
490 getClassName().c_str(),
496 if (nr_iterations_ >= max_iterations_ || delta < 1) {
498 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence reached. Number of "
499 "iterations: %d out of %d. Transformation difference: %f\n",
500 getClassName().c_str(),
503 (transformation_ - previous_transformation_).array().abs().sum());
504 previous_transformation_ = transformation_;
507 PCL_DEBUG(
"[pcl::%s::computeTransformation] Convergence failed\n",
508 getClassName().c_str());
510 final_transformation_ = previous_transformation_ * guess;
516 template <
typename Po
intSource,
typename Po
intTarget>
519 Eigen::Matrix4f& t,
const Vector6d& x)
const
523 R = Eigen::AngleAxisf(
static_cast<float>(x[5]), Eigen::Vector3f::UnitZ()) *
524 Eigen::AngleAxisf(
static_cast<float>(x[4]), Eigen::Vector3f::UnitY()) *
525 Eigen::AngleAxisf(
static_cast<float>(x[3]), Eigen::Vector3f::UnitX());
526 t.topLeftCorner<3, 3>().matrix() = R * t.topLeftCorner<3, 3>().matrix();
527 Eigen::Vector4f T(
static_cast<float>(x[0]),
528 static_cast<float>(x[1]),
529 static_cast<float>(x[2]),
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
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.
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
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
Eigen::Matrix< double, 6, 1 > Vector6d
An exception that is thrown when the number of correspondents is not equal to the minimum required.
iterator begin() noexcept
shared_ptr< const PointCloud< PointT > > ConstPtr
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
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.
shared_ptr< KdTree< PointT, Tree > > Ptr
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
@ NegativeGradientEpsilon
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
BFGSSpace::Status checkGradient(const Vector6d &g) override
void df(const Vector6d &x, Vector6d &df) override
double operator()(const Vector6d &x) override
void fdf(const Vector6d &x, double &f, Vector6d &df) override
A point structure representing Euclidean xyz coordinates, and the RGB color.