视觉SLAM⑧----视觉里程计Ⅱ(光流法与直接法)

目录

8.0 本章主要目标

8.1 直接法的引出

8.2 2D光流 

8.3 实践：LK光流

8.3.1 使用LK光流

8.3.3 光流实践小结

8.4 直接法 

8.4.1 直接法的推导

8.4.2 直接法的讨论

8.5 实践：直接法 

8.5.1 单层直接法

8.5.2 多层直接法 

8.5.3 结果讨论 

8.5.4 直接法优缺点总结

---

8.0 本章主要目标

1.理解光流法跟踪特征点的原理

2.理解直接法是如何估计相机位姿的

3.实现多层直接法的计算

        直接法是视觉里程计的另一个主要分支，它与特征点法有很大不同。虽然它还没有成为现在视觉里程计中的主流，但经过近几年的发展，直接法在一定程度上已经能和特征点法平分秋色。本讲，我们将介绍直接法的原理，并实现直接法中的核心部分。

8.1 直接法的引出

1.特征点法估计相机运动的缺点

Ⅰ.关键点的提取与描述子的计算非常耗时。

        实践中，SIFT目前在CPU上是无法实时计算的，而ORB也需要近20毫秒的计算时长。如果整个SLAM以30毫秒/帧的速度运行，那么一大半时间都将花在计算特征点上。
Ⅱ.使用特征点时，忽略了除特征点以外的所有信息。

        一幅图像有几十万个像素，而特征点只有几百个。只使用特征点丢弃了大部分可能有用的图像信息。
Ⅲ.相机有时会运动到特征缺失的地方，这些地方往往没有明显的纹理信息。

        例如，有时我们会面对一堵白墙，或者一个空荡荡的走廊。这些场景下特征点数量会明显减少，我们可能找不到足够的匹配点来计算相机运动。

2.如何克服这些缺点

Ⅰ.保留特征点，但只计算关键点，不计算描述子。同时，使用光流法（Optical Flow）跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，而光流本身的计算时间要小于描述子的计算与匹配。
Ⅱ.只计算关键点，不计算描述子。同时，使用直接法（Direct Method）计算特征点在下一时刻图像中的位置。这同样可以跳过描述子的计算过程，也省去了光流的计算时间。

3.光流法与直接法特点

        光流法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动时仍使用对极几何、PnP或ICP算法。这依然会要求提取到的关键点具有可区别性，即我们需要提到角点。

        在直接法中，我们会根据图像的像素灰度信息同时估计相机运动和点的投影，不要求提取到的点必须为角点，它们甚至可以是随机的选点。
        使用特征点法估计相机运动时，我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化重投影误差（Reprojection error）优化相机运动。在这个过程中，我们需要精确地知道空间点在两个相机中投影后的像素位置——这也就是我们要对特征进行匹配或跟踪的原因。同时，我们也知道，计算、匹配特征需要付出大量的计算量。相对地，在直接法中，我们并不需要知道点与点之间的对应关系，而是通过最小化光度误差（Photometric error）来求得它们。
        直接法它是为了克服特征点法的上述缺点而存在的。直接法根据像素的亮度信息估计相机的运动，可以完全不用计算关键点和描述子。既避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化(可以是渐变,不形成局部的图像梯度)直接法就能工作。根据使用像素的数量，直接法分为稀疏、稠密和半稠密三种。与特征点法只能重构稀疏特征点(稀疏地图)相比，直接法还具有恢复稠密或半稠密结构的能力。

8.2 2D光流 

1.光流与直接法的关系

        直接法是从光流演变而来的。它们非常相似，具有相同的假设条件。光流描述了像素在图像中的运动，而直接法则附带着一个相机运动模型。

2.什么是光流？ 

        光流是一种描述像素随时间在图像之间运动的方法，如图8-1所示。随着时间的流逝，同一个像素会在图像中运动，而我们希望追踪它的运动过程。

        其中，计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。

        稀疏光流以Lucas-Kanade光流为代表，并可以在SLAM中用于跟踪特征点位置。

        稠密光流以Horn-Schunck光流为代表。本节主要介绍Lucas-Kanade光流，也称为LK光流。

