点云算法对比ICP、NDT，LM，Ceres-Solver，KDTree，SVM和ComplexYOLO、CenterPoint、PNP

原创

已于 2024-01-25 14:44:01 修改 · 9.9k 阅读

153 ·

CC 4.0 BY-SA版权

文章标签：

#docker #pytorch #ubuntu

于 2021-01-29 11:38:13 首次发布

本文介绍了ICP算法的迭代配准原理、优化步骤，以及NDT配准的稳定性和概率优化方法。同时涵盖了KD树在点云匹配中的应用和LM算法在非线性最小二乘中的角色。还讨论了Ceres-Solver的优化求解器选择。

1：ICP

ICP（Iterative Closest Point），即最近点迭代算法，是最为经典的数据配准算法。其特征在于，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阀值，则迭代进行上述运算直到满足给定的误差要求.

ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。

ICP算法是基于EM(Expectation-maximization
algorithm)思想的方法，采用交替迭代法优化得到最优值。即ICP分为两步迭代优化，优化点云匹配及优化运动估计。
点云匹配是将两帧点云数据在同一个坐标系下，一帧数据中的点找到另一帧数据最近的点，就作为一对匹配点。
运动估计就是根据得到得两帧点云得匹配情况，构建最小二乘方程，求解。假设已知两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是求最小二乘方程的解，即以下方程的解R与t。
求解得到运动估计，ICP采用SVD分解求的运动估计得最优值。
除了使用逐步迭代法(最速/牛顿/LM等)求解，ICP算法采用SVD分解的方法，求得运动估计最优解。在时间占用上比使用四元数计算要大，但是比迭代法小很多，但是在精度上提高了很多。

在这里插入图片描述

https://blog.youkuaiyun.com/weixin_35695879/article/details/103125575

在这里插入图片描述

基本思想：
方法二：
针对点云匹配，
也可以找到对应的点集
利用lm算法，非线性优化6个参数，损失最小即可。。

||||||||||||||||||||||||||||||||||
方法三：
ICP简化算法，已知配对的坐标点：

https://blog.youkuaiyun.com/cangqiongxiaoye/article/details/123759326

from numpy import *
from math import sqrt
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt


def rigid_transform_3D(A, B):
    assert len(A) == len(B)
    N = A.shape[0];
    mu_A = mean(A, axis=0)
    mu_B = mean(B, axis=0)

    AA = A - tile(mu_A, (N, 1))
    BB = B - tile(mu_B, (N, 1))
    H = transpose(AA) * BB

    U, S, Vt = linalg.svd(H)
    R = Vt.T * U.T

    if linalg.det(R) < 0:
        print ("Reflection detected")
        Vt[2, :] *= -1
        R = Vt.T * U.T

    t = -R * mu_A.T + mu_B.T

    return R, t

if __name__=='__main__':
    
    # get data: R T
    R = mat(random.rand(3,3))
    t = mat(random.rand(3,1))
    print ('R',R)
    print ('t',t)
  

    U,S,Vt = linalg.svd(R)
    R = U*Vt
    print ('R= U*Vt',R)
    if linalg.det(R) < 0:
        Vt[2,:]*=-1
        R = U*Vt
    
    # get data: A B
    n = 10

    A = mat(random.rand(n,3))
    B = R*A.T + tile(t,(1,n))
    B = B.T
    print ("raw points A")
    print (A)
    print ("")

    print ("raw points B")
    print (B)
    # start : get R && T
    ret_R, ret_t = rigid_transform_3D(A,B)

    print ("get ret_R")
    print (ret_R)
    print ("")

    print ("get ret_t")
    print (ret_t)

    # ret_R ret_t 
    A2 = (ret_R*A.T)+ tile(ret_t,(1,n))
    A2 =A2.T

    err = A2-B

    err = multiply(err,err)
    err = sum(err)
    rmse = sqrt(err/n)

    print ("points A2")
    print (A2)
    print (