视觉SLAM十四讲笔记-8-1
视觉SLAM十四讲笔记-8-1
文章目录
- 视觉SLAM十四讲笔记-8-1
- 视觉里程计2
-
- 8.1 直接法的引出
- 8.2 2D光流
-
- 8.2.1 Lucas-Kanade光流
- 8.3 实践：LK光流
-
- 8.3.1 使用 LK 光流
- 8.3.2 用高斯牛顿法实现光流
- 8.3.3 多层光流
视觉里程计2
主要目标：
1.理解光流法跟踪特征点的原理；
2.理解直接法是如何估计相机位姿的；
3.实现多层直接法的计算。
直接法是视觉里程计中的另一个分支，它与特征点有很大不同。虽然它还没有成为视觉里程计的主流，但经过近几年的发展，直接法在一定程度上已经能和特征点法平分秋色。
8.1 直接法的引出
上一章中介绍了特征点法估计相机运动的方法。特征点法至少存在以下缺点：
1.关键点的提取与描述子的计算非常耗时；
2.使用特征点时，忽略了除特征点以外的所有信息。只使用特征点丢弃了大部分可能有用的图像信息；
3.相机有时会运动到特征丢失的地方，这些地方往往没有明显的纹理信息。这时可能找不到足够的匹配点来计算相机运动。
为了克服这些缺点，有以下几种思路：
1.保留特征点，但只计算关键点，不计算描述子。 同时，使用光流法(Optical Flow) 跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间。光流本身的计算时间要小于描述子的计算和匹配。(这种方法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动仍然使用对极几何、PnP 和 ICP算法。这依然会要求提取到的关键点具有可区别性)
2.只计算关键点，不计算描述子。 同时，使用直接法(Direct Method) 计算特征点在下一时刻图像中的位置。这同样可以跳过描述子的计算过程，也省去了光流的计算时间。(这种方法会根据图像的像素灰度信息同时估计相机运动和点的投影，不要求提取到的点必须是角点，甚至可以是随机的选点)
使用特征点法估计相机运动时，把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化重投影误差优化相机运动。在这个过程中需要精确地知道空间点在两个相机中投影后的像素位置。在直接法中，并不需要知道点与点之间的对应关系，而是通过最小化光度误差(Photometric error) 来求得它们。
直接法根据像素的亮度信息估计相机的运动，可以完全不用计算关键点和描述子，于是，既可以避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化(可以是渐变，不形成局部的图像梯度)，直接法就能工作。
根据像素的数量，直接法分为稀疏、稠密和半稠密三种。与特征点法只能重构稀疏特征点(稀疏地图)相比，直接法还具有恢复半稠密地图或者稠密结构的能力。
8.2 2D光流
直接法是从光流演变而来的。它们非常相似，具有相同的假设条件。光流描述了像素在图像中的运动，而直接法则附带着一个相机运动模型。为了说明直接法，先来介绍光流。
光流是一种描述像素随时间在图像之间运动的方法。
图片链接：link
如上图所示，随着时间的流逝，同一个像素会在图像中运动，希望跟踪它的运动过程。其中，计算部分像素的运动称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流以 Lucas-Kanade 光流为代表，并可以在 SLAM 中用于跟踪特征点位置。稠密光流以 Horn-Schunck 光流为代表。因此，本节主要介绍 Lucas-Kanade 光流，也称为 LK 光流。
8.2.1 Lucas-Kanade光流
在 LK 光流中，认为来自相机的图像是随时间变化的。图像可以看做时间的函数: I ( t ) I(t) I(t)。那么，一个在 t t t 时刻，位于 ( x , y ) (x,y) (x,y) 处的像素，它的灰度可以写成：
I ( x , y , t ) I(x,y,t) I(x,y,t)
这种方式把图像看成了关于位置与时间的函数，它的值域就是图像中像素的灰度。现在考虑某个固定的空间点，它在 t t t 时刻的像素坐标为 x , y x,y x,y。由于相机的运动，它的图像坐标将发生变化。希望估计这个空间点在其他时刻图像中的位置。怎么估计尼?这里要引入光流法的基本假设。
灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。
对于 t t t 时刻位于 ( x , y ) (x,y) (x,y) 处的像素，设 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)
注意灰度不变假设是一个很强的假设，实际上很可能不成立。事实上，由于物体的材质不同，像素会出现高光和阴影部分；有时，相机会自动调整曝光参数，使得图像整体变亮或者变暗。这时灰度不变假设都是不成立的，因此光流的结果也不一定可靠。暂时认为该假设成立，看看如何计算像素的运动。
对左边进行泰勒展开，保留一阶项，得：
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) \approx 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
两边除以 d t dt dt，得：
∂ 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 \frac{dx}{dt} dtdx 为像素在 x x x 轴上的运动速度，而 d y d t \frac{dy}{dt} dtdy 为像素在 y y y 轴上的运动速度，把它们记作 u , v u,v u,v。
同时， ∂ I ∂ x \frac{\partial I}{\partial x} ∂x∂I 为图像在该点处 x x x 方向的梯度， ∂ I ∂ y \frac{\partial I}{\partial y} ∂y∂I 为图像在该点处 y y y 方向的梯度，记为 I x , I y I_x,I_y Ix,Iy。把图像灰度对时间的变化量记为 I t I_t It，写成矩阵的形式：
[ I x I y ] [ u v ] = − I t \begin{bmatrix} I_x &I_y \end{bmatrix}\begin{bmatrix} u \\ v \end{bmatrix} = -I_t [IxIy][uv]=−It
想计算的是像素的运动 u , v u,v u,v，但是该式是带有两个变量的一次方程，仅凭它无法计算出 u , v u,v u,v。因此，必须引入额外的约束来计算 u , v u,v u,v。
在 LK 光流中，假设某一个窗口内的像素具有相同的运动。考虑一个大小为 w ∗ w w * w w∗w 的窗口，它含有 w 2 w^2 w2 数量的像素。该窗口内像素具有同样的运动，因此共有 w 2 w^2 w2 个方程：
[ I x I y ] k [ u v ] = − I t k , k = 1 , . . . , w 2 \begin{bmatrix} I_x &I_y \end{bmatrix}_k\begin{bmatrix} u \\ v \end{bmatrix} = -I_{tk}, 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{array}{c} {\left[I_{x}, I_{y}\right]_{1}} \\ \vdots \\ {\left[I_{x}, I_{y}\right]_{k}} \end{array}\right], b=\left[\begin{array}{c} I_{t 1} \\ \vdots \\ I_{t k} \end{array}\right] A=⎣⎢⎡[Ix,Iy]1⋮[Ix,Iy]k⎦⎥⎤,b=⎣⎢⎡It1⋮Itk⎦⎥⎤
于是整个方程为：
A [ u v ] = − b A\begin{bmatrix} u\\ v \end{bmatrix} = -b A[uv]=−b
这是一个关于 u , v u,v u,v 的超定线性方程，传统解法是求最小二乘解。最小二乘经常被用到：
[ u v ] ∗ = − ( A T A ) − 1 A T b \left[\begin{array}{l} u \\ v \end{array}\right]^{*}=-\left(A^{T} A\right)^{-1} A^{T} b [uv]∗=−(ATA)−1ATb
这样就得到了像素在图像间的运动速度 u , v u,v u,v。当 t t t 取离散时刻而不是连续时间时，可以估计某块像素在若干个图像中出现的位置。
由于像素梯度仅在局部有效，所以如果一次迭代不够好，可以迭代几次这个方程。在 SLAM 中，LK 光流经常被用来跟踪角点的运动。
8.3 实践：LK光流
8.3.1 使用 LK 光流
在本节中，用 OpenCV 的光流来追踪上面的特征点。同时也手动实现一个 LK 光流。
使用两张来自 Euroc 数据集的示例图像，在第一张图像中提取角点，然后用光流法追踪它们在第二张图像中的位置。
OpenCV 的光流在使用上十分简单，只需调用 cv::calcOpticalFlowPyrLK 函数，提供前后两张图像及对应的特征点，即可得到追踪后的点，以及各点的状态、误差。可以根据 status 变量是否为 1 来确定对应的点是否被正确追踪到。
mkdir optical_flow
cd optical_flow/
code .
//launch.json
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "g++ - 生成和调试活动文件",
"type": "cppdbg",
"request":"launch",
"program":"${workspaceFolder}/build/optical_flow",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"setupCommands": [
{
"description": "为 gdb 启动整齐打印",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
],
"preLaunchTask": "Build",
"miDebuggerPath": "/usr/bin/gdb"
}
]
}
//tasks.json
{
"version": "2.0.0",
"options":{
"cwd": "${workspaceFolder}/build" //指明在哪个文件夹下做下面这些指令
},
"tasks": [
{
"type": "shell",
"label": "cmake", //label就是这个task的名字,这个task的名字叫cmake
"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
"args":[
".."
]
},
{
"label": "make", //这个task的名字叫make
"group": {
"kind": "build",
"isDefault": true
},
"command": "make", //这个task要执行的任务是make
"args": [
]
},
{
"label": "Build",
"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
"dependsOn":[ //这个label依赖于上面两个label
"cmake",
"make"
]
}
]
}
#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)
project(OPTICALFLOW)
#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++14")
#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)
list( APPEND CMAKE_MODULE_PATH /home/lss/Downloads/g2o/cmake_modules )
set(G2O_ROOT /usr/local/include/g2o)
# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS} )
# Eigen
include_directories("/usr/include/eigen3")
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
find_package (glog 0.6.0 REQUIRED)
#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})
# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
add_executable(optical_flow optical_flow.cpp)
#链接OpenCV库,Ceres库，
target_link_libraries(optical_flow ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} glog::glog)
target_link_libraries(optical_flow Sophus::Sophus)
target_link_libraries(optical_flow ${Pangolin_LIBRARIES})
#include 
上面程序中首先用 OpenCV 的光流追踪上面的 GFTT 角点，然后用光流追踪它们在第二张图像中的位置。
8.3.2 用高斯牛顿法实现光流
单层光流：光流可以看成是一个优化问题：通过最小化灰度误差估计最优的像素偏移。
下面实现一个基于高斯牛顿法的光流。
在 OpticalFlowSingleLevel 函数中实现了单层光流函数，其中调用了 cv::paraller_for_ 并行调用 OpticalFlowTracker::calculateOpticalFlow，该函数计算指定范围内特征点的光流。这个并行 for 循环内部是 Intel tbb 库实现的，只需按照其接口，将函数本体定义出来，然后将函数作为 std::function 对象传递给它。
#include 运行结果：
在具体函数实现中，求解这样一个问题：
min Δ x , Δ y ∥ I 1 ( x , y ) − I 2 ( x + Δ x , y + Δ y ) ∥ 2 2 \min _{\Delta x, \Delta y}\left\|\mathbf{I}_{1}(x, y)-\mathbf{I}_{2}(x+\Delta x, y+\Delta y)\right\|_{2}^{2} Δx,Δymin∥I1(x,y)−I2(x+Δx,y+Δy)∥22
因此，残差为括号内部的部分，对应的雅克比为第二个图像在 x + Δ x ， y + Δ y x+\Delta x，y+\Delta y x+Δx，y+Δy 处的梯度。此外，这里的梯度也可以用第一个图像中的梯度 I 1 ( x , y ) I_1(x,y) I1(x,y) 来代替。这种代替方法称为反向光流法。在反向光流法中， I 1 ( x , y ) I_1(x,y) I1(x,y) 的梯度是保持不变的，可以在第一次迭代时保留计算的结果。在后续迭代中使用。当雅克比不变时， H H H 矩阵不变，每次迭代只需要计算残差。
8.3.3 多层光流
把光流写成优化问题，就必须假设优化的初始值靠近最优值，才能在一定程度上保障算法的收敛。如果相机运行较快，两张图像差异明显，那么单层图像光流法容易达到一个局部极小值。这种情况可以引入图像金字塔来改善。
图像金字塔是指对同一个图像进行缩放，得到不同分辨率下的图像。以原始图像作为金字塔底层，每往上一层，就对下层图像进行一定倍率的缩放，就得到了一个图像金字塔。然后，在计算光流时，先从顶层的图像开始计算，然后把上一层的追踪结果作为下一层光流的初始值。由于上层的图像相对粗糙，所以这个过程也称为由粗到精的光流。
图像金字塔和光流由粗到精的过程如下图所示，图片来源：link
由粗到精的好处在于，当原始图像的像素运动较大时，在金字塔顶层的图像来看，运动仍然在一个小的范围内。
在上述代码中添加多层光流的代码：
#include 运行结果：
LK 光流跟踪能够直接得到特征点的对应关系。这个对应关系就像是描述子的匹配，只是光流对图像的连续性和光照稳定性要求更高一些。可以用光流跟踪的特征点，用 P n P PnP PnP， I C P ICP ICP 或者对极几何来估计相机的运动。
总而言之，光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较为平滑(或者采集频率较高)。