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
,也就是 w ⃗ \vec{w} w,等於(0,0,1)。 -
曲面在u軸方向的單位切向量為 ( 1 , 0 , ∂ z ∂ u ) ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ \frac{(1,0,\frac{\partial z}{\partial u})}{\|(1,0,\frac{\partial z}{\partial u})\|} ∥(1,0,∂u∂z)∥(1,0,∂u∂z),因為如果把曲面上的某一點在u方向移動一單位長,它在z方向就會移動 d z d u \frac{dz}{du} dudz。把向量 ( 1 , 0 , ∂ z ∂ u ) (1,0,\frac{\partial z}{\partial u}) (1,0,∂u∂z)除以它的長度 ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ \|(1,0,\frac{\partial z}{\partial u})\| ∥(1,0,∂u∂z)∥就得到曲面在u軸方向的單位切向量。同理,曲面在v軸方向的單位切向量為 ( 1 , 0 , ∂ z ∂ v ) ∥ ( 1 , 0 , ∂ z ∂ v ) ∥ \frac{(1,0,\frac{\partial z}{\partial v})}{\|(1,0,\frac{\partial z}{\partial v})\|} ∥(1,0,∂v∂z)∥(1,0,∂v∂z)。
在法向量計算這裡,筆者花了不少時間嘗試自行推導出它的計算方式。最後是在3.1 Tangent Plane and Surface Normal看到了法向量的公式才解決。法向量的公式如下:
n ⃗ = ∂ z ∂ u × ∂ z ∂ v ∥ ∂ z ∂ u × ∂ z ∂ v ∥ \vec{n} = \frac{\frac{\partial z}{\partial u} \times \frac{\partial z}{\partial v}}{\|\frac{\partial z}{\partial u} \times \frac{\partial z}{\partial v}\|} n=∥∂u∂z×∂v∂z∥∂u∂z×∂v∂z
先來看分子,它是 z z z對 u u u的導數和 z z z對 v v v的導數的外積: ∂ z ∂ u × ∂ z ∂ v = ∣ u v w 1 0 ∂ z ∂ u 0 1 ∂ z ∂ v ∣ = − ∂ z ∂ u u ⃗ − ∂ z ∂ v v ⃗ + w ⃗ \frac{\partial z}{\partial u} \times \frac{\partial z}{\partial v} = \begin{vmatrix}u & v & w \\ 1 & 0 & \frac{\partial z}{\partial u} \\ 0 & 1 & \frac{\partial z}{\partial v} \end{vmatrix}= -\frac{\partial z}{\partial u}\vec{u} - \frac{\partial z}{\partial v}\vec{v} + \vec{w} ∂u∂z×∂v∂z=∣∣∣∣∣∣u10v01w∂u∂z∂v∂z∣∣∣∣∣∣=−∂u∂zu−∂v∂zv+w
對照代碼一看,發現兩者完全吻合!至於公式中除以分母歸一化的過程,在代碼中則是直接調用Eigen::Vector3d::normalize()
函數達成。
以下是曾經試過,但沒推出來的方法:
-
計算平面法向量在 ( 1 , 0 , ∂ z ∂ u ) ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ \frac{(1,0,\frac{\partial z}{\partial u})}{\|(1,0,\frac{\partial z}{\partial u})\|} ∥(1,0,∂u∂z)∥(1,0,∂u∂z)方向及 ( 1 , 0 , ∂ z ∂ v ) ∥ ( 1 , 0 , ∂ z ∂ v ) ∥ \frac{(1,0,\frac{\partial z}{\partial v})}{\|(1,0,\frac{\partial z}{\partial v})\|} ∥(1,0,∂v∂z)∥(1,0,∂v∂z)方向(兩個切向量方向)的分量,讓
result.normal
減去它們之後,即可得到曲面的法向量方向,之後再對它做normalize即可得到單位長度的曲面法向量。平面法向量在 ( 1 , 0 , ∂ z ∂ u ) ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ \frac{(1,0,\frac{\partial z}{\partial u})}{\|(1,0,\frac{\partial z}{\partial u})\|} ∥(1,0,∂u∂z)∥(1,0,∂u∂z)方向的分量計算如下:
result.normal
與 ( 1 , 0 , ∂ z ∂ u ) ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ \frac{(1,0,\frac{\partial z}{\partial u})}{\|(1,0,\frac{\partial z}{\partial u})\|} ∥(1,0,∂u∂z)∥(1,0,∂u∂z)做內積,然後乘上該方向的單位向量。即 ( ( 0 , 0 , 1 ) ⋅ ( 1 , 0 , ∂ z ∂ u ) ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ ) ( 1 , 0 , ∂ z ∂ u ) ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ = ∂ z ∂ u ∥ ( 1 , 0 , ∂ z ∂ u ) ∥ 2 ( 1 , 0 , ∂ z ∂ u ) ((0,0,1) \cdot \frac{(1,0,\frac{\partial z}{\partial u})}{\|(1,0,\frac{\partial z}{\partial u})\|}) \frac{(1,0,\frac{\partial z}{\partial u})}{\|(1,0,\frac{\partial z}{\partial u})\|} = \frac{\frac{\partial z}{\partial u}}{\|(1,0,\frac{\partial z}{\partial u})\|^2}(1,0,\frac{\partial z}{\partial u}) ((0,0,1)⋅∥(1,0,∂u∂z)∥(1,0,∂u∂z))∥(1,0,∂u∂z)∥(1,0,∂u∂z)=∥(1,0,∂u∂z)∥2∂u∂z(1,0,∂u∂z) = … -
( 0 , 0 , 1 ) − ( 1 ∂ z ∂ u , 0 , 0 ) u ⃗ − ( 0 , 1 ∂ z ∂ v , 0 ) v ⃗ (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} (0,0,1)−(∂u∂z1,0,0)u−(0,∂v∂z1,0)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 ( g u , g v ) = ( g u − u ) 2 + ( g v − v ) 2 + ( g w − w ) 2 K(gu,gv) = (gu-u)^2+(gv-v)^2+(gw-w)^2 K(gu,gv)=(gu−u)2+(gv−v)2+(gw−w)2
-
計算距離平方函數對
gu
和gv
的微分,消掉係數2,得到兩個函數F1
和F2
F 1 = 1 2 ∂ K ∂ g u = 1 2 ( 2 ( g u − u ) + 2 ( g w − w ) g w g u ) = g u − u + ( g w − w ) g w g u 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} F1=21∂gu∂K=21(2(gu−u)+2(gw−w)gugw)=gu−u+(gw−w)gugw
F 2 = 1 2 ∂ K ∂ g v = 1 2 ( 2 ( g v − v ) + 2 ( g w − w ) g w g v ) = g v − v + ( g w − w ) g w g v 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} F2=21∂gv∂K=21(2(gv−v)+2(gw−w)gvgw)=gv−v+(gw−w)gvgw
-
我們希望函數
F1
及F2
為0,這代表在該處有距離的局部極(小)值,即 [ F 1 ( g u , g v ) F 2 ( g u , g v ) ] \begin{bmatrix}F_1(gu,gv) \\ F_2(gu,gv)\end{bmatrix} [F1(gu,gv)F2(gu,gv)] -
接下來是使用梯度下降法(?)來優化 [ F 1 F 2 ] \begin{bmatrix}F_1 \\ F_2\end{bmatrix} [F1F2],首先求出 [ F 1 F 2 ] \begin{bmatrix}F_1 \\ F_2\end{bmatrix} [F1F2]的Jacobian矩陣,然後用它來計算 [ g u g v ] \begin{bmatrix}gu \\ gv\end{bmatrix} [gugv]的更新量 -
為了推導出 [ g u g v ] \begin{bmatrix}gu \\ gv\end{bmatrix} [gugv]的更新量,首先將 [ F 1 F 2 ] \begin{bmatrix}F_1 \\ F_2\end{bmatrix} [F1F2]使用一階近似分解為 [ F 1 F 2 ] ≃ [ F 1 u F 1 v F 2 u F 2 v ] [ g u g v ] = [ e 1 e 2 ] \begin{bmatrix}F_1 \\ F_2\end{bmatrix} \simeq \begin{bmatrix}F_{1u} & F_{1v} \\ F_{2u} & F_{2v}\end{bmatrix}\begin{bmatrix}gu\\ gv\end{bmatrix} = \begin{bmatrix}e_1\\ e_2\end{bmatrix} [F1F2]≃[F1uF2uF1vF2v][gugv]=[e1e2]
-
我們希望更新後的
gu
,gv
能使得上式的計算結果為0,即 [ F 1 u F 1 v F 2 u F 2 v ] [ g u + Δ g u g v + Δ g v ] = [ 0 0 ] \begin{bmatrix}F_{1u} & F_{1v} \\ F_{2u} & F_{2v}\end{bmatrix} \begin{bmatrix}gu+ \Delta gu \\ gv+ \Delta gv\end{bmatrix} = \begin{bmatrix}0 \\ 0\end{bmatrix} [F1uF2uF1vF2v][gu+Δgugv+Δgv]=[00] -
接着從上式計算 Δ g u \Delta gu Δgu和 Δ g v \Delta gv Δgv: [ F 1 u F 1 v F 2 u F 2 v ] [ g u + Δ g u g v + Δ g v ] = [ F 1 u F 1 v F 2 u F 2 v ] [ g u g v ] + [ F 1 u F 1 v F 2 u F 2 v ] [ Δ g u Δ g v ] = [ 0 0 ] \begin{bmatrix}F_{1u} & F_{1v} \\ F_{2u} & F_{2v}\end{bmatrix} \begin{bmatrix}gu+ \Delta gu \\ gv+ \Delta gv\end{bmatrix} = \begin{bmatrix}F_{1u} & F_{1v} \\ F_{2u} & F_{2v}\end{bmatrix} \begin{bmatrix}gu \\ gv \end{bmatrix}+\begin{bmatrix}F_{1u} & F_{1v} \\ F_{2u} & F_{2v}\end{bmatrix} \begin{bmatrix}\Delta gu \\ \Delta gv\end{bmatrix} = \begin{bmatrix}0 \\ 0\end{bmatrix} [F1uF2uF1vF2v][gu+Δgugv+Δgv]=[F1uF2uF1vF2v][gugv]+[F1uF2uF1vF2v][ΔguΔgv]=[00]
-
由此可知 [ F 1 u F 1 v F 2 u F 2 v ] [ Δ g u Δ g v ] = − [ e 1 e 2 ] \begin{bmatrix}F_{1u} & F_{1v} \\ F_{2u} & F_{2v}\end{bmatrix} \begin{bmatrix}\Delta gu \\ \Delta gv\end{bmatrix} = -\begin{bmatrix}e_1\\ e_2\end{bmatrix} [F1uF2uF1vF2v][ΔguΔgv]=−[e1e2],因此 [ Δ g u Δ g v ] = − [ F 1 u F 1 v F 2 u F 2 v ] − 1 [ e 1 e 2 ] \begin{bmatrix}\Delta gu \\ \Delta gv\end{bmatrix} = -\begin{bmatrix}F_{1u} & F_{1v} \\ F_{2u} & F_{2v}\end{bmatrix}^{-1}\begin{bmatrix}e_1\\ e_2\end{bmatrix} [ΔguΔgv]=−[F1uF2uF1vF2v]−1[e1e2]
-
更新 [ g u g v ] \begin{bmatrix}gu \\ gv\end{bmatrix} [gugv]為 [ g u + Δ g u g v + Δ g v ] \begin{bmatrix}gu + \Delta gu \\ gv+ \Delta gv\end{bmatrix} [gu+Δgugv+Δgv]
-
接着更新 g w gw 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);
}