PCL - MLS代碼研讀（六）- 各式投影函數

前言

本篇終於接近MLS模塊的核心部分，主要介紹各式投影函數。

其中投影到“平面”的函數為projectPointToMLSPlane，投影到“曲面”的函數包括projectPointSimpleToPolynomialSurface和projectPointOrthogonalToPolynomialSurface。

另外有兩個函數作為他們對外的接口，分別是projectPoint和projectQueryPoint。

projectPointToMLSPlane

給定局部坐標系下的點坐標(u,v)，將該點投影到擬合“平面”上。計算該點在全局坐標系下的坐標result.point。

pcl::MLSResult::MLSProjectionResults
pcl::MLSResult::projectPointToMLSPlane (const double u, const double v) const
{
  //投影到擬合"平面"上，所以沒有w座標，且normal即為平面的法向量
  MLSProjectionResults result;
  result.u = u;
  result.v = v;
  result.normal = plane_normal;
  //由原點mean在平面上移動
  //沒有w座標
  result.point = mean + u * u_axis + v * v_axis;

  return (result);
}

注意因為是投影到平面上，所以該點的法向量也就簡單地設為平面的法向量：

  result.normal = plane_normal;

另外因為局部坐標系是由平面上的一點mean和平面的法向量plane_normal來定義。所以我們可以知道點(u,v)投影到平面後，與局部坐標系原點mean在u_axis方向的距離就是u；同理，它的投影點與mean在v_axis方向的距離為v，因此投影點坐標可由以下算式計算：

  result.point = mean + u * u_axis + v * v_axis;

projectPointSimpleToPolynomialSurface

先將點投影到擬合"平面"上，然後再沿平面法向量方向移動到"曲面"上。

pcl::MLSResult::MLSProjectionResults
pcl::MLSResult::projectPointSimpleToPolynomialSurface (const double u, const double v) const
{
  MLSProjectionResults result;
  double w = 0;

  //這三行跟投影到擬合"平面"一樣(projectPointToMLSPlane函數)
  result.u = u;
  result.v = v;
  result.normal = plane_normal;

  if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
  {
    const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
    w = d.z;
    //怎麼算的?
    //曲面投影點的法向量是平面法向量減去該投影點在u,v兩個軸的切向量?
    //既然找得到投影點的切向量，為何不直接拿切向量算外積就好了?是正負號的問題?
    result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
    result.normal.normalize ();
  }

  //前三項跟投影到擬合"平面"上一樣，最後一項是在平面法向量方向上移動
  //從mean點分別往u_axis,v_axis及plane_normal方向偏移u,v及w即為投影的點
  result.point = mean + u * u_axis + v * v_axis + w * plane_normal;

  return (result);
}

首先看到這三行，可以發現與projectPointToMLSPlane函數一致，也就是將點投影到擬合“平面“上：

  result.u = u;
  result.v = v;
  result.normal = plane_normal;

接着是將點沿着法向量方向移動到“曲面”上，因為是沿着平面法向量移動，而平面法向量方向也就是局部坐標系的w方向，所以在這個過程中，點的u,v坐標會保持不變。

w坐標可由PCL - MLS代碼研讀（四）- 偏微分計算函數介紹過的getPolynomialPartialDerivative函數計算而來。具體而言，就是將u,v帶入多項式而得到：

    const PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
    w = d.z;

投影點坐標的計算：

  result.point = mean + u * u_axis + v * v_axis + w * plane_normal;

與projectPointToMLSPlane函數相比，多了w * plane_normal這一項，這也是開頭所說的“沿平面法向量方向移動到曲面上”。

投影點法向量的計算方式如下：

    result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
    result.normal.normalize ();

代碼很簡短，不過沒有背景知識實在無法一眼就看出它的意涵。所以下面先簡要介紹一下：

• result.normal原本是平面的法向量plane_normal，也就是$\vec{w}$，等於(0,0,1)。

• 曲面在u軸方向的單位切向量為$\frac{(1,0,\frac{\partial z}{\partial u})}{\Vert (1,0,\frac{\partial z}{\partial u})\Vert }$，因為如果把曲面上的某一點在u方向移動一單位長，它在z方向就會移動$\frac{dz}{du}$。把向量$(1,0,\frac{\partial z}{\partial u})$除以它的長度$\Vert (1,0,\frac{\partial z}{\partial u})\Vert$就得到曲面在u軸方向的單位切向量。同理，曲面在v軸方向的單位切向量為$\frac{(1,0,\frac{\partial z}{\partial v})}{\Vert (1,0,\frac{\partial z}{\partial v})\Vert }$。

