基于移动最小二乘曲面的点云对齐（二） 优化点云对齐迭代点
本文重点
上一篇博客介绍了基本隐式曲面的生成，以及点云对齐的基本操作，但是发现精度达不到理想要求，本文通过优化迭代点和步长设置优化点云对齐到隐式曲面的精度。
一、优化迭代点
接上文图
图1 点P向法截线在g0处的曲率圆做投影
其中交点Q1(X,Y,Z)和Q2(m,n,l)可以通过下面的方程解得
$\left\{ {\begin{array}{*{20}{c}}{{{\left( {X - {O_{x0}}} \right)}^2} + {{\left( {Y - {O_{y0}}} \right)}^2} + {{\left( {Z - {O_{z0}}} \right)}^2} = \frac{1}{{k_0^2}}}\\{\left[ {{n_0},P{g_0},\left( {X - {x_0},Y - {y_0},Z - {z_0}} \right)} \right] = 0}\\{\frac{{X - {O_{x0}}}}{{m - {O_{x0}}}} = \frac{{Y - {O_{y0}}}}{{n - {O_{y0}}}} = \frac{{Z - {O_{z0}}}}{{l - {O_{z0}}}}}\end{array}} \right.$
(公式8)
如图5所示，设直线PO与法截线的交点为M，可以将法截线上的曲线g0M，曲率圆上的小圆弧或者弦长||g0Q||作为步长Δs。然而计算隐式曲线段g0M或者小圆弧的长度比较麻烦，所以一般采用弦长||g0Q||作为步长Δs，即
|Δs| = ||g0Q|| (公式9)
这里规定步长的符号始终与g0P·t0(单位切向量)的符号相同，即
signal(Δs) = signal{ ( P-[x0, y0 ,z0 ] )·t0} (公式10)
随着g0的变化步长Δs也跟着变化，同时公式9的步长Δs满足|Δs|<θ/k0,这里θ为Og0与OQ的夹角并且满足0<θ<π/2,k0为法截线C0在g0处的曲率，由公式8解得。
然而若直接将公式9中的步长Δs代入公式8中计算，得到的g点往往偏离法截线C0较远，即|F(g)|较大，考虑到二阶泰勒逼近方法的收敛条件以及保证计算出来的g偏离法截线C0较小，因此需要控制步长|Δs|≤α（通常0<α<1）, α的值可以依据实际情况取定，α越小截断误差就越小。当|Δs|>α时，进行如下处理:令,N为正整数，考虑到曲率圆的小圆弧与弦长||g0Q||满足下列关系小圆弧>||g0Q||.做放大处理$\frac{{\left\| {{g_{0Q}}} \right\|}}{N} < \frac{\theta }{{{k_0}N}} < \frac{\pi }{{2{k_0}N}} \le \alpha $，即${\rm{N}} \ge \frac{\pi }{{2{k_0}N}}$，取
${\rm{N}} = \left| {\frac{\pi }{{2{k_0}N}}} \right|$
综上所述，步长
$\left| {\Delta {\rm{s}}} \right| = \left\{ {\begin{array}{*{20}{c}}{\left\| {{g_0}Q} \right\|\;,if\left\| {{g_0}Q} \right\| \le \alpha }\\{\frac{{\left\| {{g_0}Q} \right\|}}{N},else}\end{array}} \right.$
(公式11)
同时Δs符号满足公式10.
显然随着点g不断逼近目标点H，由公式11所表示的步长Δs不断趋于0.当步长Δs小于某一给定的微小量Ls时（Ls通常取10-6），认为计算出来的点即为所要求的目标点H。然而将公式11中的步长Δs代入公式8得到的点g虽然不会偏离法截面S0，但可能会偏离隐式曲面S较远，即|F(g)|>Le(Le为容许误差，通常取10-6)，因此需要对g进行误差矫正。
二、基于梯度的误差矫正方法
由公式8可知，点g位于法截面S0上，但是可能会偏离法截线C0较远，即会偏离隐式曲面S较远，|F(g)|=eF>Le,由公式8有g=g0+Δg,其中Δg=t0Δs+ (c0Δs2)1/2为迭代增矢量。
矫正误差的基本思想是：利用矫正矢量δg=[ δx δy δz ]来修正点g，即g’ = g + δg，g和g’分别为矫正前后的迭代值，并且使得|F(g’)|<Le。由于点g并未偏离法截面S0，即|G(g)|=0，矫正之后的点g’仍然需要满足|G(g’)|=0，因此矫正矢量δg位于法截面S0内，同时矫正矢量δg与迭代增矢量Δg垂直并指向隐式曲面S，故有
$\left\{ {\begin{array}{*{20}{c}}{\Delta F \cdot \delta g = - {{\rm{e}}_F}}\\{\Delta G \cdot \delta g = 0}\\{\Delta g \cdot \delta g = 0}\end{array}} \right.$ （方程4）
求解方程4得到
δg = [ -eF 0 0 ] [ ΔFT ΔGT ΔgT]-1
故改进后的迭代点为
g’ = g + [ -eF 0 0 ] [ΔFT ΔGT ΔgT]-1 （公式12）
将公式8代入公式12得到
g’ = g + t0Δs + c0Δs2(1/2) + [ -eF 0 0 ] [ ΔFT ΔGT ΔgT]-1 (公式13)
三、 算法实现
点到隐式曲面的正交投影算法
Step1. 给出初始点g0 = [x0,y0,z0]以及步长上限α.
Step2. 构造g0点处的法截面S0和法截线C0，并求出法截线C0在g0点处的单位切向量t0、曲率向量c0，以及曲率k0，同时求出垂足点Q以及步长Δs.
Step3. 按照公式(公式8)计算出点g.
Step4. 判断|F(g)|是否大于Le,如果|F(g)|>Le，对点g进行误差矫正，使得矫正后的点g’满足|F(g’)|<Le，并令gnew = g’，否则，令gnew = g.
Step5. 判断gnew是否为所要求的目标点H，即判断||是否小于Ls,如果||≤Ls，令H = gnew，执行下一步，否则，令g0 = gnew，转Step2.
Step6. 输出H的值，算法结束。
四、点到MLS曲面最近点计算
（隐式曲面的MLS情况，与上述算法基本相同，如有看不懂可以看隐式曲面求解正交投影部分）
如图4.1所示,一点云数据P定义的MLS曲面为S(x),点q到S(x)的正交投影计算过程如下：
Step1: 利用k邻域经典计算方法（即ANN算法）从P中计算距点q的邻域点集{Pi},并计算在点q处的加权法矢量nq，并令i = 0,ai = nq；
Step2: 计算从q出发沿ai的直线与S(x)的交点xi；
Step3: 利用公式${\rm{n}}\left( {{{\rm{x}}_i}} \right) = \;\frac{{\mathop \sum \nolimits_{j = 1}^k {n_{{p_j}}}{v_j}}}{{\left\| {\mathop \sum \nolimits_{j = 1}^k {n_{{p_j}}}{v_j}} \right\|}}$(公式2，请先看博客（1）再来此，以下未出现公式相同出处)计算交点xi处的加权法矢量nxi，并判断方向矢量ai与nxi是否重合（判断|ai x nxi|是否小于阈值ε，ε可参考几何建模核心如ACIS，一般取1.0x10-6），若|ai x nxi|≥ε，即ai与nxi不重合，继续下面正交投影点计算过程；
Step4: 由ai与nxi可构成法截面Plqxi，计算Plqxi与S(x)的法截线在点xi处的曲率圆圆心ci；
Step5: 令i = i+1,取曲率圆中心ci与点q的连线方向，即ai = qci/||qci||,继续3-5步直到|ai x nxi|<ε，即ai与nxi重合。
图4.1
法截面（S(x)计算）: 对于隐式曲面MLS曲面S(x)(曲面方程为g(x,y,z))，在点xi处的法矢量nxi可由下式计算：
\[{{\rm{n}}_{{\rm{xi}}}} = {\left. {\frac{{g\left( {x,y,z} \right)}}{{\left\| {g\left( {x,y,z} \right)} \right\|}}} \right|_{x = {x_i}}}\] (公式4.1)
式中，g(x,y,z)为MLS曲面的梯度，其值为
${\rm{g}}\left( {\rm{x}} \right) = {\left[ {\begin{array}{*{20}{c}}{\frac{{\partial g\left( {x,y,z} \right)}}{{\partial x}}}&{\frac{{\partial g\left( {x,y,z} \right)}}{{\partial y}}}&{\frac{{\partial g\left( {x,y,z} \right)}}{{\partial z}}}\end{array}} \right]^T}$ (公式4.2)
令法矢量nxi，坐标点xi及点q所定义的法截面Plqxi方程为F(x),则F(x)可定义为
${\rm{F}}\left( {\bf{x}} \right):\left( {{\rm{q}} - {{\rm{x}}_{\rm{i}}}} \right) \times {{\rm{n}}_{{\rm{xi}}}} \cdot \left( {{\rm{x}} - {{\rm{x}}_{\rm{i}}}} \right) = 0$ (公式4.3)
由上式可知
${\rm{F}}\left( {\rm{x}} \right) = \left( {{\rm{q}} - {{\rm{x}}_{\rm{i}}}} \right) \times {{\rm{n}}_{{\rm{xi}}}}$
${\left[ {{\rm{F}}\left( {\rm{x}} \right)} \right]^T} \cdot {n_{xi}} = 0$
${\left[ {{\rm{F}}\left( {\rm{x}} \right)} \right]^T} \cdot g\left( x \right) = 0$
当(q-xi)/ ||q-xi||趋近于nxi，直线qxi与nxi趋于重合，xi趋近于q在S(x)上的投影点。
点xi处曲率圆中心ci计算 : 由法截面及MLS曲面数学定义，法截面Plqxi与MLS曲面S(x)的法截线可表示为
$\left\{ {\begin{array}{*{20}{c}}{F\left( {\bf{x}} \right) = 0}\\{g\left( {x,y,z} \right) = 0}\end{array}} \right.$ (公式4.4)
令g(x,y,z) = g(x),设弧长参数为r，用公式4.4关于弧长参数求导，可得该曲线的一阶导数为
$\left\{ {\begin{array}{*{20}{c}}{{{\left[ {F\left( {\bf{x}} \right)} \right]}^T}\frac{{\partial x}}{{\partial r}} = 0}\\{{{\left[ {g\left( x \right)} \right]}^T}\frac{{\partial x}}{{\partial r}} = 0}\end{array}} \right.$ (公式4.5)
由上式可知，矢量$\frac{{\partial x}}{{\partial r}}$分别与梯度矢量F(x)及g(x)垂直，且为单位矢量，因此$\frac{{\partial x}}{{\partial r}}$可由下式得到：
$\frac{{\partial x}}{{\partial r}} = \frac{{F\left( {\rm{x}} \right) \times {\rm{g}}\left( {\rm{x}} \right)}}{{\left\| {F\left( {\rm{x}} \right) \times {\rm{g}}\left( {\rm{x}} \right)} \right\|}}$ (公式4.6)
五、点MLS曲面ICP多视数据对齐
输入工件多视点云数据Q1,Q2,…,Qm，在Imageware等商业软件中交互变换点云，可实现Q2,…,Qm到Q1定义坐标系下的粗对齐，得到粗对齐后的点云Q2’,…,Qm’.以上述点到MLS曲面最近点计算为基础，通过以下步骤即可实现为Q2’,…,Qm’到Q1基于点到MLS曲面ICP对齐。
Step1: 基于点到MLS曲面最近点计算方法，计算Q2’中每一点到MLS曲面(由Q1定义)的最近点，形成对应点对，令i=2,利用ICP计算得到变换矩阵Ti,并变换点云Qi’得到变换后点云Qi”;
Step2: 以合并的点云[Q1,Q2”,…,Qi”]为对齐目标点云，利用点到MLS曲面最近点的计算方法，计算点云Qi+1’到[Q1,Q2”,…,Qi”]定义的MLS曲面的对应点对，进而利用ICP计算变换矩阵Ti+1,并用Ti+1变换Qi+1’得到Qi+1”;
Step3: 令i= i+1，重复步骤2直至实现其他所有点云到合并点云MLS曲面的对齐。
【 结束 】
