SVO详解

首先科普下计算机视觉和机器视觉

计算机视觉

主要是质的分析,比如分类识别,这是一个杯子那是一条狗。或者做身份确认,比如人脸识别,车牌识别。或者做行为分析,比如人员入侵等。

机器视觉

主要侧重对量的分析,比如用视觉去测量一个零件的直径。

接下来开始说论文的重点

我们对于矩阵求逆的时候,要主要必须得保证这个矩阵是一个方阵才行哦

相机类型

刚才介绍了普通的相机,现在介绍下RGB-D相机

  1. 红外结构光来测量像素距离,例如kinect1
  2. 飞行时间法(ToF)测量像素距离,例如kinect2

相机映射模型

由相似三角形比例得:这里负号是指反向的意思

由于相机内置旋转算法,所以我们拍出来的照片都是正面的

照片上一点像素

化成齐次矩阵

其中 是内参,至此就是

我们现在将推到 ,不过在此之前我们必须要知道距离

上式就是像素到相机坐标的公式

内参属于相机出产自带的属性,我们可以通过棋盘标定法来实现相机的标定

棋盘标定法

推导外参

接下来推倒外参,外参表示相机运动的参数

向量到矩阵的变换

外积计算

,这个公式转换很最重要在后文中有提及

旋转矩阵

刚体变换

相机运动是一个刚体运动,他保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化,这种变换属于欧式变换。欧式变换包括了旋转和平移。首先我们考虑旋转。我们设某个正交单位基 经过一次旋转后变成了 ,那么同一向量 ,在两个坐标系下的坐标分别是 。根据定义,有:

两边同时左乘一个 \begin{bmatrix}
e^T_{1}\
e^T_{2}\
e^T_{3}\
\end{bmatrix}。

其中 就是旋转矩阵 此处解释了旋转矩阵为啥是乘法,旋转矩阵满足一下性质

  1. 秩为1
  2. 该矩阵与其转置相乘等于单位矩阵
    我们把旋转矩阵的集合定义如下:

在欧式变换里,除了考虑旋转,还有平移。考虑到世界坐标系下的一个向量 ,经过旋转和平移之后得到

变换矩阵

假设我们做了两次变换: ,满足

这样一来向量 计算比较复杂

引入齐次坐标和变换矩阵

其中, 是变换矩阵(Transform Matrix),本文我们定义 表示 的齐次坐标, 表示 的齐次坐标, 表示 的齐次坐标,

区分齐次和非齐次坐标符号很麻烦,我们直接就用 ,代表齐次坐标。

关于变换矩阵 ,它的结构比较特殊,左上角为旋转矩阵,右上角为平移向量,左下角为0向量,右下角为1。这种特殊矩阵又称为特殊欧式群(Special Euclidean Group):

同理该矩阵的逆

旋转向量

假设有一个旋转轴为 ,角度为 ,显然他对于的旋转向量为 。从旋转向量都旋转矩阵的转换过程由罗德里格斯公式:

这里 是向量到反对称矩阵的转换符,见前面公式。

补充一个知识点,单位矩阵 I=\begin{bmatrix}1&0&0\0&1&0\0&0&1\end{bmatrix}

对于转角 ,有:

因此

注: ,表示求矩阵 $N$的主对角元素之和。$tr(I)=3$,因为由于前面旋转矩阵推导可知,$R$是3\3矩阵。 $\vec n^{\wedge}$ 是对角线为0的反对称矩阵,所以 $tr(\vec n^{\wedge})=0$。*

关于转轴 $\vec n$,由于旋转前后旋转轴不发生变化,所以

李群

李群的主要核心是表述连续变化的特性。变换矩阵包含的实际意义就是旋转和平移,所以肯定是连续的。故旋转矩阵和变换矩阵都是李群的一种,分别是特殊正交群和特殊欧式群。

李代数

我们知道旋转矩阵的一个性质

它是有时间意义的,随着时间不断的变化,所以可以写成对时间的函数:

对时间进行求导

所以 是反对称矩阵。由前面介绍查积所知,向量到矩阵就是一个反对称矩阵。

所以 必然对应一个向量

于是

