视觉SLAM十四讲-高翔 第8讲 视觉里程计2
视觉里程计
1.直接法的引出
尽管特征点法在视觉里程计中占
据主流地位，研究者们认识到它至少有以下几个缺点：
- 关键点的提取与描述子的计算非常耗时。实践当中，SIFT 目前在 CPU 上是无法实时计算的，而 ORB 也需要近 20 毫秒的计算。如果整个 SLAM 以 30 毫秒/帧的速度运行，那么一大半时间都花在计算特征点上。
- 使用特征点时，忽略了除特征点以外的所有信息。一张图像有几十万个像素，而特征点只有几百个。只使用特征点丢弃了大部分可能有用的图像信息。
- 相机有时会运动到特征缺失的地方，往往这些地方没有明显的纹理信息。例如，有时我们会面对一堵白墙，或者一个空荡荡的走廓。这些场景下特征点数量会明显减少，我们可能找不到足够的匹配点来计算相机运动。
使用特征点法估计相机运动时，我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化重投影误差 （Reprojection error）来优化相机运动。在这个过程中，我们需要精确地知道空间点在两个相机中投影后的像素位置——这也就是我们为何要对特征进行匹配或跟踪的理由。同时，我们也知道，计算、匹配特征需要付出大量的计算量。相对的，在直接法中，我们并不需要知道点与点之间之间的对应关系，而是通过最小化光度误差（Photometric error）来求得它们。
2. 光流（Optical Flow）
光流是一种描述像素随着时间，在图像之间运动的方法，如图 8-1 所示。随着时间的经过，同一个像素会在图像中运动，而我们希望追踪它的运动过程。计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流以 Lucas-Kanade 光流为代表，并可以在 SLAM 中用于跟踪特征点位置。
2.1 Lucas-Kanade 光流
在 LK 光流中，我们假设某一个窗口内的像素具有相同的运动。
3. 实践：LK光流
THU数据集处理如果遇到报错，参考：
使用TUM的associate.py脚本报错AttributeError: ‘dict_keys‘ object has no attribute ‘remove‘
useLK.cpp
#include
#include CMakeLIsts.txt
cmake_minimum_required( VERSION 2.8 )
project( useLK )
set( CMAKE_BUILD_TYPE Release )
find_package( OpenCV )
include_directories( ${OpenCV_INCLUDE_DIRS} )
add_executable( useLK useLK.cpp )
target_link_libraries( useLK ${OpenCV_LIBS} )
输出结果
4. 直接法（Direct Methods）
4.1 直接法的推导
4.2 直接法的讨论
在我们上面的推导中，P 是一个已知位置的空间点，它是怎么来的呢？在 RGB-D 相机下，我们可以把任意像素反投影到三维空间，然后投影到下一个图像中。如果在单目相机中，这件事情要更为困难，因为我们还需考虑由 P 的深度带来的不确定性。详细的深度估计放到 13 讲中讨论。现在我们先来考虑简单的情况，即 P 深度已知的情况。
根据 P 的来源，我们可以把直接法进行分类：
- P 来自于稀疏关键点，我们称之为稀疏直接法。通常我们使用数百个至上千个关键点，并且像 L-K 光流那样，假设它周围像素也是不变的。这种稀疏直接法不必计算描述子，并且只使用数百个像素，因此速度最快，但只能计算稀疏的重构。
- P 来自部分像素。我们看到式（8.16）中，如果像素梯度为零，整一项雅可比就为零，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方。这称之为半稠密（Semi-Dense）的直接法，可以重构一个半稠密结构。
- P 为所有像素，称为稠密直接法。稠密重构需要计算所有像素（一般几十万至几百万个），因此多数不能在现有的 CPU 上实时计算，需要 GPU 的加速。但是，如前面所讨论的，梯度不明显的点，在运动估计中不会有太大贡献，在重构时也会难以估计位置。
5. 实践：RGB-D 的直接法
5.1 稀疏直接法
直接法是由以下顶点和边组成的：
- 优化变量为一个相机位姿，因此需要一个位姿顶点。由于我们在推导中使用了李代数，故程序中使用李代数表达的 SE(3) 位姿顶点。与上一章一样，我们将使用“Ver-texSE3Expmap”作为相机位姿。
- 误差项为单个像素的光度误差。由于整个优化过程中 I 1 (p 1 ) 保持不变，我们可以把它当成一个固定的预设值，然后调整相机位姿，使 I 2 (p 2 ) 接近这个值。于是，这种边只连接一个顶点，为一元边。由于 g2o 中本身没有计算光度误差的边，我们需要自己定义一种新的边。
direct_sparse.cpp
#include
#include > DirectBlock; // 求解的向量是6＊1的
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
optimizer.setVerbose( true );
*/
// 初始化g2o(新版本用法)
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的
std::unique_ptr<DirectBlock::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>());
std::unique_ptr<DirectBlock> solver_ptr ( new DirectBlock ( std::move(linearSolver))); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
optimizer.setVerbose( true );
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
pose->setId ( 0 );
optimizer.addVertex ( pose );
// 添加边
int id=1;
for ( Measurement m: measurements )
{
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
);
edge->setVertex ( 0, pose );
edge->setMeasurement ( m.grayscale );
edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
edge->setId ( id++ );
optimizer.addEdge ( edge );
}
cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
optimizer.initializeOptimization();
optimizer.optimize ( 30 );
Tcw = pose->estimate();
}
CMakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( directMethod )
set( CMAKE_BUILD_TYPE Release )
# 添加cmake模块路径
list( APPEND CMAKE_MODULE_PATH /home/ql/slamebook/lib/g2o/cmake_modules)
find_package( OpenCV )
include_directories( ${OpenCV_INCLUDE_DIRS} )
find_package( G2O )
include_directories( ${G2O_INCLUDE_DIRS} )
include_directories( "/usr/include/eigen3" )
set( G2O_LIBS
g2o_core g2o_types_sba g2o_solver_csparse g2o_stuff g2o_csparse_extension
)
add_executable( direct_sparse direct_sparse.cpp )
target_link_libraries( direct_sparse ${OpenCV_LIBS} ${G2O_LIBS} )
# add_executable( direct_semidense direct_semidense.cpp )
# target_link_libraries( direct_semidense ${OpenCV_LIBS} ${G2O_LIBS} )
5.6 直接法优缺点总结
最后，我们总结一下直接法的优缺点。大体来说，它的优点如下：
• 可以省去计算特征点、描述子的时间。
• 只要求有像素梯度即可，无须特征点。因此，直接法可以在特征缺失的场合下使用。比较极端的例子是只有渐变的一张图像。它可能无法提取角点类特征，但可以用直接法估计它的运动。
• 可以构建半稠密乃至稠密的地图，这是特征点法无法做到的。
另一方面，它的缺点也很明显：
• 非凸性——直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小，只在运动很小时直接法才能成功。
• 单个像素没有区分度。找一个和他像的实在太多了！——于是我们要么计算图像块，要么计算复杂的相关性。由于每个像素对改变相机运动的“意见”不一致。只能少数服从多数，以数量代替质量。
• 灰度值不变是很强的假设。如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时亦会出现这种情况。特征点法对光照具有一定的容忍性，而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失败。针对这一点，目前的直接法开始使用更细致的光度模型标定相机，以便在曝光时间变化时也能让直接法工作。