diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index 2eae855abbf..65041fbabd7 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -49,8 +49,9 @@ namespace pcl { * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf * The approach is based on using anisotropic cost functions to optimize the alignment * after closest point assignments have been made. - * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and - * FLANN. + * The original code uses GSL and ANN while in ours we use FLANN and Newton's method + * for optimization (call `useBFGS` to switch to BFGS optimizer, however Newton + * is usually faster and more accurate). * \author Nizar Sallem * \ingroup registration */ @@ -111,6 +112,7 @@ class GeneralizedIterativeClosestPoint using Matrix3 = typename Eigen::Matrix; using Matrix4 = typename IterativeClosestPoint::Matrix4; + using Matrix6d = Eigen::Matrix; using AngleAxis = typename Eigen::AngleAxis; PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -135,7 +137,7 @@ class GeneralizedIterativeClosestPoint const PointCloudTarget& cloud_tgt, const pcl::Indices& indices_tgt, Matrix4& transformation_matrix) { - estimateRigidTransformationBFGS( + estimateRigidTransformationNewton( cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); }; } @@ -214,6 +216,23 @@ class GeneralizedIterativeClosestPoint const pcl::Indices& indices_tgt, Matrix4& transformation_matrix); + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using an iterative non-linear Newton approach. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing + * the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing + * the correspondences of the interest points from \a indices_src + * \param[in,out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformationNewton(const PointCloudSource& cloud_src, + const pcl::Indices& indices_src, + const PointCloudTarget& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix); + /** \brief \return Mahalanobis distance matrix for the given point index */ inline const Eigen::Matrix3d& mahalanobis(std::size_t index) const @@ -276,6 +295,21 @@ class GeneralizedIterativeClosestPoint return k_correspondences_; } + /** \brief Use BFGS optimizer instead of default Newton optimizer + */ + void + useBFGS() + { + rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src, + const pcl::Indices& indices_src, + const PointCloudTarget& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) { + estimateRigidTransformationBFGS( + cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); + }; + } + /** \brief Set maximum number of iterations at the optimization step * \param[in] max maximum number of iterations for the optimizer */ @@ -447,6 +481,8 @@ class GeneralizedIterativeClosestPoint df(const Vector6d& x, Vector6d& df) override; void fdf(const Vector6d& x, double& f, Vector6d& df) override; + void + dfddf(const Vector6d& x, Vector6d& df, Matrix6d& ddf); BFGSSpace::Status checkGradient(const Vector6d& g) override; @@ -459,6 +495,26 @@ class GeneralizedIterativeClosestPoint const pcl::Indices& tgt_indices, Matrix4& transformation_matrix)> rigid_transformation_estimation_; + +private: + void + getRDerivatives(double phi, + double theta, + double psi, + Eigen::Matrix3d& dR_dPhi, + Eigen::Matrix3d& dR_dTheta, + Eigen::Matrix3d& dR_dPsi) const; + + void + getR2ndDerivatives(double phi, + double theta, + double psi, + Eigen::Matrix3d& ddR_dPhi_dPhi, + Eigen::Matrix3d& ddR_dPhi_dTheta, + Eigen::Matrix3d& ddR_dPhi_dPsi, + Eigen::Matrix3d& ddR_dTheta_dTheta, + Eigen::Matrix3d& ddR_dTheta_dPsi, + Eigen::Matrix3d& ddR_dPsi_dPsi) const; }; } // namespace pcl diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index b3ce37af2f3..3e1bcb62ed6 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -129,19 +129,17 @@ GeneralizedIterativeClosestPoint::computeCovar template void -GeneralizedIterativeClosestPoint::computeRDerivative( - const Vector6d& x, const Eigen::Matrix3d& dCost_dR_T, Vector6d& g) const +GeneralizedIterativeClosestPoint::getRDerivatives( + double phi, + double theta, + double psi, + Eigen::Matrix3d& dR_dPhi, + Eigen::Matrix3d& dR_dTheta, + Eigen::Matrix3d& dR_dPsi) const { - Eigen::Matrix3d dR_dPhi; - Eigen::Matrix3d dR_dTheta; - Eigen::Matrix3d dR_dPsi; - - double phi = x[3], theta = x[4], psi = x[5]; - - double cphi = std::cos(phi), sphi = sin(phi); - double ctheta = std::cos(theta), stheta = sin(theta); - double cpsi = std::cos(psi), spsi = sin(psi); - + const double cphi = std::cos(phi), sphi = std::sin(phi); + const double ctheta = std::cos(theta), stheta = std::sin(theta); + const double cpsi = std::cos(psi), spsi = std::sin(psi); dR_dPhi(0, 0) = 0.; dR_dPhi(1, 0) = 0.; dR_dPhi(2, 0) = 0.; @@ -177,10 +175,97 @@ GeneralizedIterativeClosestPoint::computeRDeri dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta; dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta; dR_dPsi(2, 2) = 0.; +} + +template +void +GeneralizedIterativeClosestPoint::computeRDerivative( + const Vector6d& x, const Eigen::Matrix3d& dCost_dR_T, Vector6d& g) const +{ + Eigen::Matrix3d dR_dPhi; + Eigen::Matrix3d dR_dTheta; + Eigen::Matrix3d dR_dPsi; + getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi); - g[3] = matricesInnerProd(dR_dPhi, dCost_dR_T); - g[4] = matricesInnerProd(dR_dTheta, dCost_dR_T); - g[5] = matricesInnerProd(dR_dPsi, dCost_dR_T); + g[3] = (dR_dPhi * dCost_dR_T).trace(); + g[4] = (dR_dTheta * dCost_dR_T).trace(); + g[5] = (dR_dPsi * dCost_dR_T).trace(); +} + +template +void +GeneralizedIterativeClosestPoint::getR2ndDerivatives( + double phi, + double theta, + double psi, + Eigen::Matrix3d& ddR_dPhi_dPhi, + Eigen::Matrix3d& ddR_dPhi_dTheta, + Eigen::Matrix3d& ddR_dPhi_dPsi, + Eigen::Matrix3d& ddR_dTheta_dTheta, + Eigen::Matrix3d& ddR_dTheta_dPsi, + Eigen::Matrix3d& ddR_dPsi_dPsi) const +{ + const double sphi = std::sin(phi), stheta = std::sin(theta), spsi = std::sin(psi); + const double cphi = std::cos(phi), ctheta = std::cos(theta), cpsi = std::cos(psi); + ddR_dPhi_dPhi(0, 0) = 0.0; + ddR_dPhi_dPhi(1, 0) = 0.0; + ddR_dPhi_dPhi(2, 0) = 0.0; + ddR_dPhi_dPhi(0, 1) = -cpsi * stheta * sphi + spsi * cphi; + ddR_dPhi_dPhi(1, 1) = -cpsi * cphi - spsi * stheta * sphi; + ddR_dPhi_dPhi(2, 1) = -ctheta * sphi; + ddR_dPhi_dPhi(0, 2) = -spsi * sphi - cpsi * stheta * cphi; + ddR_dPhi_dPhi(1, 2) = -spsi * stheta * cphi + cpsi * sphi; + ddR_dPhi_dPhi(2, 2) = -ctheta * cphi; + + ddR_dPhi_dTheta(0, 0) = 0.0; + ddR_dPhi_dTheta(1, 0) = 0.0; + ddR_dPhi_dTheta(2, 0) = 0.0; + ddR_dPhi_dTheta(0, 1) = cpsi * ctheta * cphi; + ddR_dPhi_dTheta(1, 1) = spsi * ctheta * cphi; + ddR_dPhi_dTheta(2, 1) = -stheta * cphi; + ddR_dPhi_dTheta(0, 2) = -cpsi * ctheta * sphi; + ddR_dPhi_dTheta(1, 2) = -spsi * ctheta * sphi; + ddR_dPhi_dTheta(2, 2) = stheta * sphi; + + ddR_dPhi_dPsi(0, 0) = 0.0; + ddR_dPhi_dPsi(1, 0) = 0.0; + ddR_dPhi_dPsi(2, 0) = 0.0; + ddR_dPhi_dPsi(0, 1) = -spsi * stheta * cphi + cpsi * sphi; + ddR_dPhi_dPsi(1, 1) = spsi * sphi + cpsi * stheta * cphi; + ddR_dPhi_dPsi(2, 1) = 0.0; + ddR_dPhi_dPsi(0, 2) = cpsi * cphi + spsi * stheta * sphi; + ddR_dPhi_dPsi(1, 2) = -cpsi * stheta * sphi + spsi * cphi; + ddR_dPhi_dPsi(2, 2) = 0.0; + + ddR_dTheta_dTheta(0, 0) = -cpsi * ctheta; + ddR_dTheta_dTheta(1, 0) = -spsi * ctheta; + ddR_dTheta_dTheta(2, 0) = stheta; + ddR_dTheta_dTheta(0, 1) = -cpsi * stheta * sphi; + ddR_dTheta_dTheta(1, 1) = -spsi * stheta * sphi; + ddR_dTheta_dTheta(2, 1) = -ctheta * sphi; + ddR_dTheta_dTheta(0, 2) = -cpsi * stheta * cphi; + ddR_dTheta_dTheta(1, 2) = -spsi * stheta * cphi; + ddR_dTheta_dTheta(2, 2) = -ctheta * cphi; + + ddR_dTheta_dPsi(0, 0) = spsi * stheta; + ddR_dTheta_dPsi(1, 0) = -cpsi * stheta; + ddR_dTheta_dPsi(2, 0) = 0.0; + ddR_dTheta_dPsi(0, 1) = -spsi * ctheta * sphi; + ddR_dTheta_dPsi(1, 1) = cpsi * ctheta * sphi; + ddR_dTheta_dPsi(2, 1) = 0.0; + ddR_dTheta_dPsi(0, 2) = -spsi * ctheta * cphi; + ddR_dTheta_dPsi(1, 2) = cpsi * ctheta * cphi; + ddR_dTheta_dPsi(2, 2) = 0.0; + + ddR_dPsi_dPsi(0, 0) = -cpsi * ctheta; + ddR_dPsi_dPsi(1, 0) = -spsi * ctheta; + ddR_dPsi_dPsi(2, 0) = 0.0; + ddR_dPsi_dPsi(0, 1) = -cpsi * stheta * sphi + spsi * cphi; + ddR_dPsi_dPsi(1, 1) = -cpsi * cphi - spsi * stheta * sphi; + ddR_dPsi_dPsi(2, 1) = 0.0; + ddR_dPsi_dPsi(0, 2) = -spsi * sphi - cpsi * stheta * cphi; + ddR_dPsi_dPsi(1, 2) = -spsi * stheta * cphi + cpsi * sphi; + ddR_dPsi_dPsi(2, 2) = 0.0; } template @@ -259,6 +344,116 @@ GeneralizedIterativeClosestPoint:: "solver didn't converge!"); } +template +void +GeneralizedIterativeClosestPoint:: + estimateRigidTransformationNewton(const PointCloudSource& cloud_src, + const pcl::Indices& indices_src, + const PointCloudTarget& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) +{ + // need at least min_number_correspondences_ samples + if (indices_src.size() < min_number_correspondences_) { + PCL_THROW_EXCEPTION(NotEnoughPointsException, + "[pcl::GeneralizedIterativeClosestPoint::" + "estimateRigidTransformationNewton] Need " + "at least " + << min_number_correspondences_ + << " points to estimate a transform! " + "Source and target have " + << indices_src.size() << " points!"); + return; + } + // Set the initial solution + Vector6d x = Vector6d::Zero(); + // translation part + x[0] = transformation_matrix(0, 3); + x[1] = transformation_matrix(1, 3); + x[2] = transformation_matrix(2, 3); + // rotation part (Z Y X euler angles convention) + // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations + x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2)); + x[4] = std::asin( + std::min(1.0, std::max(-1.0, -transformation_matrix(2, 0)))); + x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0)); + + // Set temporary pointers + tmp_src_ = &cloud_src; + tmp_tgt_ = &cloud_tgt; + tmp_idx_src_ = &indices_src; + tmp_idx_tgt_ = &indices_tgt; + + // Optimize using Newton + OptimizationFunctorWithIndices functor(this); + Eigen::Matrix hessian; + Eigen::Matrix gradient; + double current_x_value = functor(x); + functor.dfddf(x, gradient, hessian); + Eigen::Matrix delta; + int inner_iterations_ = 0; + do { + ++inner_iterations_; + // compute descent direction from hessian and gradient. Take special measures if + // hessian is not positive-definite (positive Eigenvalues) + Eigen::SelfAdjointEigenSolver> eigensolver(hessian); + Eigen::Matrix inverted_eigenvalues = + Eigen::Matrix::Zero(); + for (int i = 0; i < 6; ++i) { + const double ev = eigensolver.eigenvalues()[i]; + if (ev < 0) + inverted_eigenvalues(i, i) = 1.0 / eigensolver.eigenvalues()[5]; + else + inverted_eigenvalues(i, i) = 1.0 / ev; + } + delta = eigensolver.eigenvectors() * inverted_eigenvalues * + eigensolver.eigenvectors().transpose() * gradient; + + // simple line search to guarantee a decrease in the function value + double alpha = 1.0; + double candidate_x_value; + bool improvement_found = false; + for (int i = 0; i < 10; ++i, alpha /= 2) { + Vector6d candidate_x = x - alpha * delta; + candidate_x_value = functor(candidate_x); + if (candidate_x_value < current_x_value) { + PCL_DEBUG("[estimateRigidTransformationNewton] Using stepsize=%g, function " + "value previously: %g, now: %g, " + "improvement: %g\n", + alpha, + current_x_value, + candidate_x_value, + current_x_value - candidate_x_value); + x = candidate_x; + current_x_value = candidate_x_value; + improvement_found = true; + break; + } + } + if (!improvement_found) { + PCL_DEBUG("[estimateRigidTransformationNewton] finishing because no progress\n"); + break; + } + functor.dfddf(x, gradient, hessian); + if (gradient.head<3>().norm() < translation_gradient_tolerance_ && + gradient.tail<3>().norm() < rotation_gradient_tolerance_) { + PCL_DEBUG("[estimateRigidTransformationNewton] finishing because gradient below " + "threshold: translation: %g<%g, rotation: %g<%g\n", + gradient.head<3>().norm(), + translation_gradient_tolerance_, + gradient.tail<3>().norm(), + rotation_gradient_tolerance_); + break; + } + } while (inner_iterations_ < max_inner_iterations_); + PCL_DEBUG("[estimateRigidTransformationNewton] solver finished after %i iterations " + "(of max %i)\n", + inner_iterations_, + max_inner_iterations_); + transformation_matrix.setIdentity(); + applyState(transformation_matrix, x); +} + template inline double GeneralizedIterativeClosestPoint:: @@ -377,6 +572,152 @@ GeneralizedIterativeClosestPoint:: gicp_->computeRDerivative(x, dCost_dR_T, g); } +template +inline void +GeneralizedIterativeClosestPoint:: + OptimizationFunctorWithIndices::dfddf(const Vector6d& x, + Vector6d& gradient, + Matrix6d& hessian) +{ + Matrix4 transformation_matrix = gicp_->base_transformation_; + gicp_->applyState(transformation_matrix, x); + const Eigen::Matrix4f transformation_matrix_float = + transformation_matrix.template cast(); + const Eigen::Matrix4f base_transformation_float = + gicp_->base_transformation_.template cast(); + // Zero out gradient and hessian + gradient.setZero(); + hessian.setZero(); + // Helper matrices + Eigen::Matrix3d dR_dPhi; + Eigen::Matrix3d dR_dTheta; + Eigen::Matrix3d dR_dPsi; + gicp_->getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi); + Eigen::Matrix3d ddR_dPhi_dPhi; + Eigen::Matrix3d ddR_dPhi_dTheta; + Eigen::Matrix3d ddR_dPhi_dPsi; + Eigen::Matrix3d ddR_dTheta_dTheta; + Eigen::Matrix3d ddR_dTheta_dPsi; + Eigen::Matrix3d ddR_dPsi_dPsi; + gicp_->getR2ndDerivatives(x[3], + x[4], + x[5], + ddR_dPhi_dPhi, + ddR_dPhi_dTheta, + ddR_dPhi_dPsi, + ddR_dTheta_dTheta, + ddR_dTheta_dPsi, + ddR_dPsi_dPsi); + Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T1 = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T2 = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T3 = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T1b = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T2b = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d dCost_dR_T3b = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d hessian_rot_phi = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d hessian_rot_theta = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d hessian_rot_psi = Eigen::Matrix3d::Zero(); + Eigen::Matrix hessian_rot_tmp = Eigen::Matrix::Zero(); + + int m = static_cast(gicp_->tmp_idx_src_->size()); + for (int i = 0; i < m; ++i) { + // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp + const auto& src_idx = (*gicp_->tmp_idx_src_)[i]; + Vector4fMapConst p_src = (*gicp_->tmp_src_)[src_idx].getVector4fMap(); + // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp + Vector4fMapConst p_tgt = + (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap(); + Eigen::Vector4f p_trans_src(transformation_matrix_float * p_src); + // The last coordinate is still guaranteed to be set to 1.0 + // The d here is the negative of the d in the paper + const Eigen::Vector3d d(p_trans_src[0] - p_tgt[0], + p_trans_src[1] - p_tgt[1], + p_trans_src[2] - p_tgt[2]); + const Eigen::Matrix3d& M = gicp_->mahalanobis(src_idx); + const Eigen::Vector3d Md(M * d); // Md = M*d + gradient.head<3>() += Md; // translation gradient + hessian.block<3, 3>(0, 0) += M; // translation-translation hessian + p_trans_src = base_transformation_float * p_src; + const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]); + dCost_dR_T.noalias() += p_base_src * Md.transpose(); + dCost_dR_T1b += p_base_src[0] * M; + dCost_dR_T2b += p_base_src[1] * M; + dCost_dR_T3b += p_base_src[2] * M; + hessian_rot_tmp.noalias() += + Eigen::Map>{M.data()} * + (Eigen::Matrix() << p_base_src[0] * p_base_src[0], + p_base_src[0] * p_base_src[1], + p_base_src[0] * p_base_src[2], + p_base_src[1] * p_base_src[1], + p_base_src[1] * p_base_src[2], + p_base_src[2] * p_base_src[2]) + .finished(); + } + gradient.head<3>() *= 2.0 / m; // translation gradient + dCost_dR_T *= 2.0 / m; + gicp_->computeRDerivative(x, dCost_dR_T, gradient); // rotation gradient + hessian.block<3, 3>(0, 0) *= 2.0 / m; // translation-translation hessian + // translation-rotation hessian + dCost_dR_T1.row(0) = dCost_dR_T1b.col(0); + dCost_dR_T1.row(1) = dCost_dR_T2b.col(0); + dCost_dR_T1.row(2) = dCost_dR_T3b.col(0); + dCost_dR_T2.row(0) = dCost_dR_T1b.col(1); + dCost_dR_T2.row(1) = dCost_dR_T2b.col(1); + dCost_dR_T2.row(2) = dCost_dR_T3b.col(1); + dCost_dR_T3.row(0) = dCost_dR_T1b.col(2); + dCost_dR_T3.row(1) = dCost_dR_T2b.col(2); + dCost_dR_T3.row(2) = dCost_dR_T3b.col(2); + dCost_dR_T1 *= 2.0 / m; + dCost_dR_T2 *= 2.0 / m; + dCost_dR_T3 *= 2.0 / m; + hessian(3, 0) = (dR_dPhi * dCost_dR_T1).trace(); + hessian(4, 0) = (dR_dTheta * dCost_dR_T1).trace(); + hessian(5, 0) = (dR_dPsi * dCost_dR_T1).trace(); + hessian(3, 1) = (dR_dPhi * dCost_dR_T2).trace(); + hessian(4, 1) = (dR_dTheta * dCost_dR_T2).trace(); + hessian(5, 1) = (dR_dPsi * dCost_dR_T2).trace(); + hessian(3, 2) = (dR_dPhi * dCost_dR_T3).trace(); + hessian(4, 2) = (dR_dTheta * dCost_dR_T3).trace(); + hessian(5, 2) = (dR_dPsi * dCost_dR_T3).trace(); + hessian.block<3, 3>(0, 3) = hessian.block<3, 3>(3, 0).transpose(); + // rotation-rotation hessian + int lookup[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}}; + for (int l = 0; l < 3; ++l) { + for (int i = 0; i < 3; ++i) { + double phi_tmp = 0.0, theta_tmp = 0.0, psi_tmp = 0.0; + for (int j = 0; j < 3; ++j) { + for (int k = 0; k < 3; ++k) { + phi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPhi(j, k); + theta_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dTheta(j, k); + psi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPsi(j, k); + } + } + hessian_rot_phi(i, l) = phi_tmp; + hessian_rot_theta(i, l) = theta_tmp; + hessian_rot_psi(i, l) = psi_tmp; + } + } + hessian_rot_phi *= 2.0 / m; + hessian_rot_theta *= 2.0 / m; + hessian_rot_psi *= 2.0 / m; + hessian(3, 3) = (dR_dPhi.transpose() * hessian_rot_phi).trace() + + (ddR_dPhi_dPhi * dCost_dR_T).trace(); + hessian(3, 4) = (dR_dPhi.transpose() * hessian_rot_theta).trace() + + (ddR_dPhi_dTheta * dCost_dR_T).trace(); + hessian(3, 5) = (dR_dPhi.transpose() * hessian_rot_psi).trace() + + (ddR_dPhi_dPsi * dCost_dR_T).trace(); + hessian(4, 4) = (dR_dTheta.transpose() * hessian_rot_theta).trace() + + (ddR_dTheta_dTheta * dCost_dR_T).trace(); + hessian(4, 5) = (dR_dTheta.transpose() * hessian_rot_psi).trace() + + (ddR_dTheta_dPsi * dCost_dR_T).trace(); + hessian(5, 5) = (dR_dPsi.transpose() * hessian_rot_psi).trace() + + (ddR_dPsi_dPsi * dCost_dR_T).trace(); + hessian(4, 3) = hessian(3, 4); + hessian(5, 3) = hessian(3, 5); + hessian(5, 4) = hessian(4, 5); +} + template inline BFGSSpace::Status GeneralizedIterativeClosestPoint:: diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index 0d3b7dcdb6c..0b2d33712aa 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -585,6 +585,68 @@ TEST (PCL, GeneralizedIterativeClosestPoint) EXPECT_LT (reg.getFitnessScore (), 0.0001); } +TEST (PCL, GeneralizedIterativeClosestPointBFGS) +{ // same as above, but uses BFGS optimizer + using PointT = PointXYZ; + PointCloud::Ptr src (new PointCloud); + copyPointCloud (cloud_source, *src); + PointCloud::Ptr tgt (new PointCloud); + copyPointCloud (cloud_target, *tgt); + PointCloud output; + + GeneralizedIterativeClosestPoint reg; + reg.useBFGS (); + reg.setInputSource (src); + reg.setInputTarget (tgt); + reg.setMaximumIterations (50); + reg.setTransformationEpsilon (1e-8); + + // Register + reg.align (output); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 0.0001); + + // Check again, for all possible caching schemes + for (int iter = 0; iter < 4; iter++) + { + bool force_cache = static_cast (iter/2); + bool force_cache_reciprocal = static_cast (iter%2); + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + // Ensure that, when force_cache is not set, we are robust to the wrong input + if (force_cache) + tree->setInputCloud (tgt); + reg.setSearchMethodTarget (tree, force_cache); + + pcl::search::KdTree::Ptr tree_recip (new pcl::search::KdTree); + if (force_cache_reciprocal) + tree_recip->setInputCloud (src); + reg.setSearchMethodSource (tree_recip, force_cache_reciprocal); + + // Register + reg.align (output); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 0.001); + } + + // Test guess matrix + Eigen::Isometry3f transform = Eigen::Isometry3f (Eigen::AngleAxisf (0.25 * M_PI, Eigen::Vector3f::UnitX ()) + * Eigen::AngleAxisf (0.50 * M_PI, Eigen::Vector3f::UnitY ()) + * Eigen::AngleAxisf (0.33 * M_PI, Eigen::Vector3f::UnitZ ())); + transform.translation () = Eigen::Vector3f (0.1, 0.2, 0.3); + PointCloud::Ptr transformed_tgt (new PointCloud); + pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); // transformed_tgt is now a copy of tgt with a transformation matrix applied + + GeneralizedIterativeClosestPoint reg_guess; + reg_guess.useBFGS (); + reg_guess.setInputSource (src); + reg_guess.setInputTarget (transformed_tgt); + reg_guess.setMaximumIterations (50); + reg_guess.setTransformationEpsilon (1e-8); + reg_guess.align (output, transform.matrix ()); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 0.0001); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, GeneralizedIterativeClosestPoint6D) { @@ -593,7 +655,7 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) PointCloud::Ptr src_full (new PointCloud); copyPointCloud (cloud_with_color, *src_full); PointCloud::Ptr tgt_full (new PointCloud); - sampleRandomTransform (delta_transform, M_PI/0.1, .03); + sampleRandomTransform (delta_transform, 0.25*M_PI, .03); pcl::transformPointCloud (cloud_with_color, *tgt_full, delta_transform); PointCloud output; @@ -616,7 +678,7 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) // Register reg.align (output); EXPECT_EQ (output.size (), src->size ()); - EXPECT_LT (reg.getFitnessScore (), 0.003); + EXPECT_LT (reg.getFitnessScore (), 1e-4); // Check again, for all possible caching schemes for (int iter = 0; iter < 4; iter++) @@ -637,7 +699,7 @@ TEST (PCL, GeneralizedIterativeClosestPoint6D) // Register reg.align (output); EXPECT_EQ (output.size (), src->size ()); - EXPECT_LT (reg.getFitnessScore (), 0.003); + EXPECT_LT (reg.getFitnessScore (), 1e-4); } }