在法向量計算這裡，筆者花了不少時間嘗試自行推導出它的計算方式。最後是在3.1 Tangent Plane and Surface Normal看到了法向量的公式才解決。法向量的公式如下：

$\vec{n}=\frac{\frac{\partial z}{\partial u}\times \frac{\partial z}{\partial v}}{\Vert \frac{\partial z}{\partial u}\times \frac{\partial z}{\partial v}\Vert }$

先來看分子，它是$z$對$u$的導數和$z$對$v$的導數的外積：$\frac{\partial z}{\partial u}\times \frac{\partial z}{\partial v}=\left|\begin{array}{ccc}u& v& w\\ 1& 0& \frac{\partial z}{\partial u}\\ 0& 1& \frac{\partial z}{\partial v}\end{array}\right|=-\frac{\partial z}{\partial u}\vec{u}-\frac{\partial z}{\partial v}\vec{v}+\vec{w}$

對照代碼一看，發現兩者完全吻合！至於公式中除以分母歸一化的過程，在代碼中則是直接調用Eigen::Vector3d::normalize()函數達成。

以下是曾經試過，但沒推出來的方法：

• 計算平面法向量在$\frac{(1,0,\frac{\partial z}{\partial u})}{\Vert (1,0,\frac{\partial z}{\partial u})\Vert }$方向及$\frac{(1,0,\frac{\partial z}{\partial v})}{\Vert (1,0,\frac{\partial z}{\partial v})\Vert }$方向（兩個切向量方向）的分量，讓result.normal減去它們之後，即可得到曲面的法向量方向，之後再對它做normalize即可得到單位長度的曲面法向量。

  平面法向量在$\frac{(1,0,\frac{\partial z}{\partial u})}{\Vert (1,0,\frac{\partial z}{\partial u})\Vert }$方向的分量計算如下：result.normal與$\frac{(1,0,\frac{\partial z}{\partial u})}{\Vert (1,0,\frac{\partial z}{\partial u})\Vert }$做內積，然後乘上該方向的單位向量。即$((0,0,1)\cdot \frac{(1,0,\frac{\partial z}{\partial u})}{\Vert (1,0,\frac{\partial z}{\partial u})\Vert })\frac{(1,0,\frac{\partial z}{\partial u})}{\Vert (1,0,\frac{\partial z}{\partial u})\Vert }=\frac{\frac{\partial z}{\partial u}}{\Vert (1,0,\frac{\partial z}{\partial u}){\Vert }^2}(1,0,\frac{\partial z}{\partial u})$ = …

• $(0,0,1)-(\frac{1}{\frac{\partial z}{\partial u}},0,0)\vec{u}-(0,\frac{1}{\frac{\partial z}{\partial v}},0)\vec{v}$，然後normalize

projectPointOrthogonalToPolynomialSurface

這個函數與projectPointSimpleToPolynomialSurface相同，都是將點投影到曲面上。他們之間的差異是：projectPointSimpleToPolynomialSurface是沿法向量投影，projectPointOrthogonalToPolynomialSurface則是在曲面上尋找離query_point最近的點來當作投影點。