右乘一个

时,则还未开始旋转,所以 。 由一阶泰勒展开得:

将上式带入

我们看到 反映了 的导数性质,故称它在SO(3)原点附近的正切空间上。引用书上原话,我不是很明白

由齐次微分方程性质()得

通解

由于初始值,我们考虑的是时,此时 为常数,故简化 所以其特解为

对其换种写法也即

一般我们说 的元素是3维向量或者3维反对称矩阵。

的关系对应为

同理我们可以推导

每个 的元素 是六维向量,前三维是平移向量,记做 。 后三维是旋转向量,记做 。同时我们这里的 不再单纯表示向量到反对称矩阵,而是向量到矩阵的转换, 表示矩阵到向量的转换。

指数映射

由于 是三维向量,我们可以换种方式来定义它,通过模长 和方向 ,这里 是长度为1的方向向量,对于 (后面我们不说明的情况下就是 ),有以下两个性质:

PS:这是人家前人发现的,我们直接用就行了

本文通过假设 ,强行往里带入可以验证上述性质成立

由麦克劳林公式得

由上式得

将上式关于 的次方推导公式,带入

指数映射

按照之前推导$exp(\phi^{\wedge})$ 的思路,将$a^{\wedge}$ 平方和立方的计算规律带入得:

图像像素灰度在k的时刻表示为I_k:Ω⊂R^2⊢R, 其中Ω是图像的整个域(二维的)。任意3维点p 在可见的场景表面S⊂R^3,通过相机投影模型 映射到图像的坐标

其中,前注k表示在k时刻相机框架点坐标的表示。投影函数π由相机内参(由校准之后得到)决定。3维点可以由图像坐标u恢复,不过需要给定反向投影函数π-1和深度

总结下李群、李代数之间的关系

李群是一个矩阵(R,T),李代数是一个向量 $\phi$,$\xi$,每个李代数都可以对应一个反对称矩阵 $\phi^{\wedge}$,$\xi^{\wedge}$,同时我们对这个反对称矩阵求指数就是李群了。也即:

为什么要提出李群,李代数

李群为了更好的描述相机的运动,李代数是为了进一步更好的计算李群(单纯的李群是不容易直接进行数学运算的)

假设我们知道一个世界坐标系上的点p(路标点),运动了T之后,它的观测量为z,则理论上:

然而我们知道真实情况系必然存在误差,所以应该是

我们通常比较想知道计算理想和真实情况下的误差,使其最小

假设我们有N个观测点和路标点,则

我们的目标就是求$T$,使得整体误差$J$最小。

BCH近似

其中$J_l=J=\frac{sin\theta}{\theta}I+(1-\frac{sin\theta}{\theta})aa^T+\frac{1-cos\theta}{\theta}a^{\wedge}$,它的逆

右乘雅克比仅需要对自变量取符号即可:

将 $\phi_1^{\wedge}$ 与$J_l^{-1}\phi_1$对应,所以 $\phi_1$与 $(J_l\phi_1)^{\wedge}$ 对应。即:

李代数求导

由导数定义可知

从叉积公式得 $\vec a\times \vec b=a^{\wedge}b=-ab^{\wedge}$

自此,我们推导出旋转后的点相对于李代数的导数:

扰动模型(左乘)

SE(3)

其中, 是已知深度域
相机的位置和方向在k时刻用刚体变换 表示 SE(3)
我们可以映射一个3D点从世界坐标框架到参考相机框架
Tk,w就是上面说的外参,
两个连续帧直接相关变化可以用 计算
在优化过程中,我们需要最小的表示变化,因此,使用李代数se(3) 对应恒等于正切空间SE(3)。我们用代数元素ξ=〖(ω,υ)〗^T∈R^6表示旋转坐标,其中w是角速度和v线性速度。旋转坐标ξ用指数映射到SE(3)

对极约束

$s_{1}$ 是一个数值,表示距离

对其进行归于化,得

两边同时乘以$t^{\wedge}$,由于$t^{\wedge}t=0$

两边同时乘以$x_{2}^{T}$

由于向量也是一维矩阵,所以左边满足矩阵乘法结合律

