直接法里程计设计

最新推荐文章于 2024-08-04 18:19:50 发布

嘀嗒嗒~

最新推荐文章于 2024-08-04 18:19:50 发布

阅读量1k

点赞数 26

分类专栏： slam学习文章标签：计算机视觉图像处理人工智能

本文链接：https://blog.csdn.net/paul1994/article/details/136509880

版权

slam学习专栏收录该内容

6 篇文章 0 订阅

订阅专栏

文章目录

1、Lucas-Kanade 光流
2、直接法
- 2.1、直接法的数学描述
- 2.2、直接法雅可比矩阵求解
3、Lucas-Kanade 稀疏光流实践
4、直接法实践
5、光流计算视差
- 5.1、源码实现
- 5.2、测试结果
6、直接法计算视差
- 6.1、源码实现
- 6.2、测试结果

1、Lucas-Kanade 光流

https://blog.csdn.net/qq_41368247/article/details/82562165

三种光流法程序实现：

https://github.com/xingdi-eric-yuan/optical-flow

https://blog.csdn.net/xiaoyufei117122/article/details/53693627

孔径问题说明参考：

https://blog.csdn.net/u012841922/article/details/84941667

https://elvers.us/perception/aperture/

为了解决孔径问题，我们需要选择角点的程序实现：

https://blog.csdn.net/u012841922/article/details/86519833

1.2、光流法的灰度不变假设

光流问题描述
在 LK 光流中，我们认为来自相机的图像是随时间变化的。图像的灰度可以看作时间的函数： $I (t)$ 。那么，一个在 t 时刻，位于 $(x, y)$ 处的像素，它的灰度可以写成 $I (x, y, t)$ ，这种方式把图像看成了关于位置与时间的函数，它的值域就是图像中像素的灰度。
现在考虑某个固定的空间点，它在 t 时刻图像 ${I_1}$ 中的像素坐标为 $(x, y)$ 。由于相机的运动，它的图像坐标将发生变化。我们希望估计这个空间点在其他时刻图像 ${I_2}$ 中对应的像素位置。
灰度不变假设
灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。对于 t 时刻位于 $(x, y)$ 处的像素，我们设 $t + d t$ 时刻，它运动到 $(x + d x, y + d y)$ 处。由于灰度不变，我们有：
$I (x + d x, y + d y, t + d t) = I (x, y, t)$
灰度不变假设是一个很强的假设，实际当中很可能不成立。事实上，由于物体的材质不同，像素会出现高光和阴影部分；有时，相机会自动调整曝光参数，使得图像整体变亮或变暗。这些时候灰度不变假设都是不成立的，因此光流的结果也不一定可靠。
时间连续或运动是“小运动”：即时间的变化不会引起目标位置的剧烈变化，相邻帧之间位移要比较小。

1.3、Lucas-Kanade 光流解析法

解析法有3个假设
1. 亮度恒定不变(所有光流法必须满足)：目标像素在不同帧间运动时外观上是保持不变的，对于灰度图像，假设在整个被跟踪期间，像素亮度不变。亮度恒定，就是同一点随着时间的变化，其亮度不会发生改变，这是基本光流法的假定（所有光流法变种都必须满足），用于得到光流法基本方程: .
2. 小运动(所有光流法必须满足)：时间连续或者运动是“小运动”。图像运动相对于时间来说比较缓慢，实际应用中指时间变化相对图像中运动比例要足够小，这样目标在相邻帧间的运动就比较小。小运动，这个也必须满足，就是时间的变化不会引起位置的剧烈变化，这样灰度才能对位置求偏导（换句话说，小运动情况下我们才能用前后帧之间单位位置变化引起的灰度变化去近似灰度对位置的偏导数），只有满足小运动我们才能进行泰勒展开：
3. 区域一致性：同一场景中同一表面上的邻近点运动情况相似，且这些点在图像上的投影也在邻近区域。一个场景上邻近的点投影到图像上也是邻近点，且邻近点速度一致。这是Lucas-Kanade光流法特有的假定，因为光流法基本方程约束只有一个，而要求x，y方向的速度，有两个未知变量。我们假定特征点邻域内做相似运动，就可以连立n多个方程求取x，y方向的速度（n为特征点邻域总点数，包括该特征点）。
求解形式
三个偏导具体定义
Determining Optical Flow 论文第七章中具体定义三个偏导的求解方式，三个偏导都需要需要使用到2个图片的信息，具体定义如下：
Lk光流法缺陷

这个算法假设窗口内光流一致，然而这样的窗口不易选择。窗口越小，越容易出现孔径问题，窗口越大，越无法保证窗口内光流的一致性。孔径问题具体参考：

https://blog.csdn.net/u012841922/article/details/84941667

https://elvers.us/perception/aperture/

为了解决孔径问题，我们需要选择角点：

https://blog.csdn.net/u012841922/article/details/86519833

在这里插入图片描述

这个窗口内所有x轴方向的梯度为0 $I_x(i,j)=0)$ ，只能算出v，而无法算出u。

1.4、Lucas-Kanade 光流最小二乘法

正向：第一幅图是用作计算error；第二幅图用来计算偏导以及计算error；

反向：第一幅图是用作计算error和偏导；第二幅图用来计算error。

2、直接法

直接法也是基于灰度不变假设

2.1、直接法的数学描述

考虑某个空间点 P 和两个时刻的相机。 P 的世界坐标为 [X, Y, Z]，它在两个相机上成像，记非齐次像素坐标为 p1; p2。我们的目标是求第一个相机到第二个相机的相对位姿变换。我们以第一个相机为参照系，设第二个相机旋转和平移为 R， t（对应李代数为 ξ）。同时，两相机的内参相同，记为 K。为清楚起见，我们列写完整的投影方程：
$\begin{array}{l}{p_1} = {\left[ {\begin{array}{ccccccccccccccc}u\\v\\1\end{array}} \right]_1} = \frac{1}{{{z_1}}}KP,\\{p_2} = {\left[ {\begin{array}{ccccccccccccccc}u\\v\\1\end{array}} \right]_2} = \frac{1}{{{z_2}}}K(RP + t) = \frac{1}{{{z_2}}}K(\exp ({\xi ^ \wedge })){P_{1:3}}\end{array}$

其中 Z₁是 P 的深度， Z₂是 P 在第二个相机坐标系下的深度，也就是 RP + t 的第三个坐标值。由于 exp(ξ^) 只能和齐次坐标相乘，所以我们乘完之后要取出前三个元素。在直接法中，由于没有特征匹配，我们无从知道哪一个 p₂与 p₁ 对应着同一个点。直接法的思路是根据当前相机的位姿估计值，来寻找 p₂的位置。但若相机位姿不够好， p₂的外观和 p₁会有明显差别。于是，为了减小这个差别，我们优化相机的位姿，来寻找与 p₁更相似的 p₂。这同样可以通过解一个优化问题，但此时最小化的不是重投影误差，而是光度误差（Photometric Error），也就是 P 的两个像的亮度误差：

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