图8-1 LK光流法示意图

 3.Lucas-Kanade光流

        在LK光流中，我们认为来自相机的图像是随时间变化的。图像可以看作时间的函数：。那么，一个在时刻，位于处的像素，它的灰度可以写成。
        这种方式把图像看成了关于位置与时间的函数，它的值域就是图像中像素的灰度。现在考虑某个固定的空间点，它在时刻的像素坐标为。由于相机的运动，它的图像坐标将发生变化。我们希望估计这个空间点在其他时刻图像中的位置。

        怎么估计呢？引入光流法的基本假设。

        灰度不变假设:同一个空间点的像素灰度值，在各个图像中是固定不变的。
        对于时刻位于处的像素，我们设时刻它运动到处。由于灰度不变，我们有：

式8-1 灰度不变假设

         注意：灰度不变假设是一个很强的假设，实际中很可能不成立。

        事实上，由于物体的材质不同，像素会出现高光和阴影部分；有时，相机会自动调整曝光参数，使得图像整体变亮或变暗。这时灰度不变假设都是不成立的，因此光流的结果也不一定可靠。然而，从另一方面来说，所有算法都是在一定假设下工作的。如果我们什么假设都不做，就没法设计实用的算法。所以，让我们暂且认为该假设成立，看看如何计算像素的运动。
        对左边进行泰勒展开，保留一阶项，得：

式8-2 对式8-1进行泰勒展开

         因为我们假设了灰度不变，于是下一个时刻的灰度等于之前的灰度，从而：

                                                                                            (8-3)

        两边除以，有：                                                         (8-4)

        其中为像素在轴上的运动速度，而为轴上的速度，把它们记为。

        为图像在该点处方向的梯度，另一项则是在方向的梯度，记为。​

        ​​​​​​把图像灰度对时间的变化量记为，写成矩阵形式，有：

                                                                                                            (8-5)

        我们想计算的是像素的运动，但是该式是带有两个变量的一次方程，仅凭它无法计算出。因此,必须引人额外的约束来计算。在LK光流中，我们假设某一个窗口内的像素具有相同的运动。
        考虑一个大小为的窗口，它含有数量的像素。该窗口内像素具有同样的运动
，因此我们共有个方程：

                                 

式8-6 利用约束计算u和v

         记A：

                ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                                                    (8-7)

        于是方程可化为：

        这是一个关于的超定线性方程，传统解法是求最小二乘解。最小二乘经常被用到：

式8-8 最小二乘解算u，v

         这样就得到了像素在图像间的运动速度。当取离散的时刻而不是连续时间时，我们可以估计某块像素在若干个图像中出现的位置。由于像素梯度仅在局部有效，所以如果一次迭代不够好，我们会多迭代几次这个方程。在SLAM中，LK光流常被用来跟踪角点的运动。

 4.笔者推导光流法

图8-2 笔者推导

8.3 实践：LK光流

8.3.1 使用LK光流

1.代码问题及解决

代码问题：详见

图8-3 代码问题

OpenCV error: ‘CV_BGR2GRAY’ was not declared in this scope错误_熊铁树的博客-CSDN博客

 的评论3.

        使用几张示例图像，用OpenCV的光流来追踪上面的特征点。同时，也将手动实现一个LK光流，以达到加深理解的效果。我们使用两张来自Euroc数据集的示例图像，在第一张图像中提取角点，然后用光流追踪它们在第二张图像中的位置。首先，我们使用OpenCV中的LK光流：

   // use opencv's flow for validation
    vector pt1, pt2;
    for (auto &kp: kp1) pt1.push_back(kp.pt);
    vector status;
    vector error;
    cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);

        OpenCV的光流在使用上十分简单，只需调用cv::calcOpticalFlowPyrLK函数，提供前后两张图像及对应的特征点，即可得到追踪后的点，以及各点的状态、误差。我们可以根据status变量是否为1来确定对应的点是否被正确追踪到。该函数还有一些可选的参数，我们只使用默认参数。

2.用高斯牛顿法实现光流

单层光流：

        光流也可以看成一个优化问题:通过最小化灰度误差估计最优的像素偏移。所以，类似于之前实现的各种高斯牛顿法化，我们现在也来实现一个基于高斯牛顿法的光流。

/// Optical flow tracker and interface
class OpticalFlowTracker {
public:
    OpticalFlowTracker(
        const Mat &img1_,
        const Mat &img2_,
        const vector &kp1_,
        vector &kp2_,
        vector &success_,
        bool inverse_ = true, bool has_initial_ = false) :
        img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
        has_initial(has_initial_) {}

