PCL ICP使用OMP加速(ICP_OMP)

CV陈智君

已于 2023-04-17 21:27:08 修改

阅读量2k

点赞数 3

分类专栏： pcl 文章标签：算法人工智能 c++ slam

于 2023-04-17 21:14:54 首次发布

本文链接：https://blog.csdn.net/u014493244/article/details/130208393

版权

pcl 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

最近项目中使用到了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 tree
        PointTarget 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 choice
  if (!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 tree
        PointTarget 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_reciprocal
        PointSource 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>::Ptr
			correspondence_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 transformation
    PointCloudSource input_transformed;
    if (cloud == nullptr) {
        transformPointCloud(*input_, input_transformed, final_transformation_);
    }
    else {
        input_transformed = *cloud;
    }

    // For each point in the source dataset
    double 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 target
        tree_->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 score
            fitness_score += nn_dists[0];
            nr++;
        }
    }

    if (nr > 0)
        return (fitness_score / nr);
    else
        return (std::numeric_limits<double>::max());

}
#endif    // PCL_ICP_2D_OMP_IMPL_H_

使用时直接构造IterativeClosestPointOMP的对象即可，参数设置与pcl自带的icp一致。