**能够做这种优化的理由，仍是基于灰度不变假设。**在直接法中，我们假设一个空间点在各个视角下，成像的灰度是不变的。我们有许多个（比如 N 个）空间点 Pi，那么，整个相机位姿估计问题变为：
$e = {I_1}({p_1}) - {I_2}({p_2}).$
注意这里 e 是一个标量，所以没有加粗。同样的，优化目标为该误差的二范数，暂时取不加权的形式，为：
$\mathop {min}\limits_\xi J(\xi ){\text{ }} = \parallel e{\parallel ^2}e = {I_1}({p_1}) - {I_2}({p_2}).$
能够做这种优化的理由，仍是基于灰度不变假设。在直接法中，我们假设一个空间点在各个视角下，成像的灰度是不变的。我们有许多个（比如 N 个）空间点 P_i，那么，整个相机位姿估计问题变为：
在这里插入图片描述

2.2、直接法雅可比矩阵求解

在这里插入图片描述

3、Lucas-Kanade 稀疏光流实践

光流法计算偏导时特别需要注意2个点：第一个是使用t时刻图片I1还是使用t+dt时刻的图片I2的像素，第二个是计算(x,y)位置的偏导还是(x+dx,y+dy)的位置。

正向：第一幅图是用作计算error；第二幅图用来计算偏导以及计算error；

反向：第一幅图是用作计算error和偏导；第二幅图用来计算error。

3.1、光流文献综述

按此文的分类，光流法可分为哪几类？

算法分类	应用例子	可以应用的场景
Forwards Additive(正向加性)	Lucas-Kanade (1981)	Any warp
Forwards Compositional（正向组合）	Shum-Szeliski (2000)	Any semi-group
Inverse Additive（逆向加性）	Hager-Belhumeur (1998)	Simple-linear 2D+
Inverse Compositional（逆向组合）	Baker-Matthews (2001)	Any group

forward 和 inverse 有何差别？
正向法在第二个图像处求梯度，而逆向法直接用第一个图像处的梯度来代替。如果模板中有很多噪声，则应该使用正向算法。如果输入图像中噪声更多，那就采用逆向算法。逆向算法只用计算一次Hessian矩阵，而正向算法需要在每次迭代过程中都计算一遍Hessian矩阵。因此，逆向算法更高效，计算量较小。

3.2、正向光流法

设有图像 1.png， 2.png，我们在 1.png 中使用OpenCV提取了 GFTT 角点，然后希望在 2.png中追踪这些关键点。设两个图分别为 $I_1,I_2$ ，第一张图中提取的点集为 $P={p_i}$ ，其中 ${p_i} = {\left[ {\begin{array}{ccccccccccccccc}{{x_i}}&{{y_i}}\end{array}} \right]^T}$ 为像素坐标值。考虑第 i个点，我们希望计算，满足：
在这里插入图片描述

即最小化二者灰度差的平方，其中 $\sum\limits_W {}$ 表示将某个窗口（Window）的所有像素点的灰度差求和（而不是单个像素，因为问题有两个未知量，单个像素只有一个约束，是欠定的）。实践中，取此 window 为 8×8 大小的小块，即从 x_i - 4 取到 x_i + 3， y 坐标亦然。显然，这是一个 forward-addtive 的光流，而上述最小二乘问题可以用 Gauss-Newton 迭代求解。

3.2.1、最小二乘法的像素误差定义

${e_i} = {I_1}({x_i},{y_i}) - {I_2}({x_i} + \vartriangle {x_i},{y_i} + \vartriangle {y_i})$
在最小二乘法中，我们需要求 $\vartriangle {x_i},\vartriangle {y_i}$ 使得 ${e_i}$ 的平方和最小，所以目标函数是一个代表灰度的标量函数 ${I_2}({x_i} + \vartriangle {x_i},{y_i} + \vartriangle {y_i})$ ，高斯牛顿法的关键是求取目标函数的雅可比矩阵，也就是偏导数。

3.2.2、误差相对于自变量的导数定义（雅可比矩阵求取）

待估计的变量是 $\vartriangle {x_i},\vartriangle {y_i}$ ，定义相应的导数为：
在这里插入图片描述
正向：第一幅图是用作计算error；第二幅图用来计算偏导以及计算error；
反向：第一幅图是用作计算error和偏导；第二幅图用来计算error。

3.3、反向光流法

3.3.1、正向光流法的限制

Gauss-Newton 的计算依赖于 ${I_2}$ 在 $({x_i} + \Delta {x_i},{\text{ }}{y_i} + \Delta {y_i})$ 处的梯度信息。然而，角点提取算法仅保证了 ${I_1}$ 在 $({x_i} ,{\text{ }}{y_i})$ 处是角点（可以认为角度点存在明显梯度），但对于 ${I_2}$ ，我们并没有办法假设 ${I_2}$ 在 $({x_i} + \Delta {x_i},{\text{ }}{y_i} + \Delta {y_i})$ 处亦有梯度，从而 Gauss-Newton 并不一定成立。
反向的光流法（inverse）则做了一个巧妙的技巧，即用 ${I_1}({x_i},{\text{ }}{y_i})$ 处的梯度，替换掉原本要计算的 ${I_2}({x_i} + \Delta {x_i},{\text{ }}{y_i} + \Delta {y_i})$ 的梯度。这样做的好处有：

${I_1}({x_i},{\text{ }}{y_i})$ 是GFTT 方法提取的角点，梯度总有意义；
${I_1}({x_i},{\text{ }}{y_i})$ 处的梯度不随迭代改变，所以只需计算一次，就可以在后续的迭代中一直使用，节省了大量计算时间。

3.3.2、反向光流法梯度计算

反向的光流法做了一个巧妙的技巧，即用 ${I_1}({x_i},{\text{ }}{y_i})$ 处的梯度，替换掉原本要计算的 ${I_2}({x_i} + \Delta {x_i},{\text{ }}{y_i} + \Delta {y_i})$ 处的梯度。相应的导数为：
在这里插入图片描述

3.4、正反向光流法程序实现

3.4.1、程序实现

#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <execution>

using namespace std;
using namespace cv;

// this program shows how to use optical flow

string file_1 = "./1.png";  // first image
string file_2 = "./2.png";  // second image

// 打印函数执行时间
template <typename FuncT>
void evaluate_and_call(FuncT func, const std::string &func_name = "",
                       int times = 10) {
  double total_time = 0;
  for (int i = 0; i < times; ++i) {
    auto t1 = std::chrono::steady_clock::now();
    func();
    auto t2 = std::chrono::steady_clock::now();
    total_time +=
        std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1)
            .count() *
        1000;
  }

  std::cout << "方法 " << func_name
            << " 平均调用时间/次数: " << total_time / times << "/" << times
            << " 毫秒." << std::endl;
}

// TODO implement this funciton
/**
 * 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<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse = false
);

void OpticalFlowSingleLevelMT(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse
);
// TODO implement this funciton
/**
 * multi level optical flow, scale of pyramid is set to 2 by default
 * the image pyramid will be create inside the function
 * @param [in] img1 the first pyramid
 * @param [in] img2 the second pyramid
 * @param [in] kp1 keypoints in img1
 * @param [out] kp2 keypoints in img2
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse set true to enable inverse formulation
 */
void OpticalFlowMultiLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse = false
);

/** 实际验证OpenCV提供的光流法接口计算的匹配特征点也是浮点数，打印出来是有小数点的。
 * 双线性插值法的作用一般适用于图像缩放，但此处是为了解决高斯牛顿迭代法计算dx和dy时会产生小数的问题；
 * 由于此函数只用于特征点附近8*8的计算，因此不需要考虑边界点。
 * 即像素点x=x+dx为浮点型小数，通过双线性插值就能解决这个问题。
 * get a gray scale value from reference image (bi-linear interpolated)
 * @param img
 * @param x
 * @param y
 * @return
 * 使用std::floor来获取7.8的不大于它的最大整数，即7
 */
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    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) {

    // images, note they are CV_8UC1, not CV_8UC3
    Mat img1 = imread(file_1, 0);
    Mat img2 = imread(file_2, 0);

    // key points, using GFTT here.
    vector<KeyPoint> kp1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(img1, kp1);

    // now lets track these key points in the second image
    // first use single level LK in the validation picture
    vector<KeyPoint> kp2_single;
    vector<KeyPoint> kp2_singleMT;
    vector<bool> success_single;
    // OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);
    OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single, true);
    evaluate_and_call([&]() { OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single, true); },
                        "OpticalFlowSingleLevel time", 1);
    evaluate_and_call([&]() { OpticalFlowSingleLevelMT(img1, img2, kp1, kp2_singleMT, success_single, true); },
                        "OpticalFlowSingleLevelMT time", 1);
    // then test multi-level LK
    vector<KeyPoint> kp2_multi;
    vector<bool> success_multi;
    OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi);

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

    // plot the differences of those functions  CV_GRAY2BGR 改成  cv::COLOR_GRAY2BGR
    Mat img2_single;
    cv::cvtColor(img2, img2_single, cv::COLOR_GRAY2BGR);
    for (int i = 0; i < kp2_single.size(); i++) {
        if (success_single[i]) {
            cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 1);
            cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
        }
        // std::cout << "kp2:" << i << "coordinates: (" << kp2_single[i].pt.x << ", " << kp2_single[i].pt.y << ")" << std::endl; 
    }

    Mat img2_multi;
    cv::cvtColor(img2, img2_multi, cv::COLOR_GRAY2BGR);
    for (int i = 0; i < kp2_multi.size(); i++) {
        if (success_multi[i]) {
            cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 1);
            cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat img2_CV;
    cv::cvtColor(img2, img2_CV, cv::COLOR_GRAY2BGR);
    for (int i = 0; i < pt2.size(); i++) {
        if (status[i]) {
            cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 1);
            cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
        }
        /* OpenCV金字塔流光法计算出来匹配点也是浮点数，需要想办法变成整数*/
        // std::cout << "cv:kp2:" << i << "coordinates: (" << pt2[i].x << ", " << pt2[i].y << ")" << std::endl; 
    }

    cv::imshow("single level", img2_single);
    cv::imwrite("single_pk2.jpg", img2_single);
    cv::imshow("multi level", img2_multi);
    cv::imwrite("multi_pk2.jpg", img2_multi);
    cv::imshow("opencv", img2_CV);
    cv::imwrite("OpenCV_pk2.jpg", img2_CV);
    cv::waitKey(0);

    return 0;
}

/* 
功能：单层流光法实现
计算的假设条件：
1、单个点的灰度不变性
2、8*8区域内点的dx和dy相同，即运动偏移相同，所以dx和dy的一维变量
已知入参：
    2张灰度图片 img1，img2；
    一张灰度图片特征点：kp1
未知解参：
    第2张图片对应的特征点 kp2；
    每个特征点的匹配情况：success
设置参数：inverse：正向/反向
 */
void OpticalFlowSingleLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse
) {

    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    bool have_initial = !kp2.empty();

    for (size_t i = 0; i < kp1.size(); i++) {
        auto kp = kp1[i];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (have_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
        for (int iter = 0; iter < iterations; iter++) {
            Eigen::Matrix2d H = Eigen::Matrix2d::Zero();    // Hessian矩阵
            Eigen::Vector2d b = Eigen::Vector2d::Zero();    // bias
            cost = 0;

            if (kp.pt.x + dx <= half_patch_size || kp.pt.x + dx >= img1.cols - half_patch_size ||
                kp.pt.y + dy <= half_patch_size || kp.pt.y + dy >= img1.rows - half_patch_size) {   // go outside
                succ = false;
                break;
            }

            // 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++) {

                    // TODO START YOUR CODE HERE (~8 lines)
                    double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y+y) - GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);
                    Eigen::Vector2d J;  // Jacobian
                    if (inverse == false) {
                        // Forward Jacobian
						J = -1.0 * Eigen::Vector2d(
							0.5*( GetPixelValue(img2, kp.pt.x + x+ dx + 1, kp.pt.y+y+dy) - GetPixelValue(img2, kp.pt.x + x + dx-1 , kp.pt.y + y + dy)),
							0.5*( GetPixelValue(img2, kp.pt.x + x+dx, kp.pt.y+y+dy +1) - GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy-1))
						);
                        /* 注意这里是指反向光流，由于使用imag1的梯度，因此不同点计算的雅可比是不同的，但是每次迭代时都是一样的，
                           每次迭代此雅可比不需要更新，等效于 (inverse == false && iter == 0)  */
                    } else if(iter == 0){
                        // Inverse Jacobian
                        // 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;
                    /*  */
                    if(inverse==false||iter==0){
                        H += J * J.transpose();
                    }
                    b += -error * J;
                    cost += error * error;
                    // TODO END YOUR CODE HERE
                }

            // compute update
            // TODO START YOUR CODE HERE (~1 lines)
            Eigen::Vector2d update = H.ldlt().solve(b);
            // TODO END YOUR CODE HERE

            if (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) {
                // cout << "cost increased: " << cost << ", " << lastCost << endl;
                break;
            }

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

        success.push_back(succ);

        // set kp2，如果 kp2 已经初始化了且分配了空间，则可以使用数组方式赋值，如果没有初始化则只能 push_back
        if (have_initial) {
            kp2[i].pt = kp.pt + Point2f(dx, dy);
        } else {
            KeyPoint tracked = kp;
            tracked.pt += cv::Point2f(dx, dy);
            kp2.push_back(tracked);
        }
    }
}

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

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

    /* create pyramids，创建图像金字塔，pyr[0]代表原图，pyr[1]代表原图/2，pyr[2]代表原图/4，pyr[3]代表原图/8， */
    vector<Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE (~8 lines)
    for (int i = 0; i < pyramids; i++) {
        Mat img1temp, img2temp;
        if(i == 0){
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        }else{
            /* 图像的长宽都缩小到一半，针对小数采用双线性插值法处理 */
            cv::resize(pyr1[i-1], img1temp,
                       Size(pyr1[i-1].cols * pyramid_scale, pyr1[i-1].rows * pyramid_scale));//Size_(_Tp _width, _Tp _height);
            cv::resize(pyr2[i-1], img2temp,
                       Size(pyr2[i-1].cols * pyramid_scale, pyr2[i-1].rows * pyramid_scale));//Size_(_Tp _width, _Tp _height);
            pyr1.push_back(img1temp);
            pyr2.push_back(img2temp);
        }
    }
    /* 关键点进行缩放，原图的特征点坐标分别乘以4层的缩放因子（1，0.5，0.25，0.125）得到4层的特征点，
        小数部分不处理，因为光流法计算本身就有小数点的情况。 */
    vector<KeyPoint> 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);
    }  
    // TODO END YOUR CODE HERE

    // coarse-to-fine LK tracking in pyramids
    // TODO START YOUR CODE HERE
    for(int level = pyramids-1; level >= 0; level--)
    {
       //from coarse to fine //由粗至精， 自顶向下
       success.clear();
        OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse);
        if(level > 0)
        {   /* 将上一次计算的2幅图片的关键点坐标乘2，得到下一层关键点坐标的初始值 */
            for(auto &kp : kp1_pyr)
                kp.pt /= pyramid_scale;
            for(auto &kp : kp2_pyr)
                kp.pt /= pyramid_scale;
        }
    }
    /* 将最终得到的第二幅图片的关键点坐标更新到 kp2 */
	for (auto &kp: kp2_pyr)
        kp2.push_back(kp);
    // TODO END YOUR CODE HERE
    // don't forget to set the results into kp2
}

