点云配准 -- 2D NDT 算法

1 背景

上一篇文章中介绍了什么是点云配准和经典的 ICP 算法。

点云配准(Point Cloud Registration):输入两幅点云 $P_s$ (source) 和 $P_t$ (target) ,输出一个变换 $T$ 使得 $T(P_s)$ 和 $P_t$ 的重合程度尽可能高。

点云配准的一个常见应用就是用来解决无人车的定位(localization)问题。

什么是无人车的定位呢?就是确定无人车在世界中的什么位置,具体来说,是确定无人车在我们预先构建的地图中的哪个位置。

定位的精度直接影响感知模块和规划控制模块的效果,一般来说对无人车的定位精度要求都在 厘米 级别(平均误差 10 厘米以内)。

如何实现无人车的定位?这取决于你使用什么样的传感器配置。

传统的单点 GPS 定位精度在米级别(3到5米),而我国城市主干道的单一车道宽一般是 3.75 米,因此单独靠 GPS 无法实现可靠的定位系统。

使用差分GPS(differential GPS-DGPS,DGPS)可以有效提高 GPS 的定位精度,常见的 RTK(Real time kinematic) 载波相位差分技术可以达到厘米级精度,基本满足自动驾驶定位的需求。但 RTK 需要架设基站,且较容易受卫星状况、天气状况、数据传输状况的影响。

使用 GPS 属于全局定位的方法,还有一类相对定位的方法,比如使用 IMU 和里程计(Odometry),根据上一时刻的位置和方位推断现在的位置和方位,也叫航迹推算,优点是输出频率高、短时间内精度高,缺点是定位误差会随着时间累积。

其实仔细思考一下人开车的过程,我们的大脑是没有 GPS 和 IMU 这种类似的东西的,我们靠的是我们的眼睛,先对环境进行感知、观察,然后同大脑里已经见过的位置场景进行“匹配”,从而实现了定位。这是一种基于环境特征匹配的定位。

无人车的“眼睛”是什么呢?虽然基于相机的视觉方案潜力巨大,但目前业界使用最普遍的还是 3D 激光雷达(Lidar)。我们可以使用 Lidar 进行基于环境特征的定位,用我们每一帧得到的点云和预先制作的地图进行匹配,从而得到实时的车的位置和姿态,这就可以利用点云配准。

我们可以使用 ICP 算法完成点云配准,但 ICP 算法对位姿初值较敏感,且最近邻点搜索较耗时。

NDT(Normal Distribution Transform,正态分布变换) 算法是一种基于概率分布的点云配准算法,其比 ICP 算法耗时更加稳定,实际定位测试效果也更加鲁棒。本文先只介绍 2D NDT 算法,主要是为了给出 NDT 算法的基本思想和原理,3D 的情况留到下一篇。


2 正态分布变换(Normal Distributions Transform)

The Normal Distributions Transform: A New Approach to Laser Scan Matching 这篇论文提出了用于匹配两幅 2D 激光扫描点云的 NDT 算法。

More

<命に嫌われている(被生命所厌恶)> by AZKi

听了 AZKi 的这首《命に嫌われている(被生命所厌恶)》,实在是太喜欢了,这几天一直单曲循环。

我去 B 站搜了原版以及很多其他翻唱的版本,不知道是不是先入为主导致的,都没有 AZKi 的演唱带给我的这种浑身颤抖的感觉,一种由内而外的共鸣。上一次有类似感受的歌是 まじ娘 翻唱的《心做し(无心)》。AZKi 这种独特的哭腔在歌曲的关键部分总能准确地击中我心里最敏感的那根弦,让人忍不住眼眶湿润。

标题里的“【传达给大家】”究竟是在传达什么呢?我觉得就是“活下去”。

“被生命所厌恶”是一句再正确不过的话了,人一出生,从某种意义上,就是为了走向死亡。但这首歌希望你活下去。

贫穷也好,富贵也好,请活下去。

丑陋也好,美丽也好,请活下去。

悲伤也好,幸福也好,请活下去。