    void calculateOpticalFlow(const Range &range);

private:
    const Mat &img1;
    const Mat &img2;
    const vector &kp1;
    vector &kp2;
    vector &success;
    bool inverse = true;
    bool has_initial = false;
};
/**
 * single level optical flow
 * @param [in] img1 the first image
 * @param [in] img2 the second image
 * @param [in] kp1 keypoints in img1
 * @param [in|out] kp2 keypoints in img2, if empty, use initial guess in kp1
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse use inverse formulation?
 */
void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector &kp1,
    vector &kp2,
    vector &success,
    bool inverse = false,
    bool has_initial_guess = false
);
void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {
    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    for (size_t i = range.start; i < range.end; i++) {
        auto kp = kp1[i];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (has_initial) {
            dx = kp2[i].pt.x - kp.pt.x;
            dy = kp2[i].pt.y - kp.pt.y;
        }

        double cost = 0, lastCost = 0;
        bool succ = true; // indicate if this point succeeded

        // Gauss-Newton iterations
        Eigen::Matrix2d H = Eigen::Matrix2d::Zero();    // hessian
        Eigen::Vector2d b = Eigen::Vector2d::Zero();    // bias
        Eigen::Vector2d J;  // jacobian
        for (int iter = 0; iter < iterations; iter++) {
            if (inverse == false) {
                H = Eigen::Matrix2d::Zero();
                b = Eigen::Vector2d::Zero();
            } else {
                // only reset b
                b = Eigen::Vector2d::Zero();
            }

            cost = 0;

            // compute cost and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++) {
                    double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
                                   GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
                    if (inverse == false) {
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
                                   GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
                                   GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
                        );
                    } else if (iter == 0) {
                        // in inverse mode, J keeps same for all iterations
                        // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
                                   GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
                            0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
                                   GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
                        );
                    }
                    // compute H, b and set cost;
                    b += -error * J;
                    cost += error * error;
                    if (inverse == false || iter == 0) {
                        // also update H
                        H += J * J.transpose();
                    }
                }

            // compute update
            Eigen::Vector2d update = H.ldlt().solve(b);

            if (std::isnan(update[0])) {
                // sometimes occurred when we have a black or white patch and H is irreversible
                cout << "update is nan" << endl;
                succ = false;
                break;
            }

            if (iter > 0 && cost > lastCost) {
                break;
            }

            // update dx, dy
            dx += update[0];
            dy += update[1];
            lastCost = cost;
            succ = true;

            if (update.norm() < 1e-2) {
                // converge
                break;
            }
        }

        success[i] = succ;

        // set kp2
        kp2[i].pt = kp.pt + Point2f(dx, dy);
    }
}

        我们在OpticalFlowSingleLevel函数中实现了单层光流函数，其中调用了cv:parallel_for_并行调用OpticalFlowTracker::calculateOpticalFlow，该函数计算指定范围内特征点的光流。        

        这个并行 for循环内部是Intel tbb库实现的，我们只需按照其接口，将函数本体定义出来，然后将函数作为std::function对象传递给它。
        具体函数实现中（即 calculateOpticalFlow )，我们求解这样一个问题：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        

        因此，残差为括号内部的部分，对应的雅可比为第二个图像在处的梯度。此外，这里的梯度也可以用第一个图像的梯度来代替。这种代替的方法为反向( Inverse)光流法。在反向光流中，的梯度是保持不变的，所以我们可以在第一次迭代时保留计算的结果，在后续迭代中使用。当雅可比不变时，矩阵不变，每次迭代只需算残差，这可以节省一部分计算量。

多层光流：

       我们把光流写成了优化问题，就必须假设优化的初始值靠近最优值，才能在一定程度上保障算法的收敛。如果相机运动较快，两张图像差异较明显，那么单层图像光流法容易达到一个局部极小值。这种情况可以通过引入图像金字塔来改善。
        图像金字塔是指对同一个图像进行缩放，得到不同分辨率下的图像，如下图所示。以原始图像作为金字塔底层，每往上一层，就对下层图像进行一定倍率的缩放，就得到了一个金字塔。

        然后，在计算光流时，先从顶层的图像开始计算，然后把上一层的追踪结果，作为下一层光流的初始值。由于上层的图像相对粗糙，所以这个过程也称为由粗至精(Coarse-to-fine)的光流，也是实用光流法的通常流程。

