Sensors
文章目录
自动驾驶中的传感器。
各种传感器
惯性测量单元（IMU, inertial measurement unit） 通常指由 3个加速度计， 3 个陀螺仪和算法处理单元组合，加速度计和陀螺仪安装在互相垂直的测量轴上。是测量物体三轴姿态角（或角速度）以及加速度的装置。一般而言，IMU 要安装在被测物体的重心上。我们把传统的IMU 和与车身、GPS等信息融合的算法组合在一起的系统称为广义的、针对自动驾驶的IMU。
加速度计，可以测量低俗的静态加速度。加速度计在自由落体时，其输出是0.其设计原理： 加速度计测量加速度是通过比力来测量，而不是通过加速度，加速度若是绕着重力加速度的轴旋转，则测量值不会改变，也就是说加速度计无法感知这种水平旋转。
陀螺仪，测量角速度。他测量的是角度的倒数，所以需要将角速度和时间积分才能得到角度。
陀螺仪于加速度计之间的关系
机体好似一条船，姿态就是航向（船头的方位），重力是灯塔，陀螺（角速度积分）是舵手，加速度计是瞭望手。舵手负责估计和把握航向，如果舵手很牛逼，那么方向很准确。否则，这个时候就需要一个瞭望手来观察误差。
瞭望手通过灯塔方向和船的当前航向，算出灯塔理论上的应该在的方位和实际的方位，这个时候就有了误差。舵手收到瞭望手的报告，可以相应的做出调整。
IMU 在自动驾驶中的作用：高精定位。有两层含义。
（1）得到自车和周围环境之间的相对位置，即相对定位
（2）得到自车的精准经纬度，即绝对定位，自动驾驶对于周边环境的理解需要高精地图，联合感知等计数的辅助。
这两个技术叠加在一块，能大大提高自动驾驶车辆的安全性。之所以使用经纬度来描述这些信息，因为世界上最通用的观测坐标系就是由经纬度定义的坐标系。
GPS可以为车辆提供精度为米级的绝对定位，查分GPS 或 RTK GPS可以为车辆提供精度为厘米级的决定丁文，然而并不是所有路段所有时间都可以得到良好的 GPS信号。所以 RTK GPS的输出一般要和 IMU ，汽车自身的传感器（轮速计，方向转角器）进行融合。
里程计（odometry）
里程计是衡量我们从初始位姿和终点位姿的一个标准，通俗讲，如果要实现机器人的定位和导航，就需要知道机器人行进了多少距离，是往哪个方向行进的。
视觉里程计（visual odometry）
机器人每个时刻的位置和姿态使用 $(x_t,y_t,z_t,r,p,y)$ ， 其中 $(x_t,y_t,z_t)$ 表示机器人在世界坐标系中的位置信息，$(r,p,y)$ 表示机器人的姿态，分别是 roll, pitch, yaw. 可以分成 单目视觉里程计和双目（立体）视觉里程计。
工作步骤
1.获得图像 2.对获得图像进行畸变处理 3.通过FAST算法对图像进行特征检测，通过KLT算法跟踪这些特征到图像中，如果跟踪特征有所丢失，特征数小于某个阈值，则重新进行特征检测 4.通过带RANSAC的5点算法来估计两幅图像的本质矩阵 5.通过计算的本质矩阵进行估计R,t 6.对尺度信息进行估计，最终确定旋转矩阵和平移向量
陀螺仪（gyroscope）
陀螺仪是一种传感器，是指在工程中具有固定点，绕对称轴做高速自转的对称刚体，其自转轴称为陀螺主轴。目前传统上机械陀螺仪正在被淘汰，有高精度需求的地方使用的是激光陀螺仪，在普及方面使用的是微机电陀螺仪，iPhone 手机中就是使用它来检测手机的姿态。
IMU
一般包含了三个单轴的加速计和三个单轴的陀螺仪。
IMU 的原理和黑暗中走小碎步相似。在黑暗中，由于自己对步长的估计和实际走的距离存在误差，走的步数越来越多时，自己估计的位置与实际的位置相差会越来越远。走第一步时，估计位置与实际位置还比较接近；但随着步数增多，估计位置与实际位置的差别越来越大。根据此方法推广到三维，就是惯性测量单元的原理。
摄像头
广泛用于物体识别和物体追踪场景，比如车道线检测、交通识别等。一般会使用多个摄像头；一般 50m 以内；成本低；可识别物体；依赖光线和天气。
激光雷达
用于障碍物识别、绘制地图、辅助定位，准确率非常高。200m 以内；容易受到恶劣天气影响，成本高昂。
毫米波雷达
阴雨天、雾霾天能够辅助感知获取物体的位置和速度；难以识别行人。（毫米波能够穿透雾、烟、灰尘）
毫米波雷达对高度的检测本来就不好，比如说障碍物的高度。
radar 一般用于通用型避障，经常应用在高速路上，一般不在车库使用；有效范围可以达到 100-200 米。
毫米波雷达是基于电磁波。毫米波的频段比较特殊，其频率高于无线电，低于可见光和红外线，频率大致范围是10GHz—200GHz。
超声波
近处高敏感，常用于安全冗余设别检测车辆的碰撞安全问题。基于机械波，依靠介质震动传播。
- 摄像头：具有丰富的纹理特征，感知范围可以有多种组合
- 激光雷达：感知范围远，精度高，不收光照影响。如果雨雪天气比较小，那么影响是有限的。
- 毫米波雷达：成本低，对感知的方向角敏感
- 超声波雷达：成本低，功耗低，适合近距离感知
自动驾驶相关传感器底层技术可以分为两类
- 一类为光学图像投射到图像传感器上，然后转为电信号就是摄像头。
- 传感器发射信号，然后接受反射回来的额信号。通过测量所经过的时间或位移，根据光飞行时间（光速）计算距离，这就是雷达。根据发射波长的不同来不同的命名。
自动驾驶中涉及到的感知器的具体分类
GPS全球定位系统-使用卫星信号以及地图实现米级的定位感知精度。
惯量导航系统-使用IMU和GPS数据计算车辆位置、方向和速度。
激光雷达（光探测和测距）-通过激光和反射测量距离和探测物体。
雷达（无线电探测和测距）-使用射频波计算远程目标的距离、速度和角度。
摄像头-多个摄影机创建环境的视觉表示。
红外传感器-利用红外光谱探测物体（如行人）。
超声波传感器-使用超声波测量短程距离。
量距传感器-车轮跳动传感器和转向角传感器。
当前基于各种冗余理论，以上感知都是融合使用的，再通过强大的计算能力做后盾运算出汽车的位置。
摄像头
2D 摄像头依靠强大的处理器、算法和神经网络处理这些图像。
立体视觉需要使用两个摄像头来实现，两个摄像头根据彼此的相对位置准确放置。将这两个来源的图像相关联，可以生成深度图。但立体视觉很受摄像头光线环境影响。
立体视觉并不会比 单个摄像头的事情少。
radar
使用无线电波检测一定距离内的物体，并确定其速度和倾向。依靠来自多个 RADAR 传感器的信息（通过车载电脑来解读）来识别车辆或危险物的距离、方向和相对速度。
radar 几乎不受恶劣天气的影响，能够在黑暗、潮湿或有雾的条件下可靠的工作。当前的 24 GHz 传感器分辨率有限，图像模糊不清，随着更准确的 77 GHz RADAR 传感器的推出，这一问题将迎刃而解。
毫米波雷达：精度较低，极端天气下表现较好。
毫米波雷达观察世界的方式于激光雷达有所不同。激光雷达的原理是光的直线传播，因此在测量时能直接获取障碍物在笛卡尔坐标系下 $x$ 轴，$y$ 轴和 $z$ 轴方向上的距离；毫米波雷达的原理是多普勒效应，它所测量的数据都是在极坐标系下的。
毫米波雷达能够测量障碍物在极坐标系下离雷达的距离ρ 方向角$\theta$以及距离的变化率（径向速度）ρ'
激光 lidar，又称 3D 激光扫描，和 radar 的工作原理相似，不同点是 lidar 发射的是快速激光信号， radar 发射的是无线电波。
lidar 的优点包括准确性和精度，可以为自动驾驶车辆提供较远距离内周围事物的 3D 图像。LiDAR 与摄像头相比极其准确，因为激光器不会受阴影、日光或其他汽车前灯光线的影响。缺点包括成本高、尺寸大，存在干扰和干涉，在雾、雪和雨天的穿透能力有限。另外，LiDAR 不提供摄像头通常所看到的信息，例如标牌上的文字或者交通信号灯的颜色。
超声波ultrasonic-超声波传感器可发出短的超声波脉冲，遇到障碍物会反射回来。随后会接收并处理回声信号。超声波传感器在雾、雨、雪等恶劣天气以及弱光条件下表现出色，且价格相对并不昂贵。缺点包括反应时间长、视野有限，比 LiDAR 的精度要低。另外，超声波传感器难以检测包括快速移动的小物体或多个物体。
远红外far-infrared (FIR)摄像头，又称热感摄像头，可以收集来自物体、人员和周围事物的热辐射。通过感应远超可见光的红外光谱。
camera 在物体分类和车道线检测中表现较好，但是在恶劣天气、弱光条件下表现不佳；Lidar 在物体检测和弱光条件下的性能较好，但在车道线检测中表现较差； rader 在恶劣天气和弱光添加下表现较好，但是在物体分类和车道线检测上表现不行。使用多种传感器数据，根据他们各自的工作特点，优势互补，提高对场景的数据标示性和数据鲁棒性。
IMU中的计算
坐标系
2D 坐标系一般默认： $x$ 轴朝右， $y$ 轴朝上。
3D 坐标系一般有两种习俗，左手坐标系和右手坐标系。手指和轴的对应关系：
right -$x$ 轴
up - $y$ 轴
front - $z$ 轴
下图左边是左手坐标系，右边是右手坐标系。
WGS-84 坐标系（World Geodetic System -1984 Coordinate System） 是一种国际上采用的以地球质心（总椭圆的几何中心）为原点的大地坐标系。其坐标系的Z轴指向BIH（国际时间服务机构）定义的协议地球极（CTP）方向，X轴指向BIH定义的零子午面和CTP赤道的交点，Y轴与Z轴、X轴垂直构成右手坐标系。
WGS -84 坐标系下经纬度定义：
**1.大地纬度：**过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。
**2.大地经度：**过用户点P的子午面与零子午线之间的夹角。经度值在-180°到+180°之间。向东为正，向西为负。
**3.大地高度：**过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。
|
|
GCJ-02坐标系
GCJ-02坐标系是由中国国家测绘局（G表示国家，C表示测绘，J表示局）制订的地理信息系统的坐标系统，是在WGS84经纬度的基础上执行加密算法而成。因GPS得到的经纬度直接在GCJ-02坐标系下会定位到错误的地点。
根据规定，国内出版的各种地图系统（包括电子地图），必须采用GCJ-02坐标系。高德地图、Google地图国内版等都是使用GCJ-02坐标系，而百度使用的是在GCJ-02基础上再一次加密的BD-09坐标系。
世界坐标系
NED 坐标： X轴向北，Y轴向东，Z轴向下
ENU 坐标：X轴向东，Y轴向北，Z轴向上
NWU 坐标：X轴向北，Y轴向西，Z轴向上（这个坐标系最符合人类想象）
自身坐标系
FRD坐标：X轴向前，Y轴向右，Z轴向下
FLU坐标：X轴向前，Y轴向左，Z轴向上
导航坐标系
导航坐标系又称为局部平面坐标系等，常用的导航坐标系有东北天和北东地两种。
东北天坐标系（ENU），也称为站心坐标系，主要用于表示以观察者为中心的其他物体的运动规律。其坐标定义为，Z轴与椭球法线重合，向上为正；Y与椭球短半轴重合（北向为正）；X轴与椭球长半轴重合（东向为正）。
北东地坐标系（NED），主要方便在平面下导航使用。其坐标定义为，z轴与椭球法线重合，向下为正；X与椭球短半轴重合（北向为正）；Y轴与椭球长半轴重合（东向为正）。
|
|
IMU (Inertial measurement unit，惯性测量单元) ：实时测量自身的姿态，200hz 或者更高。包含了 3个单轴的加速度计和 3个单轴的陀螺仪，加速计测量物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号。IMU 只提供相对定位信息，即自体从某时刻开始相对于某个初始位置的运动轨迹和姿态。将 IMU 的相对定位和 RTK GPS 的绝对定位进行融合后，就产生了两个无可替代的优点：
- IMU 可以验证 RTK GPS 结果的自洽性，并对无法自洽的绝对定位数据进行滤波和修正
- IMU 可以在 RTK GPS 信号消失之后，仍然提供持续若干秒的亚米级定位精度。
GPS + IMU 作为主流的高精定位方法，还可以在 GPS信号发生漂移的时候对 GPS 信号进行纠偏。（特别是在地震带上 GPS 漂移）
按照旋转的坐标系可以分为内旋（intrinsic rotation）和外旋 (extrinsic rotation)
内旋 (intrinsic rotation)： 绕物体自身的坐标系（object-space） 旋转。指绕物体的 $x$ 轴旋转一定角度后，绕着 $y$ 轴进行旋转，每次旋转都会改变下一次旋转的轴。这种情况下旋转的轴是动态的
外旋（extrinsic rotation）: 绕惯性系（upright-space） 旋转，无论旋转几次，轴都是固定的，不会变的。
欧拉角转旋转矩阵
在计算坐标变换时，旋转更方便的表示形式是旋转矩阵（rotation matrix）。三维空间的旋转矩阵可以表示成 $3*3$ 的矩阵，将欧拉角转换成旋转矩阵的计算方法如下，假设欧拉角 yaw，pitch，roll 的角度为 alpha, beta, gamma, 则
绕 IMU 的 $x$ 轴旋转：横滚角 roll
绕 IMU 的 $y$ 轴旋转：俯仰角 pitch
绕 IMU 的 $z$ 轴旋转：航向角 yaw
roll， ailerons 副翼
pitch, elevator 电梯升降机
yaw, rudder 方向盘，方向舵
旋转矩阵
姿态的旋转选用 ZYX 顺序的 3次旋转方式（下面的公式是一一对应的）。
解这个方程，可以得等到 roll 和 pitch 角度（由于绕 $z$ 旋转时，感受到的重力加速度是不变的，因此加速度计无法计算 yaw 角度）
$$
\begin{gathered}
\text {roll}=\arctan \left(\frac{a_{y}}{a_{z}}\right) \
\text { pitch }=-\arctan \left(\frac{a_{x}}{\sqrt{a_{y}{ }^{2}+a_{z}^{2}}}\right)
\end{gathered}
$$
yaw, pitch, roll 对应于欧拉角，按照以下公式可以将转换矩阵转化成欧拉角
$$
\begin{aligned}
\phi &=\arctan 2\left(A_{31}, A_{32}\right) \
\theta &=\arccos \left(A_{33}\right) \
\psi &=-\arctan 2\left(A_{13}, A_{23}\right)
\end{aligned}
$$
python 代码实现：
|
|
这个是雷达的文件，可以 load 进入，目前需要解决的是 行列的具体含义。
|
|
四种坐标系
为什么要相机标定？
我们拍摄的图像是二维的，但真实世界是三维的。如果从三维变成二维，相机就能起到这样的作用。可以把相机看做一个函数，输入是一个三维场景，输出是我们二维得到的图像（相机得到的是灰度图，我们平时看到的 RGB 彩色图是通过 RGB 三通道来实现的，每个通道可以认为是一张灰度图）。从三维世界到二维世界的这个映射关系是不可逆的，所以无法仅通过一张二维图来得到真实的三维世界。
相机标定就是通过输入带有标定 pattern 的标定板来计算相机参数，用简单的数学模型来表达复杂的成像过程。求解这个数学模型，就是求解相机的参数，包括相机的内参、外餐和畸变参数。有了这个数学模型，我们就可以对相机拍摄的图像进行畸变矫正，也可以使用多个相机拍摄图像进行三维重建，以及其他计算机视觉的应用。
在针孔相机的成像过程中，物体所折射的光线，透过 pinhole 后，光线投影在底片上，会在底片上形成一个左右相反、上下颠倒的该物体的影像。
世界坐标系（world coordinates），单位m
相机坐标系（camera coordinates），单位m
图像坐标系（Film coordinates），单位 mm
像素坐标系（pixel coordinates），单位 pixel
相机标定、相机内参和相机外参
这个是大概的坐标系转换流程，上面的符号不一定和下面的匹配
像素（图像）坐标系
图像一般以左上角为原点以像素为单位建立 u-v 坐标系。在OpenCV 中的坐标定义如下。其中 $u-v$ 是像素坐标系， $x-y$ 是图像坐标系。
（1）世界坐标系向相机坐标系转换
世界坐标系和相机坐标系都是三维的，所以可以通过平移变换将两个坐标系的原点重合，然后做旋转变换。原点重合之后，先固定一个轴，然后变换另外两个轴，就可以得到最后的旋转变换矩阵。
相机坐标系 $P_c$ 只是单个相机下的坐标，需要把所有相机拍的转换到同一个坐标系下，即世界坐标系下 $P_w$，经过旋转和平移。
$$
P_{c}=\left(\begin{array}{c}
x_{c} \
y_{c} \
z_{c} \
1
\end{array}\right)=\left(\begin{array}{cc}
R & T \
0^{3} & 1
\end{array}\right)\left(\begin{array}{c}
x_{w} \
y_{w} \
z_{w} \
1
\end{array}\right)=R P_{w}+T
$$
其中 $R$ 是一个 $33$ 的旋转矩阵， $T$ 是一个 $31$ 的平移矩阵。 $RT$ 放在一起是一个 $4*4$ 的矩阵，表示相机的外参（camera extrinsics） 矩阵。下面是外参矩阵（外参矩阵由旋转 R 和变换 T 组成，相机坐标系的原点在其光学中心，其 x 轴和 y轴确定了图像平面）：
$$
\left(\begin{array}{cc}
R & T \
0^{3} & 1
\end{array}\right)
$$
相机外参，比如相机的位置、旋转方向，用于描述在静态场景下的相机的运动或者在相机固定时，运动物体的刚性运动。
刚性运动：只有物体的位置（平移变换）和朝向（旋转变换）发生改变，形状不变，得到的变化是刚性变换
（2）相机坐标系向图像坐标系的转换
图像坐标系是二维，从相机坐标系到图像坐标系，属于透视投影关系，从 3D 到 2D。可以看出是小孔成像原理的镜像位置到了 Image Point 上。
（3）图像坐标系向像素坐标系转换
图像坐标系和像素坐标系都是二维的，所以对单位做一个变换就行。
（4）像素坐标系和相机坐标系的转换
（将后面两个转换结合起来了）
使用 $P_x$ 表示像素坐标系，$P_c$ 表示相机坐标系， $P_w$ 表示世界坐标系
像素坐标系相比于相机坐标系进行了缩放和原点的平移。（像素坐标系一般在图像的左上角）， 像素坐标系 $P_x (u, v)$
$$
\begin{aligned}
&u=f_{x} \frac{x_{c}}{z_{c}}+c_{x} \
&v=f_{y} \frac{y_{c}}{z_{c}}+c_{y}
\end{aligned}
$$
其中 $f_{x}, f_{y}, c_{x}, c_{y}$的单位是像素，$c_x, c_y$ 是原点的平移尺寸。
$$
z_{c}\left(\begin{array}{l}
u \
v \
1
\end{array}\right)=\left(\begin{array}{ccc}
f_{x} & 0 & c_{x} \
0 & f_{y} & c_{y} \
0 & 0 & 1
\end{array}\right)\left(\begin{array}{l}
x_{c} \
y_{c} \
z_{e}
\end{array}\right)=K P_{c}
$$
从像素坐标系到相机坐标系
因此 $z_{c} P_{x t}=K P_{c}$
$$
K=\left(\begin{array}{ccc}
f_{x} & 0 & c_{x} \
0 & f_{y} & c_{y} \
0 & 0 & -1
\end{array}\right)
$$
其中 $K$ 表示相机的内参( camera intrinsics) 矩阵。即像素坐标 $P_x$ 乘以深度 $z_c$ = 相机内参 乘以相机坐标 $P_c$
另外的一种写法
Intrinsic parameters 包括焦距（focal length）和光学中心（optical center， or principle point） 和偏斜系数（skew coefficient）。
$$
K=\left[\begin{array}{ccc}
f_{x} & 0 & 0 \
s & f_{y} & 0 \
c_{x} & c_{y} & 1
\end{array}\right]
$$
其中 $[c_{x} , c_{y}]$ 是以像素为单位的光学中心，通常是图片的中心
$(f_{x},f_{y})$ 是以像素为单位的焦距, $f_x =F/p_x, f_y = F/ p_y$
F 是世界单位下的焦距，一般是用毫米表示
$(p_x,p_y)$ 是世界单位下的像素大小
$s=f_x tan α$ 是偏斜系数，如果图片的坐标轴不相互垂直，那么偏斜系数大小就不为零。像素的偏斜被定义为：
（4）像素坐标系到世界坐标系
$$
z_{c}\left[\begin{array}{l}
u \
v \
1
\end{array}\right]=K \cdot\left[\begin{array}{cc}
R & T \
0 & 1
\end{array}\right]\left[\begin{array}{l}
x_{w} \
y_{w} \
z_{w} \
1
\end{array}\right]
$$
相机深度 $z_c$ 乘以像素坐标 $P_x$ = 相机内参 $K$ 乘以相机外参$RT$ 乘以世界坐标 $P_w$
K 是相机内参，包括相机焦距、光轴于图像平面的焦点位置等内部参数。
相机外参 RT 表示世界坐标系和相机坐标系的转换关系，是相机在世界坐标系下的位置姿态矩阵，认为是相机的外餐。
如果世界坐标系设置为相机坐标系（即两者重合），那么这个外参就是一个单位矩阵
引入齐次的目的是将所有的变换（缩放、旋转、平移）统一起来，因为分开的话，平移是矩阵加法，旋转是矩阵乘法，引入齐次之后就变成了乘法。
总结
世界坐标可以通过 extrinsic parameters 转变到相机坐标系；相机坐标系可以用 intrinsic paramters 投影到图像平面。
进一步的资料：
图像畸变
相机标定算法能够使用 extrinsic and intrinsic parameters 计算 camera matrix，extrinsic parameters 表示从三维世界坐标系到三维相机坐标系的刚体转变（a grid transformation），即物体不会发生形变。
内参矩阵反应对的了相机自身的属性，比如相机的焦距、像素大小，是需要标定才能知道的这些参数。
需要注意点是，真实的镜头还会有径向和切向畸变，这些畸变属于相机的内参。畸变还有其他类型的畸变，但是没有径向畸变、切向畸变显著，所以忽略不计。畸变解释了为什么像素点没有落在理论的位置上（产生的偏移和变形）。畸变是相机本身的固有特性，和相机内参相同，标定一次之后即可。
径向畸变来自于透镜形状。 切向畸变来自于整个摄像机的组装过程。
径向畸变（radial distortion）指的是光线在投过透镜边缘发生的弯曲比在光学中心发生的弯曲程度大的现象，透镜越小，发生的径向畸变越大。
如下图，光线在原理透镜中心的地方比靠近中心的地方更加弯曲。对于常用的普通透镜来说，这种现象更加严重。筒形畸变在便宜的网络摄像机中非常厉害，但在高端摄像机中不明显，因为这些透镜系统做了很多消除径向畸变的工作。
径向畸变能够表示为以下的模式，畸变点表示为 $(x_{\text {distorted }} , y_{\text {distorted }} )$
$$
\begin{aligned}
x_{\text {distorted }} &=x\left(1+k_{1} * r^{2}+k_{2} * r^{4}+k_{3} * r^{6}\right) \
y_{\text {distorted }} &=y\left(1+k_{1} * r^{2}+k_{2} * r^{4}+k_{3} * r^{6}\right)
\end{aligned}
$$
其中 $x,y$ 表示无畸变的像素坐标， $k_1,k_2, k_3$ 是透镜径向畸变的系数， $r^2 = x^2 +y^2$。一般来说两个系数就能完成标定任务，但是比较复杂的畸变，比如在广角镜头（wide-angle lenses）中，就需要选择 3 个系数（包括 $k_3$）
切向畸变（薄透镜畸变和离心畸变）, Tangential Distortion 是指当透镜和图片平面不平行时出现的畸变：
切向畸变是由于透镜制造上的缺陷使得透镜本身与图像平面不平行而产生的。
此时的畸变点表示为：
$$
\begin{aligned}
x_{\text {distorted }} &=x+\left[2 * p_{1} * x * y+p_{2} *\left(r^{2}+2 * x^{2}\right)\right] \
y_{\text {distorted }} &=y+\left[p_{1} *\left(r^{2}+2 * y^{2}\right)+2 * p_{2} * x * y\right]
\end{aligned}
$$
其中：
$(x,y)$ 是无畸变的像素坐标
$p_1, p_2$ 是透镜切向畸变系数
$r^2 = x^2 + y^2$
Image distortion: 将真实世界的三维图像转换成二维时，这种转换并不是完美的。
pinhole camera model（针孔相机模式）
实际物体，经过镜头的变换，从 3D 转换成 2D，经过转换后，在镜头边缘的图像会发生变形。
types of distortion
-
radial distortion 径向畸变，相机的镜头通常都是弧线，光线通过弧线时会发生扭曲。实际为直线的物体，会变得像曲线一样。
-
tangential distortion 切向畸变 当镜头和摄像机胶片并不平行时，会导致物体看起来更远或更近，距离不准确。如鱼眼镜头或广角镜头，就是故意达到这种效果。
distortion coefficients and correction 畸变系数和校正
滤波
线速度（linear velocity）和角速度（angular velocity）
线速度被定义为物体和固定点之间位移的变化率。线速度的定义可以看做是物体在单位时间内的位移。
角速度是角运动中讨论的一个事件。像旋转风扇的叶片或转动的轮子这样的运动都有角速度。
里程计
我们希望测量一个运动物体的轨迹。比如在汽车轮胎上安装计数码盘，可以得到轮胎转动的距离，从而得到汽车的估计。或者测量汽车的速度，加速度，通过时间来计算它的位移。完成这样运动估计的装置（包括硬件和算法）叫做里程计（odometry）。
视觉里程计（Visual Odometry）
视觉里程计是通过拍摄的图像估计相机的运动。主要可以分为特征点法和直接方法。其中特征点法目前占据最主流，能够在噪声大，相机运动较快时工作，但地图则是稀疏特征点。直接方法不需要提特征，能够建立稠密地图，但存在计算量大，鲁棒性不好的缺陷。
滤波
在信号系统领域，滤波是指将信号中特定波段的频率过滤掉。在机器人领域，暂时没有明确的定义，姑且认为：通过不断地观测，使得对目标状态的估计变得更加准确。
filtering is weighing （滤波即加权），滤波的作用是给不同的信号加上不同的权重。
贝叶斯滤波
贝叶斯函数
（1）先验概率密度函数 $$ f_{X_{k}}^{-}(x)=\int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v $$ （2）似然概率密度函数 $$ f_{Y_{k} \mid X_{k}}\left(y_{k} \mid x\right)=f_{R_{k}}\left[y_{k}-h(x)\right] $$ （3）后验概率密度函数 $$ f_{X_{k}}^{+}(x)=\eta_{k} \cdot f_{R_{k}}\left[y_{k}-h(x)\right] \cdot f_{X_{k}}^{-}(x) $$
卡尔曼滤波 (Kalman Filter)
卡尔曼滤波器是一个状态估计器，它利用传感器融合、信息融合来提高系统的精度。通常，我们要观测一个系统的状态，有两种方法。一种是通过系统的状态转移方程，并结合上一时刻的状态推测下一时刻的状态。另一种是借助辅助系统（量测系统）的测量得到系统状态。这两种方法都有各自的不确定性，卡尔曼滤波可以将两者做到最优结合（加权平均），使得我们估计的状态的不确定性小于其中任何一种。所以权重的选择至关重要，它意味着我们更信任哪一种方法的得出的状态（当然是更加信任不确定较小的状态）
kalman filter 也可以被认为是一种数据融合算法（data fusion algorithm）。Kalman Filter 的巨大成功归功于其小的计算需求，优雅的递归属性以及作为具有高斯误差统计的一维线性系统的最优估计器的状态。
从感性上认识卡尔曼滤波的基本原理，包含预测（prediction）和测量值更新（measurement update）两大过程。预测和测量值更新的交替执行，实现了卡尔曼滤波在状态估计中的闭环。
kalman 滤波是在时域上运用状态空间，递推得到一种滤波算法，便于在计算机上实时实现，计算量和存储量小。
对以上的公式逐个分析：
(1) $x$ 表示状态向量，通过左乘一个矩阵 $F$ ，加上外部影响 $\mu$，得到预测的状态向量 x'。 这里的 $F$ 叫做状态转移矩阵。
(2) 公式中的 $P$（状态协方差矩阵 state covariance matrix） 表示系统的不确定程度，在初始化时候很大，随着越来越多的数据注入到滤波器中，不确定性会变小。$Q$ 表示过程噪声（process covariance matrix），即无法使用 公式(1) 表示的噪声，比如车辆运动时突然到了上坡，这样影响是无法用之前的状态转移方程估计的。协方差矩阵对角线是方差，非对角线表示两两变量之间的联系。状态之间的关系
(3) 观测的第一个公式，用来计算观测值 $z$ 和预测值 $x'$ 之间差值 $y$
(4) 这两个公式是卡尔曼滤波中很重要的变量- 卡尔曼增益 $K$ (Kalman Gain) ：就是求差值 $y$ 的权值。其中 $R$ 是测量噪声矩阵（measurement covariance matrix），表示测量值和真值之间的差值，一般情况下，传感器的厂家会提供。$S$ 只是为了简化公式，写得临时变量，不用在意。为求得卡尔曼增益 $K$ 需要使用测量矩阵 $H$。 $$ S=H P^{\prime} H^{T}+R \quad K=P^{\prime} H^{T} S^{-1} $$ 求解 $H$，可以从公式(3) 中入手，此处省略雅克比公式和高数求导。最终可求。
(5) 最后两个公式卡尔曼滤波的闭环， $x =x' +Ky$ 是完成了当前状态 $x$ 的更新，不仅考虑了上一时刻的预测值，也考虑了测量值和整个系统的噪声。第二个公式是根据卡尔曼增益 $K$ 更新了系统的不确定度 $P$ ，用于下一周期的运算。 $I$ 是状态向量同纬度的单位矩阵。
卡尔曼滤波的核心：预测、测量和反馈。卡尔曼滤波的目标：从不确定信息中挤出尽可能多的信息。
卡尔曼滤波是以贝叶斯滤波为理论基础，并通过假设状态量、观测量都服从正太分布，假设过程噪声、观测噪声均服从均值为0 的正太分布，以及假设状态转移函数和观测函数均为线性函数，实现对连续型随机过程的递推状态估计。简言之，卡尔曼滤波是在贝叶斯滤波框架下求解线性高斯问题。
卡尔曼滤波是贝叶斯滤波在线性高斯系统下的一种滤波算法，对于非线性系统，则衍生出来扩展卡尔曼滤波。同时，无论是卡尔曼还是扩展卡尔曼，都是参数化的滤波方法。对于无法用参数表示的，则采用粒子滤波。
卡尔曼滤波和粒子滤波都是基于贝叶斯滤波框架下的滤波算法。贝叶斯滤波的基本思想就是根据上一时刻的状态对当前状态进行预测，并根据此时的观测进行更新。
卡尔曼滤波的默认假定是，世界充满噪声，任何测量结果都有噪声，状态转移过程会有噪声。卡尔曼滤波另一个重要假定模型是这样的，一个系统会处在各种不同的状态，并且会在状态之间转化来转化去。幸运的是我们可以通过测量的结果猜测到系统当前在一个什么状态。
卡尔曼滤波非常适合不断变化的系统，它的优点还有内存占用较小（只需保留前一个状态），速度快，是实时问题和嵌入式系统的理想选择。
卡尔曼滤波是利用多次观察和估计达到目的的，我们也只能一步步调整我们的观察和估计值，来渐渐达到准确的测量，所以整个算法是递归的，需要多次重复调整。调整的过程也很简单，就是把实测值（称出来的体重）和估计值（书上得来的体重）比较一下，如果估计值比测量值小，那就把估计值加上他们之间的偏差作为新的估计值，当然前面要加个系数，就是我们前面说的加权系数，这个地方我要写个公式，因为很简单就能说明白。
卡尔曼假设测量过程中的噪声是高斯模型。但有时候高斯模型并不实用，这个时候有了 extended kalman filter 和 particle filter。particle filter是针对多个对象的，比如在测量过程中，不仅涉及到汽车还涉及到卡车等，所以 particle filter 传递的是高斯混合分布，每一个峰代表一个物体。
kalman filter 可以很好追寻物体的轨迹。卡尔曼滤波器是最佳的线性滤波器。
局限性
（1）Kalman Filter 只能减小均值为0的测量噪声带来的影响。只要噪声均值（期望）为0，那么不管方差多大，只要迭代次数足够多，那效果都很好。反之，噪声期望不为0，那么估计值就还是与实际值有偏差。
（2）当系统状态方程不符合线性假设时，采用卡尔曼滤波无法获得理想的最优估计。
在描述机器人状态时常常不满足卡尔曼滤波的假设,为了仍然能够使用卡尔曼滤波，我们采用对非线性系统进行线性化等方法来扩大卡尔曼滤波的使用范围。扩展卡尔曼滤波与卡尔曼滤波主要区别在于：对状态方程和观测方程泰勒展开。
应用场景
飞机在飞行过程中，遇到的干扰通常是时变非平稳的噪声，此时运用卡尔曼滤波可有效去除干扰，得到较真实的状态估计数据。
卡尔曼滤波系列课程学习：这个就是 deepsort 的讲解
https://www.bilibili.com/video/BV1UL4y1p7qB?p=1
卡尔曼滤波是 is a algorithm for estimating the state of system, for example the speed and position of the car.
past estimation and current observation
追踪算法- deepsort
追踪是序号化，保证 num_id 的一致性。当前帧和之前帧是否同一个实体。
开车如何定位自己的位置？
（1）提供加速度， （2）里程表信息，比如仪器误差（3）GPS信息，比如精度误差；都有一定的误差
卡尔曼滤波
结合已知信息估计最优位置；本质是优化估计算法。
bbox 是检测器给出的结果，kalman 滤波 也可以给出一个估计值。
- 先要给出状态向量（比如位置 速度），然后计算下一时刻的状态向量，涉及到状态转移矩阵，需要考虑高斯噪声（这里假设是均值是 0，有时多有时少，但整体上均值是 0，方差不为 0）
- 根据观测值和估计值，然后根据权重，求解最后的数值
先验估计：数学模型（使用状态向量和状态转移矩阵得到的结果），有偏差（不可能考虑到所有的因素）
后验估计：使用测量器得到的结果，$K$ 属于卡尔曼增益，是后验估计的权重
当前的观测值和（之后时间或历史时间）的观测值必然是存在联系的，这种联系通过协方差矩阵来表示。协方差矩阵K 表示状态之间的关系。
第二个步骤就是 update。
卡尔曼增益 K 的目的使得最优估计值的方差更小
当观测没有噪音的时候，那么最优估计就是观测值。
当状态估计没有噪音时候，先验估计等于后验估计
最终问题需要考虑的状态
均值 mean：8 维向量表示为 $x =[c_x, c_y, r, h, v_x, v_y, v_r, v_h]$ ，其中中心点坐标$(c_x, c_y)$， 宽高比 $r$，高$h$，以及各自的速度变化值。协方差矩阵表示目标位置信息的不确定性，由 $8*8$ 的矩阵表示。
其中 $h $ 会随着远近进行变化
每个 track 都是预测下一时刻的状态，并给予检测到的结果来修正（匀速、线性，追踪通常是一帧一帧处理）。注意卡尔曼滤波是线性变化，不可能跳帧（1， 3， 10 ）。
比较好的教程
飞机在飞行过程中有外来噪声输入，传感器采集到有干扰的数据送给滤波估计器，滤波器负责去除这些噪声还原飞机真实的数据送给控制器，控制器得到飞机的真实数据，控制输出到飞机。这样飞机有了真实的数据，就可以更好的控制飞机，达到稳定飞行的目的。
扩展卡尔曼（EKF）和经典卡尔曼（KF）的却比在于测量矩阵$H$ 的计算。 EKF 对非线性函数进行泰勒展开后，进行一阶线性化的截断，忽略其余高阶项，进而完成非线性函数的近似线性化。正是由于忽视了部分高阶项，所以 EKF 的状态估计会损失一些精度。
由于雷达测量是非线性的，所以我们需要再雷达测量更新周期中需要使用扩展卡尔曼滤波器，而激光测量是线性的，我们在激光测量中使用卡尔曼滤波器，在预测周期中两者都可以使用卡尔曼滤波器。
尽管扩展卡尔曼滤波能解决非线性问题，但是仍有一些缺点：
如果需要用解析方法求雅可比矩阵，则计算很困难的。
如果用数值分析的方法求出雅可比矩阵，则计算开销很大。
扩展卡尔曼滤波器只适用于具有可微分模型的系统。
- 粒子滤波( particle filter) 没有找到很好的讲解资料
- 直方图滤波（ Histogram Filter）没有找到很好的讲解资料
文章作者 jijeng
上次更新 2022-01-12