[人工智能] 基于ICP算法计算点集之间的变换矩阵（旋转、平移）

开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> 人工智能 -> 基于ICP算法计算点集之间的变换矩阵（旋转、平移） -> 正文阅读

[人工智能]基于ICP算法计算点集之间的变换矩阵（旋转、平移）

前言

本文主要是计算两个激光雷达之间的变换矩阵，即计算两组点云之间的变换矩阵。其中处理的点云数据主要是由x,y,z,intensity组成的，代表空间位置x,ry,z 和每个点云对应的反射强度intensity；

这里计算点集之间的变换矩阵，用到每个点云的x,y,z信息，可表示为n*3的数组；两组激光雷达点云，可以表示为2和n*3的数组。

首先使用ICP点云匹配算法，计算两组点云之n*3的数组间对应的点；然后基于SVD算法求出两个对应点集合的旋转矩阵R和转移矩阵t。

一、基于ICP匹配对应点

基于点的匹配算法中常用最多的是 ICP（Iterative Closest Point）算法；其实质是基于两个集合中最近点对的搜索，并且估计将它们对齐的“刚性变换”。然后，将刚性变换应用于一个集合的点，并迭代该过程直到收敛。

优点：原理简单、易于理解和实现，同时配准精度较高。

弊端：ICP 算法在迭代初期就必须确定初始位置，如果初始位置估计不准确，与实际位置相差较大，则可能会产生局部最优解。并且，ICP 算法在每次迭代时都要对点集内所有点进行对应点搜索。

应用：ICP 算法不需要事先对目标进行特征提取和分割，可以直接对点云进行处理。

使用ICP进行点云配准，基本思路是：对于两片待配准的点云，首先寻找两个点云中的对应点间的对应关系，计算出两片点云间的坐标变换参数，然后通过三维刚体变换，将两片待配准点云的坐标系调整到一致，从而完成两片点云在三维空间中的配准。

ICP 配准算法流程图，如下所示：

?在迭代初期，数据点可能和其对应点距离很远，但随着迭代次数的增加它们距离越来越近。ICP 算法可以单调收敛到局部最小值，可能导致陷入局部极值，所以初值的选取尤为重要。

二、计算变换矩阵

假设激光雷达1的点云数据，表示为A数组，结构是n*3；激光雷达2的点云数据，表示为B数组，结构也是n*3；A和B数组已经通过ICP算法，进行一一匹配了。

注意：这里的点最少需要3个，即n>=3；点多一些，计算得更精准一些。

2.1 思路

对于无噪声数据，两个点集之间的旋转矩阵R 和平移向量t，可以表示为：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? $\large B=R*A+t$ ? （R 是一个 3×3 旋转矩阵，t 是 1×3平移向量。）

对于有噪声数据，它将最小化最小二乘误差，两个点集之间的旋转矩阵R 和平移向量t，可以表示为：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? $\large err=\sum_{i=1}^{N}\left \| RA^{i} +t -B^{i} \right \|^{2}$

无噪声数据、或有噪声数据，计算R和t，通常都需要三步：

计算点集合的中心点（质心）
将点集合移动到原点，然后找到最优旋转矩阵R
计算转移矩阵t

2.2?计算中心点

这里是点的平均值，计算如下：

2.3?将点集合移动到原点

需要将点集重新中心化，生成新点集合 $\large A^{'},B^{'}$ ，公式如下：

?2.4?计算协方差矩阵H

计算点集之间的协方差矩阵H，公式如下：

2.5 计算旋转矩阵R

通过SVD方法获得矩阵的U、S和V,可以计算点集之间的旋转矩阵R，公式如下：

?2.6 计算平移向量t

通过R可以获得转移矩阵t，公式如下：

?关于SVD补充一下：

?奇异值分解（SVD）维基百科：https://en.wikipedia.org/wiki/Singular_value_decomposition

?SVD的数学推导文档：https://igl.ethz.ch/projects/ARAP/svd_rot.pdf

三、实现代码案例

大佬们有相关的代码开源了，Python实现的：GitHub - ClayFlannigan/icp: iterative closest point

这里考虑了有噪声的数据；?

import numpy as np
from sklearn.neighbors import NearestNeighbors


def best_fit_transform(A, B):
    '''
    计算在 m 个空间维度上映射对应点 A 到 B 的最小二乘最佳拟合变换
     输入：
       A：对应点的Nxm numpy数组
       B：对应点的Nxm numpy数组
     返回：
       T：(m+1)x(m+1) 将 A 映射到 B 的齐次变换矩阵
       R：mxm 旋转矩阵
       t: mx1 平移向量
    '''

    assert A.shape == B.shape

    # 获取维数
    m = A.shape[1]

    # 将点转换为它们的质心（中心点）
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)
    AA = A - centroid_A
    BB = B - centroid_B

    # rotation matrix
    H = np.dot(AA.T, BB)
    U, S, Vt = np.linalg.svd(H)
    R = np.dot(Vt.T, U.T)

    # special reflection case
    if np.linalg.det(R) < 0:
       Vt[m-1,:] *= -1
       R = np.dot(Vt.T, U.T)

    # translation
    t = centroid_B.T - np.dot(R,centroid_A.T)

    # homogeneous transformation
    T = np.identity(m+1)
    T[:m, :m] = R
    T[:m, m] = t

    return T, R, t


def nearest_neighbor(src, dst):
    '''
    为 src 中的每个点在 dst 中找到最近的（欧几里得）邻居
     输入：
         src: Nxm 点数组
         dst: Nxm 点数组
     输出：
         距离：最近邻居的欧几里德距离
         索引：最近邻居的 dst 索引 
    '''

    assert src.shape == dst.shape

    neigh = NearestNeighbors(n_neighbors=1)
    neigh.fit(dst)
    distances, indices = neigh.kneighbors(src, return_distance=True)
    return distances.ravel(), indices.ravel()


def icp(A, B, init_pose=None, max_iterations=20, tolerance=0.001):
    '''
    迭代最近点方法：找到将点 A 映射到点 B 的最佳拟合变换
     输入：
         A: Nxm numpy 源 mD 点数组
         B：目标 mD 点的 Nxm numpy 数组
         init_pose: (m+1)x(m+1) 齐次变换
         max_iterations: max_iterations 后退出算法
         tolerance：容差收敛标准
     输出：
         T：将 A 映射到 B 的最终齐次变换
         距离：最近邻居的欧几里得距离（误差）
         i：要收敛的迭代次数 
    '''

    assert A.shape == B.shape

    # 获取维数
    m = A.shape[1]

    # 使点同种类的(points homogeneous)，复制它们以保持原件
    src = np.ones((m+1,A.shape[0]))
    dst = np.ones((m+1,B.shape[0]))
    src[:m,:] = np.copy(A.T)
    dst[:m,:] = np.copy(B.T)

    # 应用初始姿态估计
    if init_pose is not None:
        src = np.dot(init_pose, src)

    prev_error = 0

    for i in range(max_iterations):
        # 找到当前源点和目标点之间的最近邻居
        distances, indices = nearest_neighbor(src[:m,:].T, dst[:m,:].T)

        # 计算当前源点和最近目标点之间的变换
        T,_,_ = best_fit_transform(src[:m,:].T, dst[:m,indices].T)

        # 更新当前源  矩阵相乘 T*src
        src = np.dot(T, src)

        # 检查错误
        mean_error = np.mean(distances)
        if np.abs(prev_error - mean_error) < tolerance:
            break
        prev_error = mean_error

    # 计算最终变换
    T,_,_ = best_fit_transform(A, src[:m,:].T)

    return T, distances, i

效果：