图8-4 光流金字塔的作用

        由粗至精的好处在于，当原始图像的像素运动较大时，在金字塔顶层的图像看来，运动仍然在一个很小范围内。例如，原始图像的特征点运动了20个像素，很容易由于图像非凸性导致优化困在极小值里。但现在假设有缩放倍率为0.5倍的金字塔，那么往上两层图像里，像素运动就只有5个像素了，这时结果就明显好于直接在原始图像上优化。
        代码如下：

void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector &kp1,
    vector &kp2,
    vector &success,
    bool inverse) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    vector pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {
            Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast>(t2 - t1);
    cout << "build pyramid time: " << time_used.count() << endl;

    // coarse-to-fine LK tracking in pyramids
    vector kp1_pyr, kp2_pyr;
    for (auto &kp:kp1) {
        auto kp_top = kp;
        kp_top.pt *= scales[pyramids - 1];
        kp1_pyr.push_back(kp_top);
        kp2_pyr.push_back(kp_top);
    }

    for (int level = pyramids - 1; level >= 0; level--) {
        // from coarse to fine
        success.clear();
        t1 = chrono::steady_clock::now();
        OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);
        t2 = chrono::steady_clock::now();
        auto time_used = chrono::duration_cast>(t2 - t1);
        cout << "track pyr " << level << " cost time: " << time_used.count() << endl;

        if (level > 0) {
            for (auto &kp: kp1_pyr)
                kp.pt /= pyramid_scale;
            for (auto &kp: kp2_pyr)
                kp.pt /= pyramid_scale;
        }
    }

    for (auto &kp: kp2_pyr)
        kp2.push_back(kp);
}

        这段代码构造了一个四层的、倍率为0.5的金字塔，并调用单层光流函数实现了多层光流。在主函数中，我们分别对两张图像测试了OpenCV的光流、单层光流和多层光流的表现，计算了它们的运行时间：

liuhongwei@liuhongwei-virtual-machine:~/桌面/ch8/build$ ./optical_flow 
build pyramid time: 9.023e-05
track pyr 3 cost time: 0.00071656
track pyr 2 cost time: 0.000984707
track pyr 1 cost time: 0.00066838
track pyr 0 cost time: 0.000655536
optical flow by gauss-newton: 0.00330038
optical flow by opencv: 0.00297861

        从运行时间上看，多层光流法的耗时和OpenCV的大致相当。光流的对比图如图8-5所示。从结果图上看，多层光流与OpenCV的效果相当，单层光流要明显弱于多层光流。

图8-5 光流对比图

8.3.3 光流实践小结

        我们看到，LK光流跟踪能够直接得到特征点的对应关系。

        这个对应关系就像是描述子的匹配，只是光流对图像的连续性和光照稳定性要求更高一些。我们可以通过光流跟踪的特征点，用PnP、ICP或对极几何来估计相机运动。
        光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较平滑（或采集频率较高）。

8.4 直接法 

8.4.1 直接法的推导

        在光流中，我们会首先追踪特征点的位置，再根据这些位置确定相机的运动。这样一种两步走的方案，很难保证全局的最优性。（离散不连续）

        那能不能在后一步中，调整前一步的结果呢？例如，如果认为相机右转了15°，那么光流能不能以这个15°运动作为初始值的假设，调整光流的计算结果呢？直接法就是遵循这样的思路得到的结果。
        如下图所示，考虑某个空间点和两个时刻的相机。的世界坐标为，它在两个相机上成像，记像素坐标为。

