基于移动最小二乘曲面的点云对齐（二） —— 优化点云对齐迭代点

本文重点

上一篇博客介绍了基本隐式曲面的生成，以及点云对齐的基本操作，但是发现精度达不到理想要求，本文通过优化迭代点和步长设置优化点云对齐到隐式曲面的精度。

一、优化迭代点

接上文图

图1 点P向法截线在g₀处的曲率圆做投影

其中交点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，可以将法截线上的曲线g₀M，曲率圆上的小圆弧或者弦长||g₀Q||作为步长Δs。然而计算隐式曲线段g₀M或者小圆弧的长度比较麻烦，所以一般采用弦长||g₀Q||作为步长Δs，即

|Δs| = ||g₀Q|| (公式9)

这里规定步长的符号始终与g₀P·t₀(单位切向量)的符号相同，即

signal(Δs) = signal{ ( P-[x₀, y₀ ,z₀ ] )·t₀} (公式10)

随着g0的变化步长Δs也跟着变化，同时公式9的步长Δs满足|Δs|<θ/k₀,这里θ为Og₀与OQ的夹角并且满足0<θ<π/2,k₀为法截线C₀在g₀处的曲率，由公式8解得。

然而若直接将公式9中的步长Δs代入公式8中计算，得到的g点往往偏离法截线C₀较远，即|F(g)|较大，考虑到二阶泰勒逼近方法的收敛条件以及保证计算出来的g偏离法截线C₀较小，因此需要控制步长|Δs|≤α（通常0<α<1）, α的值可以依据实际情况取定，α越小截断误差就越小。当|Δs|>α时，进行如下处理:令,N为正整数，考虑到曲率圆的小圆弧与弦长||g₀Q||满足下列关系小圆弧>||g₀Q||.做放大处理$\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虽然不会偏离法截面S₀，但可能会偏离隐式曲面S较远，即|F(g)|>Le(Le为容许误差，通常取10^-6)，因此需要对g进行误差矫正。

二、基于梯度的误差矫正方法

由公式8可知，点g位于法截面S₀上，但是可能会偏离法截线C₀较远，即会偏离隐式曲面S较远，|F(g)|=e_F>Le,由公式8有g=g₀+Δg,其中Δg=t₀Δs+ (c₀Δs²)1/2为迭代增矢量。

矫正误差的基本思想是：利用矫正矢量δg=[ δx δy δz ]来修正点g，即g’ = g + δg，g和g’分别为矫正前后的迭代值，并且使得|F(g’)|<Le。由于点g并未偏离法截面S₀，即|G(g)|=0，矫正之后的点g’仍然需要满足|G(g’)|=0，因此矫正矢量δg位于法截面S₀内，同时矫正矢量δ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 = [ -e_F 0 0 ] [ ΔF^T ΔG^T Δg^T]^-1

故改进后的迭代点为

g’ = g + [ -e_F 0 0 ] [ΔF^T ΔG^T Δg^T]^-1 （公式12）

将公式8代入公式12得到

g’ = g + t0Δs + c₀Δs²(1/2) + [ -e_F 0 0 ] [ ΔF^T ΔG^T Δg^T]^-1 (公式13)

三、算法实现

点到隐式曲面的正交投影算法

Step1. 给出初始点g₀ = [x₀,y₀,z₀]以及步长上限α.

Step2. 构造g₀点处的法截面S₀和法截线C₀，并求出法截线C₀在g₀点处的单位切向量t₀、曲率向量c₀，以及曲率k₀，同时求出垂足点Q以及步长Δs.

Step3. 按照公式(公式8)计算出点g.

Step4. 判断|F(g)|是否大于Le,如果|F(g)|>Le，对点g进行误差矫正，使得矫正后的点g’满足|F(g’)|<Le，并令g_new = g’，否则，令g_new = g.

Step5. 判断g_new是否为所要求的目标点H，即判断||是否小于Ls,如果||≤Ls，令H = g_new，执行下一步，否则，令g₀ = g_new，转Step2.

Step6. 输出H的值，算法结束。

四、点到MLS曲面最近点计算

（隐式曲面的MLS情况，与上述算法基本相同，如有看不懂可以看隐式曲面求解正交投影部分）

如图4.1所示,一点云数据P定义的MLS曲面为S(x),点q到S(x)的正交投影计算过程如下：

Step1: 利用k邻域经典计算方法（即ANN算法）从P中计算距点q的邻域点集{Pi},并计算在点q处的加权法矢量n_q，并令i = 0,a_i = n_q；

Step2: 计算从q出发沿a_i的直线与S(x)的交点x_i；

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）再来此，以下未出现公式相同出处)计算交点x_i处的加权法矢量n_xi，并判断方向矢量a_i与n_xi是否重合（判断|a_ix n_xi|是否小于阈值ε，ε可参考几何建模核心如ACIS，一般取1.0x10^-6），若|a_ix n_xi|≥ε，即a_i与n_xi不重合，继续下面正交投影点计算过程；

Step4: 由a_i与n_xi可构成法截面Pl_qxi，计算Pl_qxi与S(x)的法截线在点x_i处的曲率圆圆心c_i；

Step5: 令i = i+1,取曲率圆中心c_i与点q的连线方向，即a_i = qc_i/||qc_i||,继续3-5步直到|a_ix n_xi|<ε，即a_i与n_xi重合。

图4.1

4.1 MLS曲面相关参数计算

法截面（S(x)计算）: 对于隐式曲面MLS曲面S(x)(曲面方程为g(x,y,z))，在点x_i处的法矢量n_xi可由下式计算：

\[{{\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)

令法矢量n_xi，坐标点x_i及点q所定义的法截面Pl_qxi方程为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||趋近于n_xi，直线q_xi与n_xi趋于重合，xi趋近于q在S(x)上的投影点。

点x_i处曲率圆中心c_i计算 : 由法截面及MLS曲面数学定义，法截面Pl_qxi与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多视数据对齐

输入工件多视点云数据Q₁,Q₂,…,Q_m，在Imageware等商业软件中交互变换点云，可实现Q₂,…,Q_m到Q1定义坐标系下的粗对齐，得到粗对齐后的点云Q₂’,…,Q_m’.以上述点到MLS曲面最近点计算为基础，通过以下步骤即可实现为Q₂’,…,Q_m’到Q1基于点到MLS曲面ICP对齐。

Step1: 基于点到MLS曲面最近点计算方法，计算Q₂’中每一点到MLS曲面(由Q₁定义)的最近点，形成对应点对，令i=2,利用ICP计算得到变换矩阵T_i,并变换点云Q_i’得到变换后点云Q_i”;

Step2: 以合并的点云[Q₁,Q₂”,…,Q_i”]为对齐目标点云，利用点到MLS曲面最近点的计算方法，计算点云Q_i+1’到[Q₁,Q₂”,…,Q_i”]定义的MLS曲面的对应点对，进而利用ICP计算变换矩阵T_i+1,并用T_i+1变换Q_i+1’得到Q_i+1”;

Step3: 令i= i+1，重复步骤2直至实现其他所有点云到合并点云MLS曲面的对齐。

【结束】

原文地址：https://www.cnblogs.com/fujj/p/9678646.html