PCL ICP使用OMP加速(ICP_OMP)
最近项目中使用到了pcl库的ICP算法，但是pcl库的ICP算法在点云数量较多时计算速度较慢。ICP算法中频繁的使用了kdtree最近邻搜索，而每次迭代中每个点的最近邻搜索是可以并行计算的，因此想到了使用omp来给ICP加速。但是pcl库中没有相应的实现，本人又比较懒，不想从头重新撸一个ICP算法，因此就分析了pcl中ICP的源代码，发现可以继承并使用omp优化。
打开ICP的源代码，在registration/icp.h文件中，IterativeClosestPoint就是ICP算法实现的类，IterativeClosestPoint构造函数中构造了三个对象：
transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar> ());correspondence_estimation_.reset (new pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>);convergence_criteria_.reset(new pcl::registration::DefaultConvergenceCriteria<Scalar> (nr_iterations_, transformation_, *correspondences_));其中transformation_estimation_定义了ICP最近点匹配后计算变换矩阵的方法，默认采用SVD分解计算，convergence_criteria_定义了判断点云是否收敛的方法，correspondence_estimation_定义了最近点搜索的方法，是我们要优化和替换的对象。
首先，新建correspondence_estimation_omp.h文件和correspondence_estimation_omp.hpp文件，在correspondence_estimation_omp.h中定义CorrespondenceEstimationOMP类，继承自CorrespondenceEstimation类，correspondence_estimation_omp.h中的内容如下：
#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_OMP_H_
#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_OMP_H_#include <string>#include <pcl/pcl_base.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h>
#include <pcl/pcl_macros.h>#include <pcl/registration/correspondence_estimation.h>namespace pcl
{namespace registration{/ \\brief @b CorrespondenceEstimation represents the base class for* determining correspondences between target and query point* sets/features. Code example: \\code* pcl::PointCloud<pcl::PointXYZRGBA>::Ptr source, target;* // ... read or fill in source and target* pcl::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> est;* est.setInputSource (source);* est.setInputTarget (target); pcl::Correspondences all_correspondences;* // Determine all reciprocal correspondences* est.determineReciprocalCorrespondences (all_correspondences);* \\endcode \\author Radu B. Rusu, Michael Dixon, Dirk Holz* \\ingroup registration*/template <typename PointSource, typename PointTarget, typename Scalar = float>class CorrespondenceEstimationOMP : public CorrespondenceEstimation<PointSource, PointTarget, Scalar>{public:/ \\brief Empty constructor. */CorrespondenceEstimationOMP(){corr_name_ = "CorrespondenceEstimationOMP";}/ \\brief Empty destructor */virtual ~CorrespondenceEstimationOMP() {}/ \\brief Determine the correspondences between input and target cloud.* \\param[out] correspondences the found correspondences (index of query point, index of target point, distance)* \\param[in] max_distance maximum allowed distance between correspondences*/virtual void determineCorrespondences (pcl::Correspondences &correspondences,double max_distance = std::numeric_limits<double>::max ()) override;/ \\brief Determine the reciprocal correspondences between input and target cloud.* A correspondence is considered reciprocal if both Src_i has Tgt_i as a * correspondence, and Tgt_i has Src_i as one. \\param[out] correspondences the found correspondences (index of query and target point, distance)* \\param[in] max_distance maximum allowed distance between correspondences*/virtual void determineReciprocalCorrespondences (pcl::Correspondences &correspondences,double max_distance = std::numeric_limits<double>::max ()) override;/ \\brief Clone and cast to CorrespondenceEstimationBase */virtual boost::shared_ptr< CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> > clone () const{Ptr copy (new CorrespondenceEstimationOMP<PointSource, PointTarget, Scalar> (*this));return (copy);}};}
}#include "impl/correspondence_estimation_omp.hpp"#endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_OMP_H_ */
correspondence_estimation_omp.hpp中的内容如下：
#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_OMP_H_
#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_OMP_H_#include <pcl/common/io.h>
#include <pcl/common/copy_point.h>
///
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::CorrespondenceEstimationOMP<PointSource, PointTarget, Scalar>::determineCorrespondences (pcl::Correspondences &correspondences, double max_distance)
{if (!initCompute ())return;double max_dist_sqr = max_distance * max_distance;unsigned int nr_valid_correspondences = 0;pcl::Correspondences temp_correspondences;temp_correspondences.resize (indices_->size ());// Check if the template types are the same. If true, avoid a copy.// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!if (isSamePointType<PointSource, PointTarget> ()){// Iterate over the input set of source indices
#pragma omp parallel for reduction(+:nr_valid_correspondences)for (int i = 0;i< indices_->size(); ++i) {int idx = (*indices_)[i];std::vector<int> index(1);std::vector<float> distance(1);tree_->nearestKSearch (input_->points[idx], 1, index, distance);if (distance[0] > max_dist_sqr) {continue;}temp_correspondences[i].index_query = idx;temp_correspondences[i].index_match = index[0];temp_correspondences[i].distance = distance[0];nr_valid_correspondences++;}}else{// Iterate over the input set of source indices
#pragma omp parallel for reduction(+:nr_valid_correspondences)for (int i = 0; i < indices_->size(); ++i) {int idx = (*indices_)[i];// Copy the source data to a target PointTarget format so we can search in the treePointTarget pt;copyPoint (input_->points[idx], pt);std::vector<int> index(1);std::vector<float> distance(1);tree_->nearestKSearch (pt, 1, index, distance);if (distance[0] > max_dist_sqr) {continue;}temp_correspondences[i].index_query = idx;temp_correspondences[i].index_match = index[0];temp_correspondences[i].distance = distance[0];nr_valid_correspondences++;}}correspondences.clear();correspondences.reserve(nr_valid_correspondences);for (const auto& corr : temp_correspondences) {if (corr.index_match < 0) {continue;}correspondences.push_back(corr);}deinitCompute ();
}///
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::CorrespondenceEstimationOMP<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (pcl::Correspondences &correspondences, double max_distance)
{if (!initCompute ())return;// setup tree for reciprocal search// Set the internal point representation of choiceif (!initComputeReciprocal())return;double max_dist_sqr = max_distance * max_distance;pcl::Correspondences temp_correspondences;temp_correspondences.resize(indices_->size());unsigned int nr_valid_correspondences = 0;// Check if the template types are the same. If true, avoid a copy.// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!if (isSamePointType<PointSource, PointTarget> ()){// Iterate over the input set of source indices
#pragma omp parallel for reduction(+:nr_valid_correspondences)for (int i = 0; i < indices_->size (); ++i){int idx = (*indices_)[i];std::vector<int> index(1);std::vector<float> distance(1);tree_->nearestKSearch (input_->points[idx], 1, index, distance);if (distance[0] > max_dist_sqr) {continue;}int target_idx = index[0];std::vector<int> index_reciprocal(1);std::vector<float> distance_reciprocal(1);tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0]) {continue;}temp_correspondences[i].index_query = idx;temp_correspondences[i].index_match = index[0];temp_correspondences[i].distance = distance[0];}}else{// Iterate over the input set of source indices
#pragma omp parallel for reduction(+:nr_valid_correspondences)for (int i = 0; i < indices_->size(); ++i) {int idx = (*indices_)[i];std::vector<int> index(1);std::vector<float> distance(1);// Copy the source data to a target PointTarget format so we can search in the treePointTarget pt_src;copyPoint (input_->points[idx], pt_src);tree_->nearestKSearch (pt_src, 1, index, distance);if (distance[0] > max_dist_sqr)continue;int target_idx = index[0];std::vector<int> index_reciprocal(1);std::vector<float> distance_reciprocal(1);// Copy the target data to a target PointSource format so we can search in the tree_reciprocalPointSource pt_tgt;copyPoint (target_->points[target_idx], pt_tgt);tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0]) {continue;}temp_correspondences[i].index_query = idx;temp_correspondences[i].index_match = index[0];temp_correspondences[i].distance = distance[0];}}correspondences.clear();correspondences.reserve(nr_valid_correspondences);for (const auto& corr : temp_correspondences) {if (corr.index_match < 0) {continue;}correspondences.push_back(corr);}deinitCompute ();
}//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation<T,U>;#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_OMP_H_ */
至此我们完成了基于omp的最近邻搜索方法的构建。
其实到这里ICP_OMP就基本完成了，使用的时候，先构建ICP对象，再调用setCorrespondenceEstimation函数将默认的最近邻搜索方法更换为我们优化后的对象即可，具体使用方法如下：
pcl::registration::CorrespondenceEstimationOMP<PointXYZ, PointXYZ>::Ptrcorrespondence_estimation(new pcl::registration::CorrespondenceEstimationOMP<PointXYZ, PointXYZ>());
pcl::IterativeClosestPoint<PointXYZ, PointXYZ>::Ptr icp(new pcl::IterativeClosestPoint<PointXYZ, PointXYZ>());
icp_->setCorrespondenceEstimation(correspondence_estimation);但是在使用过程中，我们发现icp的结果会使用hasConverged()函数来判断是否可用，hasConverged()的作用是计算匹配点云与目标点云的平均匹配距离的，icp自带的hasConverged()函数也没有采用omp加速，在点云数量较大时速度较慢，因此也需要进一步的优化，但是icp默认的hasConverged()函数并没可以替换接口，因此我们需要重建一个icp类。
重建icp_omp类首先需要新建icp_omp.h和icp_omp.hpp两个文件，在icp_omp.h中定义IterativeClosestPointOMP类，继承自IterativeClosestPoint类，主要对icp自带的hasConverged()函数采用omp加速，icp_omp.h中的内容如下
#ifndef PCL_ICP_OMP_H_
#define PCL_ICP_OMP_H_// PCL includes
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/registration/default_convergence_criteria.h>
#include <pcl/registration/icp.h>
#include "correspondence_estimation_omp.h"
namespace pcl
{/ \\brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. * The transformation is estimated based on Singular Value Decomposition (SVD). The algorithm has several termination criteria: <ol>* <li>Number of iterations has reached the maximum user imposed number of iterations (via \\ref setMaximumIterations)</li>* <li>The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \\ref setTransformationEpsilon)</li>* <li>The sum of Euclidean squared errors is smaller than a user defined threshold (via \\ref setEuclideanFitnessEpsilon)</li>* </ol>* Usage example:* \\code* IterativeClosestPoint<PointXYZ, PointXYZ> icp;* // Set the input source and target* icp.setInputCloud (cloud_source);* icp.setInputTarget (cloud_target); // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)* icp.setMaxCorrespondenceDistance (0.05);* // Set the maximum number of iterations (criterion 1)* icp.setMaximumIterations (50);* // Set the transformation epsilon (criterion 2)* icp.setTransformationEpsilon (1e-8);* // Set the euclidean distance difference epsilon (criterion 3)* icp.setEuclideanFitnessEpsilon (1); // Perform the alignment* icp.align (cloud_source_registered); // Obtain the transformation that aligned cloud_source to cloud_source_registered* Eigen::Matrix4f transformation = icp.getFinalTransformation ();* \\endcode \\author Radu B. Rusu, Michael Dixon* \\ingroup registration*/template <typename PointSource, typename PointTarget, typename Scalar = float>class IterativeClosestPointOMP : public IterativeClosestPoint<PointSource, PointTarget, Scalar>{public:typedef typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource PointCloudSource;typedef typename PointCloudSource::Ptr PointCloudSourcePtr;typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;typedef typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget PointCloudTarget;typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;typedef PointIndices::Ptr PointIndicesPtr;typedef PointIndices::ConstPtr PointIndicesConstPtr;typedef boost::shared_ptr<IterativeClosestPointOMP<PointSource, PointTarget, Scalar> > Ptr;typedef boost::shared_ptr<const IterativeClosestPointOMP<PointSource, PointTarget, Scalar> > ConstPtr;/ \\brief Empty constructor. */IterativeClosestPointOMP(): IterativeClosestPoint<PointSource, PointTarget, Scalar>(){reg_name_ = "IterativeClosestPointOMP";transformation_estimation_.reset(new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>());correspondence_estimation_.reset(new pcl::registration::CorrespondenceEstimationOMP<PointSource, PointTarget, Scalar>);convergence_criteria_.reset(new pcl::registration::DefaultConvergenceCriteria<Scalar>(nr_iterations_, transformation_, *correspondences_));};/ \\brief Empty destructor */virtual ~IterativeClosestPointOMP() {}double getFitnessScore(const PointCloudSourceConstPtr& cloud,double max_range = std::numeric_limits<double>::max());};
}#include "impl/icp_omp.hpp"
#endif //#ifndef PCL_ICP_2D_OMP_H_
icp_omp.hpp中的内容如下
#ifndef PCL_ICP_2D_OMP_IMPL_H_
#define PCL_ICP_2D_OMP_IMPL_H_
#include <cmath>
#include <pcl/registration/eigen.h>
#include <pcl/registration/boost.h>//
template <typename PointSource, typename PointTarget, typename Scalar> double
pcl::IterativeClosestPointOMP<PointSource, PointTarget, Scalar>::getFitnessScore(const PointCloudSourceConstPtr& cloud, double max_range) {// Transform the input dataset using the final transformationPointCloudSource input_transformed;if (cloud == nullptr) {transformPointCloud(*input_, input_transformed, final_transformation_);}else {input_transformed = *cloud;}// For each point in the source datasetdouble fitness_score = 0.0;int nr = 0;#pragma omp parallel for reduction(+:fitness_score,nr)for (int i = 0; i < input_transformed.points.size(); ++i) {std::vector<int> nn_indices(1);std::vector<float> nn_dists(1);// Find its nearest neighbor in the targettree_->nearestKSearch(input_transformed.points[i], 1, nn_indices, nn_dists);// Deal with occlusions (incomplete targets)if (nn_dists[0] <= max_range) {// Add to the fitness scorefitness_score += nn_dists[0];nr++;}}if (nr > 0)return (fitness_score / nr);elsereturn (std::numeric_limits<double>::max());}
#endif // PCL_ICP_2D_OMP_IMPL_H_使用时直接构造IterativeClosestPointOMP的对象即可，参数设置与pcl自带的icp一致。