图8-6 直接法推导

        我们的目标是求第一个相机到第二个相机的相对位姿变换。

        我们以第一个相机为参照系，设第二个相机的旋转和平移为（对应李群为)。同时，两相机的内参相同，记为。我们列写完整的投影方程：

                                                      

                                                                (8-9)

        其中是的深度，是在第二个相机坐标系下的深度，也就是的第3个坐标值。由于只能和齐次坐标相乘，所以我们乘完之后要取出前3个元素。
        回忆特征点法中，由于我们通过匹配描述子知道了的像素位置，所以可以计算重投影的位置。

        但在直接法中，由于没有特征匹配，我们无从知道哪一个与对应着同一个点。
        直接法的思路是根据当前相机的位姿估计值寻找的位置。但若相机位姿不够好，的外观和会有明显差别。于是，为了减小这个差别，我们优化相机的位姿，来寻找与更相似的。这同样可以通过解一个优化问题完成，但此时最小化的不是重投影误差，而是光度误差，也就是的两个像素的亮度误差：

         注意，这里的是一个标量。同样地，优化目标为该误差的二范数，暂时取不加权的形式，为： 

        能够做这种优化的理由，仍是基于灰度不变假设。我们假设一个空间点在各个视角下成像的灰度是不变的。我们有许多个(比如N个)空间点P，那么，整个相机位姿估计问题变为：

                                

        注意，这里的优化变量是相机位姿，而不像光流那样优化各个特征点的运动。为了求解这个优化问题，我们关心误差是如何随着相机位姿变化的，需要分析它们的导数关系。因此，定义两个中间变量：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​     

        这里的为在第二个相机坐标系下的坐标，而为它的像素坐标。显然是的函数，是的函数，从而也是的函数。考虑李代数的左扰动模型，利用一阶泰勒展开，因为:

                                                        ​​​​​​​

        所以：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        

         其中为的左扰动。我们看到，一阶导数由于链式法则分成了3项，而这3项都是容易计算的：

Ⅰ.为处的像素。

Ⅱ.为投影方程关于相机坐标系下的三维点的导数。记，根据第7讲的推导，导数为

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        

Ⅲ.为变换后的三维点对变换的导数，这在李代数一讲介绍过：

        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        

在实践中，由于后两项只与三维点有关，而与图像无关，我们经常把它合并在一起：

        这个2×6的矩阵在第7讲中也出现过。于是，我们推导出误差相对于李代数的雅可比矩阵： 

        对于N个点的问题，我们可以用这种方法计算优化问题的雅可比矩阵，然后使用高斯牛顿法或列文伯格—马夸尔特方法计算增量，迭代求解。至此，我们推导了直接法估计相机位姿的整个流程，下面通过程序来演示直接法是如何使用的。

8.4.2 直接法的讨论

        在上面的推导中，P是一个已知位置的空间点，它是怎么来的呢？

        在RGB-D相机下，我们可以把任意像素反投影到三维空间，然后投影到下一幅图像中。如果在双目相机中，那么同样可以根据视差来计算像素的深度。如果在单目相机中，这件事情要更为困难，因为我们还须考虑由P的深度带来的不确定性。详细的深度估计放到第13讲中讨论。现在我们先来考虑简单的情况，即Р深度已知的情况。
        根据P的来源，我们可以把直接法进行分类：
1.P来自于稀疏关键点，我们称之为稀疏直接法。通常，我们使用数百个至上千个关键点，并且像L-K光流那样，假设它周围像素也是不变的。这种稀疏直接法不必计算描述子，并且只使用数百个像素，因此速度最快,但只能计算稀疏的重构。
2.P来自部分像素。如果像素梯度为零，那么整项雅可比矩阵就为零，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方。这称为半稠密(Semi-Dense)的直接法，可以重构一个半稠密结构。
3.P为所有像素，称为稠密直接法。稠密重构需要计算所有像素(一般几十万至几百万个),
因此多数不能在现有的CPU上实时计算，需要GPU的加速。但是，如前面讨论的，像素梯度不明显的点，在运动估计中不会有太大贡献，在重构时也会难以估计位置。
        可以看到，从稀疏到稠密重构，都可以用直接法计算。它们的计算量是逐渐增长的。稀疏方法可以快速地求解相机位姿，而稠密方法可以建立完整地图。具体使用哪种方法，需要视机器人的应用环境而定。特别地，在低端的计算平台上，稀疏直接法可以做到非常快速的效果，适用于实时性较高且计算资源有限的场合。

8.5 实践：直接法 

