【视觉SLAM十四讲】视觉里程计—直接法
本文为视觉 SLAM 学习总结，讲解直接法的相关知识。
本讲内容概要
- 光流法
- 直接法原理
直接法的引出
我们在上一讲中的特征法中，提取特征点计算描述子十分耗时，并且特征匹配使用暴力匹配的方法，时间复杂度为 O(n^2)，但计算位姿较快。我们希望找一种方法能够代替特征法中的特征点提取和匹配的过程。
我们可以通过光流 的方法寻找匹配点，也可以使用直接法 ，不匹配点对。光流描述了像素在图像中的运动，直接法附带一个相机运动模型。
光流
一般分为稀疏光流和稠密光流。稀疏以 LK 光流为代表，稠密以 HS 光流为代表，本质上是估计像素在不同时刻图像中的运动。因为 SLAM 中我们只需要对特征点进行匹配并计算位姿，因此仅介绍稀疏的 LK 光流。
设 t 时刻位于 x,y 处的像素点的灰度值为 I(x,y,t)，在 t+dt 时刻，该像素运动到 (x+dx,y+dy)。
灰度不变性假设：同一空间点的像素灰度值，在各图像中不变。则有：
I(x+dx,y+dy,t+dt)=I(x,y,t)
灰度不变假设是理想状态，实际中不成立。
对 t+dt 时刻的灰度进行泰勒展开保留一阶项：
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
由于灰度不变，则：
\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0
因此
\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}
设 \frac{dx}{dt},\frac{dy}{dt} 分别为 u,v。这是一个二元一次方程，欠定，需要引入额外的约束。假设一个窗口 (w\times w) 内光度不变，我们共有 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
记：
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\left[ \begin{matrix} u \\ v \end{matrix} \right]=-b
关于 u,v 超定线性方程的求解，传统解法是求最小二乘解（详细推导见《矩阵论》）：
\left[ \begin{matrix} u \\ v \end{matrix} \right]^*=-(A^TA)^{-1}A^Tb
我们可以将光流看作最小化像素误差的非线性优化。
每次泰勒一阶近似，在离优化点较远时效果不佳，往往需要迭代多次。
运动较大时使用图像金字塔，缩放分辨率，从粗到精
只需提取第一个图像的角点，不同计算描述子，直接跟踪像素轨迹。
实践：光流
首先我们对第一帧提取 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，可以为 0。在第一帧看到了点 P，投影为 p_1，按照初始估计， P 在第二帧投影为 p_2。
投影关系：
p_1=\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right]_1=\frac{1}{Z_1}KP
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}
为了估计相机的位姿，我们需要最小化光度误差：
e=I_1(p_1)-I_2(p_2)
优化的目标函数为：
\min\limits_\xi J(\xi)=||e||^2
我们仍然基于灰度不变假设，有许多个空间点 P：
\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})
我们关心误差 e 是如何随着相机位姿 \xi 变化的，需要分析它们的导数，使用李代数上的扰动模型，给 \exp(\xi) 左乘一个小扰动 \exp(\delta\xi)：
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)
对 \exp(\delta\hat{\xi}) 一阶泰勒展开：
≈I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K(1+\delta\hat{\xi})\exp(\hat{\xi})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)
记 q=\delta\hat{\xi}\exp(\hat{\xi})P),\quad u=\frac{1}{Z_2}Kq，为 P 在扰动后位于第二个相机坐标系下的坐标，u 为其像素坐标。
利用一阶泰勒展开有：
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(\xi)-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial q}\frac{\partial q}{\partial \delta\xi}\partial \delta\xi
其中：
\frac{\partial I_2}{\partial u} 为 u 处的像素梯度
\frac{\partial u}{\partial q} 为像素对投影点的导数，记 q=[X,Y,Z]^T，根据视觉里程计中的推导：
\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]
为投影点对位姿的导数，李代数中已经推导：
\frac{\partial q}{\partial \delta\xi}=[I,\hat{-q}]
雅可比矩阵为：
J=-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial \delta\xi}
可以看出，直接法的雅可比有一个图像梯度因子，因此在图像不明显的地方对相机运动估计的贡献很小。
根据使用图像信息的不同，直接法可分为：
- 稀疏直接法，只处理稀疏角点或关键点
- 稠密直接法：使用所有像素
- 半稠密直接法：使用部分梯度明显的像素
实践：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 建立优化问题，将定义的点和边放入即可。
得到图像间的匹配关系及相机位姿：
半稠密直接法
梯度大于某个值时，将其作为被测量量。
直接法的讨论
由于直接法的目标函数优化包含了像素的信息，因此图像的性质直接影响了优化的进行。如果图像间同一像素忽明忽暗，则灰度梯度不再是凸函数，会陷入局部最优解。可以构建图像金字塔，当图像分辨率很小时，图像较为模糊，灰度变化不大，非凸性变弱。因此先在粗糙的图像下计算位姿，不断提高分辨率，找到更精确的位姿。
同时直接法假设了灰度不变性，容易受到光照的影响。
优势：
- 没有特征提取和匹配的时间，不必显式估计每个点的运动，计算规模较小
- 选择的点只需要有像素梯度，不必是角点
- 可稠密或半稠密。可以选很多点
劣势：与光流相似，对图像质量要求较高。