pcl::MLSResult::MLSProjectionResults
pcl::MLSResult::projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const
{
  //在曲面上尋找距離query_point最近的點,把它當成query_point在曲面上的投影點

  //(gu,gv)表示最後投影的位置
  double gu = u;
  double gv = v;
  double gw = 0;

  MLSProjectionResults result;
  result.normal = plane_normal;
  //order > 1:如果不是這樣,就不必擬合曲面
  //c_vec.size () >= (order + 1) * (order + 2) / 2:適定或超定方程組
  if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && std::isfinite (c_vec[0]))
  {
    PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv);
    //gw,即d.z是(gu,gv)點在曲面上點的z座標值
    gw = d.z;
    double err_total;
    //一開始(gu,gv,gw)跟(u,v,w)的距離
    const double dist1 = std::abs (gw - w);
    //用於記錄迭代過程中(gu,gv,gw)跟(u,v,w)的距離
    double dist2;
    /**
     * 牛頓法
     * 欲優化的損失函數為 被投影點query_point(u,v,w) 及 投影點(gu,gv,gw) 的距離
     * 即K(gu,gv)為(gu-u)^2 + (gv-v)^2 + (gw-w)^2
     * 注意因為gw是由gu,gv唯一決定,所以K函數的自變量只有gu,gv兩個
     * 將K函數對gu,gv各自做偏微分,令他們為0,期望找到最小值
     * dK/dgu = 2(gu-u)+2(gw-w)*(dgw/dgu)
     * dK/dgv = 2(gv-v)+2(gw-w)*(dgw/dgv)
     * 設F1(gu) = (gu-u)+(gw-w)*(dgw/dgu)
     * 設F2(gv) = (gv-v)+(gw-w)*(dgw/dgv)
     * 
     * 這裡是把距離平方對gu,gv的微分[F1]當作誤差函數來優化?
     *                          [F2]
     * 不是直接優化距離平方?
     * 
     * 欲求F1 = 0及F2 = 0這個方程組的根gu,gv
     * 
     **/
    do
    {
      //將F1函數代入當前的gu,得到常數e1
      double e1 = (gu - u) + d.z_u * gw - d.z_u * w;
      //將F2函數代入當前的gv,得到常數e2
      double e2 = (gv - v) + d.z_v * gw - d.z_v * w;

      //建構Jacobian矩陣
      //F1對gu微分
      const double F1u = 1 + d.z_uu * gw + d.z_u * d.z_u - d.z_uu * w;
      //F1對gv微分
      const double F1v = d.z_uv * gw + d.z_u * d.z_v - d.z_uv * w;

      //F2對gu微分
      const double F2u = d.z_uv * gw + d.z_v * d.z_u - d.z_uv * w;
      //F2對gv微分
      const double F2v = 1 + d.z_vv * gw + d.z_v * d.z_v - d.z_vv * w;

      /**
       * Jacobian矩陣
       * 誤差函數(F1,F2) 對 兩個自變量(gu,gv) 的微分
       * [F1u F1v]
       * [F2u F2v]
       **/
      Eigen::MatrixXd J (2, 2);
      J (0, 0) = F1u;
      J (0, 1) = F1v;
      J (1, 0) = F2u;
      J (1, 1) = F2v;

      /**
       * err也就是
       * [F1(gu)] = [e1]
       * [F2(gv)]   [e2]
       **/
      Eigen::Vector2d err (e1, e2);
      //Jacobian的反矩陣
      Eigen::Vector2d update = J.inverse () * err;
      /**
       * 牛頓法,把步長選擇為f(x)(?)
       * x' = x - J^-1 * f(x)
       **/
      gu -= update (0);
      gv -= update (1);

      //gw是gu及gv的函數，代入新的(gu,gv)得到
      d = getPolynomialPartialDerivative (gu, gv);
      gw = d.z;
      //更新後的(gu,gv,gw)跟(u,v,w)的距離
      dist2 = std::sqrt ((gu - u) * (gu - u) + (gv - v) * (gv - v) + (gw - w) * (gw - w));

      err_total = std::sqrt (e1 * e1 + e2 * e2);

    //err_total <= 1e−8(收斂)或dist2 >= dist1(發散)時跳出
    } while (err_total > 1e-8 && dist2 < dist1);

    //如果算法發散,則採用一開始的(u,v,d.z)
    if (dist2 > dist1) // the optimization was diverging reset the coordinates for simple projection
    {
      gu = u;
      gv = v;
      d = getPolynomialPartialDerivative (u, v);
      gw = d.z;
    }

    result.u = gu;
    result.v = gv;
    //法向量的算法跟projectPointSimpleToPolynomialSurface一樣
    //任一向量減去它在曲面切向量方向的分量,得到曲面法向量方向的分量
    result.normal -= (d.z_u * u_axis + d.z_v * v_axis);
    //上面只得到法向量的方向,這裡把它的長度設為1
    result.normal.normalize ();
  }

  //優化得到(gu,gv),投影點即曲面上(gu,gv)處的點
  result.point = mean + gu * u_axis + gv * v_axis + gw * plane_normal;

  return (result);
}

這段程序有點長，以下是它的主要步驟：