8.5.1 单层直接法

        现在，我们来演示如何使用稀疏的直接法。由于不涉及GPU编程，稠密的直接法就省略了。同时，为了保持程序简单，我们使用带深度的数据而非单目数据，这样可以省略单目的深度恢复部分。基于特征点的深度恢复(即三角化)已经在第7讲介绍过，而基于块匹配的深度恢复将在后面介绍。所以本节我们考虑双目的稀疏直接法。
        求解直接法最后等价于求解一个优化问题，因此可以使用g2o或Ceres这些优化库来帮助求解，也可以自己实现高斯牛顿法。和光流类似，直接法也可以分为单层直接法和金字塔式的多层直接法。我们同样先来实现单层直接法，进而拓展到多层直接法。
        在单层直接法中，类似于并行的光流，我们也可以并行地计算每个像素点的误差和雅可比，为此我们定义一个求雅可比的类：

/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
    JacobianAccumulator(
        const cv::Mat &img1_,
        const cv::Mat &img2_,
        const VecVector2d &px_ref_,
        const vector depth_ref_,
        Sophus::SE3d &T21_) :
        img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    /// accumulate jacobians in a range
    void accumulate_jacobian(const cv::Range &range);

    /// get hessian matrix
    Matrix6d hessian() const { return H; }

    /// get bias
    Vector6d bias() const { return b; }

    /// get total cost
    double cost_func() const { return cost; }

    /// get projected points
    VecVector2d projected_points() const { return projection; }

    /// reset h, b, cost to zero
    void reset() {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector depth_ref;
    Sophus::SE3d &T21;
    VecVector2d projection; // projected points

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost = 0;
};

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector depth_ref,
    Sophus::SE3d &T21
);

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector depth_ref,
    Sophus::SE3d &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.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[img.step] +
        xx * yy * data[img.step + 1]
    );
}

int main(int argc, char **argv) {

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
    return 0;
}

void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector depth_ref,
    Sophus::SE3d &T21) {

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // solve update and put it into estimation
        Vector6d update = H.ldlt().solve(b);;
        T21 = Sophus::SE3d::exp(update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) {
            // converge
            break;
        }

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast>(t2 - t1);
    cout << "direct method for single layer: " << time_used.count() << endl;

    // plot the projected pixels here
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, cv::COLOR_BGR2GRAY);
    VecVector2d projection = jaco_accu.projected_points();
    for (size_t i = 0; i < px_ref.size(); ++i) {
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] > 0 && p_cur[1] > 0) {
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                     cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("current", img2_show);
    cv::waitKey();
}

void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {

    // parameters
    const int half_patch_size = 1;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) {

        // compute the projection in the second image
        Eigen::Vector3d point_ref =
            depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)   // depth invalid
            continue;

        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
            v > img2.rows - half_patch_size)
            continue;

        projection[i] = Eigen::Vector2d(u, v);
        double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
            Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
        cnt_good++;

        // and compute error and jacobian
        for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++) {

                double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
                               GetPixelValue(img2, u + x, v + y);
                Matrix26d J_pixel_xi;
                Eigen::Vector2d J_img_pixel;

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                J_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;

                J_img_pixel = Eigen::Vector2d(
                    0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
                    0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
                );

                // total jacobian
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();

                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
            }
    }

   if (cnt_good) {
        // set hessian, bias and cost
        unique_lock lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
}

        在这个类的accumulate_jacobian函数中，我们对指定范围内的像素点，按照之前的推导计算像素误差和雅可比矩阵，最后加到整体的H矩阵中。然后，定义一个函数来迭代这个过程：

void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector depth_ref,
    Sophus::SE3d &T21) {

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // solve update and put it into estimation
        Vector6d update = H.ldlt().solve(b);;
        T21 = Sophus::SE3d::exp(update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) {
            // converge
            break;
        }

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

        该函数根据计算的H和b，求出对应的位姿更新量，然后更新到当前的估计值上。因为我们在理论部分已经把细节都介绍清楚了，所以这部分代码看起来不会特别困难。

8.5.2 多层直接法 

        然后，类似于光流，我们再把单层直接法拓展到金字塔式的多层直接法上，用Coarse-to-fine的过程计算相对运动。这部分代码和光流的也非常相似：
        

void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector depth_ref,
    Sophus::SE3d &T21) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {
            cv::Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
    }

        需要注意的是，直接法求雅可比的时候带上了相机的内参，而当金字塔对图像进行缩放时，对应的内参也需要乘以相应的倍率。