不是什么“活着才有希望”、“活着才有未来”,不是什么为了“我们爱的人”或者“爱我们的人”而活着,而是单单强调“活下去”。

希望听这首歌的你活下去,仅此而已。

被生命厌恶著。
最后总有一天会死亡。
不管是你还是我终有一日都会有如枯萎的叶腐朽而去。
尽管如此我们还是奋力地活着
奋力地拥抱着生命活下去
扼杀着 挣扎著 欢笑着 背负着
活下去、活下去、活下去、活下去、活下去啊。

三维点云配准 -- ICP 算法

1 问题描述

点云配准(Point Cloud Registration)指的是输入两幅点云 $P_s$ (source) 和 $P_t$ (target) ,输出一个变换 $T$ 使得 $T(P_s)$ 和 $P_t$ 的重合程度尽可能高。变换 $T$ 可以是刚性的(rigid),也可以不是,本文下面只考虑刚性变换,即变换只包括旋转、平移。

点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主要是为精配准提供较好的变换初值;精配准则是给定一个初始变换,进一步优化得到更精确的变换。

目前应用最广泛的点云精配准算法是迭代最近点算法(Iterative Closest Point, ICP)及各种变种 ICP 算法。

2 算法描述

对于 $T$ 是刚性变换的情形,点云配准问题可以描述为:

\begin{equation}
R^{\ast}, t^{\ast} = \mathop{\arg\min}_{R, t} \frac{1}{|P_s|} \sum_{i=1}^{|P_s|} || p_t^i - (R \cdot p_s^i + t) ||^2
\end{equation}

这里 $p_s$ 和 $p_t$ 是源点云和目标点云中的一一对应点。

ICP 算法的直观想法如下:

  • 如果我们知道两幅点云上点的对应关系,那么我们可以用 Least Squares 来求解 R, t 参数;
  • 怎么知道点的对应关系呢?如果我们已经知道了一个大概靠谱的 R, t 参数,那么我们可以通过贪心的方式找两幅点云上点的对应关系(直接找距离最近的点作为对应点)。

ICP 算法实际上就是交替进行上述两个步骤,迭代进行计算,直到收敛。

ICP 一般算法流程为:

  1. 点云预处理
    • 滤波、清理数据等
  2. 匹配
    • 应用上一步求解出的变换,找最近点
  3. 加权
    • 调整一些对应点对的权重
  4. 剔除不合理的对应点对
  5. 计算 loss
  6. 最小化 loss,求解当前最优变换
  7. 回到步骤 2. 进行迭代,直到收敛

整体上来看,ICP 把点云配准问题拆分成了两个子问题:

  • 找最近点
  • 找最优变换

More

Seam Carving -- 基于内容的图像缩放算法

1 背景

我们经常会有缩放图像的需求,然而直接缩放的问题是,如果宽高缩放比例不一致,会导致图像内容发生形变“失真”。

Seam Carving 算法是下面论文中提出的一种图像缩放算法,它的好处是可以尽可能保持图像中“重要区域”的比例,避免由于直接缩放造成的“失真”。

@inproceedings{avidan2007seam,
title={Seam carving for content-aware image resizing},
author={Avidan, Shai and Shamir, Ariel},
booktitle={ACM Transactions on graphics (TOG)},
volume={26},
number={3},
pages={10},
year={2007},
organization={ACM}
}

compare_resizing

上图是几种缩放方法的对比,左侧是 seam carving 结果,中间是直接缩放,右侧是 crop ,可以发现 seam carving 方法很好地保持了原图中大部分“信息”,且看起来画面中的主要物体也没有出现比例“失真”的情况(比如图片底部的岩石,直接缩放比例变化很大,crop 的话直接就没了)。

2 算法原理

基本思想

算法的基本思想非常直观,先考虑下沿着宽的方向进行缩放,缩放实质是删去了若干条纵向的像素“路径”(或者 seam,缝隙),直接缩放删去的路径都是竖直的长条,相当于沿着图像竖直方向做了均匀的降采样。那么我们为什么一定要删去竖直的“路径”呢,如果能保持删去路径后,剩余的图像部分还是“平滑”的,或者说删去的路径是最不重要的,那么不就实现了基于图像内容的缩放了吗?