/* 多线程并行化 */
void OpticalFlowSingleLevelMT(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse
) {
    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    bool have_initial = !kp2.empty();

    /* 这里给kp2分配空间以及设置vector内元素的数量，以便下面能使用数组下标方式访问 */
    if(!have_initial) {
        kp2.reserve(kp1.size());
        kp2.resize(kp1.size());
    }

    /* 并行化，使用一个int型的vector来做并行化的迭代器 */
    vector<int> indexs;
    for (int i = 0; i < kp1.size(); i++) {
        indexs.push_back(i);
    }

    for_each(execution::par_unseq, indexs.begin(), indexs.end(),
             [&] (auto &i){
        auto kp = kp1[i];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (have_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
            {
                H = Eigen::Matrix2d::Zero();
            }
            b = Eigen::Vector2d::Zero();
            cost = 0;

            if (kp.pt.x + dx <= half_patch_size || kp.pt.x + dx >= img1.cols - half_patch_size ||
                kp.pt.y + dy <= half_patch_size || kp.pt.y + dy >= img1.rows - half_patch_size) {   // go outside
                succ = false;
                break;
            }

            // 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++) {

                    // TODO START YOUR CODE HERE (~8 lines)
                    double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y+y) - GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);

                    if (inverse == false) {
                        // Forward Jacobian
                        J = -1.0 * Eigen::Vector2d(
                                0.5*( GetPixelValue(img2, kp.pt.x + x+ dx + 1, kp.pt.y+y+dy) - GetPixelValue(img2, kp.pt.x + x + dx-1 , kp.pt.y + y + dy) ),
                                0.5*( GetPixelValue(img2, kp.pt.x + x+dx, kp.pt.y+y+dy +1) - GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy-1) )
                        );

                    } else if(iter == 0){
                        // Inverse Jacobian
                        // 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;
                    if(inverse==false || iter==0){
                        H += J * J.transpose();
                    }
                    b += -error * J;
                    cost += error * error;
                    // TODO END YOUR CODE HERE
                }

            // compute update
            // TODO START YOUR CODE HERE (~1 lines)
            Eigen::Vector2d update = H.ldlt().solve(b);
            // TODO END YOUR CODE HERE

            if (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) {
                //cout << "cost increased: " << cost << ", " << lastCost << endl;
                break;
            }

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

        success.push_back(succ);

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

#if 0
void cv::calcOpticalFlowPyrLK	(	InputArray 	prevImg,
InputArray 	nextImg,
InputArray 	prevPts,
InputOutputArray 	nextPts,
OutputArray 	status,             /* 每个特征点的匹配状态，匹配成功则相应bit置1 */
OutputArray 	err,
Size 	winSize = Size(21, 21),     /* 光流法计算需要使用一个小窗口的数据点，窗口大小指定为cv::Size(8, 8)  */
int 	maxLevel = 3,               /* 金字塔层数，设置为3则为4层图像 */
TermCriteria 	criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),   /* 光流法计算迭代的终止条件，可以指定次数会误差 */
int 	flags = 0,
double 	minEigThreshold = 1e-4 
)	

使用实例：
// use opencv's flow for validation
vector<Point2f> pt1, pt2;
for (auto &kp: kp1) pt1.push_back(kp.pt);
vector<uchar> status;
vector<float> error;
cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error, cv::Size(8, 8));
#endif

3.4.2、测试结果

单层正向光流法的计算结果与OpenCV计算结果对比：

左侧图片是正向光流法计算结果，右侧图片是OpenCV计算结果；红框中可以看出单层正向光流的运动方向相对比较混乱，而OpenCV方向比较统一，按理同一个物体的运动方向是一致的，因此单层正向光流的计算效果并不好。
单层反向光流法的计算结果与OpenCV计算结果对比：

左侧图片是反向光流法计算结果，右侧图片是OpenCV计算结果；红框中可以看出单层反向光流的运动量很少，很不明显，而OpenCV运动量比较明显且方向统一，因此单层反向光流的计算效果并不好。

3.5、金字塔光流法

3.5.1、所谓 coarse-to-fine 是指怎样的过程

图像金字塔是指对一个图像进行缩放，得到不同分辨率下的图像，如下图所示。以原始图像作为金字塔底层，每往上一层，就对下一层图像进行一定倍率的缩放，就得到了一个金字塔。然后，在计算光流时，先从顶层的图像开始计算，然后把上一层的追踪结果，作为下一层光流的初始值。由于上层图像相对粗糙，所以这个过程也称为由粗至精（coarseto-fine）的光流。
金字塔光流法使用了上次单层lk光流的程序，直接分了4个图层，进行了4次单层lk光流法的计算，实现过程注意以下问题：

金字塔图像使用cv::resize实现，程序中使用4层（加上原图四层）。
原图提取的特征点如何处理，像素坐标为小数如何处理；原图的特征点坐标分别乘以4层的缩放因子（1，0.5，0.25，0.125）得到4层的特征点，小数部分不处理，因为光流法计算本身就有小数点的情况。
dx和dy在每层如何传递下来。不传递dx和dy，而是直接传递2幅图片的特征点坐标（小数类型），然后将特征点坐标乘以2得到下一层的关键点坐标初始值，重新计算。

3.5.2、光流法中的金字塔用途和特征点法中的金字塔有何差别?

光流法中使用图像金字塔，可以使得优化过程更容易跳出，由于原始图像像素运动较大导致的优化结果困在极小值的情况。
而特征点法金子塔中，金字塔的作用是解决Fast角点，由于远处看着像角点的地方，近处就可能不是角点的尺度问题，给Fast角点增加尺度不变性。

