pcl源码分析之Harris关键点（二）

---

前言

前文已经总结了一下pcl源码提取Harris关键点的流程和大致原理，这篇文章对关键函数进行详细分析，点云3DHarris角点检测算法推导，可以参考文章1，以及文章2，结合起来看，才能成为自己的东西~~

一、responseHarris响应函数

1.responseHarris函数

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseHarris (PointCloudOut &output) const
{
   
  PCL_ALIGN (16) float covar [8]; 
  output.resize (input_->size ());
#pragma omp parallel for \ //for循环多线程，这个之前讲过，不再赘述
  default(none) \
  shared(output) \
  firstprivate(covar) \
  num_threads(threads_)
  for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx)
  {
   
    const PointInT& pointIn = input_->points [pIdx];
    output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
    if (isFinite (pointIn))
    {
   
      pcl::Indices nn_indices;
      std::vector<float> nn_dists;
      tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);//查找该点近邻区域
      calculateNormalCovar (nn_indices, covar);//利用近邻区域的法向量构造协方差矩阵

      float trace = covar [0] + covar [5] + covar [7];//A矩阵的迹
      if (trace != 0)
      {
   
        //A矩阵的行列式
        float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6]
                  - covar [2] * covar [2] * covar [5]
                  - covar [1] * covar [1] * covar [7]
                  - covar [6] * covar [6] * covar [0];
		//响应函数，从公式看直接就是：det - 0.04f * trace * trace，这里多加了一个k（0.04）
        output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace;
      }
    }
    //坐标不变
    output [pIdx].x = pointIn.x;
    output [pIdx].y = pointIn.y;
    output [pIdx].z = pointIn.z