Python-open3d点云配准

见贤思齐

文章目录ICP算法鲁棒核ICP测试ICP算法ICP,即IterativeClosestPoint,迭代点算法。ICP算法有多种形式，其中最简单的思路就是比较点与点之间的距离，对于点云P={pi},Q={qi}P=\{p_i\},Q=\{q_i\}P={pi​},Q={qi​}而言，如果二者是同一目标，通过旋转、平移等操作可以实现重合的话，那么只需要固定QQQ而不断地旋转或平移PPP，最终二者一定能最完美地重合。设R,tR,tR,t分别为旋转矩阵和平移矩阵，在完美匹配的情况下，必有qi=Rpi+tq_i=Rp_i+tqi​=Rpi​+t。但三维点云不具备栅格特征，故而很难保证qiq_iqi​和pip_ipi​一一对应，所以求解问题变为优化问题，目标函数为arg min⁡R,t12∑i=1n∥qi−Rpi−t∥2\argmin_{R,t}\frac{1}{2}\sum^n_{i=1}\Vertq_i-Rp_i-t\Vert^2R,targmin​21​i=1∑n​∥qi​−Rpi​−t∥21992年Chen和Medioni对此方案进行了改进，提出了点对面的预估方法，其目标函数为arg min⁡R,t12∑i=1n[(qi−Rpi)⋅np]2\argmin_{R,t}\frac{1}{2}\sum^n_{i=1}[(q_i-Rp_i)\cdotn_p]^2R,targmin​21​i=1∑n​[(qi​−Rpi​)⋅np​]2其中npn_pnp​是点ppp的法线，这种方案显然效率更高。在使用ICP算法前后，两个点云的叠加图像变化如下基于Open3d实现的代码如下importnumpyasnpimportopen3daso3dpipreg=o3d.pipelines.registrationpcd=o3d.data.DemoICPPointClouds()src=o3d.io.read_point_cloud(pcd.paths[0])tar=o3d.io.read_point_cloud(pcd.paths[1])th=0.02trans_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,andcorrespondence_setsizeof74056Accesstransformationtogetresult.'''12345678910111213141516171819202122【registration_icp】即为open3d实现的点云配准函数，在示例中，输入的5个参数分别为点云PPP、目标点云QQQ、同名点未匹配时的最大距离、初始变化矩阵、变换方法。【TransformationEstimationPointToPoint】即点对点的目标函数。匹配完成后，打印的变换矩阵为[0.839246440.01006041−0.543908670.64639961−0.151023440.96521988−0.214916040.751660790.521911230.26169520.81146378−1.503035330.0.0.1.]]⎡⎣⎢⎢⎢0.83924644−0.151023440.521911230.0.010060410.965219880.26169520.−0.54390867−0.214916040.811463780.0.646399610.75166079−1.503035331.]⎤⎦⎥⎥⎥[0.839246440.01006041−0.543908670.64639961−0.151023440.96521988−0.214916040.751660790.521911230.26169520.81146378−1.503035330.0.0.1.]]​0.83924644−0.151023440.521911230.​0.010060410.965219880.26169520.​−0.54390867−0.214916040.811463780.​0.646399610.75166079−1.503035331.]​​绘图代码如下fromcopyimportdeepcopysrcDraw=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)123456789101112131415鲁棒核现有点云P,QP,QP,Q，若二者存在一一对应的点列pi∈P,qi∈Qp_i\inP,q_i\inQpi​∈P,qi​∈Q，通过对QQQ进行矩阵变换TTT，以期二者完全配准，那么对于第iii个点而言，记ri(T)=(pi−Tqi)⋅npir_i(T)=(p_i-Tq_i)\cdotn_{p_i}ri​(T)=(pi​−Tqi​)⋅npi​​，npin_{p_i}npi​​为点pip_ipi​的法向量，即qiq_iqi​在经过矩阵变换后与pip_ipi​在其法向量方向的残差。则点对面ICP的目的，就是让下面的函数取值最小E(T)=∑i=1Nri(T)2E(T)=\sum_{i=1}^Nr_i(T)^2E(T)=i=1∑N​ri​(T)2在这个优化问题中，rir_iri​的作用举足轻重，任何对rir_iri​的形式上的更改，都会直接影响优化结果，例如将这个优化问题改写成重复加权最小二乘法的形式E(T)=∑i=1Nwiri(T)2E(T)=\sum_{i=1}^Nw_ir_i(T)^2E(T)=i=1∑N​wi​ri​(T)2让rir_iri​乘上一个权重，那么在具体匹配的过程中，就可以降低某些特殊值的影响。如果这个权重本身就是rir_iri​的函数，那么加权过程可以写成更加抽象的形式E(T)=∑i=1Nρ[ri(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{\vertr\vert}\to\rho(r)=\vertr\vertω(r)=∣r∣1​→ρ(r)=∣r∣L2损失L2Lossω(r)=1→ρ(r)=r22\omega(r)=1\to\rho(r)=\frac{r^2}2ω(r)=1→ρ(r)=2r2​柯西核CauchyLossω(r)=11+(rk)2→ρ(r)=k22log⁡(1+(rk)2)\omega(r)=\frac1{1+(\fracrk)^2}\to\rho(r)=\frac{k^2}{2}\log(1+(\fracrk)^2)ω(r)=1+(kr​)21​→ρ(r)=2k2​log(1+(kr​)2)GM核GMLossω(r)=k(k+r2)2→ρ(r)=r2/2k+r2\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核HuberLossTukey核TukeyLossHuber核以及Tukey核相对复杂，表示如下Huber核ω(r)={1∣r∣⩽kk∣r∣otherwise⁡→ρ(r)={r22∣r∣