视觉SLAM笔记（44） RGB-D 的直接法
视觉SLAM中的RGB-D直接法是一种基于深度图像和RGB颜色信息的里程计（Visual Odometry）方法，用于估计相机运动和构建环境地图。该方法通过优化问题求解位姿估计，在没有单目视觉或深度重建的情况下完成实时定位。
核心内容
稀疏直接法
- 以相机位姿为顶点构建图优化问题。
- 使用基于光度误差的一元边（单像素边），无需深度信息或三角测量。
- 边定义为从参考帧到当前帧像素点的光度误差，计算雅可比矩阵用于优化。
定义直接法的边- 边继承自g2o::BaseUnaryEdge模板类。
- computeError()函数计算光度误差（当前帧像素亮度与参考帧预测值之差）。
- linearizeOplus()函数计算雅可比矩阵（李代数梯度），支持图优化求解。
使用直接法估计相机运动- 在TUM公开数据集上运行程序：读取RGB-D图像序列，提取特征点或高梯度像素作为图节点。
- 使用g2o进行优化求解位姿增量。
- 绘制关键点在后续帧中的投影效果示例。
半稠密直接法- 增加更多像素作为图节点（如梯度显著或深度有效）。
- 提高优化精度但增加计算开销。
- 示例显示半稠密方法能更稳定地估计相机运动。
讨论与优缺点- 直接法则完全依赖于优化过程中的局部梯度引导。
- 优点：无复杂特征提取需求；缺点：易受图像非凸性和噪声影响；适合小步长优化场景。
该方法结合了RGB颜色信息与深度数据的优势，在资源受限环境下具有良好的实时性与鲁棒性。
视觉SLAM笔记（44） RGB-D 的直接法
- 1. Sparse Direct Method
- 2. Edge Detection in Direct Method
- 3. Estimating Camera Motion via Direct Method
- 4. Partially Dense Direct Methods
- 5. Analysis of Direct Methods
1. 稀疏直接法
今天我们将讲解如何利用稀疏的方法实现直接法。为简化程序流程，我们选择RGB-D数据而不是单目相机数据来获取深度信息。与传统的单目相机深度估计不同，在本节中我们主要关注基于特征点的深度恢复方法，并已有关于三角测量的知识已经有较为详细的介绍视觉SLAM笔记（34） 三角测量。而对于基于块匹配的方法，则将在后续内容中详细讲解。
在 RGB-D 框架下采用稀疏式直接法的 VO 实现（Visual Odometry），其核心在于通过视觉信息实现环境定位与导航任务。这种基于 Direct Method 的方法在 Direct 法中最终归结为解决一个优化问题。这些优化库（如 g2o 和 Ceres）为我们提供了有效的解决方案。以 g2o 为例开展实验研究时，则要求将该方法与图优化模型相结合。
显然，直接法是由以下顶点和边组成的：
优化目标为单个相机的位姿估计问题，则需要构建一个对应的顶点
基于李代数理论的推导框架下，在程序实现中采用 SE(3) 李群参数化的位姿顶点表示方法
如同 视觉SLAM笔记（37） 求解 PnP 中的做法，
本论文将采用 VertexSE3Expmap 作为相机的位姿表示方法
误差项仅限于单像素级别的光度误差。
在优化过程中保持不变的是变量I_1(p_1)。
通过调整相机的姿态和位置，
使得对应的I_2(p_2)接近该固定值。
这样的边仅连接一个顶点，
因此被定义为一元边。
g2o框架中并未提供直接计算光度误差的边类型。
在上述建模中
在本研究中,我们首先定义了一种用于直接法位姿估计的基础边.随后,在此基础上构建了一个基于该边的信息优化模型,并通过求解该模型完成了相应的优化任务.
2. 定义直接法的边
为计算光度误差的边界建立定义，请编写一段代码如下所示：
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3ProjectDirect() {}
EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
: x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
{}
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;
}
}
// plus in manifold
virtual void linearizeOplus( )
{
if ( level() == 1 )
{
_jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
return;
}
VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book
double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;
float u = x*fx_*invz + cx_;
float v = y*fy_*invz + cy_;
// jacobian from se3 to u,v
// NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
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;
}
// dummy read and write functions because we don't care...
virtual bool read ( std::istream& in ) {}
virtual bool write ( std::ostream& out ) const {}
protected:
// get a gray scale value from reference image (bilinear interpolated)
inline float getPixelValue ( float x, float y )
{
uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
float xx = x - floor ( x );
float yy = y - floor ( y );
return float (
( 1-xx ) * ( 1-yy ) * data[0] +
xx* ( 1-yy ) * data[1] +
( 1-xx ) *yy*data[ image_->step ] +
xx*yy*data[image_->step+1]
);
}
public:
Eigen::Vector3d x_world_; // 3D point in world frame
float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
cv::Mat* image_=nullptr; // reference image
};
AI助手该边继承自类g2o::BaseUnaryEdge。在继承过程中，请确保为该边指定测量值的维度和类型，并记录与该边相关联的顶点编号。同时，请将空间坐标点P、相机内参以及对应的图像信息存储到该边所属的成员变量中。
为了让 g2o 优化该边对应的误差，需要覆写两个虚函数：
computeError()计算误差值linearizeOplus()计算雅可比
从观察角度来看，在本研究中所进行的雅可比矩阵计算与误差相对于基于李代数的雅可比矩阵一致地呈现出相同的结果。
在程序实现中，在处理误差计算时采用了I_2(p_2) - I_1(p_1)的形式。
进而我们可以省略前者的负号，并直接将像素梯度与像素到李代数的空间梯度相乘。
在程序运行过程中，在相机的姿态信息中使用了浮点数值表示法，并将其映射至图像平面位置。为了提高图像细节的精度，在计算每个像素的亮度时需要执行插值运算。其中一种常用的方法是双线性内插法，在实际应用中还可以采用更高阶的插值算法以获得更好的效果。然而这通常会导致较高的计算开销
3. 使用直接法估计相机运动
在定义了g2o边之后，在节点与边的基础上构建图结构后, 就可以调用g2o进行优化计算了。在这个实验中, 我们主要关注并读取并解析RGB-D图像序列。作为示例, 我们采用TUM公开数据集中的数据来进行分析。基于第一个图像, 我们采用直接法来求解后续每个框架的位姿信息。首先, 在参考帧中, 通过FAST算法检测出目标区域的关键特征点(无需提取描述子), 然后利用直接法确定这些特征点在后续帧中的位置以及相机的姿态信息。通过上述步骤, 我们最终在一个二维框架内完成了特征点定位和相机姿态估计的过程, 构建了一个基础的稀疏直接方法。最后, 在第二个框架中绘制特征点的位置。
$ ./direct_sparse ../../042/data/
AI助手在绘图完成后短暂暂停以便观察特征点之间的相对位置关系的同时，在控制台中会实时显示迭代误差值的下降趋势
当两个图像十分接近时，
直接法会通过调整相机的位置和姿态，
使大部分像素能够顺利追踪。
然而，
在时间跨度稍长的情况下，
例如在0-9帧之间进行对比分析，
发现相机位置与姿态估计不够精确，
导致特征点发生明显偏移。
4. 半稠密直接法
该系统可通过扩展原有算法实现半稠密直接法形式。
在参考帧内首先提取出具有显著梯度的像素点，并利用直接法对该区域进行图像优化处理。
如图所示，在源代码文件direct\_semidense.cpp中进行如下修改：
// select the pixels with high gradiants
for (int x = 10; x < gray.cols - 10; x++)
for (int y = 10; y < gray.rows - 10; y++)
{
Eigen::Vector2d delta(
gray.ptr<uchar>(y)[x + 1] - gray.ptr<uchar>(y)[x - 1],
gray.ptr<uchar>(y + 1)[x] - gray.ptr<uchar>(y - 1)[x]
);
if (delta.norm() < 50)
continue;
ushort d = depth.ptr<ushort>(y)[x];
if (d == 0)
continue;
Eigen::Vector3d p3d = project2Dto3D(x, y, d, fx, fy, cx, cy, depth_scale);
float grayscale = float(gray.ptr<uchar>(y)[x]);
measurements.push_back(Measurement(p3d, grayscale));
}
AI助手此次改动极为微小，
将原先作为稀疏特征点处理的像素改为具有明显梯度分布的像素。
这种修改导致图优化过程中增加了大量连接。
在估计相机姿态的过程中，
这些新增加的边能够充分利用每一个像素的信息而不局限于仅依赖少数关键特征点。
这种方法被称作半稠密优化方法（Semi-dense）。
把参与估计的像素取出来并把它们在图像中显示出来
通过分析参与估计的像素信息可以看出，在空间中这些点是被固定下来的。在相机旋转的过程中可以看出这些点的位置并没有发生任何变化。这也验证了我们对相机运动的估计具有较高的准确性。此外还可以通过分析使用的像素数量与优化时间之间的关系来进一步改进算法性能。随着所使用的像素数量增加 在保证图像质量的前提下 优化时间也会相应增长 因此为了实现更高的实时性 我们必须选择具有较高精度特征的好像素 或者采取降分辨率处理的方式来减少计算负担。
5. 直接法的讨论
相较于特征点法而言，直接法完全依赖于优化算法来计算相机位姿。
我们可以代入优化算法的视角去进行设想
设想对于参考图像而言，在获取到灰度值为229的像素
同时，在已知该空间点的深度信息下，则能够确定在空间中对应的位置
此时获得了一张新的图像后，则需计算其相机姿态
该姿态是基于初始值不断优化迭代得出的
假设初始值较为粗略，在该初始值下, 空间点P经投影后的像素灰度值为126
因此该像素的误差计算结果为229−126=103
为了减小该误差, 在微调相机姿态的同时可采取措施使该像素亮度增加
如何确定往哪个方向微调可以使像素亮度增加？
这表明需要利用图像局部的梯度信息来确定调整方向。
沿着u轴移动一步导致灰度值从当前值减少到123。
同样地，在v轴上移动一步导致灰度值进一步减少到108。
在这个像素周围观察到梯度向量为 [-3, -18]。
通过优化算法对相机进行微调以使其图像在左上方方向上增强。
在这个过程中，在像素级别的局部范围内使用梯度来近似其周围的灰度分布情况。需要注意的是真实图像并非光滑连续的，在这种情况下该方法的有效性会有所下降。这表明该梯度仅在邻近区域有效。
然而，在优化算法的设计中，并非仅需关注单个像素的信息；还需综合考虑其他相邻像素的意见。经过数学推导得出该增量的具体值为 exp(ξ)。加入该增量后，在图像空间中完成一次迭代操作：原图像点 I₂被映射至 I₂′；同时相应投影点的位置也被移动至亮度更高的区域。观察到，在这一更新过程中误差值呈现出显著下降的趋势。
在理想情况下，期望误差会不断下降，最后收敛
实际情况如何？
然而，图像通常是一个很强烈的非凸函数
在实际应用中，如果遵循梯度方向移动，在面对图像自身的非凸特性（可能由噪声引起）时，容易陷入局部极小值而导致优化过程受阻。这种现象表明，在存在较大程度非凸性或噪声干扰的情况下，默认采用直接法可能会出现效果不佳的问题。因此，在相机运动幅度较小时，并且确保梯度场的非凸性较弱的情况下，直接法才具有可行性。
在实现过程中，在处理阶段仅计算了单个像素之间的差异。\n然而，在实际应用中发现单个像素缺乏足够的区分能力。\n因此，在这种情况下通常会采用小块图像块的方法，并结合更为复杂的相似度量指标。\n例如归一化相关性（NCC）等。\n相反地，在本实现中为了保持与理论推导的一致性，则采用了误差平方和作为衡量标准
参考：
相关推荐：
视觉SLAM笔记（43） 基于直接的方法
视觉SLAM笔记（42） 基于光流法的特征点追踪技术
视觉SLAM笔记（41） 光流追踪技术
视觉SLAM笔记（40） 特征点匹配中的缺陷问题
视觉SLAM笔记（39） 基于ICP算法的空间定位解决方案