【视觉SLAM十四讲】视觉里程计—直接法
本文为视觉 SLAM 学习总结,讲解直接法的相关知识。
本讲内容概要
- 光流法
- 直接法原理
直接法的引出
我们在上一讲中的特征法中,提取特征点计算描述子十分耗时,并且特征匹配使用暴力匹配的方法,时间复杂度为 O ( n 2 ) O(n^2) O(n2),但计算位姿较快。我们希望找一种方法能够代替特征法中的特征点提取和匹配的过程。
我们可以通过光流的方法寻找匹配点,也可以使用直接法,不匹配点对。光流描述了像素在图像中的运动,直接法附带一个相机运动模型。
光流
一般分为稀疏光流和稠密光流。稀疏以 LK 光流为代表,稠密以 HS 光流为代表,本质上是估计像素在不同时刻图像中的运动。因为 SLAM 中我们只需要对特征点进行匹配并计算位姿,因此仅介绍稀疏的 LK 光流。

设 t t t 时刻位于 x , y x,y x,y 处的像素点的灰度值为 I ( x , y , t ) I(x,y,t) I(x,y,t),在 t + d t t+dt t+dt 时刻,该像素运动到 ( x + d x , y + d y ) (x+dx,y+dy) (x+dx,y+dy)。
灰度不变性假设:同一空间点的像素灰度值,在各图像中不变。则有:
I
(
x
+
d
x
,
y
+
d
y
,
t
+
d
t
)
=
I
(
x
,
y
,
t
)
I(x+dx,y+dy,t+dt)=I(x,y,t)
I(x+dx,y+dy,t+dt)=I(x,y,t)
灰度不变假设是理想状态,实际中不成立。
对
t
+
d
t
t+dt
t+dt 时刻的灰度进行泰勒展开保留一阶项:
I
(
x
+
d
x
,
y
+
d
y
,
t
+
d
t
)
≈
I
(
x
,
y
,
t
)
+
∂
I
∂
x
d
x
+
∂
I
∂
y
d
y
+
∂
I
∂
t
d
t
I(x+dx,y+dy,t+dt)≈I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt
I(x+dx,y+dy,t+dt)≈I(x,y,t)+∂x∂Idx+∂y∂Idy+∂t∂Idt
由于灰度不变,则:
∂
I
∂
x
d
x
+
∂
I
∂
y
d
y
+
∂
I
∂
t
d
t
=
0
\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0
∂x∂Idx+∂y∂Idy+∂t∂Idt=0
因此
∂
I
∂
x
d
x
d
t
+
∂
I
∂
y
d
y
d
t
=
−
∂
I
∂
t
\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}
∂x∂Idtdx+∂y∂Idtdy=−∂t∂I
设
d
x
d
t
,
d
y
d
t
\frac{dx}{dt},\frac{dy}{dt}
dtdx,dtdy 分别为
u
,
v
u,v
u,v。这是一个二元一次方程,欠定,需要引入额外的约束。假设一个窗口
(
w
×
w
)
(w\times w)
(w×w) 内光度不变,我们共有
w
2
w^2
w2 个方程:
[
I
x
I
y
]
k
[
u
v
]
=
−
I
t
k
,
k
=
1
,
…
,
w
2
\left[ \begin{matrix} I_x & I_y \end{matrix} \right]_k\left[ \begin{matrix} u \\ v \end{matrix} \right]=-I_{tk},\quad k=1,…,w^2
[IxIy]k[uv]=−Itk,k=1,…,w2
记:
A
=
[
[
I
x
I
y
]
1
⋮
[
I
x
I
y
]
k
]
,
b
=
[
I
t
1
⋮
I
t
k
]
A=\left[ \begin{matrix} [I_x & I_y]_1\\ \vdots\\ [I_x & I_y]_k \end{matrix} \right],\quad b=\left[ \begin{matrix} I_{t1}\\ \vdots\\ I_{tk} \end{matrix} \right]
A=⎣⎢⎡[Ix⋮[IxIy]1Iy]k⎦⎥⎤,b=⎣⎢⎡It1⋮Itk⎦⎥⎤
于是整个方程:
A
[
u
v
]
=
−
b
A\left[ \begin{matrix} u \\ v \end{matrix} \right]=-b
A[uv]=−b
关于
u
,
v
u,v
u,v 超定线性方程的求解,传统解法是求最小二乘解(详细推导见《矩阵论》):
[
u
v
]
∗
=
−
(
A
T
A
)
−
1
A
T
b
\left[ \begin{matrix} u \\ v \end{matrix} \right]^*=-(A^TA)^{-1}A^Tb
[uv]∗=−(ATA)−1ATb
-
我们可以将光流看作最小化像素误差的非线性优化。
-
每次泰勒一阶近似,在离优化点较远时效果不佳,往往需要迭代多次。
-
运动较大时使用图像金字塔,缩放分辨率,从粗到精
-
只需提取第一个图像的角点,不同计算描述子,直接跟踪像素轨迹。
实践:光流
首先我们对第一帧提取 FAST 角点:
if (index ==0 )
{
// 对第一帧提取FAST特征点
vector<cv::KeyPoint> kps;
cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
detector->detect( color, kps );
for ( auto kp:kps )
keypoints.push_back( kp.pt );
last_color = color;
continue;
}
对其他帧用 LK 跟踪特征点:
vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;
for ( auto kp:keypoints )
prev_keypoints.push_back(kp);
vector<unsigned char> status;
vector<float> error;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"LK Flow use time:"<<time_used.count()<<" seconds."<<endl;
把跟丢的点删掉:
// 把跟丢的点删掉
int i=0;
for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)
{
if ( status[i] == 0 )
{
iter = keypoints.erase(iter);
continue;
}
*iter = next_keypoints[i];
iter++;
}
cout<<"tracked keypoints: "<<keypoints.size()<<endl;
if (keypoints.size() == 0)
{
cout<<"all keypoints are lost."<<endl;
break;
}
画出 keypoints:
cv::Mat img_show = color.clone();
for ( auto kp:keypoints )
cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
cv::imshow("corners", img_show);
cv::waitKey(0);
last_color = color;
角点固定不动,边缘会沿着边滑动,因为点沿着边看起来一样,区块附近点很相似,更容易移动到别的地方去。

