---

前言

前文已经总结了一下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;}output.height = input_->height;output.width = input_->width;
}

2.calculateNormalCovar 函数

//
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const pcl::Indices& neighbors, float* coefficients) const
{unsigned count = 0;//这里数组为啥不直接弄个9或者6，弄成9，直接和矩阵3*3对应；弄成6，忽略矩阵里相同部分，弄成8多尴尬~~// indices        0   1   2   3   4   5   6   7// coefficients: xx  xy  xz  ??  yx  yy  yz  ??#ifdef __SSE__//方法一，这个未深究，主要看下面// accumulator for xx, xy, xz__m128 vec1 = _mm_setzero_ps();// accumulator for yy, yz, zz__m128 vec2 = _mm_setzero_ps();__m128 norm1;__m128 norm2;float zz = 0;for (const auto &neighbor : neighbors){if (std::isfinite ((*normals_)[neighbor].normal_x)){// nx, ny, nz, hnorm1 = _mm_load_ps (&((*normals_)[neighbor].normal_x));// nx, nx, nx, nxnorm2 = _mm_set1_ps ((*normals_)[neighbor].normal_x);// nx * nx, nx * ny, nx * nz, nx * hnorm2 = _mm_mul_ps (norm1, norm2);// accumulatevec1 = _mm_add_ps (vec1, norm2);// ny, ny, ny, nynorm2 = _mm_set1_ps ((*normals_)[neighbor].normal_y);// ny * nx, ny * ny, ny * nz, ny * hnorm2 = _mm_mul_ps (norm1, norm2);// accumulatevec2 = _mm_add_ps (vec2, norm2);zz += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;++count;}}if (count > 0){norm2 = _mm_set1_ps (static_cast<float>(count));vec1 = _mm_div_ps (vec1, norm2);vec2 = _mm_div_ps (vec2, norm2);_mm_store_ps (coefficients, vec1);_mm_store_ps (coefficients + 4, vec2);coefficients [7] = zz / static_cast<float>(count);}elsestd::fill_n(coefficients, 8, 0);
#else//方法二：主要看这个std::fill_n(coefficients, 8, 0);//循环遍历每个近邻点，求协方差矩阵A//对于二维图像，求A时I是像素；对于点云，这里用到的是法向量for (const auto& index : neighbors){//不为nan或者无穷大if (std::isfinite ((*normals_)[index].normal_x)){coefficients[0] += (*normals_)[index].normal_x * (*normals_)[index].normal_x;//Ixi * Ixicoefficients[1] += (*normals_)[index].normal_x * (*normals_)[index].normal_y;//Ixi * Iyicoefficients[2] += (*normals_)[index].normal_x * (*normals_)[index].normal_z;//Ixi * Izicoefficients[5] += (*normals_)[index].normal_y * (*normals_)[index].normal_y;//Iyi * Iyicoefficients[6] += (*normals_)[index].normal_y * (*normals_)[index].normal_z;//Iyi * Izicoefficients[7] += (*normals_)[index].normal_z * (*normals_)[index].normal_z;//Izi * Izi++count;}}//这里做了归一化处理，也即除了近邻点的个数，从公式看，并未要求做归一化，当然做了也可以if (count > 0){float norm = 1.0 / float (count);coefficients[0] *= norm;coefficients[1] *= norm;coefficients[2] *= norm;coefficients[5] *= norm;coefficients[6] *= norm;coefficients[7] *= norm;}
#endif
}

二、其他响应函数

其他响应方式和Harris响应不同

1.responseNoble函数

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseNoble (PointCloudOut &output) const
{...//响应函数 R = det(A) / trace(A)output [pIdx].intensity = det / trace;...
}

2.responseLowe函数

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseLowe (PointCloudOut &output) const
{...//响应函数 R = det(A) / trace(A) * trace(A)output [pIdx].intensity = det / (trace * trace);...
}

3.responseCurvature函数

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseCurvature (PointCloudOut &output) const
{PointOutT point;for (std::size_t idx = 0; idx < input_->size(); ++idx){point.x = (*input_)[idx].x;point.y = (*input_)[idx].y;point.z = (*input_)[idx].z;//直接拿曲率作为强度值point.intensity = (*normals_)[idx].curvature;output.push_back(point);}// does not change the orderoutput.height = input_->height;output.width = input_->width;
}

4.responseTomasi函数

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudOut &output) const
{...covariance_matrix.coeffRef (0) = covar [0];covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];covariance_matrix.coeffRef (4) = covar [5];covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];covariance_matrix.coeffRef (8) = covar [7];//计算covariance_matrix特征值,取第一个值，这里没有排序，所以不一定是最大值//SortEigenValuesAndVectors(eigen_vector3, eigen_values);EIGEN_ALIGN16 Eigen::Vector3f eigen_values;pcl::eigen33(covariance_matrix, eigen_values);output [pIdx].intensity = eigen_values[0];...
}

三.refineCorners函数

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOut &corners) const
{Eigen::Matrix3f nnT;Eigen::Matrix3f NNT;Eigen::Vector3f NNTp;constexpr unsigned max_iterations = 10;
#pragma omp parallel for \shared(corners) \firstprivate(nnT, NNT, NNTp) \num_threads(threads_)for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx){unsigned iterations = 0;do {NNT.setZero();NNTp.setZero();PointInT corner;corner.x = corners[cIdx].x;corner.y = corners[cIdx].y;corner.z = corners[cIdx].z;pcl::Indices nn_indices;std::vector<float> nn_dists;tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);for (const auto& index : nn_indices){if (!std::isfinite ((*normals_)[index].normal_x))continue;//每个角点法向量构造3*3矩阵nnT = (*normals_)[index].getNormalVector3fMap () * (*normals_)[index].getNormalVector3fMap ().transpose();NNT += nnT;//矩阵相加NNTp += nnT * (*surface_)[index].getVector3fMap ();//和角点近邻点相乘}//LDLT分解， 求解方程组A x = bconst Eigen::LDLT<Eigen::Matrix3f> ldlt(NNT);//小于这个阈值范围，则更新角点的坐标值if (ldlt.rcond() > 1e-4)corners[cIdx].getVector3fMap () = ldlt.solve(NNTp);//角点先前坐标值与更新后的坐标值差的欧氏距离小于一个阈值，则认为当前角点细化结束const auto diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();if (diff <= 1e-6) {break;}} while (++iterations < max_iterations);}
}

---

总结

pcl提取Harris关键点的源码分析就此结束，还是很有收获的~~