PCL - MLS代碼研讀（七）- 曲面擬合函數

computeMLSSurface

這個函數是MLS模塊的精華所在，可以與論文對應的地方最多，建議與PCL MLS論文Computing and Rendering Point Set Surfaces研讀筆記交互參看。

宣告兩個變數

宣告兩個變數：共變異數矩陣和鄰域內所有點的重心。

//計算cloud[index]這個點的擬合曲面
template <typename PointT> void
pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
                                   pcl::index_t index,
                                   const pcl::Indices &nn_indices,
                                   double search_radius,
                                   int polynomial_order,
                                   std::function<double(const double)> weight_func)
{
  // Compute the plane coefficients
  //3*3 matrix
  EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
  Eigen::Vector4d xyz_centroid;

計算鄰域內所有點的重心

  // Estimate the XYZ centroid
  // 鄰域點的重心,即論文裡的點q
  pcl::compute3DCentroid (cloud, nn_indices, xyz_centroid);

計算平面參數model_coefficients

計算平面參數model_coefficients：用待投影點周圍的鄰居（索引為nn_indeices）與重心xyz_centroid計算共變異數矩陣covariance_matrix，然後求解它的特徵值eigen_value及特徵向量eigen_vector，最小的特徵值所對應的特徵向量即為擬合平面的法向量model_coefficients.head<3>。接着將平面上一點xyz_centroid代入平面方程，即可求出平面的最後一個係數model_coefficients[3]。這部分的理論基礎寫在尋找平面H。

（論文中共變異數矩陣有加權，但代碼中沒有？）

  // Compute the 3x3 covariance matrix
  pcl::computeCovarianceMatrix (cloud, nn_indices, xyz_centroid, covariance_matrix);
  EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigen_value;
  EIGEN_ALIGN16 Eigen::Vector3d eigen_vector;
  //這裡model_coefficients代表的是一個擬合"平面"!!我們接下來還要計算擬合"曲面"!!
  //這個平面即論文圖三裡的平面H
  //平面是擬合鄰域內的點得出來的
  Eigen::Vector4d model_coefficients (0, 0, 0, 0);
  //pcl::eigen33: determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix
  //即計算矩陣的eigenvalue及eigenvector，注意它回傳的是最小的eigenvalue及其對應的eigenvector
  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
  //最小的eigenvector定義了擬合平面的法向量
  //論文裡尋找平面H的第一步:固定t=0求使得n^T * covariance_matrix * n最小化的n
  //該最小化問題的解即為covariance_matrix最小特徵值所對應的特徵向量
  model_coefficients.head<3> ().matrix () = eigen_vector;
  //擬合平面要穿過xyz_centroid
  //平面H的方程: (normal, H上的點) + model_coefficients[3] = 0
  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);

被投影點query_point

被投影點query_point，即論文裡的r點。注意到它在cast之前加了一個template關鍵字，這將留到.template cast＜＞中template的作用裡繼續探討。

  //論文裡的r點
  query_point = cloud[index].getVector3fMap ().template cast<double> ();

檢查eigen_vector合理性

檢查算出來的eigen_vector是否合理，如果不合理，只更新mean為query_point，並將此次的計算標註為無效（valid = false）後便退出。

  if (!std::isfinite(eigen_vector[0]) || !std::isfinite(eigen_vector[1]) || !std::isfinite(eigen_vector[2]))
  {
    // Invalid plane coefficients, this may happen if the input cloud is non-dense (it contains invalid points).
    // Keep the input point and stop here.
    //找不到擬合平面
    //這個函數的輸出是valid跟mean?
    valid = false;
    mean = query_point;
    return;
  }

計算局部坐標系原點

計算局部坐標系原點，也就是query_point在平面上的投影點。這部分的理論基礎可以參考尋找點r的擬合平面(或說座標系) H。

  // Projected query point
  valid = true;
  //query_point到擬合平面的有號距離
  const double distance = query_point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
  //將query_point沿"平面"法向量上移動到"平面"上，即為query_point在"平面"上的投影
  //論文裡圖三將r點移到紅點的過程,得到論文裡的點q
  //mean.dot (model_coefficients.head<3> ()) + model_coefficients[3] = distance - distance * model_coefficients.head<3> ().dot(model_coefficients.head<3> ()) = 0, 代表mean點在平面上
  mean = query_point - distance * model_coefficients.head<3> ();