3.5.3、程序实现

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

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

    /* create pyramids，创建图像金字塔，pyr[0]代表原图，pyr[1]代表原图/2，pyr[2]代表原图/4，pyr[3]代表原图/8， */
    vector<Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE (~8 lines)
    for (int i = 0; i < pyramids; i++) {
        Mat img1temp, img2temp;
        if(i == 0){
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        }else{
            /* 图像的长宽都缩小到一半，针对小数采用双线性插值法处理 */
            cv::resize(pyr1[i-1], img1temp,
                       Size(pyr1[i-1].cols * pyramid_scale, pyr1[i-1].rows * pyramid_scale));//Size_(_Tp _width, _Tp _height);
            cv::resize(pyr2[i-1], img2temp,
                       Size(pyr2[i-1].cols * pyramid_scale, pyr2[i-1].rows * pyramid_scale));//Size_(_Tp _width, _Tp _height);
            pyr1.push_back(img1temp);
            pyr2.push_back(img2temp);
        }
    }

    /* 关键点进行缩放，原图的特征点坐标分别乘以4层的缩放因子（1，0.5，0.25，0.125）得到4层的特征点，
        小数部分不处理，因为光流法计算本身就有小数点的情况。 */
    vector<KeyPoint> 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);
    }  
    // TODO END YOUR CODE HERE

    // coarse-to-fine LK tracking in pyramids
    // TODO START YOUR CODE HERE
    for(int level = pyramids-1; level >= 0; level--)
    {
       //from coarse to fine //由粗至精， 自顶向下
       success.clear();
        OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse);
        if(level > 0)
        {   /* 将上一次计算的2幅图片的关键点坐标乘2，得到下一层关键点坐标的初始值 */
            for(auto &kp : kp1_pyr)
                kp.pt /= pyramid_scale;
            for(auto &kp : kp2_pyr)
                kp.pt /= pyramid_scale;
        }
    }
    /* 将最终得到的第二幅图片的关键点坐标更新到 kp2 */
	for (auto &kp: kp2_pyr)
        kp2.push_back(kp);
    // TODO END YOUR CODE HERE
    // don't forget to set the results into kp2
}

3.5.4、测试结果

正向4层金字塔光流法

左图是自己实现的4层金字塔正向光流法的计算结果，右图是OpenCV的4层金字塔计算结果，对比2个图片发现实现效果很接近，效果都比较好。
反向4层金字塔光流法
左图是自己实现的4层金字塔反向光流法的计算结果，右图是OpenCV的4层金字塔计算结果，对比2个图片发现实现效果很接近，效果都比较好。

3.6、并行化

3.6.1、并行化注意事项

并行化层次，计算光流法时有多个层次的循环（多个关键点计算，每个关键点迭代多次，每次迭代要计算W2个点）；一般都是选取最大层次，并且相互独立的层次进行并行化。并行化实现过程注意细节：

C++中的vector在并行化情况下需要使用数组下标方式访问，而实现数组下标方式访问有2种方式。
a. 要求有下面2点：reserve函数用于分配足够的空间，resize用于告诉vector里面存在元素；如果resize不执行，那数组访问方式 kp2[i]将无法访问读写数据，因为vector认为里面0个元素。
```
    kp2.reserve(kp1.size());
    kp2.resize(kp1.size());
```
b. 定义vector的时候确认元素大小以及初始化，以int类型举例；
```
std::vector\<int\> vec(10,2);
```
c. 获取定义一个vector，然后使用 desc.push_back(d)来确定vector的空间以及大小。
并行化也可以使用原子变量 std::atomic<int> atm_count(0) 的方式作为vector的数组下标进行访问。

3.6.2、并行化源码实现

src\optical_flow.cpp

