IMLS -ICP
Kong Liangqian Lv6
 2021-05-23 18:38:29

推荐书

多视图几何（multiple view geometer），状态估计(state estimate for robotic)

内容非常全，对初学者不是很友好。

视觉里程计：给出两张图来判断相机是如何运动的

后端：让总体的轨迹和地图更精确

ICP

当已知两个点集的对应的点对的时候，可以直接使用ICP方法直接一步到位！计算两个点集的R和t，很快

当位置对应点的时候，需要进行迭代金酸

IMLS-ICP（效果不错）

  1. 点云中隐藏这真实的曲面，最好的做法就是把参考帧点云中把曲面重建出来
  2. 选择具有代表性的激光点来进行匹配，若两个主法向量是差不多的话，就是差不多的面

激光点有一个可观性的问题，

代表点的选取

曲率小比较好

重建曲面

定义了一个函数，由一个点云的集合来表示一个曲面的函数

通过给定一个距离公式$I^p$，隐藏了一个曲面

IMLS -ICP匹配方法代码思想：

MATCH过程：结果需要返回一个当前帧和上一帧的一个增量

目标（上一帧）是targetCloud，本帧是sourceTarget

  1. 计算上一帧target每一个点的法向量。

    • 寻找每一个点附近的20个点作为平面
    • 计算SVD，特征值最小的特征向量就是法向量
    • 检查数据合法性

  2. 把当前的位姿，转换到上一帧的坐标系中。然后和上一帧转换到原点的数据（target）进行投影匹配，得到和本帧一一匹配的点

    • 循环每一个本帧的点$x_i$，利用knn找到上一帧中与$x_i$最近的点$\hat{x}_i$, 以及$\hat{x}_i$的法向量

    • 判断此$\hat{x}_i$是否有法向量，若没有则取消$x_i$的匹配

    • 判断$\hat{x}_i$和$x_i$的距离，若太远则取消$x_i$的匹配

    • 计算点到上一帧中隐藏曲面的距离

      • 建立上一帧数据的KD Tree

      • 寻找$x_i$最近的在上一帧中的最近的20个点，判断这些是否为inf, 是否有法向量，没有的话都不要，最后将点及他们的法向量存好

      • 如果筛完后的点小于3个，则认为没有匹配点，return False

      • 利用公式计算此点$x_i$到曲面的距离

        这里，

        $P_k$：前n帧激光数据组成的子图

        $\bold{n_i}$：点云集$P_k$中的点$p_i$的法向量

        $I^{P_k}(x)$：表示点$x$到点云集合$P_k$隐藏曲面的距离

  • 判断距离是否为nan，inf等，如果是，取消此点的匹配

  • 计算$x_i$（这个$x_i$是已经先转换到了上一帧坐标系中了）在曲面上的投影

    此$y_i$就是曲面上的投影点，并同时记录$\hat{x}_i$的法向量

  1. 若匹配后的点云数量太少，判断匹配失败
  2. 计算两帧之间的位移，从当前的source到target，返回一个位姿增量matrix3d.可以参考point-to-line ICP论文
  3. 更新result位姿，判断位姿增量是否过小，过小可以提前结束，否则继续迭代。从2开始
  4. 返回总的位姿变化
 Comments