計算曲率

  //curvature是eigen_value/trace?
  //維基上說mean curvature是trace/n?
  curvature = covariance_matrix.trace ();
  // Compute the curvature surface change
  if (curvature != 0)
    curvature = std::abs (eigen_value / curvature);

這部分不是很確定是什麼意思。根據維基百科 - Mean Curvature所說：

More abstractly, the mean curvature is the trace of the second fundamental form divided by n (or equivalently, the shape operator). 

mean curvature是second fundamental form的trace除以n，但是這裡卻是用eigen_value去除trace得到curvature？

計算局部坐標系的三個軸向

平面的法向量即model_coefficients前三個數組成的向量。v_axis取為任一個與plane_normal正交的單位向量，u_axis則取為兩者的外積。

  // Get a copy of the plane normal easy access
  plane_normal = model_coefficients.head<3> ();

  //根據曲面的法向量得到兩個單位正交向量
  //plane_normal, u_axis及v_axis定義了一個座標系
  // Local coordinate system (Darboux frame)
  v_axis = plane_normal.unitOrthogonal ();
  u_axis = plane_normal.cross (v_axis);

設定三個變數

• num_neighbors：鄰居數量
• order：多項式階數
• nr_coeff：雙自變數多項式係數的數量。對於二次多項式，有$a+bv+c{v}^2+du+euv+f{v}^2$，共有6項，正好符合公式的$(order+1)\ast (order+2)/2=6$

  // Perform polynomial fit to update point and normal
  
  num_neighbors = static_cast<int> (nn_indices.size ());
  order = polynomial_order;
  if (order > 1)
  {
    //原因同calculatePrincipleCurvatures處註解
    const int nr_coeff = (order + 1) * (order + 2) / 2;

    //鄰居數量要>=係數數量才有解
    if (num_neighbors >= nr_coeff)
    {

權重函數

weight_func是一個高斯函數，接受query_point鄰居與mean的距離（注意不是與query_point的距離！）平方為參數，回傳該點的權重。它被用於計算鄰域內各點的權重。

      //用於計算鄰域內各點的權重
      //論文裡公式(4)的h直接取search_radius?
      if (!weight_func)
        weight_func = [=] (const double sq_dist) { return this->computeMLSWeight (sq_dist, search_radius * search_radius); };

computeMLSWeight函數的定義詳見PCL - MLS代碼研讀（二）- MLSResult - computeMLSWeight。

宣告四個變數

• weight_vec：長度等於鄰居數量num_neighbors的向量，它的每個元素代表鄰域內各點的權重
• P：多項式各項組成的矩陣，大小為係數數量nr_coeff乘上鄰居數量num_neighbors
• f_vec：鄰居點在局部坐標系下的w坐標（即高度）
• P_weight_Pt：

      // Allocate matrices and vectors to hold the data used for the polynomial fit
      Eigen::VectorXd weight_vec (num_neighbors);
      //多項式的各項(不含係數)
      Eigen::MatrixXd P (nr_coeff, num_neighbors);
      //儲存各鄰居在擬合平面定義的座標系下的z座標
      //裡面的各元素即論文裡的圖3中的fi
      Eigen::VectorXd f_vec (num_neighbors);
      Eigen::MatrixXd P_weight_Pt (nr_coeff, nr_coeff);

計算鄰域內各點的權重

weight_vec是一個長度等於鄰居數量num_neighbors的向量，它的每個元素代表鄰域內各點的權重。權重的計算如下：將鄰居點與mean距離（注意不是與query_point的距離！詳見projection property）的平方代入高斯函數所得到。

      //計算weight_vec,即鄰域內各點的權重
      //updating only distances for the weights for speed->什麼意思?本來不是這樣算?
      // Update neighborhood, since point was projected, and computing relative
      // positions. Note updating only distances for the weights for speed
      std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > de_meaned (num_neighbors);
      for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
      {
        //對鄰域內的每個點，計算其以mean為原點的座標，即de_meaned[ni]
        de_meaned[ni][0] = cloud[nn_indices[ni]].x - mean[0];
        de_meaned[ni][1] = cloud[nn_indices[ni]].y - mean[1];
        de_meaned[ni][2] = cloud[nn_indices[ni]].z - mean[2];
        //de_meaned[ni].dot (de_meaned[ni]):鄰居到擬合平面原點距離的平方
        //該點的權重是由該點到原點的距離計算出來的
        weight_vec (ni) = weight_func (de_meaned[ni].dot (de_meaned[ni]));
      }

計算鄰居點在局部坐標系下的坐標

計算u_coord,v_coord及f_vec，這三者構成了鄰居ni在局部坐標系下的坐標。

      //計算f_vec及P
      //f_vec:局部坐標系下各點的高度height,也就是論文圖3裡的f(i)
      //P:記錄鄰域內每個點在局部坐標系下的多項式各項,如:[1, v, v^2, u, uv, u*2]
      // Go through neighbors, transform them in the local coordinate system,
      // save height and the evaluation of the polynomial's terms
      for (std::size_t ni = 0; ni < static_cast<std::size_t>(num_neighbors); ++ni)
      {
        // Transforming coordinates
        //鄰居ni在擬合平面座標系下的座標:(u_coord, v_coord, f_vec(ni))
        const double u_coord = de_meaned[ni].dot(u_axis);
        const double v_coord = de_meaned[ni].dot(v_axis);
        f_vec (ni) = de_meaned[ni].dot (plane_normal);

計算多項式各項組成的矩陣

P是一個nr_coeff乘以num_neighbors的矩陣，P(j,ni)代表鄰居點ni代入多項式的第j項。此處就是在計算P：

        //預先計算多項式的各項
        //如果order為2, P(:, ni) = [1, v, v^2, u, uv, u*2]
        // Compute the polynomial's terms at the current point
        int j = 0;
        double u_pow = 1;
        for (int ui = 0; ui <= order; ++ui)
        {
          double v_pow = 1;
          for (int vi = 0; vi <= order - ui; ++vi)
          {
            //j:多項式的第幾項
            //u_pow:動態更新,為u_coord的ui次方
            //v_pow:動態更新,為v_coord的ui次方
            P (j++, ni) = u_pow * v_pow;
            v_pow *= v_coord;
          }
          u_pow *= u_coord;
        }
      }

使用矩陣分解計算多項式的係數

計算多項式的係數c_vec，使用矩陣分解的方法。

這裡將多項式的計算表示成矩陣和向量的乘積，矩陣P是將每個鄰居點坐標代入得到的多項式各項，向量c_vec是多項式的係數矩陣（未知）。然後列一個等式，等號左邊是矩陣和向量的乘積，右邊是各點在局部坐標系下的高度（即w坐標）。

在大多數情況下，鄰居點並不會構成一個完美的曲面，所以會有一定的誤差。我們希望這個誤差能最小化，將這個問題表述成最小二乘的形式：

$P^T\ast c\_vec=f\_vec$

如果$P^T$可逆，那直接對$f\_vec$左乘$(P^T{)}^{-1}$即可得到解。但是因為$P^T$不一定可逆，所以需要在等號兩邊都乘上$P\ast weight\_vec.asDiagonal()$，得到：

$P\ast weight\_vec.asDiagonal()\ast P^T\ast c\_vec=P\ast weight\_vec.asDiagonal()\ast f\_vec$

然後再同乘偽逆求解。在程序中，則是透過調用llt函數，使用矩陣分解的方式來求解。

其中llt分解即Cholesky分解，詳見Cholesky 分解。

      //計算多項式的係數
      // Computing coefficients
      /**
       * 鄰域各點帶入多項式得到num_neighbors個等式,
       * 等式左邊是 多項式係數 與 各點多項式各項(視為已知數) 的乘積,
       * 等式右邊是 各點在局部坐標系下的高度
       * 
       * 欲求解此線性系統,先將它描述成一個最小二乘問題:
       * A^T * A * x = A^T * b
       * 其中A代表num_neighbors組多項式係數所構成的矩陣
       * b代表num_neighbors個點各自在局部坐標系下的高度
       * x則是待求解的多項式係數
       * 求解x,得到線性方程組的解
       * 
       * 最小二乘問題可透過SVD,QR分解,LU分解,Cholesky分解(即LLT分解),LDLT分解等方式求解
       **/ 
      //P_weight: 加權後的多項式各項
      //注意到P與最小二乘問題中的A是互為轉置的關係
      const Eigen::MatrixXd P_weight = P * weight_vec.asDiagonal(); // size will be (nr_coeff_, nn_indices.size ());
      //P_weight_Pt:最小二乘問題等號左邊的東西(不包含x)
      P_weight_Pt = P_weight * P.transpose ();
      //c_vec:在這裡是最小二乘問題等號右邊的東西
      c_vec = P_weight * f_vec;
      //solveInPlace,求解P_weight_Pt * x = c_vec後,將解x存入c_vec中
      //這行結束後c_vec變成多項式的係數
      P_weight_Pt.llt ().solveInPlace (c_vec);
    }
  }
}