前言

本篇延續PCL - MLS代碼研讀（二），繼續介紹坐標計算函數。

三維getMLSCoordinates

此函數計算全局坐標系下的三維點pt在局部坐標系（u,v,w坐標系）下的表示：

void
pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v, double &w) const
{
  //mean: 所有鄰居的重心，即擬合平面座標系的原點
  //獲取pt在局部座標系下的三維座標
  Eigen::Vector3d delta = pt - mean;
  u = delta.dot (u_axis);
  v = delta.dot (v_axis);
  w = delta.dot (plane_normal);
}

首先將pt減去局部坐標系原點mean的坐標，得到delta。然後計算delta在u_axis，v_axis，plane_normal三個方向上的投影（有號）長度，即為u,v,w坐標。

注：代碼中用到的mean成員變數是在pcl::MLSResult::computeMLSSurface函數中計算出來的。

二維getMLSCoordinates

與上面的函數類似，只是少算了w坐標。

void
pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const
{
  //獲取pt在局部座標系下的二維座標
  Eigen::Vector3d delta = pt - mean;
  u = delta.dot (u_axis);
  v = delta.dot (v_axis);
}

getPolynomialValue

double
pcl::MLSResult::getPolynomialValue (const double u, const double v) const
{
  // Compute the polynomial's terms at the current point
  // Example for second order: z = a + b*y + c*y^2 + d*x + e*x*y + f*x^2
  //在二階的情況下:(ui, vi) = (0, 0), (0, 1), (0, 2), (1, 0), (1, 1), (2, 0)
  int j = 0;
  double u_pow = 1;
  double result = 0;
  for (int ui = 0; ui <= order; ++ui)
  {
    double v_pow = 1;
    for (int vi = 0; vi <= order - ui; ++vi)
    {
      result += c_vec[j++] * u_pow * v_pow;
      v_pow *= v;
    }
    u_pow *= u;
  }

  return (result);
}

如果曲面多項式的階數為二，那麼我們可以用$w=a+b\ast v+c\ast v^2+d\ast u+e\ast u\ast v+f\ast u^2$來表示它。

代碼中的u_pow便表示數學式中的$1$,$u$,或$u^2$，它會在迭代過程中動態改變，v_pow亦然。

result便是對多項式的係數c_vec[j]與u_pow，v_pow的乘積做累加，得到最終的多項式值。

注：代碼中用到的c_vec成員變數是在pcl::MLSResult::computeMLSSurface函數中計算出來的。