NormalEstimation

determine The problem of the normal of a point on the surface is similar to the problem of estimating the normal of a tangent plane of the surface fitted by the adjacent point , Therefore, after conversion, it becomes a problem of least square plane fitting . therefore The solution to estimate the surface normals is to analyze the eigenvectors and eigenvalues of a covariance matrix ( perhaps PCA- Principal component analysis ), This covariance matrix starts from the Nearest neighbor element Created in . The largest principal component is the direction with the largest variance after projection to a certain direction （ The direction with the largest amount of information ）; The normal vector is projected to a certain direction , The direction with the least amount of information , Therefore need PCA Transformation , Project the point cloud to the direction with the smallest eigenvalue .

There are several concepts above :

• Definition of plane and distance from point to line
• Least squares and principal component analysis (PCA)
• Covariance matrix , And its eigenvectors and eigenvalues

Normal vector solution steps

The target plane can consist of a point $x$ And a normal vector $\vec{n}$ To express , And a point $p_i\in P^k$ The distance to the plane is :
$$d_i=(p_i-x)\cdot \vec{n}$$

That is, take a point on the plane , The distance from a point outside the plane to the plane , It is the projection of the vector composed of these two points on the normal vector .

If the distance from the point to the plane is 0, namely $d_i=0$, Then we can find the normal vector of the plane . So go through the following steps ：

1. Find the centroid of all points $p^k$
   $$x=\bar{p}=\frac{1}{k}\cdot \sum _{i=1}^k{p}_i$$

2. Calculating the center of mass $p^k$ The covariance matrix of $C\in {\mathbb{R}}^{3\times 3}$, And its eigenvectors and eigenvalues
   $$C=\frac{1}{k}\sum _{i=1}^kϵ\cdot (p_i-\bar{p})\cdot (p_i-\bar{p}{)}^T,C\cdot \overrightarrow{v_j}={\lambda }_j\cdot \overrightarrow{v_j},j\in \{0,1,2\}$$

   among $ϵ$ Usually it is 1.$C$ Is a symmetric and positive semidefinite matrix and its eigenvalue is a real number ${\lambda }_j\in \mathbb{R}$. Eigenvector $\overrightarrow{v_j}$ To form the orthogonal frame, Corresponding to the center of mass $p^k$ The principal component of (principal components). If $0\le {\lambda }_0\le {\lambda }_1\le {\lambda }_2$, Minimum eigenvalue ${\lambda }_0$ Eigenvector of $\overrightarrow{v_0}$ It's the normal vector $+\vec{n}=\{n_x,n_y,n_z\}$ perhaps $-\vec{n}$ Approximation of . perhaps ,$\vec{n}$ It can be used in spherical coordinate system $\varphi ,\theta$ Express :
   $$\varphi =\arctan (\frac{n_z}{n_y}),\theta =\arctan \frac{\sqrt{(n_y^2+n_z^2)}}{n_x}$$

   You can see , By principal component analysis (PCA) To calculate its direction has Dichotomy , The normal direction of the whole point cloud data set cannot be uniformly oriented . As shown in the figure below ：

   

3. Calculate the normal direction and viewpoint
   The solution to this problem is to use viewpoints $V_p$. For all normals $\overrightarrow{n_i}$ Orientation only needs to make them consistent in the direction of the viewpoint , Satisfy the following equation :
   $$\overrightarrow{n_i}\cdot (V_p-p_i)>0$$

   

PCL Normal vector estimation

First look at it. PCL Template of normal vector estimation . It usually includes the following steps ：

• Set the input point cloud , And its indexing method
• Set the radius of the adjacent point
• Calculate the normal vector

#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 ()*
}

The main function above is compute template function .compute The key of template function is computeFeature function , Let's take a closer look at , stay PCL Source code normal_3d.hpp In the file .

1. computeFeature function

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 Template function has two corresponding functions API It's our theory above ,computePointNormal and flipNormalTowardsViewpoint, That is, corresponding to the second and third steps above . Then take a deep look .

2. computePointNormal template function

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;
  }

This template function , First, calculate the covariance matrix of the point cloud , adopt computeMeanAndCovarianceMatrix template function , Then calculate the plane parameters .

3. computeMeanAndCovarianceMatrix template function

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));
}

Here you are Shifted data/with estimate of mean. This gives very good accuracy and good performance. In reference to 2 It is explained in detail in .

4. solvePlaneParameters template function

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;
}

Get eigenvectors and eigenvalues .

5. flipNormalTowardsViewpoint template function

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

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