/* 多线程并行化 */
void OpticalFlowSingleLevelMT(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse
) {
    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    bool have_initial = !kp2.empty();

    /* 这里给kp2分配空间以及设置vector内元素的数量，以便下面能使用数组下标方式访问 */
    if(!have_initial) {
        kp2.reserve(kp1.size());
        kp2.resize(kp1.size());
    }

    /* 并行化，使用一个int型的vector来做并行化的迭代器 */
    vector<int> indexs;
    for (int i = 0; i < kp1.size(); i++) {
        indexs.push_back(i);
    }

    for_each(execution::par_unseq, indexs.begin(), indexs.end(),
             [&] (auto &i){
        auto kp = kp1[i];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (have_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
            {
                H = Eigen::Matrix2d::Zero();
            }
            b = Eigen::Vector2d::Zero();
            cost = 0;

            if (kp.pt.x + dx <= half_patch_size || kp.pt.x + dx >= img1.cols - half_patch_size ||
                kp.pt.y + dy <= half_patch_size || kp.pt.y + dy >= img1.rows - half_patch_size) {   // go outside
                succ = false;
                break;
            }

            // 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++) {

                    // TODO START YOUR CODE HERE (~8 lines)
                    double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y+y) - GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);

                    if (inverse == false) {
                        // Forward Jacobian
                        J = -1.0 * Eigen::Vector2d(
                                0.5*( GetPixelValue(img2, kp.pt.x + x+ dx + 1, kp.pt.y+y+dy) - GetPixelValue(img2, kp.pt.x + x + dx-1 , kp.pt.y + y + dy) ),
                                0.5*( GetPixelValue(img2, kp.pt.x + x+dx, kp.pt.y+y+dy +1) - GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy-1) )
                        );

                    } else if(iter == 0){
                        // Inverse Jacobian
                        // 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;
                    if(inverse==false || iter==0){
                        H += J * J.transpose();
                    }
                    b += -error * J;
                    cost += error * error;
                    // TODO END YOUR CODE HERE
                }

            // compute update
            // TODO START YOUR CODE HERE (~1 lines)
            Eigen::Vector2d update = H.ldlt().solve(b);
            // TODO END YOUR CODE HERE

            if (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) {
                //cout << "cost increased: " << cost << ", " << lastCost << endl;
                break;
            }

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

        success.push_back(succ);

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

src\CMakeLists.txt

find_package(OpenCV REQUIRED)

IF(OpenCV_FOUND)

MESSAGE(STATUS "Found OpenCV: \${OpenCV_LIBRARIES}")

include_directories(\${OpenCV_INCLUDE_DIRS})

add_executable(lk optical_flow.cpp)

target_link_libraries(lk \${OpenCV_LIBRARIES})

target_link_libraries(lk tbb)

ENDIF(OpenCV_FOUND)

3.6.3、并行化比较结果

并行化能极大缩小程序执行的时间。

第一次测试

方法 OpticalFlowSingleLevel time 平均调用时间/次数: 174.985/1 毫秒.

方法 OpticalFlowSingleLevelMT time 平均调用时间/次数: 95.9404/1 毫秒.

第二次测试

xb@ukylin:~/learn/practice/p6-vo2/build/bin$ ./lk

方法 OpticalFlowSingleLevel time 平均调用时间/次数: 167.264/1 毫秒.

方法 OpticalFlowSingleLevelMT time 平均调用时间/次数: 106.17/1 毫秒.

第三次测试

xb@ukylin:~/learn/practice/p6-vo2/build/bin$ ./lk

方法 OpticalFlowSingleLevel time 平均调用时间/次数: 175.332/1 毫秒.

方法 OpticalFlowSingleLevelMT time 平均调用时间/次数: 68.0124/1 毫秒.

第四次测试

xb@ukylin:~/learn/practice/p6-vo2/build/bin$ ./lk

方法 OpticalFlowSingleLevel time 平均调用时间/次数: 113.757/1 毫秒.

方法 OpticalFlowSingleLevelMT time 平均调用时间/次数: 26.7483/1 毫秒.

3.7、讨论

我们优化两个图像块的灰度之差真的合理吗?哪些时候不够合理?你有解决办法吗?
优化两个图像块的灰度值之差是在两图灰度值不会变的强前提之下的，如果两张图片中，色彩变化剧烈，那么光流发可能效果就不会那么合理。高博课中提到，可以使用图像灰度相对变化来进行光流法，对图像中每个像素值减去均值，得到相对变化来做光流法。
图像块大小是否有明显差异?取 16x16 和 8x8 的图像块会让结果发生变化吗?
如下图所示，分别为8×8与16×16的图像块时，利用opencv处理所的的结果。本实验中，8×8与16×16的图像块并无太大差异。推测可能是由于本题的两张图片中像素变化范围并不大，如果运动范围很大，那么可能调整图像块会有一定影响。

可以看出，肯定是倍率越小，光流法得到的点就越少，但是倍率的影响要比金字塔的影响更快，在倍率为0.25时，直接没能找到相应的点。

4、直接法实践

注意：直接法要求获取空间点的深度信息进行计算，当前程序是根据参考图像和视差信息计算出空间点的三维坐标，然后将坐标乘以估计的位姿T，获取空间点在当前图像的三维坐标。

4.1、单层直接法

4.1.1、单层法理论计算

在1.2章节有详细计算过程

4.1.2、作业问题回答

单层直接法的误差项是什么
在上一题的光流法中，我们是进行了特征匹配的，相当于知道了点P在两张图片上的投影像素位置p1,p2 ，所以可以计算重投影的位置。而直接法是事先不知道点p1对应哪个p2 ，根据当前相机位置的估计值来寻找点p2的位置，进而通过对比两个点的光度误差来优化相机位姿。本题的误差项是光度误差：
误差相对于自变量的雅克比维度是多少？如何求解？
误差相对于自变量的雅可比维度为2×6，具体计算方法如下：
窗口可以取多大？是否可以取单个点？
程序实现中我们使用均匀分布获取了1000个点，其实有这1000个单点做最小二乘拟合是足够的；因为该方法不去要做特征点匹配，该方法需要优化的是一个点的光度误差，而不是某个特定点的光度误差。
代码中针对每个点都选取了8×8的窗口，因此程序实际取点数为1000*（8*8）个点，将这6.4万个点进行拟合计算，每个点取8*8的窗口有什么作用，是否有必要

作用：根据程序实现，在计算雅可比矩阵时分为2部分：第1部分是图像灰度梯度，第2部分是像素点对位姿变换的导数；针对每个关键点的窗口点进行计算时，第2部分是不变的，但是第1部分的图像灰度梯度窗口内8*8个点都是不一样的，所以在每个点取8*8的窗口作用是保证图像梯度的数值更加合理，鲁棒性更强；
必要性：但是图像灰度梯度的获取方式有很多更好的方法，没必要取8*8的窗口进行迭代，花了大代价而收获不多。比如：Sobel 算子是一个离散微分算子 (discrete differentiation operator)，它用来计算图像灰度函数的近似梯度，是一种联合高斯平滑加微分运算，对噪声有较强的抵抗能力。

4.1.3、直接法程序实现

src\direct_method.cpp

#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <Eigen/Core>
#include <vector>
#include <string>
#include <boost/format.hpp>
// #include <pangolin/pangolin.h>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
// 内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线
double baseline = 0.573;
// paths
string left_file = "./left.png";
string disparity_file = "./disparity.png";
boost::format fmt_others("./%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// TODO implement this function
/**
 * 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<double> depth_ref,
        Sophus::SE3d &T21
);

// TODO implement this function
/**
 * 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<double> depth_ref,
        Sophus::SE3d &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    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 = 1000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        /* 产生1000个随机点的坐标，为了避免取到像素边界，因此像素点坐标范围为 （40，图像行/列像素数量-40）
        RNG::uniform()产生一个均匀分布的随机数;          RNG::gaussian()产生一个高斯分布的随机数 */
        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<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        /* 得到1000个像素点以及像素点对应的深度 */
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    /* 默认值为的R为单位矩阵，即没有旋转；偏移为(0,0,0)，即没有偏移 */
    Sophus::SE3d T_cur_ref;
    // std::cout << "T_def = \n" << T_cur_ref.matrix() << std::endl;
    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);    // first you need to test single layer
        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<double> depth_ref,
        Sophus::SE3d &T21
) {

    // parameters
    int half_patch_size = 4;
    int iterations = 100;

    double cost = 0, lastCost = 0;
    int nGood = 0;  // good projections
    VecVector2d goodProjection;

    for (int iter = 0; iter < iterations; iter++) {
        nGood = 0;
        goodProjection.clear();

        // Define Hessian and bias
        Matrix6d H = Matrix6d::Zero();  // 6x6 Hessian
        Vector6d b = Vector6d::Zero();  // 6x1 bias

        for (size_t i = 0; i < px_ref.size(); i++) {

            // compute the projection in the second image
            // TODO START YOUR CODE HERE
            /* 在t时刻选取的1000个参考点像素坐标和深度信息转换为实际相机坐标 */
            Eigen::Vector3d point_ref = depth_ref[i] * Eigen::Vector3d((px_ref[i][0]-cx)/fx, (px_ref[i][1]-cy)/fy, 1);
            /* 根据相机t时刻的参考点坐标point_ref和相机的运动位姿T21,计算t+1时刻P点的相机坐标 point_cur */
            Eigen::Vector3d point_cur = T21 * point_ref;
            if(point_cur[2]<0)
                continue; //深度无效

            /* 将t+1时刻P点的相机坐标 point_cur转换为像素坐标，然后就能计算2个图片的梯度信息 */
            float u =0, v = 0;
            u = fx * point_cur[0]/point_cur[2] +cx;
            v = fy * point_cur[1]/point_cur[2] +cy;
            /* 边界判断，因为选点就考虑过边界问题，rng.uniform(boarder, left_img.cols - boarder); 所以这里不会产生边界问题；
            由于相机运动，参考图像中的点可能在投影之后，跑到后续图像的外部，所以这里也要做img2的边界校验。最后的目标函数要对投影在内部的点求平均误差，而不是对所有点求平均。 */
            if(u<half_patch_size || u>img2.cols-half_patch_size || v<half_patch_size || v>img2.rows - half_patch_size)
                continue;

            nGood++;
            goodProjection.push_back(Eigen::Vector2d(u, v));
            /* （X,Y,Z）为t+1时刻P点在相机坐标系中的坐标 */
            float X = point_cur[0], Y = point_cur[1], Z = point_cur[2];
            float Z2 = Z*Z, Z_inv = 1.0/Z, Z2_inv = Z_inv*Z_inv;

            // and compute error and jacobian
            /* 用于最小二乘法的1000个均匀分布的关键点其实够了，这里针对每个关键点又取了一个8*8的窗口计算，相当于一共有1000*（8*8）个点；每个点取8*8的窗口有什么作用，是否有必要；
                根据程序实现，在计算雅可比矩阵时分为2部分：第1部分是图像灰度梯度，第2部分是像素点对位姿变换的导数；
                下面程序实现针对每个关键点的第2部分是不变的，但是第1部分的图像灰度梯度窗口内8*8个点都是不一样的，所以下面程序的作用是保证图像梯度的数值更加合理，鲁棒性更强；
                但是图像灰度梯度的获取方式有很多更好的方法，没必要取8*8的窗口进行迭代，花了大代价而收获不多。比如：
                Sobel 算子是一个离散微分算子 (discrete differentiation operator)，它用来计算图像灰度函数的近似梯度，是一种联合高斯平滑加微分运算，对噪声有较强的抵抗能力。
                 */
            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;   // pixel to \xi in Lie algebra
                    Eigen::Vector2d J_img_pixel;    // image gradients
                    /* 雅可比分为2部分，第一部分是图像灰度梯度，第二部分是像素点对位姿变化的导数，这里是计算第二部分 */
                    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;
                    /* 雅可比分为2部分，第一部分是图像灰度梯度，第二部分是像素点对位姿变化的导数，这里是计算第一部分：（u，v）是t+1时刻P点在相机的像素坐标 */
                    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();;

                    H += J * J.transpose();
                    b += -error * J;
                    cost += error * error;
                }
            // END YOUR CODE HERE
        }

        // solve update and put it into estimation
        // TODO START YOUR CODE HERE
        Vector6d update = H.ldlt().solve(b);
        T21 = Sophus::SE3d::exp(update) * T21;
        // END YOUR CODE HERE

        cost /= nGood;

        /* 一旦发现某次迭代的误差增大了，或者方程没有解，则break停止迭代；按理应该是需要continue略过此次结果，继续后面的迭代过程；
        实测发现一旦某次误差增大，后面所有的迭代误差都会增加，所以没有必要继续迭代了，因此break掉 */
        if (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;
        }
        lastCost = cost;
        // cout << "cost = " << cost << ", good = " << nGood << endl;
    }
    // cout << "good projection: " << nGood << endl;
    // cout << "T21 = \n" << T21.matrix() << endl;

