本文为视觉 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∂I​dx+∂y∂I​dy+∂t∂I​dt
由于灰度不变,则:
∂ 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∂I​dx+∂y∂I​dy+∂t∂I​dt=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∂I​dtdx​+∂y∂I​dtdy​=−∂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 [Ix​​Iy​​]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​⋮[Ix​​Iy​]1​Iy​]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​=Z1​1​KP

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​=Z1​1​K(RP+t)=Z1​1​K(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 ξmin​J(ξ)=∣∣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}) ξmin​J(ξ)=i=1∑N​eiT​ei​,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​(Z1​1​KP)−I2​(Z2​1​Kexp(δξ^​)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​(Z1​1​KP)−I2​(Z2​1​K(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​(Z1​1​KP)−I2​(Z2​1​Kexp(ξ^​)P+Z2​1​Kδξ^​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=Z2​1​Kq,为 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​(Z1​1​KP)−I2​(Z2​1​Kexp(ξ^​)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​​]=[Zfx​​0​0Zfy​​​−Z2fx​X​Z2fy​Y​​]

  • 为投影点对位姿的导数,李代数中已经推导:
    ∂ 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 建立优化问题,将定义的点和边放入即可。

得到图像间的匹配关系及相机位姿:

在这里插入图片描述

半稠密直接法

梯度大于某个值时,将其作为被测量量。

在这里插入图片描述

直接法的讨论

由于直接法的目标函数优化包含了像素的信息,因此图像的性质直接影响了优化的进行。如果图像间同一像素忽明忽暗,则灰度梯度不再是凸函数,会陷入局部最优解。可以构建图像金字塔,当图像分辨率很小时,图像较为模糊,灰度变化不大,非凸性变弱。因此先在粗糙的图像下计算位姿,不断提高分辨率,找到更精确的位姿。

同时直接法假设了灰度不变性,容易受到光照的影响。

优势:

  • 没有特征提取和匹配的时间,不必显式估计每个点的运动,计算规模较小
  • 选择的点只需要有像素梯度,不必是角点
  • 可稠密或半稠密。可以选很多点

劣势:与光流相似,对图像质量要求较高。

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