8.5.3 结果讨论 

        最后，我们用一些示例图片测试直接法的结果。我们会用到几张KittilT4自动驾驶数据集的图像。首先，我们读取第一个图像left.png，在对应的视差图disparity.png中，计算每个像素对应的深度，然后对000001.png~000005.png这五张图像，利用直接法计算相机的位姿。为了展示直接法对特征点的不敏感性，我们随机地在第一张图像中选取一些点，不使用任何角点或特征点提取算法，来看看它的结果。

int main(int argc, char **argv) {

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
    return 0;
}

        它将输出每个图像的每层金字塔上的追踪点，并输出运行时间。
        我们简单地对直接法的迭代过程做一点解释。相比于特征点法，直接法完全依靠优化来求解相机位姿。像素梯度引导着优化的方向。如果想要得到正确的优化结果，就必须保证大部分像素梯度能够把优化引导到正确的方向。
        这是什么意思呢?我们不妨设身处地地扮演一下优化算法。假设对于参考图像，我们测量到一个灰度值为229的像素。并且，由于我们知道它的深度，可以推断出空间点P的位置。
        此时，我们又得到了一幅新的图像，需要估计它的相机位姿。这个位姿是由一个初值不断地优化迭代得到的。假设我们的初值比较差，在这个初值下，空间点P投影后的像素灰度值是126。于是，此像素的误差为229-126=103。为了减小这个误差，我们希望微调相机的位姿，使像素更亮一些。
        怎么知道往哪里微调像素会更亮呢?这就需要用到局部的像素梯度。我们在图像中发现
沿轴往前走一步，该处的灰度值变成了123，即减去了3。同样地，沿v轴往前走一步，灰度值减了18，变成108。在这个像素周围，我们看到梯度是[-3,-18]，为了提高亮度，我们会建议优化算法微调相机，使P的像往左上方移动。在这个过程中，我们用像素的局部梯度近似了它附近的灰度分布，不过真实图像并不是光滑的，所以这个梯度在远处就不成立了。
        但是，优化算法不能只听这个像素的一面之词，还需要听取其他像素的建议。综合听取了许多像素的意见之后，优化算法选择了一个和我们建议的方向偏离不远的地方，计算出一个更新量。加上更新量后，图像从移动到了，像素的投影位置也变到了一个更亮的地方。我们看到，通过这次更新，误差变小了。在理想情况下，我们期望误差会不断下降，最后收敛。

        但是实际是不是这样呢?我们是否真的只要沿着梯度方向走，就能走到一个最优值呢？注意，直接法的梯度是直接由图像梯度确定的，因此我们必须保证沿着图像梯度走时，灰度误差会不断下降。然而，图像通常是一个很强烈的非凸函数。实际中，如果我们沿着图像梯度前进，很容易由于图像本身的非凸性(或噪声)落进一个局部极小值中，无法继续优化。只有当相机运动很小，图像中的梯度不会有很强的非凸性时，直接法才能成立。
        在例程中，我们只计算了单个像素的差异，并且这个差异是由灰度直接相减得到的。然而,单个像素没有什么区分性，周围很可能有好多像素和它的亮度差不多。所以，我们有时会使用小的图像块（patch )，并且使用更复杂的差异度量方式，例如归一化相关性(Normalized CrossCorrelation，NCC)等。

8.5.4 直接法优缺点总结

1.优点

①可以省去计算特征点、描述子的时间。
②只要求有像素梯度即可，不需要特征点。因此,直接法可以在特征缺失的场合下使用。比较极端的例子是只有渐变的一幅图像。它可能无法提取角点类特征，但可以用直接法估计它的运动。这一点在实用中非常关键，因为实用场景很有可能没有很多角点可供使用。
③可以构建半稠密乃至稠密的地图，这是特征点法无法做到的。

2.缺点

①非凸性。直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小，只在运动很小时直接法才能成功。针对于此，金字塔的引入可以在一定程度上减小非凸性的影响。
②单个像素没有区分度。和它像的实在太多了！于是我们要么计算图像块，要么计算复杂的相关性。由于每个像素对改变相机运动的“意见”不一致，只能少数服从多数，以数量代替质量。所以，直接法在选点较少时的表现下降明显，我们通常建议用500个点以上

③灰度值不变是很强的假设。如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时也会出现这种情况。特征点法对光照具有一定的容忍性，而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失败。针对这一点，实用的直接法会同时估计相机的曝光参数，以便在曝光时间变化时也能工作。