由于$t^{\wedge}x_{2}$ 是垂直于$t$ 和 $x_{2}$ 的向量(一维矩阵),所以左边等0,即

将上面的$x_{1},x_{2}$ 代入

令$E=t^{\wedge}R$(本质矩阵),$F=K^{-T}EK^{-1}$(基础矩阵)

根据特征点匹配求出$E$,从中分解出$R 和 t$ 出来即可
由E的性质可知,E为3*3的矩阵

对于任意的一对匹配点$x_{1}=[u_{1}, v_{1}, 1]^{T},x_{2}=[u_{2}, v_{2}, 1]^{T}$

由矩阵乘法结合律

将$E$写成向量的形式

上式可以写成

对于其他特征点也有同样的性质

光束平差法(Bundle Ajustment)

我们通过特征匹配的方式,已经可以到N组特征点

其中$z_{i}^{j}$ 下标是指第几帧图,上标表示第几个特征点,其在图像上的坐标为 $z_{i}^{j}=\left [ u,v \right ]_{i}^{j}$


上图模型表示为相机在1处,2处分别观测空间中X点位置。

其中 $\lambda_{1},\lambda_{2}$ 表示像素深度值,也就是相机的z坐标。正常的思路就行,两个公式约掉 $X^{j}$ ,使用对极约束和SVD分解,理论上我们需要8个点就可以解算出来$R, t$,这是常规思路,我们这里主要介绍图优化。
我们尝试构造一个优化问题
由于存在着精度误差,所以 $\left | \frac{1}{\lambda_{1}}CX^{j}-\left [ z_{1}^{j},1 \right ]^{T} \right |^{2},\left | \frac{1}{\lambda_{2}}C(RX^{j}+t)-\left [ z_{2}^{j},1 \right ]^{T} \right |^{2}$ 不可能都为0,所以我们只能通过调整 $R,t$ 来使他们的误差尽量下。

所以可以构造一个最优化问题,调整$R,t$使得对于所有的特征点$z^{j}$,误差二范数累计最小,也即

这个就是最小化重投影误差问题(minimizaiton of reprojection error),实际操作中,我们在调整每个$X^{j}$,使得更符合每一次观测$z^{j}$,也就是每个误差项都尽量小。由于这个原因,所以他也叫捆绑调整(Bundle Adjustment)。

BA很容易描述成图优化形式。图的结点为

  • 相机的位置位姿:一个SE(3)的元素

  • 空间中的特征点:是个XYZ坐标

边主要是有空间点到像素坐标的投影关系。也就是

注:向量转置与其本身实质和意义都是一样的,与矩阵不同

g2o

其主要步骤如下:

  1. 选择一个线性方程求解器,从 PCG, CSparse, Choldmod中选,实际则来自 g2o/solvers 文件夹中定义的文件夹。

使用Cholmod中的线性方程求解器
1
g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
  1. 选择一个 BlockSolver。

    6*3 的参数
    1
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
  2. 选择一个迭代策略,从GN, LM, Doglog中选。

    L-M 下降
    1
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );
    1. 构造结点和边

结点,空间中的点和位姿,边是有空间中的点跟像素坐标的投影关系

测试BA和对级约束求解(详见ubuntu虚拟机的代码)

对极约束

BA

最小光度误差

直接法主要根据灰度不变性假设进行实验,所以我们要实现最小光度误差
等价于一个空间中的点,在两个不同位置上的相机投影产生的光度理论上是应该一致,所以整个最小化光度误差公式应由灰度方程组成。即

$\xi$为相机变换李代数,不断调整相机位姿使得$e$的二范数值最小,这就是整个最小光度误差的原理。

由前面的投影方程可知

其中D是齐次到非齐次的转换

因为空间中可能有许多类似于上述的三维点,所以构成了一个累和公式

既然要求最小值,所以一般我们先考虑其导数的情况,对于 $\min_{\xi}J(\xi)$ 的最小值,等价于求 $e_{i}$ 最小值,一般来说,我们想要求最小值先分析其导数比较合适,但是$e_{i}$不便对其进行求导,所以我们采用最基本的导数公式

