专栏首页计算机视觉life从零开始一起学习SALM-ICP原理及应用

从零开始一起学习SALM-ICP原理及应用

小白:师兄,最近忙什么呢,都见不到你人影,我们的课也好久没更新了呢

师兄:抱歉,抱歉,最近忙于俗事。我后面一起补上,学习劲头得向你们年轻人学习啊!话说,你最近在研究什么呢?

小白:最近在看ICP,十四讲上简单提了下,还有点懵懵哒

师兄:好,那今天就说说ICP算法吧。搬个小板凳做好哈:

ICP全称Iterative Closest Point,翻译过来就是迭代最近点。I

CP在点云配准领域应用的非常广泛,因此基于深度相机、激光雷达的算法使用ICP的频率比较高。

小白:那我是学视觉SLAM的,是不是不用关注了啊(内心OS:毕竟SLAM要学的太多了)

师兄:哈哈,这个懒不能偷的。ICP现在在视觉SLAM中用的也非常多了。我总结了一下下ICP的应用场景:

ICP到底有什么用呢?

假设我们在三维空间中有两组点集,我们分别称之为P1, P2好了,P1, P2中都有几千个点,那么可以分为两种情况:

1、我们完全不知道P1, P2中每个点是如何对应的。这样的情况很常见,比如我用只有depth的深度相机或者激光雷达在两个不同的位置对着一只兔儿分别拍摄一张深度图并转化为点云图,我特喵的也不知道哪个点和哪个点对应,但是我想把这两个点云“融合”(专业词汇叫做registration,有些地方翻译为注册,有点让人不知所云,我建议翻译为配准)在一起,变成一个更完整的兔儿。

2、我们已经知道P1, P2中哪个点对应的哪个点。比如我们使用RGB-D相机分别在两个不同位置拍摄一张 RGB彩色图 + 深度图,而且彩色图和深度图是对齐好的。因为有彩色图我们就可以做特征点匹配了,因为每个特征点都对应一个深度图上的深度值,所以我们能够得到两组对应好的三维点。

小白:原来如此,看起来ICP还挺强大的,对应不对应都能搞定啊,ICP为何这么牛叉?

ICP为何这么牛叉?

师兄:来看看ICP的过程就知道了。为了方便,我们用一个二维的例子来说明吧,初始是两个不同角度下的笑脸(深红色和绿色),下面是红色笑脸如何通过ICP过程和绿色笑脸重合的:

小白:好神奇啊

师兄:这个只是一个示例,通常情况下我们在SLAM中不会遇到这种情况,都是三维空间的点。我再以点云配准为例进行说明。假设现在有两幅待配准的点云(比如上面的小兔子),ICP算法是这样配准两幅点云的:

ICP算法流程

  1. 首先对于一幅点云中的每个点,在另一幅点云中计算匹配点(最近点)
  2. 极小化匹配点间的匹配误差,计算位姿
  3. 然后将计算的位姿作用于点云
  4. 再重新计算匹配点
  5. 如此迭代,直到迭代次数达到阈值,或者极小化的能量函数变化量小于设定阈值

下面是用三维点云进行ICP的一个效果

小白:第一次计算的误差估计很大,后面会慢慢变小,直到收敛?

师兄:嗯,这是一个由粗放到精细的过程,在视觉算法中由粗到精的方法很常用。。

小白:比如图像金字塔!哈哈

师兄:嗯,熟悉的配方,熟悉的味道(/笑脸)

ICP的精髓其实就是迭代,一次次的修正错误,最后获得一个还不错的结果。

不过话是这样说,但是一般情况下数据都有不少噪音,如果噪音较大,第一种情况不一定能完全配准正确。

小白:听起来合情合理,也不麻烦,书上为啥推导那么复杂?

师兄:嗯,这就是感性到理性的过程。视觉SLAM十四讲书上从数学上推导了具体如何实现。我这里引用一下求解ICP的三个步骤:

小白:原理推导大概是能看明白了

师兄:嗯嗯,推导部分给你留了一个小小的课后作业了

小白:师兄想的周到,这样才能真正掌握呢(内心OS:真狠啊。。)

师兄:上面介绍是最简单的点和点匹配的ICP算法,实际应用中为了使得算法鲁棒,也就是在有不少噪声的情况下仍然能够得到正确的结果,很多研究者提出了不同的匹配思路:例如,极小化的误差项包括对应点的点到点的欧式距离,和对应点的点到平面距离,以及极小化对应点的颜色值误差等

小白:这么多方法,用哪个好呢?

师兄:如果你应用场景简单,数据干净基本没有噪声,最原始的点点匹配就行。如果有噪声,则最后考虑一下复杂点的方法。2003年的时候,pottman 和Hofer两位大牛的论文中证明了当两幅点云比较接近时,极小化对应点的点到平面距离比点到点距离更接近两个平面之间的真实距离,也就是说计算点到平面的距离更靠谱!

下面是个示意图

这就是ICP的原理啦!

下面是实践环节

练习

1、证明:《视觉SLAM十四讲》第174页公式7.55中的

各符号定义见书上,其中,tr表示矩阵的迹。

2、 给定一个轨迹1,数据格式:timestamp tx ty tz qx qy qz qw, 自定义一个任意的旋转矩阵和平移向量(可以尝试不同的值,甚至加一些噪声看看结果有什么变化),对轨迹1进行变换,得到一个新的轨迹2, 使用ICP算法(提示:取平移作为三维空间点)估计轨迹1,2之间的位姿,然后将该位姿作用在轨迹2

验证:ICP算法估计的旋转矩阵和平移向量是否准确;轨迹1,2是否重合。

如下是我加了一个旋转平移量后的两个轨迹,经过ICP计算好位姿后再反作用在变换后的轨迹,最终两个轨迹是重合滴!

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • C编程练习001

    正念君
  • 【C语言笔记】枚举

    这个知识点很重要,到处能用得到。其可以与typedef来进行类比进行理解(可查看【C语言笔记】define与typedef的区别?)。除此之外,其还可以与枚举e...

    正念君
  • Nginx 整合 FastDFS 实现文件服务器

    本篇衔接《FastDFS 环境搭建》内容进行讲解,上篇文章我们实现了图片上传的功能,但是无法通过 http 进行访问,本篇将解决该问题。

    IT技术小咖
  • C编程练习003

    正念君
  • 【C语言笔记】char *str与char str[]的区别

    C语言中没有特定的字符串类型,常用以下两种方式定义字符串:一种是字符数组,另一种是指向字符串的指针。如下:

    正念君
  • 【C语言笔记】volatile关键字

    volatile的作用是作为指令关键字,确保本条指令不会因编译器的优化而省略,且要求每次直接读值。

    正念君
  • 天天算法 LeetCode-101-对称二叉树

    https://leetcode-cn.com/problems/symmetric-tree/

    灵魂画师牧码
  • 【C语言笔记】变参函数

    提到变参函数,我们的感觉是不是既熟悉又陌生?感觉熟悉是因为我们平时都在使用着,如我们常使用的printf()函数与scanf()函数就是典型的变参函数。因为pr...

    正念君
  • Java 性能优化二三事之代码编写不规范,维护开发两行泪

    IT技术小咖
  • 【C语言笔记】extern关键字

    extern “C”的主要作用就是为了能够正确实现C++代码调用其他C语言代码。加上extern “C”后,会指示编译器这部分代码按C语言的进行编译,而不是C+...

    正念君

扫码关注云+社区

领取腾讯云代金券