• 計算投影點與query_point距離平方，這是有兩個自變量gu,gv的純量函數（u,v,w已知，gw由gu,gv唯一確定）

  $K(gu,gv)=(gu-u{)}^2+(gv-v{)}^2+(gw-w{)}^2$

• 計算距離平方函數對gu和gv的微分，消掉係數2，得到兩個函數F1和F2

  $F_1=\frac{1}{2}\frac{\partial K}{\partial gu}=\frac{1}{2}(2(gu-u)+2(gw-w)\frac{gw}{gu})=gu-u+(gw-w)\frac{gw}{gu}$

  $F_2=\frac{1}{2}\frac{\partial K}{\partial gv}=\frac{1}{2}(2(gv-v)+2(gw-w)\frac{gw}{gv})=gv-v+(gw-w)\frac{gw}{gv}$

• 我們希望函數F1及F2為0，這代表在該處有距離的局部極（小）值，即$\left[\begin{array}{c}{F}_1(gu,gv)\\ F_2(gu,gv)\end{array}\right]$

• 接下來是使用梯度下降法（？）來優化$\left[\begin{array}{c}{F}_1\\ F_2\end{array}\right]$，首先求出$\left[\begin{array}{c}{F}_1\\ F_2\end{array}\right]$的Jacobian矩陣，然後用它來計算$\left[\begin{array}{c}gu\\ gv\end{array}\right]$的更新量

• 為了推導出$\left[\begin{array}{c}gu\\ gv\end{array}\right]$的更新量，首先將$\left[\begin{array}{c}{F}_1\\ F_2\end{array}\right]$使用一階近似分解為$\left[\begin{array}{c}{F}_1\\ F_2\end{array}\right]\simeq \left[\begin{array}{cc}{F}_{1u}& F_{1v}\\ F_{2u}& F_{2v}\end{array}\right]\left[\begin{array}{c}gu\\ gv\end{array}\right]=\left[\begin{array}{c}{e}_1\\ e_2\end{array}\right]$

• 我們希望更新後的gu,gv能使得上式的計算結果為0，即$\left[\begin{array}{cc}{F}_{1u}& F_{1v}\\ F_{2u}& F_{2v}\end{array}\right]\left[\begin{array}{c}gu+\Delta gu\\ gv+\Delta gv\end{array}\right]=\left[\begin{array}{c}0\\ 0\end{array}\right]$

• 接着從上式計算$\Delta gu$和$\Delta gv$：$\left[\begin{array}{cc}{F}_{1u}& F_{1v}\\ F_{2u}& F_{2v}\end{array}\right]\left[\begin{array}{c}gu+\Delta gu\\ gv+\Delta gv\end{array}\right]=\left[\begin{array}{cc}{F}_{1u}& F_{1v}\\ F_{2u}& F_{2v}\end{array}\right]\left[\begin{array}{c}gu\\ gv\end{array}\right]+\left[\begin{array}{cc}{F}_{1u}& F_{1v}\\ F_{2u}& F_{2v}\end{array}\right]\left[\begin{array}{c}\Delta gu\\ \Delta gv\end{array}\right]=\left[\begin{array}{c}0\\ 0\end{array}\right]$

• 由此可知$\left[\begin{array}{cc}{F}_{1u}& F_{1v}\\ F_{2u}& F_{2v}\end{array}\right]\left[\begin{array}{c}\Delta gu\\ \Delta gv\end{array}\right]=-\left[\begin{array}{c}{e}_1\\ e_2\end{array}\right]$，因此$\left[\begin{array}{c}\Delta gu\\ \Delta gv\end{array}\right]=-{\left[\begin{array}{cc}{F}_{1u}& F_{1v}\\ F_{2u}& F_{2v}\end{array}\right]}^{-1}\left[\begin{array}{c}{e}_1\\ e_2\end{array}\right]$

• 更新$\left[\begin{array}{c}gu\\ gv\end{array}\right]$為$\left[\begin{array}{c}gu+\Delta gu\\ gv+\Delta gv\end{array}\right]$

• 接着更新$gw$和dist2

• 檢查此次迭代算法是否發散，如果是則跳出while循環。以上就是牛頓法的過程

• while循環結束後更新投影點的相關資訊，包括：result.u,result.v,result.point,result.normal

上面優化F1,F2而非距離平方的想法來自Shortest distance between point and surface。