所以我们对 $e_{i}$ 加个扰动量,即

记, $q=\delta\xi^\wedge exp(\xi^\wedge)P$,$u=\frac{1}{Z_{2}}DKq$ 所以,上式一阶泰勒展开等于

第一个式子 $\frac{\delta I_{2}}{\mathbf{u}}$ 是在 $\mathbf{u}$ 处的像素梯度
第二个式子 $\frac{\delta \mathbf{u}}{\delta q}$ 是投影方程关于相机坐标系下的三维点的导数。由前面的投影方程可知

于是,导数为

误差相对于李代数的雅克比矩阵

2018.2.25

Sophus

Li algebra
Eigen is a matrix operation library, including create Vector and Matrix

Opencv

Mat can from a image file, and it’s has many invariables, like rows, cols, channel_number

also, we learn PointCloud, its has a function push_back(), which can storage class PointT, IsometryId, ImageDepth, and ImageColor

In addition, formation from pixel frame to 3D frame. However, real World Point must use 3D frame multiple SE(3). like this:

Eigen::Vector3d pointWorld = T ** point;

this function can save pointcloud map

pcl::io::savePCDFileBinary("map.pcd", **pointCloud);

2018.2.26

point

when we define a type point, like char* p, it means p is the type char of start address, as we all known each type has different bytes, float is 4 bytes, char is 1 bytes, each address is a bytes.

char var = 's';
int var = 12;
char* p = &var;  // right

*p = var is value, and p = &var is address

OptimizationAlgorithm

Guass-Newton

对 $\Delta x$进行求导,并令其为零。
so,

将左侧定义为H,右侧定义为g

Levenberg-Marquardt

we assume $D=I$ so,

Eigen

functions:

1. Identity()

solve identity matrix(单位矩阵)

2. transpose()

transposition of a matrix(矩阵的转置)

3. norm()

求向量的模长

g2o

2018.3.1

function: img.at()

for a single channel grey scale image, img.at() is get the value of intensity

Scalar intensity = img.at<uchar>(y, x)

when consider a 3 channel image with RGB color

Vec3b intensity = img.at<Vec3b>(y, x);
uchar blue = intensity.val[0];
uchar green = intensity.val[1];
uchar red = intensity.val[2];

struct DMatch

Class for matching keypoint descriptors: query descriptor index, train descriptor index, train image index, and distance between descriptors.

class KeyPoint

Data structure for salient point detectors.

Point2f pt

coordinates of the keypoint

so, keypoints[m.queryIdx].pt.y is y direction coordinate.

pixel2cam

this forum is pixel2cam

2018.3.9

LK OpticalFlow

it’s main function is tracking and match features. it saves more time with respect to tranditional feature methods. So, when it finish match features, then, we should use SVD, PnP, ICP.

calcOpticalFlowPyrLK(prevImg, nextImg, prevPts, nextPts, status, erros)

Parameter:

status – Output status vector. Each element of the vector is set to 1 if the flow for the corresponding features has been found. Otherwise, it is set to 0.

简单说光流法是不是只做到了特征点跟踪的而已,等于替换了特征点法的特征点匹配那个步骤
如果要基于光流法得到相机的运动,是不是就需要继续补充PnP或者ICP,而且光流法如果想进一步使用BA原理进行计算运动的话,那是不是就是直接法的第一种,稀疏直接法

2018.3.14

OpticalFlow essence

  1. extract features with fast
  2. match features using intensity consistent assume
  3. if we want to farther get the pose of camera, in addition to, it needs to PnP or ICP after matching
  4. if step three is to solver pose using BA based on the assume of consistent intensity, it is called direct method.

2018.3.18

Eigen

这里是双精度,如果是单精度,只需要修改d为f即可

旋转矩阵(3*3): Eigen::Matrix3d

旋转向量(3*1):Eigen::AngleAxisd

欧拉角(3*1):Eigen::Vector3d

四元数(4*1):Eigen::Quaterniond

欧式变换矩阵(4*4):Eigen::Isometry3d

仿射变换(4*4):Eigen::Affine3d

射影变换(4*4):Eigen::Projective3d

schur消元

对于线性方程