于是该论文作者提出了可以删去“能量”最少的 seam 来实现图像缩小。

“能量” 如何定义,最容易想到的就是梯度信息:

用像素在水平和竖直方向上的一阶梯度值的之和来表示该像素点的能量,那么一条缝隙的能量就是该缝隙上所有像素点能量之和。

我们需要做的就是每次找到像素能量最小的一条缝隙,然后删去它。

seam with min energy

算法步骤

有了基本思想的铺垫,算法步骤也非常直观了,假设我们要删去 K 条 seam:

  1. 计算每个像素点的能量;
  2. 找到竖直/水平方向上的能量最小的路径,称为 seam;
  3. 移除 seam,得到新图像;
  4. 重复步骤 1 至步骤 3 K 次,得到缩放后的图像,

3 实现细节

More

韩国电影《寄生虫》观后感

韩国电影《寄生虫》,最近挺火,有人说尺度很大,中国肯定引进不了。今天花时间看完了,个人评价是一部好电影,但是给我的感觉又比较奇怪。

先说看完后最违和的一个感受:像影片中这种水平的富人,判断人的能力真有可能会这么差吗?家里突然换了一堆工作人员,就没有想到是诈骗的可能性?

其次,这部电影究竟想表达个啥?看起来像是在讽刺富人、穷人之间的阶级对立,不过这里的富人、穷人形象都不那么正面。我讨厌富人嫌弃穷人身上“味道”的态度,更瞧不起穷人一家坑蒙拐骗式的“寄生”。阶级问题自古以来就存在,想逾越阶级谈何容易,有时候靠几代人的努力都不够,还要靠机会、运气,不过本片中的男主一家,在我看来显然是选错了努力方向。

影片的结尾,看到男主幻想着自己赚了大钱买下豪宅,我还以为这是个励志片=. =

总的来说,有种强行制造冲突的感觉,看得挺爽,但看完回味起来又感觉各种不合理。

用 Python 实现“文档扫描”

“扫描全能王” 是我手机里一直都有的 App,我非常喜欢把一些纸质内容电子化,比如书中看到的喜欢的段落、日常生活中的票据、产品说明书等等。

如下图所示,只需要拍一张照片,App 就会自动识别文档的边缘,并将文档转换为“正视图”。

camscanner_demo.png

实际上这个 App 用到的算法非常简单,核心就是“边缘检测 + 透视变换”,下面我们就用 Python 和 OpenCV 实现一个简单的 Demo。我用 Tkinter 做了个简单的 GUI ,可以支持手动选择文档的角点,代码地址在:

https://github.com/insaneyilin/document_scanner

More

一个简单的 C++ 对象池实现

对象池

前一段时间的工作中,有一项任务是检查线上代码中的动态内存分配与释放,能预先分配的统一预先分配,减少线上运行时动态分配、释放造成的性能开销。对于一些大对象来说,频繁的分配、释放会影响性能,我们可以使用对象池技术(Object Pool)来进行优化。

对象池,说白了就是一个池子,预先分配了一堆对象,原先的 new 改成从池子里取对象,delete 改成把对象放回池子。也就是池子中的对象是可以重复利用的,避免了频繁的分配、释放。

按照上面的简单思路,一个对象池的基本功能应该包括:

  • 构造函数中申请一批对象
  • 析构函数中释放对象
  • 提供获取和回收对象的接口 Get()Recycle()
  • 池子空了的时候支持扩容

实现起来也不难,用一个队列维护所有对象,用的时候从队列中取,回收了 push 回队列即可。不过显式的 回收 操作用起来不是很方便,忘记回收会造成资源浪费。可以考虑利用 C++ 智能指针(smart pointer)自定义删除器(deleter)来实现自动回收。

自定义智能指针的删除器

先看一个例子(这是一个错误使用智能指针的例子,只是为了展示 deleter 做了什么):

