NormalEstimation法向量估计理论和代码---PCL源码笔记

最新推荐文章于 2024-08-02 21:21:19 发布

Tianchao龙虾

最新推荐文章于 2024-08-02 21:21:19 发布

阅读量2.1k

点赞数 5

分类专栏：技术笔记文章标签：机器学习算法 python

本文链接：https://blog.csdn.net/wuchaohuo724/article/details/125738568

版权

技术笔记专栏收录该内容

6 篇文章 1 订阅

订阅专栏

NormalEstimation

确定表面一点法线的问题近似于估计临近点所拟合表面的一个相切面法线的问题，因此转换过来以后就变成了一个最小二乘法平面拟合的问题。因此估计表面法线的解决方案就变成了分析一个协方差矩阵的特征向量和特征值(或者PCA-主成分分析)，这个协方差矩阵从查询点的近邻元素中创建。最大主成分为投影到某方向后方差最大的方向（信息量最大的方向）；而法向量为投影到某方向后，信息量最小的方向，因此需要PCA变换，将点云投影到特征值最小的方向。

上面这里有几个概念:

平面的定义和点到直线的距离
最小二乘法和主成分分析(PCA)
协方差矩阵，及其特征向量和特征值

法向量求解步骤

目标平面可以由一个点 $x$ 和一个法向量 $\vec{n}$ 来表示，且一个点 $p_i \in P^k$ 到平面的距离为:
$d_i = (p_i - x) \cdot \vec{n}$

即取平面上一点，平面外一点的到平面的距离，就是这两点组成的向量在法向量上面的投影。

如果点到平面的距离为0，即 $d_i = 0$ ，我们就可以求出平面的法向量了。因此通过以下步骤：

求所有点的质心 $p^k$
$\bar{p} = \frac{1}{k} \cdot \sum_{i=1}^k p_i$
计算质心 $p^k$ 的协方差矩阵 $\in \R^{3 \times 3}$ ，及其特征向量和特征值
$\frac{1}{k} \sum_{i=1}^k \epsilon \cdot (p_i - \bar{p}) \cdot (p_i - \bar{p})^T, C\cdot \vec{v_j} = \lambda_j \cdot \vec{v_j}, j\in \{0, 1, 2\}$

其中 $\epsilon$ 通常为1。 $C$ 是对称和半正定矩阵且它的特征值是实数 $\lambda_j \in \R$ 。特征向量 $\vec{v_j}$ 形成了orthogonal frame，相对应于质心 $p^k$ 的主成分(principal components)。 如果 $\leq \lambda_0 \leq \lambda_1 \leq \lambda_2$ ，最小特征值 $\lambda_0$ 的特征向量 $\vec{v_0}$ 就是法向量 $\vec{n} = \{n_x, n_y, n_z\}$ 或者 $\vec{n}$ 的近似。 或者， $\vec{n}$ 可以在球坐标系下用 $\phi, \theta$ 表示:
$\phi = \arctan (\frac{n_z}{n_y}) , \theta = \arctan \frac{\sqrt{(n_y^2+n_z^2)}}{n_x}$

可以看到，通过主成分分析法(PCA)来计算它的方向具有二异性，无法对整个点云数据集的法向方向进行一致性定向。 如下图所示：
计算法向与视点
解决这个问题的办法就是使用视点 $V_p$ 。对所有法线 $\vec{n_i}$ 定向只需要使它们一致朝向视点的方向，满足下面的方程式:
$\vec{n_i} \cdot (V_p - p_i) > 0$

PCL法向量估计

在这里插入图片描述

先来看看PCL法向量估计的模板。通常包含以下步骤：

设置输入点云，及其索引方法
设置临近点的半径
计算法向量

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
 
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  ... read, pass in or create a point cloud ...

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->size () should have the same size as the input cloud->size ()*
}

上面的主要函数在于compute模板函数。compute模板函数关键在于computeFeature函数，我们深入看看，在PCL源码 normal_3d.hpp文件内。

1. computeFeature 函数

template <typename PointInT, typename PointOutT> void
pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
    // Allocate enough space to hold the results
    // \note This resize is irrelevant for a radiusSearch ().
    pcl::Indices nn_indices (k_);
    std::vector<float> nn_dists (k_);

    output.is_dense = true;
  
    // Iterating over the entire index vector
    for (std::size_t idx = 0; idx < indices_->size (); ++idx)
    {
      if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
          !computePointNormal (*surface_, nn_indices, output[idx].normal[0], output[idx].normal[1], output[idx].normal[2], output[idx].curvature))
      {
        output[idx].normal[0] = output[idx].normal[1] = output[idx].normal[2] = output[idx].curvature = std::numeric_limits<float>::quiet_NaN ();

        output.is_dense = false;
        continue;
      }

      flipNormalTowardsViewpoint ((*input_)[(*indices_)[idx]], vpx_, vpy_, vpz_,
                                  output[idx].normal[0], output[idx].normal[1], output[idx].normal[2]);
    }
}

