1. 稀疏直接法

现在演示如何使用稀疏的直接法
为了保持程序简单，使用 RGB-D 数据而非单目数据，这样可以省略掉单目的深度恢复部分
基于特征点的深度恢复已经在视觉SLAM笔记（34）三角测量介绍过了
而基于块匹配的深度恢复将在后面介绍

所以这里考虑 RGB-D 上的稀疏直接法 VO（Visual Odometry，视觉里程计）
由于求解直接法最后等价于求解一个优化问题
因此可以使用 g2o 或 Ceres 这些优化库帮助我们求解
以 g2o 为例设计实验，在使用 g2o 之前，需要把直接法抽象成一个图优化问题

显然，直接法是由以下顶点和边组成的：

优化变量为一个相机位姿，因此需要一个位姿顶点
由于在推导中使用了李代数，故程序中使用李代数表达的 SE(3) 位姿顶点
与视觉SLAM笔记（37）求解 PnP 一样，将使用“VertexSE3Expmap”作为相机位姿
误差项为单个像素的光度误差
由于整个优化过程中 $I$ ₁ $(p$ ₁ $)$ 保持不变，可以把它当成一个固定的预设值
然后调整相机位姿，使 $I$ ₂ $(p$ ₂ $)$ 接近这个值
于是，这种边只连接一个顶点，为 一元边
由于 g2o 中本身没有计算光度误差的边，需要自己定义一种新的边

在上述的建模中，直接法图优化问题是由一个相机位姿顶点与许多条一元边组成的
如果使用稀疏的直接法，那大约会有几百至几千条这样的边；稠密直接法则会有几十万条边
优化问题对应的线性方程是计算李代数增量，本身规模不大（6×6）
所以主要的计算时间会花费在每条边的误差与雅可比的计算上

下面的实验中，先来定义一种用于直接法位姿估计的边
然后，使用该边构建图优化问题并求解之

2. 定义直接法的边

首先来定义计算光度误差的边
创建 VSLAM_note/044/direct_sparse.cpp 文件
按照视觉SLAM笔记（43）直接法的推导，还需要给出它的雅可比矩阵：

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
};

这个边继承自 g2o::BaseUnaryEdge
在继承时，需要在模板参数里填入测量值的维度、类型，以及连接此边的顶点
同时，把空间点 P、相机内参和图像存储在该边的成员变量中

为了让 g2o 优化该边对应的误差，需要覆写两个虚函数：

computeError() 计算误差值
linearizeOplus() 计算雅可比

可以看到，这里的雅可比计算与误差相对于李代数的雅可比矩阵是一致的：

在这里插入图片描述
注意在程序中的误差计算里，使用了 $I$ ₂ $(p$ ₂ $) − I$ ₁ $(p$ ₁ $)$ 的形式
因此前面的负号可以省去，只需把像素梯度乘以像素到李代数的梯度即可

在程序中，相机位姿是用浮点数表示的，投影到像素坐标也是浮点形式
为了更精细地计算像素亮度，要对图像进行插值
这里采用了简单的双线性插值，也可以使用更复杂的插值方式，但计算代价可能会变高一些

3. 使用直接法估计相机运动

定义了 g2o 边后，将节点和边组合成图，就可以调用 g2o 进行优化了
在这个实验中，读取数据集的 RGB-D 图像序列
图片依旧以视觉SLAM笔记（42）光流法跟踪特征点中的TUM 公开数据集为例
以第一个图像为参考帧，然后用直接法求解后续图像的位姿
在参考帧中，对第一张图像提取 FAST 关键点（不需要描述子）
并使用直接法估计这些关键点在第二个图像中的位置
以及第二个图像的相机位姿
这就构成了一种简单的稀疏直接法
最后，画出这些关键点在第二个图像中的投影

$ ./direct_sparse ../../042/data/