求H矩阵的方法,其中 $\Delta x$ 是 $\Delta x_{c}=\begin{bmatrix}
\xi_{1}, & \xi_{2}, & …, & \xi_{m}
\end{bmatrix}^{T} \in \mathbb{R}^{6m}, \Delta x_{p}=\begin{bmatrix}
p_{1}, & p_{2}, & …, & p_{n}
\end{bmatrix}^{T} \in \mathbb{R}^{3n}$ 的统称,分别表示相机位姿和空间点的变量。

针对H矩阵的性质,我们将H矩阵分解成4块,$B, E, E^T, C$,该方法的精髓在于将一次性求解 $\Delta x, \Delta p$ 变成了先求解 $\Delta x$,再求解 $\Delta p$

我们将两边同时乘以 $\begin{bmatrix}
I & -EC^{-1} \
0 & I
\end{bmatrix}$

所以,就变成了先求 $(B-EC^{-1}E^T)\Delta x_{c} = \nu - EC^{-1}\omega$

之后将 $\Delta x_{c}$ 带入到 $\Delta x_{p}=C^{-1}(\omega - E^T\Delta x_{c})$ 即可

2018.3.19

光流法的核心

为啥要进行同时左乘一个 $A^T$ ,因为左边不是一个方阵,所以没法直接移过来求逆,所以我们需要通过变换,将其变成方阵

卡尔曼滤波的推导

复合高斯分布的概念

假设随机变量 $x \sim N(u_{x},\Sigma_{xx})$,另一个变量y满足:

其中 $A, b$ 为线性变量的系数矩阵和偏移量,$w$为噪声项,$w$ 为零均值的高斯分布:$w \sim N(0, R)$

y的高斯分布即为:

状态估计

我们知道对于一个物体的状态估计应该分为预测和测量,其中预测主要是基于运动方程动模型得到的数据,测量主要是基于传感器等感知的数据

具体来说应该符合如下线性方程来描述:

$C_{k}$ 也是状态变量到测量的转换矩阵。利用马尔可夫性,我们知道了k-1时刻的后验(在k-1时刻看来)状态估计 $\hat x_{k-1}$ 以及协方差 $\hat P_{k-1}$ ,现在要根据k时刻的输入( $\hat x_{k-1}, \hat P_{k-1}$ )和观测数据( $z_{k}$ ),确定k时刻的后验分布 $\hat{x}_{k}$ 。为区分先验和后验,我们分别以 $\bar{x}_{k}$ ,$\hat{x}_{k}$ 表示。

其中假设 $x_{k-1}, z_{k}, w_{k}, v_{k}$ 是满足高斯分布,分别为 $x_{k-1} \sim N(\hat{x}_{k-1}, \hat{P}_{k-1}), w_{k} \sim N(0, R), v_{k} \sim N(0, Q)$

通过运动方程,确定 $x_{k}$ 的先验分布,根据复合高斯分布的性质,显然有

所以 $x_{k}$ , $z_{k}$ 组成的联合高斯分布的形式为:

由于 $x_{k-1} \sim N(\hat{x}_{k-1}, \hat{P}_{k-1})$,即 $x_{k} \sim N(\hat{x}_{k}, \hat{P}_{k})$ 。由贝叶斯公式 $p(x,y)=p(x|y)p(y)$,下文引自公式

贝叶斯滤波的基本假设:

  1. 马儿可夫性假设:t时刻的状态由t-1时刻的状态和t时刻的动作决定。t时刻的观测仅同t时刻的状态相关,即:
  2. 观测噪声、模型噪声等是相互独立的
  3. 静态环境,即对象周边的环境假设是不变的

2018.3.20

名词解释

先验概率 P(X):仅仅依赖主观上的经验,事先根据已有的知识的推断

后验概率 P(X|Z):是在相关证据或者背景给定并纳入考虑以后的条件概率

似然P(Z|X):已知结果去推测固有性质的可能性

贝叶斯公式:

后验分布正比于先验分布乘以似然

2018.3.25

最大似然估计(Maximize Likelihood Estimation, MLE)

在怎样的状态下,最可能产生现在观测到的数据