computeFeature模板函数就有两个对应的函数API是我们上面理论部分，computePointNormal和flipNormalTowardsViewpoint，也就是对应于上面的第二第三步骤。 接着深入看看。

2. computePointNormal模板函数

template <typename PointT> inline bool
  computePointNormal (const pcl::PointCloud<PointT> &cloud, const pcl::Indices &indices,
                      Eigen::Vector4f &plane_parameters, float &curvature)
  {
    // Placeholder for the 3x3 covariance matrix at each surface patch
    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
    // 16-bytes aligned placeholder for the XYZ centroid of a surface patch
    Eigen::Vector4f xyz_centroid;
    if (indices.size () < 3 ||
        computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0)
    {
      plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
      curvature = std::numeric_limits<float>::quiet_NaN ();
      return false;
    }
    // Get the plane normal and surface curvature
    solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
    return true;
  }

这个模板函数，首先计算点云的协方差矩阵，通过computeMeanAndCovarianceMatrix模板函数，然后计算平面参数。

3. computeMeanAndCovarianceMatrix模板函数

template <typename PointT, typename Scalar> inline unsigned int
computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                const Indices &indices,
                                Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
                                Eigen::Matrix<Scalar, 4, 1> &centroid)
{
  // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
  for(const auto& index : indices)
    if(isFinite(cloud[index])) {
      K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
    }
  std::size_t point_count;
  
  
  point_count = indices.size ();
  for (const auto &index : indices)
  {
    Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
    accu [0] += x * x;
    accu [1] += x * y;
    accu [2] += x * z;
    accu [3] += y * y;
    accu [4] += y * z;
    accu [5] += z * z;
    accu [6] += x;
    accu [7] += y;
    accu [8] += z;
  }
  


  if (point_count != 0)
  {
    accu /= static_cast<Scalar> (point_count);
    centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
    centroid[3] = 1;
    covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
    covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
    covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
    covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
    covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
    covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
    covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
    covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
    covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
  }
  return (static_cast<unsigned int> (point_count));
}

这里有 Shifted data/with estimate of mean. This gives very good accuracy and good performance. 在参考2中作出了详细的解释。

4. solvePlaneParameters模板函数


inline void
solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
                      const Eigen::Vector4f &point,
                      Eigen::Vector4f &plane_parameters, float &curvature)
{
  solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);

  plane_parameters[3] = 0;
  // Hessian form (D = nc . p_plane (centroid here) + p)
  plane_parameters[3] = -1 * plane_parameters.dot (point);
}


inline void
solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
                      float &nx, float &ny, float &nz, float &curvature)
{
  // Avoid getting hung on Eigen's optimizers
//  for (int i = 0; i < 9; ++i)
//    if (!std::isfinite (covariance_matrix.coeff (i)))
//    {
//      //PCL_WARN ("[pcl::solvePlaneParameters] Covariance matrix has NaN/Inf values!\n");
//      nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
//      return;
//    }
  // Extract the smallest eigenvalue and its eigenvector
  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);

  nx = eigen_vector [0];
  ny = eigen_vector [1];
  nz = eigen_vector [2];

  // Compute the curvature surface change
  float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
  if (eig_sum != 0)
    curvature = std::abs (eigen_value / eig_sum);
  else
    curvature = 0;
}

求出特征向量和特征值。

5. flipNormalTowardsViewpoint模板函数

template <typename PointT> inline void
  flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
                              float &nx, float &ny, float &nz)
  {
    // See if we need to flip any plane normals
    vp_x -= point.x;
    vp_y -= point.y;
    vp_z -= point.z;

    // Dot product between the (viewpoint - point) and the plane normal
    float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);

    // Flip the plane normal
    if (cos_theta < 0)
    {
      nx *= -1;
      ny *= -1;
      nz *= -1;
    }
  }

Reference

pcl::NormalRobustEstimation - computing robust normals at the origin, https://github.com/PointCloudLibrary/pcl/issues/4965
Algorithms for calculating variancek, https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
Regression asymetry and PCA (Principal Component Analysis), http://zoonek2.free.fr/UNIX/48_R/09.html#7