int main(int argc, char **argv) {
  Object *obj1 = new Object;

  std::shared_ptr<Object> p1(obj1);
  std::cout << p1.use_count() << std::endl;

  std::shared_ptr<Object> p2(obj1);
  std::cout << p2.use_count() << std::endl;

  return 0;
}

运行结果:

1
1
malloc: *** error for object 0x7f845f400340: pointer being freed was not allocated
*** set a breakpoint in malloc_error_break to debug
Abort trap: 6

报错了,提示说释放了一个未分配的指针。这是因为 std::shared_ptr 默认的 deleter 就是 delete,当两个 shared_ptr 实例出作用域时,执行了两次 delete obj1,所以出错。

我们不用默认的 deleter ,试试自己定义一个 deleter 来避免第二次 delete (再强调下,这个例子只是为了展示 deleter ,实际工作中没人会这么写的):

More

高考10年

这两天全国各地陆续发布了高考分数线,我也刚刚过完了这辈子第 28 个年头,转眼间,距离我参加高考已经 10 年了。

我对自己高考总分印象很深,因为不多不少,正好和我中考总分一样。

More

夏虫不可语冰,井蛙不可语海

今天,在百度 AI 开发者大会上,Robin 正式宣布百度和金龙合作的 L4 自动驾驶小巴量产了,一起奋斗了大半年的同事们都很开心,金龙当天的股票还涨停了,大家调侃道怎么没想到早点买金龙的股票。

晚上回到家,打开知乎,刷到这样一个答案,真是哭笑不得😂:

如何看待百度称首款 L4 级别自动驾驶巴士量产下线?

论吹牛,百度说第二,没人敢说第一

这车就是一个简单的循迹行驶,架好差分GPS基站,先用人工开一遍,录一下轨迹,然后一直按着轨迹绕圈圈跑,连感知设备都用不上。

利益相关,匿了

这位答主贴的图的确是厦门金龙研发现场的,看得出来应该是一位“内部人士”。

看完这个回答和评论区内容,先是愤怒,然后是想笑,最后是无奈。

愤怒是因为我所在的组就是做感知的,没有任何证据直接说“连感知设备都用不上”相当于完全不认可我们的工作,更何况这还是一位内部人士(不太可能是 baidu 这边的人,如果是金龙的员工,那就更让人心寒了)。

想笑是由于答主在评论区里还俨然一副专家姿态,“自己百度差分GPS”(敢情还是得用百度😐),“装了感知设备,未必代表就用了”,“你自己想想这照片我怎么拍的,再猜猜我是做什么的”。当时我就想到一句话:

夏虫不可语冰,井蛙不可语海

无奈是评论里竟然还有很多人认同答主,这才是最可悲的。像这位朋友评论说“装几个简单的雷达或者红外绕车子一周,然后if else就差不多了。。。”、“实现广义的L4不难”都让人啼笑皆非。

一件本来很让人高兴的事情,却变了味道。一方面是 baidu 近年来的公众形象问题,另一方面也反映出大众对自动驾驶的认知还远远不够。

像上面这位“内部人士”显然对于自动驾驶是没有多少了解的,甚至可以说“无知”也不为过。但可怕的不是无知,而是面对自己不熟悉的内容的傲慢与偏见。

关于“夏虫不可语冰,井蛙不可语海”,还有一个老笑话:

联合国给几个世界各地的小朋友们出题:“关于其他国家粮食短缺问题,请大家说说自己的看法”。非洲小朋友不知道什么是粮食,美国小朋友不知道什么是其他国家,欧洲小朋友不知道什么是短缺,拉美小朋友不知道什么是请,中国小朋友不知道什么是自己的看法。

很讽刺不过也很现实。人哪有那么容易理解自己没见过的事物呢?从某种意义上来说,我们每个人都是“夏虫”。但是面对无尽的未知,我们不应该抱着傲慢的态度,而应该以开放、谦卑、学习的心态来对待。

以后遇到无法沟通的人或者无脑喷子,想想这句话,也就释怀了。