直接法
光流法仅估计了相机间的平移。但
- 不知道相机怎么走,未使用深度信息,未使用相机模型,即没有考虑到相机的旋转和图像的缩放
- 灰度不变性没有考虑到相机的旋转和图像的缩放
直接法的推导
假设有两帧,运动未知,但有初始估计 R , t R,t R,t,可以为 0。在第一帧看到了点 P P P,投影为 p 1 p_1 p1,按照初始估计, P P P 在第二帧投影为 p 2 p_2 p2。

投影关系:
p
1
=
[
u
v
1
]
1
=
1
Z
1
K
P
p_1=\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right]_1=\frac{1}{Z_1}KP
p1=⎣⎡uv1⎦⎤1=Z11KP
p 2 = [ u v 1 ] 2 = 1 Z 1 K ( R P + t ) = 1 Z 1 K ( exp ( ξ ^ ) P ) 1 : 3 p_2=\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right]_2=\frac{1}{Z_1}K(RP+t)=\frac{1}{Z_1}K(\exp(\hat{\xi})P)_{1:3} p2=⎣⎡uv1⎦⎤2=Z11K(RP+t)=Z11K(exp(ξ^)P)1:3
为了估计相机的位姿,我们需要最小化光度误差:
e
=
I
1
(
p
1
)
−
I
2
(
p
2
)
e=I_1(p_1)-I_2(p_2)
e=I1(p1)−I2(p2)
优化的目标函数为:
min
ξ
J
(
ξ
)
=
∣
∣
e
∣
∣
2
\min\limits_\xi J(\xi)=||e||^2
ξminJ(ξ)=∣∣e∣∣2
我们仍然基于灰度不变假设,有许多个空间点
P
P
P:
min
ξ
J
(
ξ
)
=
∑
i
=
1
N
e
i
T
e
i
,
e
i
=
I
1
(
p
1
,
i
)
−
I
2
(
p
2
,
i
)
\min\limits_\xi J(\xi)=\sum\limits_{i=1}^Ne_i^Te_i, \quad e_i=I_1(p_{1,i})-I_2(p_{2,i})
ξminJ(ξ)=i=1∑NeiTei,ei=I1(p1,i)−I2(p2,i)
我们关心误差
e
e
e 是如何随着相机位姿
ξ
\xi
ξ 变化的,需要分析它们的导数,使用李代数上的扰动模型,给
exp
(
ξ
)
\exp(\xi)
exp(ξ) 左乘一个小扰动
exp
(
δ
ξ
)
\exp(\delta\xi)
exp(δξ):
e
(
ξ
⊕
δ
ξ
)
=
I
1
(
1
Z
1
K
P
)
−
I
2
(
1
Z
2
K
exp
(
δ
ξ
^
)
exp
(
ξ
^
)
P
)
e(\xi\oplus \delta\xi)=I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K\exp(\delta\hat{\xi})\exp(\hat{\xi})P)
e(ξ⊕δξ)=I1(Z11KP)−I2(Z21Kexp(δξ^)exp(ξ^)P)
对
exp
(
δ
ξ
^
)
\exp(\delta\hat{\xi})
exp(δξ^) 一阶泰勒展开:
≈
I
1
(
1
Z
1
K
P
)
−
I
2
(
1
Z
2
K
(
1
+
δ
ξ
^
)
exp
(
ξ
^
)
P
)
≈I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K(1+\delta\hat{\xi})\exp(\hat{\xi})P)
≈I1(Z11KP)−I2(Z21K(1+δξ^)exp(ξ^)P)
= I 1 ( 1 Z 1 K P ) − I 2 ( 1 Z 2 K exp ( ξ ^ ) P + 1 Z 2 K δ ξ ^ exp ( ξ ^ ) P ) =I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K\exp(\hat{\xi})P+\frac{1}{Z_2}K\delta\hat{\xi}\exp(\hat{\xi})P) =I1(Z11KP)−I2(Z21Kexp(ξ^)P+Z21Kδξ^exp(ξ^)P)
记 q = δ ξ ^ exp ( ξ ^ ) P ) , u = 1 Z 2 K q q=\delta\hat{\xi}\exp(\hat{\xi})P),\quad u=\frac{1}{Z_2}Kq q=δξ^exp(ξ^)P),u=Z21Kq,为 P P P 在扰动后位于第二个相机坐标系下的坐标, u u u 为其像素坐标。
利用一阶泰勒展开有:
e
(
ξ
⊕
δ
ξ
)
=
I
1
(
1
Z
1
K
P
)
−
I
2
(
1
Z
2
K
exp
(
ξ
^
)
P
)
−
∂
I
2
∂
u
∂
u
∂
q
∂
q
∂
δ
ξ
∂
δ
ξ
e(\xi\oplus \delta\xi)=I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K\exp(\hat{\xi})P)-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial q}\frac{\partial q}{\partial \delta\xi}\partial \delta\xi
e(ξ⊕δξ)=I1(Z11KP)−I2(Z21Kexp(ξ^)P)−∂u∂I2∂q∂u∂δξ∂q∂δξ
= e ( ξ ) − ∂ I 2 ∂ u ∂ u ∂ q ∂ q ∂ δ ξ ∂ δ ξ =e(\xi)-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial q}\frac{\partial q}{\partial \delta\xi}\partial \delta\xi =e(ξ)−∂u∂I2∂q∂u∂δξ∂q∂δξ
其中:
-
∂ I 2 ∂ u \frac{\partial I_2}{\partial u} ∂u∂I2 为 u u u 处的像素梯度
-
∂ u ∂ q \frac{\partial u}{\partial q} ∂q∂u 为像素对投影点的导数,记 q = [ X , Y , Z ] T q=[X,Y,Z]^T q=[X,Y,Z]T,根据视觉里程计中的推导:
∂ u ∂ q = [ ∂ u ∂ X ∂ u ∂ Y ∂ u ∂ Z ∂ v ∂ X ∂ v ∂ Y ∂ v ∂ Z ] = [ f x Z 0 − f x X Z 2 0 f y Z f y Y Z 2 ] \frac{\partial u}{\partial q}=\left[ \begin{matrix} \frac{\partial u}{\partial X} &\frac{\partial u}{\partial Y}&\frac{\partial u}{\partial Z} \\ \frac{\partial v}{\partial X} &\frac{\partial v}{\partial Y}&\frac{\partial v}{\partial Z} \\ \end{matrix} \right]=\left[ \begin{matrix} \frac{f_x}{Z} &0&-\frac{f_xX}{Z^2} \\ 0 &\frac{f_y}{Z}&\frac{f_yY}{Z^2} \\ \end{matrix} \right] ∂q∂u=[∂X∂u∂X∂v∂Y∂u∂Y∂v∂Z∂u∂Z∂v]=[Zfx00Zfy−Z2fxXZ2fyY] -
为投影点对位姿的导数,李代数中已经推导:
∂ q ∂ δ ξ = [ I , − q ^ ] \frac{\partial q}{\partial \delta\xi}=[I,\hat{-q}] ∂δξ∂q=[I,−q^]
雅可比矩阵为:
J
=
−
∂
I
2
∂
u
∂
u
∂
δ
ξ
J=-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial \delta\xi}
J=−∂u∂I2∂δξ∂u
可以看出,直接法的雅可比有一个图像梯度因子,因此在图像不明显的地方对相机运动估计的贡献很小。
根据使用图像信息的不同,直接法可分为:
- 稀疏直接法,只处理稀疏角点或关键点
- 稠密直接法:使用所有像素
- 半稠密直接法:使用部分梯度明显的像素
实践:RGB-D 的直接法
稀疏直接法
我们要优化以李群方式表示的 6 自由度的相机位姿 VertexSE3Expmap。然后计算直接法中的光度误差,定义一个一元边(只估计一个误差)。
定义误差:
virtual void computeError()
{
const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
// 相机内参计算图像坐标
float x = x_local[0]*fx_/x_local[2] + cx_;
float y = x_local[1]*fy_/x_local[2] + cy_;
// check x,y is in the image
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
{
_error ( 0,0 ) = 0.0;
this->setLevel ( 1 );
}
else {// 第二个图中灰度值-第一个图的灰度值
_error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
}
}
然后写出雅可比:
Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
jacobian_uv_ksai ( 0,4 ) = 0;
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;
jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
jacobian_uv_ksai ( 1,3 ) = 0;
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
// 计算梯度
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
然后将图像的点给它,让它计算相机的位姿。用 g2o 建立优化问题,将定义的点和边放入即可。
得到图像间的匹配关系及相机位姿:

半稠密直接法
梯度大于某个值时,将其作为被测量量。

直接法的讨论
由于直接法的目标函数优化包含了像素的信息,因此图像的性质直接影响了优化的进行。如果图像间同一像素忽明忽暗,则灰度梯度不再是凸函数,会陷入局部最优解。可以构建图像金字塔,当图像分辨率很小时,图像较为模糊,灰度变化不大,非凸性变弱。因此先在粗糙的图像下计算位姿,不断提高分辨率,找到更精确的位姿。
同时直接法假设了灰度不变性,容易受到光照的影响。
优势:
- 没有特征提取和匹配的时间,不必显式估计每个点的运动,计算规模较小
- 选择的点只需要有像素梯度,不必是角点
- 可稠密或半稠密。可以选很多点
劣势:与光流相似,对图像质量要求较高。
更多推荐
所有评论(0)