diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt index ab15ffc2cc2..c0e4f740390 100644 --- a/registration/CMakeLists.txt +++ b/registration/CMakeLists.txt @@ -44,6 +44,7 @@ set(incs "include/pcl/${SUBSYS_NAME}/ndt.h" "include/pcl/${SUBSYS_NAME}/ndt_2d.h" "include/pcl/${SUBSYS_NAME}/ppf_registration.h" + "include/pcl/${SUBSYS_NAME}/anderson_acceleration.h" "include/pcl/${SUBSYS_NAME}/impl/pairwise_graph_registration.hpp" @@ -60,10 +61,12 @@ set(incs "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_plane_lls_weighted.h" "include/pcl/${SUBSYS_NAME}/transformation_estimation_symmetric_point_to_plane_lls.h" + "include/pcl/${SUBSYS_NAME}/transformation_estimation_point_to_point_robust.h" "include/pcl/${SUBSYS_NAME}/transformation_validation.h" "include/pcl/${SUBSYS_NAME}/transformation_validation_euclidean.h" "include/pcl/${SUBSYS_NAME}/gicp.h" "include/pcl/${SUBSYS_NAME}/gicp6d.h" + "include/pcl/${SUBSYS_NAME}/fricp.h" "include/pcl/${SUBSYS_NAME}/bfgs.h" "include/pcl/${SUBSYS_NAME}/warp_point_rigid.h" "include/pcl/${SUBSYS_NAME}/warp_point_rigid_6d.h" @@ -110,8 +113,10 @@ set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_lls_weighted.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_plane_weighted.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp" + "include/pcl/${SUBSYS_NAME}/impl/transformation_estimation_point_to_point_robust.hpp" "include/pcl/${SUBSYS_NAME}/impl/transformation_validation_euclidean.hpp" "include/pcl/${SUBSYS_NAME}/impl/gicp.hpp" + "include/pcl/${SUBSYS_NAME}/impl/fricp.hpp" "include/pcl/${SUBSYS_NAME}/impl/sample_consensus_prerejective.hpp" "include/pcl/${SUBSYS_NAME}/impl/ia_fpcs.hpp" "include/pcl/${SUBSYS_NAME}/impl/ia_kfpcs.hpp" @@ -141,6 +146,7 @@ set(srcs #src/pairwise_graph_registration.cpp src/ia_ransac.cpp src/icp.cpp + src/fricp.cpp src/joint_icp.cpp src/gicp.cpp src/gicp6d.cpp @@ -157,6 +163,7 @@ set(srcs src/transformation_estimation_point_to_plane_weighted.cpp src/transformation_estimation_point_to_plane_lls.cpp src/transformation_estimation_point_to_plane_lls_weighted.cpp + src/transformation_estimation_point_to_point_robust.cpp src/transformation_validation_euclidean.cpp src/sample_consensus_prerejective.cpp ) diff --git a/registration/include/pcl/registration/anderson_acceleration.h b/registration/include/pcl/registration/anderson_acceleration.h new file mode 100644 index 00000000000..aa5d2e2e2cb --- /dev/null +++ b/registration/include/pcl/registration/anderson_acceleration.h @@ -0,0 +1,185 @@ +#pragma once + +#include +#include + +#include +#include + +#include +#include + +namespace pcl { +namespace registration { + +/** + * \brief Lightweight Anderson acceleration helper used to speed up fixed-point + * iterations in FRICP. + * + * The class stores a short history of the most recent residuals and solves the + * normal equations following the scheme described in "Fast and Robust Iterative + * Closest Point", Zhang et al., 2022. + * + * Only double precision is supported internally to maximise numerical stability. + */ +class AndersonAcceleration { +public: + AndersonAcceleration() = default; + + /** + * \brief Initialise the accelerator with a circular buffer of size \a history. + * + * \param[in] history Number of previous iterates to keep (m in the paper). + * \param[in] dimension Dimensionality of the flattened state vector. + * \param[in] initial_state Pointer to the initial state (expects + * \a dimension doubles). + */ + inline void + init(std::size_t history, std::size_t dimension, const double* initial_state) + { + if ((history == 0) || (dimension == 0)) { + PCL_THROW_EXCEPTION(pcl::BadArgumentException, + "AndersonAcceleration::init expects non-zero sizes"); + } + + history_length_ = history; + dimension_ = dimension; + iteration_ = 0; + column_index_ = 0; + initialized_ = true; + + current_u_.resize(dimension_); + current_F_.resize(dimension_); + prev_dG_.setZero(dimension_, history_length_); + prev_dF_.setZero(dimension_, history_length_); + normal_eq_matrix_.setZero(history_length_, history_length_); + theta_.setZero(history_length_); + dF_scale_.setZero(history_length_); + + current_u_ = Eigen::Map(initial_state, dimension_); + } + + inline bool + isInitialized() const + { + return initialized_; + } + + inline std::size_t + history() const + { + return history_length_; + } + + inline std::size_t + dimension() const + { + return dimension_; + } + + inline void + replace(const double* state) + { + if (!initialized_) + return; + current_u_ = Eigen::Map(state, dimension_); + } + + inline void + reset(const double* state) + { + if (!initialized_) + return; + iteration_ = 0; + column_index_ = 0; + current_u_ = Eigen::Map(state, dimension_); + } + + /** + * \brief Apply one Anderson acceleration update. + * + * \param[in] next_state Flattened state obtained from the fixed-point iteration. + * \return Reference to the accelerated state vector. + */ + inline const Eigen::VectorXd& + compute(const double* next_state) + { + if (!initialized_) { + PCL_THROW_EXCEPTION(pcl::PCLException, + "AndersonAcceleration::compute called before init"); + } + + Eigen::Map G(next_state, dimension_); + current_F_ = G - current_u_; + + constexpr double eps = 1e-14; + + if (iteration_ == 0) { + prev_dF_.col(0) = -current_F_; + prev_dG_.col(0) = -G; + current_u_ = G; + } + else { + prev_dF_.col(column_index_) += current_F_; + prev_dG_.col(column_index_) += G; + + double scale = std::max(eps, prev_dF_.col(column_index_).norm()); + dF_scale_(column_index_) = scale; + prev_dF_.col(column_index_) /= scale; + + const std::size_t m_k = std::min(history_length_, iteration_); + + if (m_k == 1) { + theta_(0) = 0.0; + const double dF_norm = prev_dF_.col(column_index_).norm(); + normal_eq_matrix_(0, 0) = dF_norm * dF_norm; + if (dF_norm > eps) { + theta_(0) = (prev_dF_.col(column_index_) / dF_norm).dot(current_F_ / dF_norm); + } + } + else { + // update Gram matrix row/column corresponding to the newest column + const Eigen::VectorXd new_inner_prod = prev_dF_.col(column_index_).transpose() * + prev_dF_.block(0, 0, dimension_, m_k); + normal_eq_matrix_.block(column_index_, 0, 1, m_k) = new_inner_prod.transpose(); + normal_eq_matrix_.block(0, column_index_, m_k, 1) = new_inner_prod; + + cod_.compute(normal_eq_matrix_.block(0, 0, m_k, m_k)); + theta_.head(m_k) = + cod_.solve(prev_dF_.block(0, 0, dimension_, m_k).transpose() * current_F_); + } + + const Eigen::ArrayXd scaled_theta = + theta_.head(m_k).array() / dF_scale_.head(m_k).array(); + current_u_ = G - prev_dG_.block(0, 0, dimension_, m_k) * scaled_theta.matrix(); + + column_index_ = (column_index_ + 1) % history_length_; + prev_dF_.col(column_index_) = -current_F_; + prev_dG_.col(column_index_) = -G; + } + + ++iteration_; + return current_u_; + } + +private: + std::size_t history_length_{0}; + std::size_t dimension_{0}; + std::size_t iteration_{0}; + std::size_t column_index_{0}; + bool initialized_{false}; + + Eigen::VectorXd current_u_; + Eigen::VectorXd current_F_; + Eigen::MatrixXd prev_dG_; + Eigen::MatrixXd prev_dF_; + Eigen::MatrixXd normal_eq_matrix_; + Eigen::VectorXd theta_; + Eigen::VectorXd dF_scale_; + Eigen::CompleteOrthogonalDecomposition cod_; + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace registration +} // namespace pcl diff --git a/registration/include/pcl/registration/fricp.h b/registration/include/pcl/registration/fricp.h new file mode 100644 index 00000000000..2b756b9023e --- /dev/null +++ b/registration/include/pcl/registration/fricp.h @@ -0,0 +1,169 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include + +namespace pcl { +/** + * \brief FastRobustIterativeClosestPoint implements the FRICP variant described in + * "Fast and Robust Iterative Closest Point", Zhang et al., 2022. + * + * The solver relies on Welsch reweighting for robustness and optional Anderson + * acceleration for faster convergence. + * + * \ingroup registration + */ +template +class FastRobustIterativeClosestPoint +: public IterativeClosestPoint { +public: + using Base = IterativeClosestPoint; + using typename Base::Matrix4; + using typename Base::PointCloudSource; + using typename Base::PointCloudTarget; + + enum class RobustFunction { NONE, WELSCH }; + + FastRobustIterativeClosestPoint(); + + void + setRobustFunction(RobustFunction f); + + [[nodiscard]] RobustFunction + getRobustFunction() const; + + void + setUseAndersonAcceleration(bool enabled); + + [[nodiscard]] bool + getUseAndersonAcceleration() const; + + void + setAndersonHistorySize(std::size_t history); + + [[nodiscard]] std::size_t + getAndersonHistorySize() const; + + void + setDynamicWelschBeginRatio(double ratio); + + void + setDynamicWelschEndRatio(double ratio); + + void + setDynamicWelschDecay(double ratio); + +protected: + void + computeTransformation(PointCloudSource& output, const Matrix4& guess) override; + +private: + using Matrix4d = Eigen::Matrix; + using Matrix3Xd = Eigen::Matrix; + using VectorXd = Eigen::VectorXd; + using Vector3d = Eigen::Vector3d; + using AndersonAccelerationType = registration::AndersonAcceleration; + + Matrix4d + convertGuessToCentered(const Matrix4& guess, + const Vector3d& source_mean, + const Vector3d& target_mean) const; + + Matrix4d + convertCenteredToActual(const Matrix4d& transform, + const Vector3d& source_mean, + const Vector3d& target_mean) const; + + bool + updateCorrespondences(const Matrix4d& transform, + const Matrix3Xd& source, + const Matrix3Xd& target, + pcl::search::KdTree& tree, + Matrix3Xd& matched_targets, + VectorXd& residuals) const; + + double + computeEnergy(const VectorXd& residuals, double nu) const; + + VectorXd + computeWeights(const VectorXd& residuals, double nu) const; + + Matrix4d + computeWeightedRigidTransform(const Matrix3Xd& source, + const Matrix3Xd& target, + const VectorXd& weights) const; + + double + findKNearestMedian(const pcl::PointCloud& cloud, + pcl::search::KdTree& tree, + int neighbors) const; + + double + computeMedian(const VectorXd& values) const; + + double + computeMedian(std::vector values) const; + + Matrix4d + matrixLog(const Matrix4d& transform) const; + + RobustFunction robust_function_; + bool use_anderson_ = true; + std::size_t anderson_history_ = 5; + double nu_begin_ratio_ = 3.0; + double nu_end_ratio_ = 1.0 / (3.0 * std::sqrt(3.0)); + double nu_decay_ratio_ = 0.5; + + static constexpr double same_threshold_ = 1e-6; + + AndersonAccelerationType anderson_; + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pcl + +#include diff --git a/registration/include/pcl/registration/impl/fricp.hpp b/registration/include/pcl/registration/impl/fricp.hpp new file mode 100644 index 00000000000..78439b3d388 --- /dev/null +++ b/registration/include/pcl/registration/impl/fricp.hpp @@ -0,0 +1,581 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions of source code must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_REGISTRATION_IMPL_FRICP_HPP_ +#define PCL_REGISTRATION_IMPL_FRICP_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +namespace pcl { + +template +FastRobustIterativeClosestPoint:: + FastRobustIterativeClosestPoint() +: robust_function_(RobustFunction::WELSCH), nu_end_ratio_(1.0 / (3.0 * std::sqrt(3.0))) +{ + this->reg_name_ = "FastRobustIterativeClosestPoint"; + this->max_iterations_ = 50; + this->transformation_epsilon_ = static_cast(1e-6); + this->min_number_correspondences_ = 4; +} + +template +void +FastRobustIterativeClosestPoint::setRobustFunction( + RobustFunction f) +{ + robust_function_ = f; +} + +template +typename FastRobustIterativeClosestPoint::RobustFunction +FastRobustIterativeClosestPoint::getRobustFunction() + const +{ + return robust_function_; +} + +template +void +FastRobustIterativeClosestPoint:: + setUseAndersonAcceleration(bool enabled) +{ + use_anderson_ = enabled; +} + +template +bool +FastRobustIterativeClosestPoint:: + getUseAndersonAcceleration() const +{ + return use_anderson_; +} + +template +void +FastRobustIterativeClosestPoint:: + setAndersonHistorySize(std::size_t history) +{ + anderson_history_ = std::max(1, history); +} + +template +std::size_t +FastRobustIterativeClosestPoint:: + getAndersonHistorySize() const +{ + return anderson_history_; +} + +template +void +FastRobustIterativeClosestPoint:: + setDynamicWelschBeginRatio(double ratio) +{ + nu_begin_ratio_ = std::max(ratio, same_threshold_); +} + +template +void +FastRobustIterativeClosestPoint:: + setDynamicWelschEndRatio(double ratio) +{ + nu_end_ratio_ = std::max(ratio, same_threshold_); +} + +template +void +FastRobustIterativeClosestPoint:: + setDynamicWelschDecay(double ratio) +{ + nu_decay_ratio_ = std::max(0.0, std::min(1.0, ratio)); + if (nu_decay_ratio_ < same_threshold_) + nu_decay_ratio_ = 0.5; +} + +template +void +FastRobustIterativeClosestPoint:: + computeTransformation(PointCloudSource& output, const Matrix4& guess) +{ + pcl::IterativeClosestPoint::initComputeReciprocal(); + + if (!this->input_ || !this->target_) { + PCL_ERROR("[pcl::%s::computeTransformation] Invalid input clouds.\n", + this->getClassName().c_str()); + return; + } + + if (this->input_->empty() || this->target_->empty()) { + PCL_ERROR("[pcl::%s::computeTransformation] Empty input point clouds.\n", + this->getClassName().c_str()); + return; + } + + std::vector source_indices; + if (this->indices_ && !this->indices_->empty()) + source_indices.assign(this->indices_->begin(), this->indices_->end()); + else { + source_indices.resize(this->input_->size()); + std::iota(source_indices.begin(), source_indices.end(), 0); + } + + if (source_indices.size() < + static_cast(this->min_number_correspondences_)) { + PCL_ERROR("[pcl::%s::computeTransformation] Not enough source points (%zu).\n", + this->getClassName().c_str(), + source_indices.size()); + return; + } + + const std::size_t source_size = source_indices.size(); + const std::size_t target_size = this->target_->size(); + + if (target_size < static_cast(this->min_number_correspondences_)) { + PCL_ERROR("[pcl::%s::computeTransformation] Not enough target points (%zu).\n", + this->getClassName().c_str(), + target_size); + return; + } + + Matrix3Xd source_mat(3, source_size); + for (std::size_t i = 0; i < source_size; ++i) { + const auto& pt = (*this->input_)[source_indices[i]]; + source_mat.col(i) = pt.getVector3fMap().template cast(); + } + Vector3d source_mean = source_mat.rowwise().mean(); + source_mat.colwise() -= source_mean; + + Matrix3Xd target_mat(3, target_size); + for (std::size_t i = 0; i < target_size; ++i) { + const auto& pt = (*this->target_)[i]; + target_mat.col(i) = pt.getVector3fMap().template cast(); + } + Vector3d target_mean = target_mat.rowwise().mean(); + target_mat.colwise() -= target_mean; + + pcl::PointCloud::Ptr target_centered( + new pcl::PointCloud); + target_centered->resize(target_size); + for (std::size_t i = 0; i < target_size; ++i) { + (*target_centered)[i].x = static_cast(target_mat(0, i)); + (*target_centered)[i].y = static_cast(target_mat(1, i)); + (*target_centered)[i].z = static_cast(target_mat(2, i)); + } + + pcl::search::KdTree tree; + tree.setInputCloud(target_centered); + + Matrix4d transform_centered = convertGuessToCentered(guess, source_mean, target_mean); + Matrix4d svd_transform = transform_centered; + Matrix4d previous_transform = transform_centered; + + Matrix3Xd matched_targets(3, source_size); + VectorXd residuals(source_size); + if (!updateCorrespondences(transform_centered, + source_mat, + target_mat, + tree, + matched_targets, + residuals)) { + PCL_ERROR( + "[pcl::%s::computeTransformation] Failed to initialize correspondences.\n", + this->getClassName().c_str()); + return; + } + + const bool use_welsch = (robust_function_ == RobustFunction::WELSCH); + double nu_limit = 1.0; + double nu_current = 1.0; + if (use_welsch) { + const double neighbor_med = findKNearestMedian(*target_centered, tree, 7); + const double residual_med = computeMedian(residuals); + nu_limit = std::max(nu_end_ratio_ * neighbor_med, same_threshold_); + nu_current = std::max(nu_begin_ratio_ * residual_med, nu_limit); + } + + const double stop_threshold = (this->transformation_epsilon_ > 0) + ? static_cast(this->transformation_epsilon_) + : 1e-6; + + this->nr_iterations_ = 0; + this->converged_ = false; + + double last_energy = std::numeric_limits::max(); + if (use_anderson_) { + const Matrix4d log_state = matrixLog(transform_centered); + anderson_.init(anderson_history_, 16, log_state.data()); + } + + bool outer_done = false; + bool converged = false; + + while (!outer_done) { + for (int iter = 0; iter < this->max_iterations_; ++iter) { + double energy = + use_welsch ? computeEnergy(residuals, nu_current) : residuals.squaredNorm(); + if (use_anderson_) { + if (energy <= last_energy) { + last_energy = energy; + } + else { + transform_centered = svd_transform; + const Matrix4d log_state = matrixLog(transform_centered); + anderson_.replace(log_state.data()); + if (!updateCorrespondences(transform_centered, + source_mat, + target_mat, + tree, + matched_targets, + residuals)) { + PCL_ERROR("[pcl::%s::computeTransformation] Unable to recompute " + "correspondences during fallback.\n", + this->getClassName().c_str()); + return; + } + energy = use_welsch ? computeEnergy(residuals, nu_current) + : residuals.squaredNorm(); + last_energy = energy; + } + } + else { + last_energy = energy; + } + + VectorXd weights = use_welsch ? computeWeights(residuals, nu_current) + : VectorXd::Ones(residuals.size()); + Matrix4d candidate = + computeWeightedRigidTransform(source_mat, matched_targets, weights); + svd_transform = candidate; + transform_centered = candidate; + + if (use_anderson_) { + const Matrix4d log_matrix = matrixLog(transform_centered); + const Eigen::VectorXd& accelerated = anderson_.compute(log_matrix.data()); + transform_centered = Eigen::Map(accelerated.data()).exp(); + } + + if (!updateCorrespondences(transform_centered, + source_mat, + target_mat, + tree, + matched_targets, + residuals)) { + PCL_ERROR("[pcl::%s::computeTransformation] Failed to update " + "correspondences.\n", + this->getClassName().c_str()); + return; + } + + const double delta = (transform_centered - previous_transform).norm(); + previous_transform = transform_centered; + ++this->nr_iterations_; + + if (delta < stop_threshold) { + converged = true; + break; + } + } + + if (!use_welsch || (std::abs(nu_current - nu_limit) < same_threshold_)) { + outer_done = true; + } + else { + nu_current = std::max(nu_current * nu_decay_ratio_, nu_limit); + last_energy = std::numeric_limits::max(); + if (use_anderson_) { + const Matrix4d log_state = matrixLog(transform_centered); + anderson_.reset(log_state.data()); + } + } + } + + this->converged_ = converged; + + const Matrix4d final_transform = + convertCenteredToActual(transform_centered, source_mean, target_mean); + this->final_transformation_ = final_transform.template cast(); + this->transformation_ = this->final_transformation_; + this->previous_transformation_ = this->final_transformation_; + + pcl::transformPointCloud(*this->input_, output, this->final_transformation_); +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint:: + convertGuessToCentered(const Matrix4& guess, + const Vector3d& source_mean, + const Vector3d& target_mean) const +{ + Matrix4d centered = Matrix4d::Identity(); + centered.block<3, 3>(0, 0) = guess.template block<3, 3>(0, 0).template cast(); + Vector3d translation = guess.template block<3, 1>(0, 3).template cast(); + centered.block<3, 1>(0, 3) = + centered.block<3, 3>(0, 0) * source_mean + translation - target_mean; + return centered; +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint:: + convertCenteredToActual(const Matrix4d& transform, + const Vector3d& source_mean, + const Vector3d& target_mean) const +{ + Matrix4d actual = transform; + actual.block<3, 1>(0, 3) = transform.block<3, 1>(0, 3) - + transform.block<3, 3>(0, 0) * source_mean + target_mean; + return actual; +} + +template +bool +FastRobustIterativeClosestPoint:: + updateCorrespondences(const Matrix4d& transform, + const Matrix3Xd& source, + const Matrix3Xd& target, + pcl::search::KdTree& tree, + Matrix3Xd& matched_targets, + VectorXd& residuals) const +{ + const Eigen::Matrix3d R = transform.block<3, 3>(0, 0); + const Eigen::Vector3d t = transform.block<3, 1>(0, 3); + pcl::PointXYZ query; + pcl::Indices nn_indices(1); + std::vector nn_sqr_dists(1); + + for (int i = 0; i < source.cols(); ++i) { + const Eigen::Vector3d current = R * source.col(i) + t; + query.x = static_cast(current.x()); + query.y = static_cast(current.y()); + query.z = static_cast(current.z()); + if (tree.nearestKSearch(query, 1, nn_indices, nn_sqr_dists) != 1) + return false; + const auto idx = nn_indices[0]; + matched_targets.col(i) = target.col(static_cast(idx)); + residuals[i] = (current - matched_targets.col(i)).norm(); + } + return true; +} + +template +double +FastRobustIterativeClosestPoint::computeEnergy( + const VectorXd& residuals, double nu) const +{ + if (nu < same_threshold_) + nu = same_threshold_; + const double denom = 2.0 * nu * nu; + const Eigen::ArrayXd dist2 = residuals.array().square(); + return (1.0 - (-dist2 / denom).exp()).sum(); +} + +template +typename FastRobustIterativeClosestPoint::VectorXd +FastRobustIterativeClosestPoint::computeWeights( + const VectorXd& residuals, double nu) const +{ + if (nu < same_threshold_) + nu = same_threshold_; + const double denom = 2.0 * nu * nu; + return (-residuals.array().square() / denom).exp().matrix(); +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint:: + computeWeightedRigidTransform(const Matrix3Xd& source, + const Matrix3Xd& target, + const VectorXd& weights) const +{ + Matrix4d transform = Matrix4d::Identity(); + VectorXd normalized = weights; + const double sum = normalized.sum(); + if (sum <= same_threshold_) { + normalized.setOnes(); + normalized /= static_cast(normalized.size()); + } + else { + normalized /= sum; + } + + const Vector3d source_mean = source * normalized; + const Vector3d target_mean = target * normalized; + const Matrix3Xd source_centered = source.colwise() - source_mean; + const Matrix3Xd target_centered = target.colwise() - target_mean; + Eigen::Matrix3d sigma = + source_centered * normalized.asDiagonal() * target_centered.transpose(); + Eigen::JacobiSVD svd(sigma, + Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose(); + if (R.determinant() < 0.0) { + Eigen::Matrix3d V = svd.matrixV(); + V.col(2) *= -1.0; + R = V * svd.matrixU().transpose(); + } + transform.block<3, 3>(0, 0) = R; + transform.block<3, 1>(0, 3) = target_mean - R * source_mean; + return transform; +} + +template +double +FastRobustIterativeClosestPoint::findKNearestMedian( + const pcl::PointCloud& cloud, + pcl::search::KdTree& tree, + int neighbors) const +{ + if (cloud.empty() || neighbors < 2) + return 0.0; + + const int k = std::min(neighbors, static_cast(cloud.size())); + if (k < 2) + return 0.0; + + std::vector local_medians(cloud.size(), 0.0); + pcl::Indices nn_indices(k); + std::vector nn_sqr_dists(k); + std::vector dists; + dists.reserve(k - 1); + + for (std::size_t i = 0; i < cloud.size(); ++i) { + if (tree.nearestKSearch(cloud[i], k, nn_indices, nn_sqr_dists) != k) + continue; + dists.clear(); + for (int j = 1; j < k; ++j) + dists.push_back(std::sqrt(nn_sqr_dists[j])); + local_medians[i] = computeMedian(dists); + } + + return computeMedian(std::move(local_medians)); +} + +template +double +FastRobustIterativeClosestPoint::computeMedian( + const VectorXd& values) const +{ + std::vector buffer(static_cast(values.size())); + for (int i = 0; i < values.size(); ++i) + buffer[static_cast(i)] = values[i]; + return computeMedian(std::move(buffer)); +} + +template +double +FastRobustIterativeClosestPoint::computeMedian( + std::vector values) const +{ + if (values.empty()) + return 0.0; + auto mid = values.begin() + values.size() / 2; + std::nth_element(values.begin(), mid, values.end()); + if (values.size() % 2 == 1) + return *mid; + auto mid_prev = values.begin() + values.size() / 2 - 1; + std::nth_element(values.begin(), mid_prev, values.end()); + return (*mid + *mid_prev) * 0.5; +} + +template +typename FastRobustIterativeClosestPoint::Matrix4d +FastRobustIterativeClosestPoint::matrixLog( + const Matrix4d& transform) const +{ + Eigen::RealSchur schur(transform); + const Matrix4d U = schur.matrixU(); + const Matrix4d R = schur.matrixT(); + std::array selected{{true, true, true}}; + Eigen::Matrix3d B = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d V = Eigen::Matrix3d::Identity(); + + for (int i = 0; i < 3; ++i) { + if (!selected[i]) + continue; + if (std::abs(R(i, i) - 1.0) <= same_threshold_) + continue; + int partner = -1; + for (int j = i + 1; j < 3; ++j) { + if (std::abs(R(j, j) - R(i, i)) < same_threshold_) { + partner = j; + selected[j] = false; + break; + } + } + if (partner > 0) { + selected[i] = false; + const double diag = std::max(-1.0, std::min(1.0, static_cast(R(i, i)))); + double theta = std::acos(diag); + if (R(i, partner) < 0.0) + theta = -theta; + B(i, partner) += theta; + B(partner, i) -= theta; + V(i, partner) -= theta / 2.0; + V(partner, i) += theta / 2.0; + const double denom = 2.0 * (1.0 - R(i, i)); + if (std::abs(denom) > same_threshold_) { + const double coeff = 1.0 - (theta * R(i, partner)) / denom; + V(i, i) -= coeff; + V(partner, partner) -= coeff; + } + } + } + + Matrix4d trimmed = Matrix4d::Zero(); + trimmed.block<3, 3>(0, 0) = B; + trimmed.block<3, 1>(0, 3) = V * R.block<3, 1>(0, 3); + return U * trimmed * U.transpose(); +} + +} // namespace pcl + +#endif // PCL_REGISTRATION_IMPL_FRICP_HPP_ diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp new file mode 100644 index 00000000000..5aa83c41179 --- /dev/null +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_point_robust.hpp @@ -0,0 +1,269 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_POINT_ROBUST_HPP_ +#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_POINT_ROBUST_HPP_ + +#include + +namespace pcl { + +namespace registration { + +template +inline void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const +{ + const auto nr_points = cloud_src.size(); + if (cloud_tgt.size() != nr_points) { + PCL_ERROR("[pcl::TransformationEstimationPointToPointRobust::" + "estimateRigidTransformation] Number " + "or points in source (%zu) differs than target (%zu)!\n", + static_cast(nr_points), + static_cast(cloud_tgt.size())); + return; + } + + ConstCloudIterator source_it(cloud_src); + ConstCloudIterator target_it(cloud_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const +{ + if (indices_src.size() != cloud_tgt.size()) { + PCL_ERROR("[pcl::TransformationSVD::estimateRigidTransformation] Number or points " + "in source (%zu) differs than target (%zu)!\n", + indices_src.size(), + static_cast(cloud_tgt.size())); + return; + } + + ConstCloudIterator source_it(cloud_src, indices_src); + ConstCloudIterator target_it(cloud_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +inline void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) const +{ + if (indices_src.size() != indices_tgt.size()) { + PCL_ERROR("[pcl::TransformationEstimationSVD::estimateRigidTransformation] Number " + "or points in source (%zu) differs than target (%zu)!\n", + indices_src.size(), + indices_tgt.size()); + return; + } + + ConstCloudIterator source_it(cloud_src, indices_src); + ConstCloudIterator target_it(cloud_tgt, indices_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Correspondences& correspondences, + Matrix4& transformation_matrix) const +{ + ConstCloudIterator source_it(cloud_src, correspondences, true); + ConstCloudIterator target_it(cloud_tgt, correspondences, false); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +inline void +TransformationEstimationPointToPointRobust:: + estimateRigidTransformation(ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4& transformation_matrix) const +{ + // Convert to Eigen format + const int npts = static_cast(source_it.size()); + source_it.reset(); + target_it.reset(); + Eigen::Matrix weights(npts); + Eigen::Matrix square_distances(npts); + for (int i = 0; i < npts; i++) { + Scalar dx = source_it->x - target_it->x; + Scalar dy = source_it->y - target_it->y; + Scalar dz = source_it->z - target_it->z; + Scalar dist2 = dx * dx + dy * dy + dz * dz; + square_distances[i] = dist2; + + source_it++; + target_it++; + } + + Scalar sigma2; + if (sigma_ < 0) + sigma2 = square_distances.maxCoeff() / 9.0; + else + sigma2 = sigma_ * sigma_; + + for (int i = 0; i < npts; i++) { + weights[i] = std::exp(-square_distances[i] / (2.0 * sigma2)); + } + weights = weights / weights.sum(); + + source_it.reset(); + target_it.reset(); + // is the source dataset + transformation_matrix.setIdentity(); + + Eigen::Matrix centroid_src, centroid_tgt; + // Estimate the centroids of source, target + computeWeighted3DCentroid(source_it, weights, centroid_src); + computeWeighted3DCentroid(target_it, weights, centroid_tgt); + source_it.reset(); + target_it.reset(); + + // Subtract the centroids from source, target + Eigen::Matrix cloud_src_demean, + cloud_tgt_demean; + demeanPointCloud(source_it, centroid_src, cloud_src_demean); + demeanPointCloud(target_it, centroid_tgt, cloud_tgt_demean); + + getTransformationFromCorrelation(cloud_src_demean, + centroid_src, + cloud_tgt_demean, + centroid_tgt, + weights, + transformation_matrix); +} + +template +void +TransformationEstimationPointToPointRobust:: + getTransformationFromCorrelation( + const Eigen::Matrix& cloud_src_demean, + const Eigen::Matrix& centroid_src, + const Eigen::Matrix& cloud_tgt_demean, + const Eigen::Matrix& centroid_tgt, + const Eigen::Matrix& weights, + Matrix4& transformation_matrix) const +{ + transformation_matrix.setIdentity(); + + // Assemble the correlation matrix H = source * weights * target' + Eigen::Matrix H = + (cloud_src_demean * weights.asDiagonal() * cloud_tgt_demean.transpose()) + .template topLeftCorner<3, 3>(); + + // Compute the Singular Value Decomposition + Eigen::JacobiSVD> svd( + H, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix u = svd.matrixU(); + Eigen::Matrix v = svd.matrixV(); + + // Compute R = V * U' + if (u.determinant() * v.determinant() < 0) { + for (int x = 0; x < 3; ++x) + v(x, 2) *= -1; + } + + Eigen::Matrix R = v * u.transpose(); + + // Return the correct transformation + transformation_matrix.template topLeftCorner<3, 3>() = R; + const Eigen::Matrix Rc(R * centroid_src.template head<3>()); + transformation_matrix.template block<3, 1>(0, 3) = + centroid_tgt.template head<3>() - Rc; + + if (pcl::console::isVerbosityLevelEnabled(pcl::console::L_DEBUG)) { + size_t N = cloud_src_demean.cols(); + PCL_DEBUG("[pcl::registration::TransformationEstimationPointToPointRobust::" + "getTransformationFromCorrelation] Loss: %.10e\n", + (cloud_tgt_demean - R * cloud_src_demean).squaredNorm() / N); + } +} + +template +inline unsigned int +TransformationEstimationPointToPointRobust:: + computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, + Eigen::Matrix& weights, + Eigen::Matrix& centroid) const +{ + Eigen::Matrix accumulator{0, 0, 0, 0}; + + unsigned int cp = 0; + + // For each point in the cloud + // If the data is dense, we don't need to check for NaN + while (cloud_iterator.isValid()) { + // Check if the point is invalid + if (pcl::isFinite(*cloud_iterator)) { + accumulator[0] += weights[cp] * cloud_iterator->x; + accumulator[1] += weights[cp] * cloud_iterator->y; + accumulator[2] += weights[cp] * cloud_iterator->z; + ++cp; + } + ++cloud_iterator; + } + + if (cp > 0) { + centroid = accumulator; + centroid[3] = 1; + } + return (cp); +} + +} // namespace registration +} // namespace pcl + +#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_POINT_ROBUST_HPP_ */ diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h new file mode 100644 index 00000000000..59f74e62ae9 --- /dev/null +++ b/registration/include/pcl/registration/transformation_estimation_point_to_point_robust.h @@ -0,0 +1,186 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#pragma once + +#include +#include +#include // for PCL_NO_PRECOMPILE + +namespace pcl { +namespace registration { +/** @b TransformationEstimationPointToPointRobust implements SVD-based estimation of + * the transformation aligning the given correspondences for minimizing the Welsch + * function instead of L2-norm, For additional details, see "Fast and Robust Iterative + * Closest Point", Juyong Zhang, Yuxin Yao, Bailin Deng, 2022. + * \note The class is templated on the source and target point types as well as on the + * output scalar of the transformation matrix (i.e., float or double). Default: float. + * \author Yuxin Yao + * \ingroup registration + */ +template +class TransformationEstimationPointToPointRobust +: public TransformationEstimation { +public: + using Ptr = shared_ptr< + TransformationEstimationPointToPointRobust>; + using ConstPtr = + shared_ptr>; + + using Matrix4 = + typename TransformationEstimation::Matrix4; + + /** \brief Constructor + * \param[in] use_umeyama Toggles whether or not to use 3rd party software*/ + TransformationEstimationPointToPointRobust() = default; + + ~TransformationEstimationPointToPointRobust() override = default; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \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[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \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[out] transformation_matrix the resultant transformation matrix + */ + inline void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::Indices& indices_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Indices& indices_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target + * point cloud using SVD. \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target + * point cloud \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Correspondences& correspondences, + Matrix4& transformation_matrix) const override; + + void + set_sigma(Scalar sigma) + { + sigma_ = sigma; + }; + +protected: + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void + estimateRigidTransformation(ConstCloudIterator& source_it, + ConstCloudIterator& target_it, + Matrix4& transformation_matrix) const; + + /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src + * * tgt' \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen + * format \param[in] centroid_src the input source centroid, in Eigen format + * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format + * \param[in] centroid_tgt the input target cloud, in Eigen format + * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix + */ + virtual void + getTransformationFromCorrelation( + const Eigen::Matrix& cloud_src_demean, + const Eigen::Matrix& centroid_src, + const Eigen::Matrix& cloud_tgt_demean, + const Eigen::Matrix& centroid_tgt, + const Eigen::Matrix& weights, + Matrix4& transformation_matrix) const; + + /** \brief Compute the 3D (X-Y-Z) centroid of a set of weighted points and return it + * as a 3D vector. + * \param[in] cloud_iterator an iterator over the input point cloud + * \param[in] weights the weights corresponding to points in the input point cloud + * \param[out] centroid the output centroid + * \return number of valid points used to determine the centroid. In case of dense + * point clouds, this is the same as the size of input cloud. + * \note if return value is 0, the centroid is not changed, thus not valid. + * The last component of the vector is set to 1, this allows to transform the centroid + * vector with 4x4 matrices. + * \ingroup common + */ + inline unsigned int + computeWeighted3DCentroid(ConstCloudIterator& cloud_iterator, + Eigen::Matrix& weights, + Eigen::Matrix& centroid) const; + + /** parameter for the Welsch function */ + Scalar sigma_ = -1; +}; + +} // namespace registration +} // namespace pcl + +#include diff --git a/registration/src/fricp.cpp b/registration/src/fricp.cpp new file mode 100644 index 00000000000..6b0a0898b42 --- /dev/null +++ b/registration/src/fricp.cpp @@ -0,0 +1,37 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include diff --git a/registration/src/transformation_estimation_point_to_point_robust.cpp b/registration/src/transformation_estimation_point_to_point_robust.cpp new file mode 100755 index 00000000000..59dc078cdfe --- /dev/null +++ b/registration/src/transformation_estimation_point_to_point_robust.cpp @@ -0,0 +1,39 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) Alexandru-Eugen Ichim + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp index ab3fec26193..d50344a6d9e 100644 --- a/test/registration/test_registration_api.cpp +++ b/test/registration/test_registration_api.cpp @@ -42,6 +42,8 @@ #include #include +#include +#include #include #include #include @@ -58,6 +60,9 @@ #include #include #include +#include +#include +#include #include #include "test_registration_api_data.h" @@ -708,6 +713,70 @@ TEST (PCL, TransformationEstimationSymmetricPointToPlaneLLS) } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, FastRobustIterativeClosestPoint) +{ + using PointT = PointXYZ; + CloudXYZ::Ptr src (new CloudXYZ); + copyPointCloud (cloud_source, *src); + CloudXYZ::Ptr tgt (new CloudXYZ); + copyPointCloud (cloud_target, *tgt); + CloudXYZ output; + + pcl::FastRobustIterativeClosestPoint reg; + reg.setInputSource (src); + reg.setInputTarget (tgt); + reg.setMaximumIterations (60); + reg.setTransformationEpsilon (1e-8); + + reg.align (output); + ASSERT_TRUE (reg.hasConverged ()); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg.getFitnessScore (), 5e-4); + + Eigen::Isometry3f transform = + Eigen::Isometry3f (Eigen::AngleAxisf (-0.15f, Eigen::Vector3f::UnitX ()) * + Eigen::AngleAxisf (0.25f, Eigen::Vector3f::UnitY ()) * + Eigen::AngleAxisf (-0.30f, Eigen::Vector3f::UnitZ ())); + transform.translation () = Eigen::Vector3f (0.08f, -0.05f, 0.12f); + CloudXYZ::Ptr transformed_tgt (new CloudXYZ); + pcl::transformPointCloud (*tgt, *transformed_tgt, transform.matrix ()); + + std::mt19937 rng (1337u); + std::normal_distribution gaussian (0.0f, 0.004f); + for (auto& p : *transformed_tgt) + { + p.x += gaussian (rng); + p.y += gaussian (rng); + p.z += gaussian (rng); + } + + std::uniform_real_distribution uniform (-0.4f, 0.4f); + for (int i = 0; i < 20; ++i) + { + PointT outlier; + outlier.x = uniform (rng); + outlier.y = uniform (rng); + outlier.z = uniform (rng) + 0.4f; + transformed_tgt->push_back (outlier); + } + transformed_tgt->width = static_cast(transformed_tgt->size ()); + transformed_tgt->height = 1; + transformed_tgt->is_dense = false; + + pcl::FastRobustIterativeClosestPoint reg_guess; + reg_guess.setInputSource (src); + reg_guess.setInputTarget (transformed_tgt); + reg_guess.setMaximumIterations (60); + reg_guess.setTransformationEpsilon (1e-8); + + reg_guess.align (output, transform.matrix ()); + ASSERT_TRUE (reg_guess.hasConverged ()); + EXPECT_EQ (output.size (), cloud_source.size ()); + EXPECT_LT (reg_guess.getFitnessScore (), 5e-4); +} + + /* ---[ */ int main (int argc, char** argv)