#if DEBUG_DRAW
    // in order to help you debug, we plot the projected pixels here
    cv::Mat img1_show, img2_show;
    cv::cvtColor(img1, img1_show, cv::COLOR_GRAY2BGR);
    cv::cvtColor(img2, img2_show, cv::COLOR_GRAY2BGR);
    for (auto &px: px_ref) {
        cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
                      cv::Scalar(0, 250, 0));
    }
    for (auto &px: goodProjection) {
        cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
                      cv::Scalar(0, 250, 0));
        // cv::circle(img2_show, px, 1, cv::Scalar(0, 250, 0), 1);
        // cv::line(img2_show, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
    }
    cv::imshow("reference", img1_show);
    cv::imshow("current", img2_show);
    // cv::waitKey();
#endif
}

void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> 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<cv::Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE
    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);
        }
    }
    // END YOUR CODE HERE

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

        // TODO START YOUR CODE HERE
        /* 相机内参 fx, fy, cx, cy 都需要和图像缩小相同的比例；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];
        // END YOUR CODE HERE
        std::cout << "pyramids level = " << level << std::endl;
        /* 多层法估计位姿T作为下层图像初始值，不需要进行缩放 */
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
        std::cout << "T21 = \n" << T21.matrix() << std::endl;
    }

}

src\CMakeLists.txt

find_package(OpenCV REQUIRED)

IF(OpenCV_FOUND)

MESSAGE(STATUS "Found OpenCV: \${OpenCV_LIBRARIES}")

include_directories(\${OpenCV_INCLUDE_DIRS})

add_executable(lk optical_flow.cpp)

target_link_libraries(lk \${OpenCV_LIBRARIES})

target_link_libraries(lk tbb)

find_package(Sophus REQUIRED)

include_directories(\${Sophus_INCLUDE_DIRS})

add_executable(direct direct_method.cpp)

target_link_libraries(direct \${OpenCV_LIBRARIES})

target_link_libraries(direct Sophus::Sophus)

\# target_link_libraries(direct_method \${OpenCV_LIBS} \${Sophus_LIBRARIES} \${Pangolin_LIBRARIES} fmt tbb)

ENDIF(OpenCV_FOUND)

4.1.4、测试结果

在这里插入图片描述

根据测试结果可知：随着时间增加，5张图片相对参考图片的位姿变化增加，能够获取到好的点不断减少，平方差在不断增大。

4.2、多层直接法

直接法也基于灰度不变假设，因此当某个时刻相机运动位姿很大时，计算结果会不准确，因此也需要使用金字塔法，有粗到细，迭代找到最小值，而不是某区域内的极小值。

4.2.1、多层直接法注意点

多层法相机内参变化
多层法估计位姿T作为下层图像初始值，不需要进行缩放
根据 coarse-to-fine 的过程，上一层图像的位姿估计结果可以作为下一层图像的初始条件。

4.2.2、多层直接法程序实现

src\direct_method.cpp

void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> 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<cv::Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE
    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);
        }
    }
    // END YOUR CODE HERE

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

        // TODO START YOUR CODE HERE
        /* 相机内参 fx, fy, cx, cy 都需要和图像缩小相同的比例；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];
        // END YOUR CODE HERE
        std::cout << "pyramids level = " << level << std::endl;
        /* 多层法估计位姿T作为下层图像初始值，不需要进行缩放 */
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
        std::cout << "T21 = \n" << T21.matrix() << std::endl;
    }

}

4.2.3、多层直接法测试结果

在这里插入图片描述
对比5个时刻的图像位姿累计的误差，可以得出与单层直接法类似的结论，随着时间增大，参考图片与当前图片的位姿变化随时间增加，因此第5张图片的误差最大。
单层法与多层法的结果进行比较，当参考图片与当前图片位姿变化不大，即T1时刻多层直接法和单层法计算累计误差基本相同；但是当当参考图片与当前图片位姿变化很大，即T5时刻多层直接法和单层法计算累计误差相差很大，多层法的效果较好。

4.3、讨论