注意上面提到的牛頓法是尋找方程式根的牛頓法，而非優化算法中用於尋找最小值的牛頓法！

Github上的相關討論：（[question] [surface] What’s the optimization algorithm used in projectPointOrthogonalToPolynomialSurface）

projectPoint

projectPoint函數使用傳入的method投影方法對傳入的三維點pt做投影。投影之前會先檢查該點附近的鄰居數量num_neighbors是否大於等於required_neighbors，如果是，表示該方程組是適定或超定方程，可以對多項式係數求解，否則直接放棄擬合曲面，直接調用projectPointToMLSPlane擬合平面。

pcl::MLSResult::MLSProjectionResults
pcl::MLSResult::projectPoint (const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors) const
{
  double u, v, w;
  getMLSCoordinates (pt, u, v, w);

  MLSResult::MLSProjectionResults proj;
  //order > 1: 如果order==1,那麼投影到平面上即可
  //num_neighbors >= required_neighbors:適定或過定方程
  //ProjectionMethod為NONE表示投影到平面
  if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
  {
    if (method == ORTHOGONAL)
      proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
    else // SIMPLE
      proj = projectPointSimpleToPolynomialSurface (u, v);
  }
  else
  {
    proj = projectPointToMLSPlane (u, v);
  }

  return  (proj);
}

projectQueryPoint

這個函數與projectPoint函數大同小異，只是少了一個參數point，直接對成員變量query_point做投影。（那為何不直接調用projectPoint？）

pcl::MLSResult::MLSProjectionResults
pcl::MLSResult::projectQueryPoint (ProjectionMethod method, int required_neighbors) const
{
  //required_neighbors在caller那邊是叫nr_coeff_，代表需要與係數數量相當的鄰居,才會形成適定或過定方程,才找得到解
  MLSResult::MLSProjectionResults proj;
  //order > 1:擬合的是曲面
  //num_neighbors >= required_neighbors:適定或過定方程
  //ProjectionMethod為NONE表示投影到平面
  if (order > 1 && num_neighbors >= required_neighbors && std::isfinite (c_vec[0]) && method != NONE)
  {
    if (method == ORTHOGONAL)
    {
      double u, v, w;
      getMLSCoordinates (query_point, u, v, w);
      proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
    }
    else // SIMPLE
    {
      //method預設為MLSResult::SIMPLE
      // Projection onto MLS surface along Darboux normal to the height at (0,0)
      //Darboux normal就是plane_normal?
      //如果階數為2，c_vec的例子: z = c_vec[0] + c_vec[1]*v + c_vec[2]*v^2 + c_vec[3]*u + c_vec[4]*u*v + c_vec[5]*u^2
      //mean: query_point在"平面"上的投影,局部坐標系的原點
      /**先將query_point投影到"平"面上,得到mean,然後再移動到"曲"面上,得到proj.point
       * 此處是論文裡圖三:由紅點(mean)移動到藍點(proj.point)的過程
       * 從投影點(也就是局部座標系的原點)開始，沿擬合平面法向量方向移動到曲面上
       **/
      //c_vec[0]:由上式的z(u,v)中的u,v帶0得來，代表曲面在原點mean(0,0)處相對原點的高度
      proj.point = mean + (c_vec[0] * plane_normal);

      // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
      //"切"向量計算:偏微分
      //如: z = c_vec[0] + c_vec[1]*v + c_vec[2]*v^2 + c_vec[3]*u + c_vec[4]*u*v + c_vec[5]*u^2
      //dz/du = c_vec[3] + c_vec[4]*v + 2*c_vec[5]*u
      //但為何這裡只考慮u及v的次方數為1的情況?
      //平面法向量減投影點切向量等於投影點在曲面上的法向量?
      //應該改成跟projectPointSimpleToPolynomialSurface裡的result.normal -= (d.z_u * u_axis + d.z_v * v_axis);一樣?
      //任何一個向量減去 曲面兩個切向量方向 的分量 之後,就只剩下與曲面垂直的分量
      proj.normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
      proj.normal.normalize ();
    }
  }
  else
  {
    //如果階數為一或無解，代表不需擬合"曲"面，直接投影到擬合"平"面上
    proj.normal = plane_normal;
    //mean本來就是query_point在擬合"平"面上的投影
    proj.point = mean;
  }

  return (proj);
}