Python-open3d点云配准
文章目录
- ICP算法
- 鲁棒核
- ICP测试
ICP算法
ICP, 即Iterative Closest Point, 迭代点算法。
ICP算法有多种形式，其中最简单的思路就是比较点与点之间的距离，对于点云 P = { p i } , Q = { q i } P=\{p_i\}, Q=\{q_i\} P={pi},Q={qi}而言，如果二者是同一目标，通过旋转、平移等操作可以实现重合的话，那么只需要固定 Q Q Q而不断地旋转或平移 P P P，最终二者一定能最完美地重合。
设 R , t R, t R,t分别为旋转矩阵和平移矩阵，在完美匹配的情况下，必有 q i = R p i + t q_i = Rp_i + t qi=Rpi+t。但三维点云不具备栅格特征，故而很难保证 q i q_i qi和 p i p_i pi一一对应，所以求解问题变为优化问题，目标函数为
arg min R , t 1 2 ∑ i = 1 n ∥ q i − R p i − t ∥ 2 \argmin_{R,t}\frac{1}{2}\sum^n_{i=1}\Vert q_i-Rp_i-t\Vert^2 R,targmin21i=1∑n∥qi−Rpi−t∥2
1992年Chen和Medioni对此方案进行了改进，提出了点对面的预估方法，其目标函数为
arg min R , t 1 2 ∑ i = 1 n [ ( q i − R p i ) ⋅ n p ] 2 \argmin_{R,t}\frac{1}{2}\sum^n_{i=1}[(q_i-Rp_i)\cdot n_p]^2 R,targmin21i=1∑n[(qi−Rpi)⋅np]2
其中 n p n_p np是点 p p p的法线，这种方案显然效率更高。
在使用ICP算法前后，两个点云的叠加图像变化如下
基于Open3d实现的代码如下
import numpy as np import open3d as o3d pipreg = o3d.pipelines.registration pcd = o3d.data.DemoICPPointClouds() src = o3d.io.read_point_cloud(pcd.paths[0]) tar = o3d.io.read_point_cloud(pcd.paths[1]) th = 0.02 trans_init = np.array([ [0.862, 0.011, -0.507, 0.5], [-0.139, 0.967, -0.215, 0.7], [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]]) reg = pipreg.registration_icp( src, tar, th, trans_init, pipreg.TransformationEstimationPointToPoint()) print(reg.transformation) # 变换矩阵 print(reg) # 表示点云配准的拟合程度 ''' fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056 Access transformation to get result. '''【registration_icp】即为open3d实现的点云配准函数，在示例中，输入的5个参数分别为点云 P P P、目标点云 Q Q Q、同名点未匹配时的最大距离、初始变化矩阵、变换方法。
【TransformationEstimationPointToPoint】即点对点的目标函数。
匹配完成后，打印的变换矩阵为
[ 0.83924644 0.01006041 − 0.54390867 0.64639961 − 0.15102344 0.96521988 − 0.21491604 0.75166079 0.52191123 0.2616952 0.81146378 − 1.50303533 0. 0. 0. 1. ] ] \begin{bmatrix} 0.83924644&0.01006041&-0.54390867&0.64639961\\ -0.15102344&0.96521988&-0.21491604&0.75166079\\ 0.52191123&0.2616952& 0.81146378&-1.50303533\\ 0.& 0.&0.&1. ]\\ \end{bmatrix} 0.83924644−0.151023440.521911230.0.010060410.965219880.26169520.−0.54390867−0.214916040.811463780.0.646399610.75166079−1.503035331.]
绘图代码如下
from copy import deepcopy srcDraw = deepcopy(src) tarDraw = deepcopy(tar) srcDraw.paint_uniform_color([1, 1, 0]) tarDraw.paint_uniform_color([0, 1, 1]) srcDrawICP = deepcopy(src) tarDrawICP = deepcopy(tarDraw) srcDrawICP.transform(reg.transformation) geo = [srcDraw, tarDraw, srcDrawICP.translate((4.5, 0, 0)), tarDrawICP.translate((4.5, 0, 0))] o3d.visualization.draw_geometries(geo)鲁棒核
现有点云 P , Q P,Q P,Q，若二者存在一一对应的点列 p i ∈ P , q i ∈ Q p_i\in P, q_i\in Q pi∈P,qi∈Q，通过对 Q Q Q进行矩阵变换 T T T，以期二者完全配准，那么对于第 i i i个点而言，记 r i ( T ) = ( p i − T q i ) ⋅ n p i r_i(T)=(p_i-Tq_i)\cdot n_{p_i} ri(T)=(pi−Tqi)⋅npi， n p i n_{p_i} npi为点 p i p_i pi的法向量，即 q i q_i qi在经过矩阵变换后与 p i p_i pi在其法向量方向的残差。
则点对面ICP的目的，就是让下面的函数取值最小
E ( T ) = ∑ i = 1 N r i ( T ) 2 E(T)=\sum_{i=1}^Nr_i(T)^2 E(T)=i=1∑Nri(T)2
在这个优化问题中， r i r_i ri的作用举足轻重，任何对 r i r_i ri的形式上的更改，都会直接影响优化结果，例如将这个优化问题改写成重复加权最小二乘法的形式
E ( T ) = ∑ i = 1 N w i r i ( T ) 2 E(T)=\sum_{i=1}^Nw_ir_i(T)^2 E(T)=i=1∑Nwiri(T)2
让 r i r_i ri乘上一个权重，那么在具体匹配的过程中，就可以降低某些特殊值的影响。如果这个权重本身就是 r i r_i ri的函数，那么加权过程可以写成更加抽象的形式
E ( T ) = ∑ i = 1 N ρ [ r i ( T ) ] E(T)=\sum_{i=1}^N\rho[r_i(T)] E(T)=i=1∑Nρ[ri(T)]
ρ \rho ρ可称为Robust核函数，open3d中封装了如下和函数
核函数 封装 L1损失 L1Loss ω ( r ) = 1 ∣ r ∣ → ρ ( r ) = ∣ r ∣ \omega(r)=\frac1{\vert r\vert }\to\rho(r)=\vert r\vert ω(r)=∣r∣1→ρ(r)=∣r∣ L2损失 L2Loss ω ( r ) = 1 → ρ ( r ) = r 2 2 \omega(r)=1\to\rho(r)=\frac{r^2}2 ω(r)=1→ρ(r)=2r2 柯西核 CauchyLoss ω ( r ) = 1 1 + ( r k ) 2 → ρ ( r ) = k 2 2 log ( 1 + ( r k ) 2 ) \omega(r)=\frac1{1+(\frac r k)^2}\to \rho(r)=\frac{k^2}{2}\log(1+(\frac r k)^2) ω(r)=1+(kr)21→ρ(r)=2k2log(1+(kr)2) GM核 GMLoss ω ( r ) = k ( k + r 2 ) 2 → ρ ( r ) = r 2 / 2 k + r 2 \omega(r)=\frac{k}{(k+r^2)^2}\to\rho(r)=\frac{r^2/2}{k+r^2} ω(r)=(k+r2)2k→ρ(r)=k+r2r2/2 Huber核 HuberLoss Tukey核 TukeyLoss Huber核以及Tukey核相对复杂，表示如下
- Huber核
ω ( r ) = { 1 ∣ r ∣ ⩽ k k ∣ r ∣ otherwise → ρ ( r ) = { r 2 2 ∣ r ∣
- Huber核