程序会在作图之后暂停，可以看到特征点的位置关系，终端也会输出迭代误差的下降过程
在这里插入图片描述 )
看到在两个图像相差不多的时候，直接法会调整相机的位姿，使得大部分像素都能够正确跟踪
但是，在稍长一点的时间内，比如说 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));
	}

这只是一个很简单的改动
把先前的稀疏特征点改成了带有明显梯度的像素
于是在图优化中会增加许多的边
这些边都会参与估计相机位姿的优化问题，利用大量的像素而不单单是稀疏的特征点
由于并没有使用所有的像素，所以这种方式又称为半稠密方法（Semi-dense）
在这里插入图片描述
把参与估计的像素取出来并把它们在图像中显示出来

可以看到参与估计的像素，像是固定在空间中一样
当相机旋转时，它们的位置似乎没有发生变化
这代表了估计的相机运动是正确的
同时，可以检查使用的像素数量与优化时间的关系
显然，当像素增多时，优化会更加费时
所以为了实时性，需要考虑使用较好的像素点，或者降低图像的分辨率

5. 直接法的讨论

相比于特征点法，直接法完全依靠优化来求解相机位姿
从式（8.16）中可以看到，像素梯度引导着优化的方向
如果想要得到正确的优化结果，就必须保证大部分像素梯度能够把优化引导到正确的方向

不妨设身处地地扮演一下优化算法
假设对于参考图像，测量到一个灰度值为 229 的像素
并且，由于知道它的深度，可以推断出空间点 $P$ 的位置
在这里插入图片描述
此时又得到了一张新的图像，需要估计它的相机位姿
这个位姿是由一个初值不断地优化迭代得到的
假设初值比较差，在这个初值下，空间点 P 投影后的像素灰度值是 126
于是，这个像素的误差为 229 − 126 = 103
为了减小这个误差，希望微调相机的位姿，使像素更亮一些

怎么知道往哪里微调，像素会更亮呢？
这就需要用到局部的像素梯度
在图像中发现，沿 $u$ 轴往前走一步，该处的灰度值变成了 123，即减去了 3
同样地，沿 v 轴往前走一步，灰度值减 18，变成 108
在这个像素周围，看到梯度是 [−3; −18]
为了提高亮度，会建议优化算法微调相机，使 $P$ 的像往左上方移动

在这个过程中，用像素的局部梯度近似了它附近的灰度分布
不过请注意真实图像并不是光滑的，所以这个梯度在远处就不成立了

但是，优化算法不能只听这个像素的一面之词，还需要听取其他像素的建议
综合听取了许多像素的意见之后，优化算法选择了一个和建议的方向偏离不远的地方
计算出一个更新量 $exp(ξ$ ^ $)$
加上更新量后，图像从 $I$ ₂ 移动到了 $I$ ₂′，像素的投影位置也变到了一个更亮的地方
看到，通过这次更新，误差变小了

在理想情况下，期望误差会不断下降，最后收敛

但是实际是不是这样呢？
是否真的只要沿着梯度方向走，就能走到一个最优值？
注意到，直接法的梯度是直接由图像梯度确定的
因此必须保证沿着图像梯度走时，灰度误差会不断下降

然而，图像通常是一个很强烈的非凸函数
在这里插入图片描述
实际当中，如果沿着图像梯度前进，很容易由于图像本身的非凸性（或噪声）落进一个局部极小值中，无法继续优化
只有当相机运动很小，图像中的梯度不会有很强的非凸性时，直接法才能成立

在例程中，只计算了单个像素的差异，并且这个差异是由灰度直接相减得到的
然而，单个像素没有什么区分性，周围很可能有好多像素和它的亮度差不多
所以，有时会使用小的图像块（patch），并且使用更复杂的差异度量方式
例如归一化相关性（Normalized Cross Correlation， NCC）等
而例程为了简单起见，使用了误差的平方和，以保持和推导的一致性

参考：

《视觉SLAM十四讲》

视觉SLAM笔记（44） RGB-D 的直接法