直接法是否可以类似光流，提出 inverse, compositional 的概念？它们有意义吗？
直接法可以类似光流，提出inverse，compositional，但是没有意义，因为直接法是直接对两个图的光度误差做优化。
请思考上面算法哪些地方可以缓存或加速？
a. 在for循环计算每个点时，可以进行代码并行加速。另外并不是块选择越大越好，选择一个合适的，较小的，效率也会提高。
b. 代码计算时，窗口大小的选取要综合考虑计算速度以及位姿精度。
在上述过程中，我们实际假设了哪两个 patch 不变？
a. 灰度不变假设。
b. 区域一致性，窗口内，当前图像空间点的相机坐标(X,Y,Z)不变，不变。
为何可以随机取点？而不用取角点或线上的点？那些不是角点的地方，投影算对了吗？
因为是直接对光度误差做优化，不需要做特征匹配，因此不要求有区分度的角点。

请总结直接法相对于特征点法的异同与优缺点
a. 优点：

-  可以省去计算特征点、描述子的时间。
-  只要求有像素梯度即可，不需要特征点。
-  可以构建半稠密乃至稠密的地图，这点是特征点无法做到的。

b. 缺点：

- 非凸性，直接法完全依靠梯度搜索，其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小值，只在运动很小时直接法才能成功。
-  单个像素没有区分度，找一个和他像的实在太多了！由于每个像素对改变相机运动的“意见”不一致。只能少数服从多数，以数量代替质量。
-  灰度值不变是很强的假设，如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时亦会出现这种情况。特征点法对光照具有一定的容忍性，而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失败。
- 直接法需要获取空间点的深度信息。

5、光流计算视差

5.1、源码实现

Lk光流法计算视差的源码文件为：src\optical_disparity.cpp，可执行文件为：build\bin\dis-optical

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

    // images, note they are CV_8UC1, not CV_8UC3
    Mat img1 = imread(file_1, 0);
    Mat img2 = imread(file_2, 0);
    cv::Mat disparity_img = cv::imread(file_display, 0);

    // key points, using GFTT here.
    vector<KeyPoint> kp1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(img1, kp1);

    // now lets track these key points in the second image
    // then test multi-level LK
    vector<KeyPoint> kp2_multi;
    vector<bool> success_multi;
    OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi);

    //计算视差
    vector<double> disp;
    vector<double> cost;
    for(int i = 0 ;i < kp1.size();i++)
    {
        if (success_multi[i]) {
            double dis = abs(kp1[i].pt.x - kp2_multi[i].pt.x); //两点之间的水平视差
            double dis_ref = disparity_img.at<uchar>(kp1[i].pt.y, kp1[i].pt.x);
            disp.push_back(dis);
            double err = abs(dis - dis_ref);
            cost.push_back(err);
            // std::cout<<"dis: "<< dis <<" "<< dis_ref << std::endl;
        }
    }
    //计算视差的平均误差
    double sum = accumulate(cost.begin(),cost.end(),0.0);
    cout<<"平均误差:"<<sum/cost.size()<<endl;

/*     for (int i = 0; i < kp1.size(); i++) {
        if (success_multi[i]) {
            std::cout << "si = " << i << " x:" << kp1[i].pt.x << ", y:" <<  kp1[i].pt.y << " x:" << kp2_multi[i].pt.x << ", y:" <<  kp2_multi[i].pt.y << std::endl;
        } else
            std::cout << "ei = " << i << " x:" << kp1[i].pt.x << ", y:" <<  kp1[i].pt.y << " x:" << kp2_multi[i].pt.x << ", y:" <<  kp2_multi[i].pt.y << std::endl;
    } */

    // plot the differences of those functions  CV_GRAY2BGR 改成  cv::COLOR_GRAY2BGR
    Mat img2_multi;
    cv::cvtColor(img2, img2_multi, cv::COLOR_GRAY2BGR);
    for (int i = 0; i < kp2_multi.size(); i++) {
        if (success_multi[i]) {
            cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 1);
            cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("multi level", img2_multi);
    cv::waitKey(0);

    return 0;
}

5.2、测试结果

在这里插入图片描述

6、直接法计算视差

6.1、源码实现

直接法计算视差的源码实现文件为：src\direct_disprity.cpp，可执行文件为：build\bin\dis-direct。

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat right_img = cv::imread(right_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 = 1000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        /* 产生1000个随机点的坐标，为了避免取到像素边界，因此像素点坐标范围为 （40，图像行/列像素数量-40）
        RNG::uniform()产生一个均匀分布的随机数;          RNG::gaussian()产生一个高斯分布的随机数 */
        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<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        /* 得到1000个像素点以及像素点对应的深度 */
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    /* 默认值为的R为单位矩阵，即没有旋转；偏移为(0,0,0)，即没有偏移 */
    Sophus::SE3d T_cur_ref;
    // std::cout << "T_def = \n" << T_cur_ref.matrix() << std::endl;

    // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);    // first you need to test single layer
    DirectPoseEstimationMultiLayer(left_img, right_img, pixels_ref, depth_ref, T_cur_ref);
    //计算视差
    vector<double> disp;
    vector<double> cost;
    for(int i = 0; i < pixels_ref.size(); i++)
    {
        double dis = abs(pixels_ref[i].x() - goodProjection[i].x()); //两点之间的水平视差
        double dis_ref = disparity_img.at<uchar>(pixels_ref[i].y(), pixels_ref[i].x());
        double err = abs(dis - dis_ref);
        disp.push_back(dis);
        // std::cout<<"dis: "<< dis <<" "<< dis_ref << std::endl;
        cost.push_back(err);
    }
    //计算视差的平均误差
    double sum = accumulate(cost.begin(),cost.end(),0.0);
    cout<<"平均误差:"<<sum/cost.size()<<endl;

    // plot the differences of those functions  CV_GRAY2BGR 改成  cv::COLOR_GRAY2BGR
    cv::Mat img2_multi;
    cv::cvtColor(right_img, img2_multi, cv::COLOR_GRAY2BGR);
    for (int i = 0; i < pixels_ref.size(); i++) {
            cv::KeyPoint tmprefkp, tmpcurkp;
            tmprefkp.pt.x = pixels_ref[i].x();
            tmprefkp.pt.y = pixels_ref[i].y();
            tmpcurkp.pt.x = goodProjection[i].x();
            tmpcurkp.pt.y = goodProjection[i].y();

            cv::circle(img2_multi, tmpcurkp.pt, 2, cv::Scalar(0, 250, 0), 1);
            cv::line(img2_multi, tmprefkp.pt, tmpcurkp.pt, cv::Scalar(0, 250, 0));
    }
    cv::imshow("multi level", img2_multi);
    cv::waitKey(0);

    return 0;
}

6.2、测试结果

在这里插入图片描述

嘀嗒嗒~

关注

26
点赞
踩
18

收藏

觉得还不错? 一键收藏
0
评论
直接法里程计设计

eiI1xiyi−I2xi△xiyi△yieiI1xiyi−I2xi△xiyi△yi在最小二乘法中，我们需要求△xi△yi△xi△yi使得ei{e_i}ei的平方和最小，所以目标函数是一个代表灰度的标量函数I2xi△xiyi△yiI2xi△xiyi△yi。
复制链接

扫一扫