SLAM十四讲之第八讲：光流法、直接法_光流法直接法

视觉里程计2

1 光流法

  光流跟踪能够直接得到特征点的对应关系。这个对应关系就像是描述子的匹配，只是光流对图像的连续性和光照稳定性要求更高一些。光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较平滑（或采集频率较高）。

1.1 2D光流

  光流是一种描述像素随时间在图像之间运动的方法。随着时间的流逝，同一个像素会在图像中运动，而我们希望追踪它的运动过程。其中，计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流以LK光流为代表(特征点匹配)。稠密光流以Horm-Schunck光流为代表(建立完整地图)。

  在LK光流中，我们认为来自相机的图像是随时间变化的。图像可以看作时间的函数：$I(t)$。那么，一个在$t$时刻，位于$(x,y)$处的像素，它的灰度可以写成

$$I(x,y,t)$$

  灰度不变假设：同一个空间点的像素灰度值，不随时间和图像变化改变，在各个图像中是固定不变的。
$$\mathbf{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)=\mathbf{I}(x,y,t)$$

  把灰度不变假设进行泰勒展开
$$I\left(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t\right)\approx I\left(x,y,t\right)+\frac{\partial I}{\partial x}\mathrm{d}x+\frac{\partial I}{\partial y}\mathrm{d}y+\frac{\partial I}{\partial t}\mathrm{d}t$$

$$\frac{\partial \mathbf{I}}{\partial x}\mathrm{d}x+\frac{\partial \mathbf{I}}{\partial y}\mathrm{d}y+\frac{\partial \mathbf{I}}{\partial t}\mathrm{d}t=0$$

$$\frac{\partial \mathbf{I}}{\partial x}\frac{\mathrm{d}x}{\mathrm{d}t}+\frac{\partial \mathbf{I}}{\partial y}\frac{\mathrm{d}y}{\mathrm{d}t}=-\frac{\partial \mathbf{I}}{\partial t}$$

  其中，$\frac{\partial \mathbf{I}}{\partial x}=I_x$,$\frac{\partial \mathbf{I}}{\partial y}=I_y$,分别是图像在该点处$x$和$y$方向上的梯度;$\frac{\mathrm{d}x}{\mathrm{d}t}=u$是像素在$x$轴上的运动速度,$\frac{\mathrm{d}y}{\mathrm{d}t}=v$是像素在$y$轴上的运动速度。把上式写为矩阵形式如下：

[ I x I y ] [ u v ] = − I t

$$\begin{bmatrix}I_x&I_y\end{bmatrix}$$ $$\begin{bmatrix}u\\\\v\end{bmatrix}$$ =-\boldsymbol{I}_t [Ix​​Iy​​] ​uv​ ​=−It​

  很明显，如果只有一个像素点，那么这个矩阵方程就是一个欠定方程组，也就是说，可能会有无穷多个解满足该方程，所以我们要引入限制条件。

  我们考虑一个$w\times w$的窗口，一共有$w^2$个像素，这个窗口内所有的像素都具有同样的运动，那么就有了$w^2$个方程。
[ I x I y ] k [ u v ] = − I t k , k = 1 , … , w 2

$$\begin{bmatrix}\boldsymbol I_x&\boldsymbol I_y\end{bmatrix}$$ _k $$\begin{bmatrix}u\\\\v\end{bmatrix}$$ =-\boldsymbol I_{tk},\quad k=1,\ldots,w^2 [Ix​​Iy​​]k​ ​uv​ ​=−Itk​,k=1,…,w2

​  上面方程组变成了超定方程组，那么一定存在唯一一个最小二乘解！这里不再推导最小二乘解的过程，具体可见。

A [ u v ] = − b . A

$$\begin{bmatrix}u\\\\v\end{bmatrix}$$ =-\boldsymbol{b}. A ​uv​ ​=−b.

[ u v ] ∗ = − ( A T A ) − 1 A T b .

$$\begin{bmatrix}u\\\\v\end{bmatrix}$$ ^*=-\big(A^{\mathrm{T}}A\big)^{-1}A^{\mathrm{T}}b. ​uv​ ​∗=−(ATA)−1ATb.

1.2 LK光流程序

LK光流的实现步骤如下：

1. 选择感兴趣的像素点或角点作为特征点。
2. 在第一帧图像中，通过特征点周围的窗口计算特征点的光流。可以使用亚像素精度的插值来提高计算精度。
3. 在下一帧图像中，使用第一步中得到的光流作为初始估计，通过在特征点周围的窗口内最小化亮度误差来重新计算光流。(初始点是两张图像的同一个坐标点)
4. 重复步骤3，直到收敛或达到最大迭代次数。

1.2.1 单层光流

• 1 光流—非线性求解方法

  单层光流的计算并不是按上面推导的公式来的，而是以下面非线性优化的角度来计算。把该问题写为优化函数，则残差函数$f$为两张图片同一特征点像素差，优化参数为$dx$和$dy$。即求f最小时的对应坐标偏移量。
$$\underset{\mathrm{dx},\mathrm{dy}}{\min }\mathrm{F}(\mathrm{dx},\mathrm{dy})={\Vert \mathrm{f}(\mathrm{dx},\mathrm{dy})\Vert }^2={\Vert {\mathbf{I}}_1\left(x,y\right)-{\mathbf{I}}_2\left(x+\Delta x,y+\Delta y\right)\Vert }_2^2$$
  残差函数$f$对参数$dx$和$dy$的导数（图像梯度$I_x，I_y$），即雅可比矩阵为
$$\mathrm{J}\left(\mathbf{m}\right)={\left[\frac{\partial \mathrm{f}}{\partial \mathrm{dx}},\frac{\partial \mathrm{f}}{\partial \mathrm{dx}}\right]}^T$$
  高斯牛顿的解为$\Delta m$即$dx$和$dy$
$$\underset{\mathbf{H}(\mathbf{m})}{\underbrace{\mathbf{J}(\mathbf{m}){\mathbf{J}}^{\mathrm{T}}}}(\mathbf{m})\Delta \mathbf{m}=\underset{\mathbf{g}(\mathbf{m})}{\underbrace{-\mathbf{J}(\mathbf{m})f(\mathbf{m})}}$$
  我们可以验证把海塞矩阵取逆，然后等式两边同除以$\Delta t$，就可以得到上面线性方程推导的最小二乘解。($\Delta m$除以$\Delta t$即$u和v$)

• 2 反向光流

  代码分为反向光流和正向光流两种，正向光流计算img2的梯度，由于参数$\Delta m$（即$dx$和$dy$）一直在改变，所以图像梯度$I_x，I_y$在不停的改变；反向光流计算的是img1的梯度，img1的特征点坐标位置不变，图像梯度保持不变，所以我们只需计算一次梯度即可。图像梯度不变，即雅可比$J$不变，也就是$H$矩阵不变，减少了计算量。

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))
//  两个像素单位的差/2    
1
2
3
4
5
6

• 3 程序解析

void OpticalFlowSingleLevel(
    const Mat& img1,const Mat& img2,const vector<KeyPoint>& kp1,
    vector<KeyPoint>& kp2,vector<bool>& success,bool inverse, bool has_initial) 
{ // success记录匹配成功点数，inverse表示用正向/反向光流，has_initial表示是否已经又img2对应img1关键点kp1坐标
    kp2.resize(kp1.size());
    success.resize(kp1.size());
    OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);  // 调用类构造函数初始化
    parallel_for_(Range(0, kp1.size()),
        std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
}

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++)  //注意range
    {
        auto kp = kp1[i];
        double dx = 0, dy = 0; // 初始化偏移参数dx，dy为0
        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 2*2
        Eigen::Vector2d b = Eigen::Vector2d::Zero();    // bias 2*1
        Eigen::Vector2d J;  // jacobian 2*1      Ix和Iy，即图像在x和y方向上的梯度
        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 残差 and 雅可比-----梯度
            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);  // dx和dy，计算小窗口的像素差，求得error最小的dxdy，
                                                                                                                  // 利用光流法是实现关键点匹配
                    
                    if (inverse == false) 
                    {  // 正向光流通过计算img2的梯度
                        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) 
                    { // 反向光流计算img1的梯度
                        // in inverse mode, J keeps same for all iterations反向光流法中，J保持不变。程序中使用的就是反向光流
                        // 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]))  // 检查一个浮点数是否为NaN（Not a Number）
            {
                cout << "update is nan" << endl;
                succ = false;
                break;
            }

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

            // 更新优化量dx，dy
            dx += update[0];
            dy += update[1];
            lastCost = cost;   // 关注的是cost最小，不是参数dxdy为0，勿混淆
            succ = true;

            if (update.norm() < 1e-2) 
            {
                // converge
                break;
            }
        }
        success[i] = succ;

        // set kp2   计算完当前img1关键点对应img2对应的像素位置，迭代下一个关键点
        kp2[i].pt = kp.pt + Point2f(dx, dy);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

1.2.2 多层光流

  单层光流法中，我们利用的是高斯牛顿法来解决基于光流的关键点匹配的优化问题。但这种迭代的优化算法通常要求一个比较好的初始值，才能保证算法收敛。上面程序中，我们假定img2的初始值即img1的关键点位置。

  如果相机运动较快，两张图像差异较明显，我们若还是按单层光流法取初始值，很容易达到一个局部极小值。这种情况可以通过引人图像金字塔，通过多层光流改善算法效果。

  图像金字塔：图像金字塔是指对同一个图像进行缩放，得到不同分辨率下的图像。以原始图像作为金字塔底层，每往上一层，就对下层图像进行一定倍率的缩放。
  单层光流：只在原始图像上进行光流法
  多层光流：从图像金字塔顶层依次向图像金字塔底层做光流法，每一层的结果作为下一层光流法的初始变换，由粗至精（Coarse-to-fine）。

• 程序解析

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

    // parameters金字塔数4，每层比例0.5
    int pyramids = 4;                   
    double pyramid_scale = 0.5;  
    double scales[] = { 1.0, 0.5, 0.25, 0.125 };


    // 为img1和img2分别创建金字塔
    vector<Mat> pyr1, pyr2; 
    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);
        }
    }

    // coarse-to-fine（有粗到精） LK tracking in pyramids
    vector<KeyPoint> kp1_pyr, kp2_pyr;
    for (auto& kp : kp1) 
    {
        auto kp_top = kp;
        kp_top.pt *= scales[pyramids - 1];  // kp_top.pt*0.125  关键点坐标的缩放
        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();    
        OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_pyr, kp2_pyr, success, inverse, true);      
        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_pyr已经是img2的特征点坐标了
        kp2.push_back(kp);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

1.2.3 opencv函数

① cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);

// opencv自定义的 LK optocal track方法
void cv::calcOpticalFlowPyrLK(cv::InputArray prevImg, cv::InputArray nextImg, 									  cv::InputArray prevPts, 
                              cv::InputOutputArray nextPts, 
                              cv::OutputArray status,
                              cv::OutputArray err, 
                              cv::Size winSize = cv::Size(21, 21), 
                              int maxLevel = 3,
                              cv::TermCriteria criteria = cv::TermCriteria((TermCriteria::COUNT) + (TermCriteria::EPS),30,(0.01000000000000000021)), 
                              int flags = 0, double minEigThreshold = (0.0001000000000000000048));
    
param1--prevImg;  // 第一帧图像
param2--nextImg;  // 第二帧
param2--prevPts;  // 第一帧关键点坐标
param4--nextPts;  // 第二帧关键点坐标

status //输出状态向量（无符号字符）;如果找到相应特征的流，则向量的每个元素设置为1，否则设置为0。
err //输出每个点的匹配误差; 误差度量的类型可以在flags参数中设置; 如果未找到流，则未定义错误。
winSize //每个金字塔等级的搜索窗口的winSize大小。
maxLevel //基于0的最大金字塔等级数;如果设置为0，则不使用金字塔（单级），如果设置为1，则使用两个级别，依此类推;如果将金字塔传递给输入，那么算法将使用与金字塔一样多的级别，但不超过maxLevel。
criteria //参数，指定迭代搜索算法的终止条件（在指定的最大迭代次数criteria.maxCount之后或当搜索窗口移动小于criteria.epsilon时）。
flags //操作标志：
OPTFLOW_USE_INITIAL_FLOW//使用初始估计，存储在nextPts中;
                        //如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计。
OPTFLOW_LK_GET_MIN_EIGENVALS//使用最小特征值作为误差测量（参见minEigThreshold描述）;
                            //如果没有设置标志，则将原稿周围的色块和移动点之间的L1距离除以
                            //窗口中的像素数，用作误差测量。
minEigThreshold //算法计算光流方程的2x2正常矩阵的最小特征值，除以窗口中的像素数;
                //如果此值小于minEigThreshold，则过滤掉相应的功能并且不处理其流程，
                //因此它允许删除坏点并获得性能提升。    
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

② cv::cvtColor(cv::InputArray src, cv::OutputArray dst, int code)

参数说明：

src：输入图像（可以是单通道或多通道图像）。
dst：输出图像，用于存储转换后的图像。
code：颜色空间转换代码，指定所需的转换类型。
	// cv::COLOR_BGR2GRAY 表示从BGR彩色到灰度的转换，
    // cv::COLOR_BGR2HSV 表示从BGR彩色到HSV颜色空间的转换。您可以根据需要选择适合的转换代码。
1
2
3
4
5
6
7

③ cv::resize(InputArray src, OutputArray dst, Size dsize, double fx = 0, double fy = 0, int interpolation = INTER_LINEAR);

  resize 函数用于将输入图像调整为指定尺寸或缩放比例的目标图像，并将结果存储在输出图像 dst 中。

src：输入图像（可以是单通道或多通道图像）。
dst：输出图像，用于存储调整尺寸后的图像。
dsize：目标图像的尺寸，可以通过指定宽度和高度的 Size 对象来设置。
fx	//在水平方向上的缩放比例因子，默认值为 0，当为 0 时，根据目标图像的尺寸自动计算缩放比例。
fy  //在垂直方向上的缩放比例因子，默认值为 0，当为 0 时，根据目标图像的尺寸自动计算缩放比例。
interpolation // 插值方法，指定如何在调整图像尺寸时进行像素值的插值，默认为 INTER_LINEAR，表示使用双线性插值。
1
2
3
4
5
6

④ cv::Range

class Range  // 用于表示一个整数范围。它通常用于指定迭代的范围或操作范围。
{
public:
    Range();
    Range(int _start, int _end);
    int size() const;
    bool empty() const;
    static Range all();

    int start, end;
};
1
2
3
4
5
6
7
8
9
10
11

⑤ cv::RNG

  cv::RNG 是 OpenCV 库中的一个类，提供了随机数生成功能。“RNG” 是 “Random Number Generator” 的缩写，表示随机数生成器。

#include <opencv2/opencv.hpp>

int main()
{
    cv::RNG rng; // 创建随机数生成器对象

    int randomInt = rng.uniform(0, 10); // 生成0到10之间的随机整数
    double randomDouble = rng.uniform(0.0, 1.0); // 生成0.0到1.0之间的随机浮点数

    std::cout << "Random Integer: " << randomInt << std::endl;
    std::cout << "Random Double: " << randomDouble << std::endl;

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

⑥ parallel_for_()并行计算函数

void parallel_for_(const Range& range, const std::function<void(const Range&)>& body, double nstripes = -1.0);

// 参数说明：

range：要迭代的范围，通常使用 Range 类型表示。
body：一个函数对象，表示要在每个迭代中执行的操作。该函数对象接受一个 Range 参数，用于指定当前的迭代范围。
nstripes：可选参数，表示线程数量。默认值为 -1，表示由 OpenCV 自动确定线程数量。
1
2
3
4
5
6
7

• 实验1

#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;

void print(const cv::Range& range)
{
    for (int i = range.start; i < range.end; i++)
    {
        std::cout << "处理第" << i << " 个数" << endl;
    }
}

int main()
{
    cv::Range test(0,100);
    // print对象接受一个Range参数
    cv::parallel_for_(test, print);  // 并行输出0-100的数
    
    cv::parallel_for_(test, std::bind(print, std::placeholders::_1)); // 这里把print捆绑，没有固定的参数，
    // auto a =  std::bind(print, std::placeholders::_1) 
    // a(Range参数)  Range参数即对应的std::placeholders::_1
    
    
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

实验2

void sum(const cv::Range& range,int c)
{
    for (int i = range.start; i < range.end; i++)
    {
        int s = i + c;
        std::cout << i << "+c = " << s <<  endl;
    }
}

int main()
{
    cv::Range test(0,100);
    //cv::parallel_for_(test, print);

    // cv::parallel_for_(test, std::bind(print, std::placeholders::_1));
    // cv::parallel_for_(test, std::bind(print, std::placeholders::_1));
    cv::parallel_for_(test, std::bind(sum, std::placeholders::_1,100));

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

1.2.4 双线性插值

  双线性插值是一种常用的插值方法，用于在像素级别之间进行平滑的值估计。它通过对周围的四个像素进行加权平均来计算一个子像素的近似值。这四个像素通常是离子像素最近的四个像素，分别位于该子像素的左上、右上、左下和右下位置。

  双线性插值的计算过程是根据这四个像素的亮度值和相对位置，通过加权平均来估计子像素的值。权重是根据子像素与四个相邻像素之间的距离计算得出的。通过这种方式，双线性插值可以在像素级别之间提供平滑的过渡，以得到更准确的近似值。后续再补充详细内容。

inline float GetPixelValue(const cv::Mat& img, float x, float y) {
    // 是否超出边界
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols - 1) x = img.cols - 2;
    if (y >= img.rows - 1) y = img.rows - 2;

    float xx = x - floor(x);  // floor(x)向下取整
    float yy = y - floor(y);
    int x_a1 = std::min(img.cols - 1, int(x) + 1);
    int y_a1 = std::min(img.rows - 1, int(y) + 1);

    return (1 - xx) * (1 - yy) * img.at<uchar>(y, x) // opencv提取的特征点的坐标都是整数，
        + xx * (1 - yy) * img.at<uchar>(y, x_a1)	 //	这里实际还是img.at<uchar>(y, x) 
        + (1 - xx) * yy * img.at<uchar>(y_a1, x)	 
        + xx * yy * img.at<uchar>(y_a1, x_a1);
}

//
// 在直接法中略有不同，读取像素没有使用at函数，直接使用行指针
inline float GetPixelValue(const cv::Mat& img, float x, float y) {
    // 检查越界
    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]
        );
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

1.2.5 C++内置类

1 std::bind

  The function template函数模板. std::bind固定某些参数，不同的调用可以只调用个别参数（Partial application）。除此之外，std::bind还可以把函数转换为函数对象

template< class F, class... Args >
/*unspecified*/ bind( F&& f, Args&&... args );
// unspecified指没有特定的返回类型
f   -  可调用 (Callable) 对象（函数对象、指向函数指针、到函数引用、指向成员函数指针或指向数据成员指针）
args  -  要绑定的参数列表，未绑定参数为命名空间 std::placeholders 的占位符 _1, _2, _3... 所替换
1
2
3
4
5

• 实验

#include <functional>
#include <iostream>

using namespace std;
using namespace std::placeholders;  // for _1, _2, _3...
									// eg std::placeholders::_1

void print(int n1, int n2, int n3, const int& n4, int n5);
int g(int n1);

class sum   // struct sum 结构体同理
{
public:
    void print_sum(int n1, int n2)
    {
        std::cout << n1 + n2 << endl;
    }

    int data = 10;
};

int main()
{   
    // 1. 绑定函数print，参数重新排序，按引用传参
    int n = 7;                                                                     // _i表示调用传入的第i个参数
    auto test1 = std::bind(print, _2, 42, _1, std::cref(n), n);   // _1 and _2 来自占位符std::placeholders，eg std::placeholders::_1
    n = 10;
    test1(1, 2, 1001);      // 1 为参数 _1 所绑定， 2 为 _2 所绑定，不会使用 1001
                                    // 实际调用print(2, 42, 1, n, 7)，而非print(1, 42, 2, n, 7)

    // 2. 嵌套绑定，占位符共享
    auto test2 = std::bind(print, _3, std::bind(g, _3), _3, 4, 5);
    test2(10, 11, 12);          // 实际调用 f(12, g(12), 12, 4, 5);    没有使用10，11


    // 3. 绑定成员函数指针
    sum s;
    auto test3 = std::bind(&sum::print_sum, &s, 95, _1);   // n1 =95   n2 = 5
    test3(5);

    // 4.  绑定mem_fn(成员函数指针)
    auto ptr_to_print_sum = std::mem_fn(&sum::print_sum);
    auto test4 = std::bind(ptr_to_print_sum, &s, 95, _1);
    test4(5);


    // 5.  绑定类成员对象
    auto test5 = std::bind(&sum::data, _1);
    std::cout <<"data = "<< test5(s) << endl;

   // 6. 绑定mem_fn（类成员对象）
    auto ptr_to_data = std::mem_fn(&sum::data);
    auto test6 = std::bind(ptr_to_data, _1);
    std::cout << test6(s) << endl;

    //7.  利用智能指针调用 members of the referenced objects
    std::cout << test6(std::make_shared<sum>(s)) << ' '
                   << test6(std::make_unique<sum>(s)) << endl;
}

void print(int n1, int n2, int n3, const int& n4, int n5)
{
    std::cout << n1 << ' ' << n2 << ' ' << n3 << ' ' << n4 << ' ' << n5 << endl;
}

int g(int n1)
{
    return n1;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70

参考链接1 参考链接2

1.3 为什么使用多层光流

  做了一个小实验，用几张尺度比较大的图像来做对比

  下面是几种方法的效果对比，实际上一旦尺度变大，不论是多层光流还是opencv，都会出现错误的匹配！就像特征点法一样，我们也可以设置一个旋转直方图（方向 or 距离，当然这样不是很合理，远点与近点，直行或倒退，这两种规律不同），来剔除误匹配！

对比1

对比2

对比3

2 直接法

2.1 直接法原理推导

  我们的目标是求第一个相机到第二个相机的相对位姿变换。我们以第一个相机为参照系，设第二个相机的旋转和平移为$R,t$（对应李群为$T$)。同时，两相机的内参相同，记为$K$。

p 1 = [ u v 1 ] 1 = 1 Z 1 K P , \boldsymbol{p}_1=

$$\begin{bmatrix}u\\\\v\\\\1\end{bmatrix}$$ _1=\frac1{Z_1}\boldsymbol{K}\boldsymbol{P}, p1​= ​uv1​ ​1​=Z1​1​KP,

p 2 = [ u v 1 ] 2 = 1 Z 2 K ( R P + t ) = 1 Z 2 K ( T P ) 1 : 3 . \boldsymbol{p}_2=

$$\begin{bmatrix}u\\\\v\\\\1\end{bmatrix}$$ _2=\frac{1}{Z_2}\boldsymbol{K}\left(\boldsymbol{R}P+t\right)=\frac{1}{Z_2}\boldsymbol{K}\left(\boldsymbol{T}P\right)_{1:3}. p2​= ​uv1​ ​2​=Z2​1​K(RP+t)=Z2​1​K(TP)1:3​.

  构建优化函数，我们要求的是最小化光度误差。注意这里优化的参数是相机位姿$T$，光流法中优化的是特征点的运动距离。
$$e={\mathbf{I}}_1\left({\mathbf{p}}_1\right)-{\mathbf{I}}_2\left({\mathbf{p}}_2\right).$$

$$\underset{\mathbf{T}}{\min }J\left(\mathbf{T}\right)=\Vert e{\Vert }^2.$$

$$\underset{\mathbf{T}}{\min }J\left(\mathbf{T}\right)=\sum _{i=1}^N{e}_i^{\mathrm{T}}{e}_i,e_i={\mathbf{I}}_1\left({\mathbf{p}}_{1,i}\right)-{\mathbf{I}}_2\left({\mathbf{p}}_{2,i}\right).$$

  定义两个中间变量，$q$是$P$在第二个相机坐标系下的坐标，而$u$是像素坐标。

q = T P , u = 1 Z 2 K q .

$$\begin{gathered}\boldsymbol{q}=T\boldsymbol{P},\\\boldsymbol{u}=\frac1{Z_2}\boldsymbol{K}\boldsymbol{q}.\end{gathered}$$ q=TP,u=Z2​1​Kq.​

  残差函数变为：
$$e(\mathbf{T})={\mathbf{I}}_1({\mathbf{p}}_1)-{\mathbf{I}}_2(\mathbf{u}),$$

  我们优化的参数是位姿，所以对位姿求导。因为img1的像素点坐标是固定的常数（不随位姿变化而改变），所以${\mathbf{I}}_1({\mathbf{p}}_1)$对于$T$的导数为0;$p_2$是变量，由$u$表示，因为在不停的寻找最佳匹配点。
$$\frac{\partial e}{\partial \mathbf{T}}=\frac{\partial {\mathbf{I}}_1({\mathbf{p}}_1)-\partial {\mathbf{I}}_2(\mathbf{u})}{\partial \mathbf{T}}=-\frac{\partial {\mathbf{I}}_2(\mathbf{u})}{\partial \mathbf{T}}$$
​  根据李代数的求导，我们选择左扰动模型（李群对加法不封闭，但李代数封闭），利用上面定义的中间变量，把$u$置换为位姿变化形式.
e ( ξ ⊕ δ ξ ) = − I 2 ( 1 Z 2 K exp ⁡ ( δ ξ ∧ ) exp ⁡ ( ξ ∧ ) P ) ≈ − I 2 ( 1 Z 2 K ( 1 + δ ξ ∧ ) exp ⁡ ( ξ ∧ ) P ) = − I 2 ( 1 Z 2 K exp ⁡ ( ξ ∧ ) P + 1 Z 2 K δ ξ ∧ exp ⁡ ( ξ ∧ ) P ) = − I 2 ( u + 1 Z 2 K δ ξ ∧ q )

$$\begin{aligned} e\left(\boldsymbol{\xi}\oplus\delta\boldsymbol{\xi}\right)& =-\boldsymbol{I}_{2}\left(\frac1{Z_{2}}\boldsymbol{K}\exp\left(\delta\boldsymbol{\xi}^{\wedge}\right)\exp\left(\boldsymbol{\xi}^{\wedge}\right)\boldsymbol{P}\right) \\ &\approx -I_{2}\left(\frac{1}{Z_{2}}K\left(1+\delta\xi^{\wedge}\right)\exp\left(\xi^{\wedge}\right)P\right) \\ &=-I_{2}\left(\frac{1}{Z_{2}}K\exp\left(\xi^{\wedge}\right)P+\frac{1}{Z_{2}}K\delta\xi^{\wedge}\exp\left(\xi^{\wedge}\right)P\right)\\ &=-I_2(u+\frac1{Z_2}K\delta\xi^\wedge q) \end{aligned}$$ e(ξ⊕δξ)​=−I2​(Z2​1​Kexp(δξ∧)exp(ξ∧)P)≈−I2​(Z2​1​K(1+δξ∧)exp(ξ∧)P)=−I2​(Z2​1​Kexp(ξ∧)P+Z2​1​Kδξ∧exp(ξ∧)P)=−I2​(u+Z2​1​Kδξ∧q)​
  或

∂ e ( ξ ⊕ δ ξ ) ∂ δ ξ = − ∂ I 2 ( 1 Z 2 K exp ⁡ ( δ ξ ∧ ) exp ⁡ ( ξ ∧ ) P ) ∂ δ ξ ≈ − ∂ I 2 ( 1 Z 2 K ( 1 + δ ξ ∧ ) exp ⁡ ( ξ ∧ ) P ) ∂ δ ξ = − ∂ I 2 ( 1 Z 2 K exp ⁡ ( ξ ∧ ) P + 1 Z 2 K δ ξ ∧ exp ⁡ ( ξ ∧ ) P ) ∂ δ ξ = − ∂ I 2 ( u + 1 Z 2 K δ ξ ∧ q ) ∂ δ ξ

$$\begin{aligned} \frac{\partial e\left(\boldsymbol{\xi}\oplus\delta\boldsymbol{\xi}\right)}{\partial\boldsymbol{\delta\xi}} & =-\frac{\partial \boldsymbol{I}_{2}{\left(\frac1{Z_{2}}\boldsymbol{K}\exp\left(\delta\boldsymbol{\xi}^{\wedge}\right)\exp\left(\boldsymbol{\xi}^{\wedge}\right)\boldsymbol{P}\right)}}{\partial\boldsymbol{\delta\xi}} \\ &\approx -\frac{\partial I_{2}\left(\frac{1}{Z_{2}}K\left(1+\delta\xi^{\wedge}\right)\exp\left(\xi^{\wedge}\right)P\right)}{\partial\boldsymbol{\delta\xi}} \\ &=-\frac{\partial I_{2}\left(\frac{1}{Z_{2}}K\exp\left(\xi^{\wedge}\right)P+\frac{1}{Z_{2}}K\delta\xi^{\wedge}\exp\left(\xi^{\wedge}\right)P\right)}{\partial\boldsymbol{\delta\xi}}\\ &=-\frac{\partial I_2(u+\frac1{Z_2}K\delta\xi^\wedge q)}{\partial\boldsymbol{\delta\xi}} \end{aligned}$$ ∂δξ∂e(ξ⊕δξ)​​=−∂δξ∂I2​(Z2​1​Kexp(δξ∧)exp(ξ∧)P)​≈−∂δξ∂I2​(Z2​1​K(1+δξ∧)exp(ξ∧)P)​=−∂δξ∂I2​(Z2​1​Kexp(ξ∧)P+Z2​1​Kδξ∧exp(ξ∧)P)​=−∂δξ∂I2​(u+Z2​1​Kδξ∧q)​​
注意$\frac{\partial e}{\partial \mathbf{T}}=\frac{\partial e}{\partial \mathbf{\delta \xi }}$

  然后我们把$I_2(p_2)$在$p_2=u$处泰勒展开（有两种形式）,这两种形式都是成立的，我们利用形式1方便后续分析。

①形式1
注意这里的$\frac{1}{Z_2}K\delta {\xi }^{\wedge }q$是像素点位置变化量，$\delta \xi$是李代数表示，求导是扰动的李代数求导
$$I_2(u+\frac{1}{Z_2}K\delta {\xi }^{\wedge }q)\approx I_2(u)+\frac{\partial I_2}{\partial \delta \xi }\delta \xi$$
②形式2
$$I_2(p_2)\approx I_2(u)+\frac{\partial I_2}{\partial \delta \xi }(p_2-u)$$

接着上面的推导，可得：
$$\begin{gathered} \frac{\partial e}{\partial \mathbf{\delta \xi }}=-\frac{\partial I_2(u+\frac{1}{Z_2}K\delta {\xi }^{\wedge }q)}{\partial \mathbf{\delta \xi }} \\ =-\frac{\partial (I_2(u)+\frac{\partial I_2}{\partial \delta \xi }\delta \xi )}{\partial \mathbf{\delta \xi }} \\ =0+\frac{\partial I_2}{\partial \delta \xi } \end{gathered}$$
​
  然后，根据求导的链式法则，我们可以把残差函数对位姿的导数写为下述形式（课本上缺失负号）：
$$\frac{\partial e}{\partial \mathbf{T}}=-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}$$

①$\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}$是图像像素的梯度，同光流法中计算一致，即$\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}=[I_x{I}_y{]}^T$。

②即像素坐标系下坐标对相机坐标系下三维坐标的导数，我们只要把$[u,v]$形式写出，对$[XYZ]$进行求导（分子布局,具体见第六讲矩阵求导笔记）。
∂ u ∂ q = [ ∂ u ∂ X ∂ u ∂ Y ∂ u ∂ Z ∂ v ∂ X ∂ v ∂ Y ∂ v ∂ Z ] = [ f x Z 0 − f x X Z 2 0 f y Z − f y Y Z 2 ] 2 × 3 \frac{\partial\boldsymbol{u}}{\partial\boldsymbol{q}}=

$$\begin{bmatrix}\dfrac{\partial u}{\partial X}&\dfrac{\partial u}{\partial Y}&\dfrac{\partial u}{\partial Z}\\\dfrac{\partial v}{\partial X}&\dfrac{\partial v}{\partial Y}&\dfrac{\partial v}{\partial Z}\end{bmatrix}$$ = $$\begin{bmatrix}\dfrac{f_x}{Z}&0&-\dfrac{f_xX}{Z^2}\\\\0&\dfrac{f_y}{Z}&-\dfrac{f_yY}{Z^2}\end{bmatrix}$$ _{2×3} ∂q∂u​= ​∂X∂u​∂X∂v​​∂Y∂u​∂Y∂v​​∂Z∂u​∂Z∂v​​ ​= ​Zfx​​0​0Zfy​​​−Z2fx​X​−Z2fy​Y​​ ​2×3​

③即变换后的三维点$q=Tp$对位姿变换*$\delta \mathbf{\xi }$的导数，这里具体过程同第四讲扰动模型求导。

$$\frac{\partial \mathbf{Tp}}{\partial \delta \mathbf{\xi }}=\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}={\left[\mathbf{I},-{\mathbf{q}}^{\wedge }\right]}_{3\times 6}$$

④由于后两项只与三维点$q$有关，而与图像无关，我们经常把它合并在一起：
∂ u ∂ δ ξ = [ f x Z 0 − f x X Z 2 − f x X Y Z 2 f x + f x X 2 Z 2 − f x Y Z 0 f y Z − f y Y Z 2 − f y − f y Y 2 Z 2 f y X Y Z 2 f y X Z ] 2 × 6 \frac{\partial\boldsymbol{u}}{\partial\delta\boldsymbol{\xi}}=

$$\begin{bmatrix}\frac{f_x}{Z}&0&-\frac{f_xX}{Z^2}&-\frac{f_xXY}{Z^2}&f_x+\frac{f_xX^2}{Z^2}&-\frac{f_xY}{Z}\\0&\frac{f_y}{Z}&-\frac{f_yY}{Z^2}&-f_y-\frac{f_yY^2}{Z^2}&\frac{f_yXY}{Z^2}&\frac{f_yX}{Z}\end{bmatrix}$$ _{2×6} ∂δξ∂u​=[Zfx​​0​0Zfy​​​−Z2fx​X​−Z2fy​Y​​−Z2fx​XY​−fy​−Z2fy​Y2​​fx​+Z2fx​X2​Z2fy​XY​​−Zfx​Y​Zfy​X​​]2×6​

我们得到了残差函数$e$关于参数位姿$T$的导数，也就是我们需要的雅可比矩阵：
$$\mathbf{J}=-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}.$$

2.2 直接法程序解析

  雅可比矩阵中包含了$P$的空间坐标，它是从何而来的呢？可以通过双目相机或RGB-D获得，也可以通过单目相机估计。书中代码直接使用带有深度的数据。

2.2.1 计算雅可比

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

        // 计算吗img2中的投影点位置
        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++;

        
        for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++) {
                
                // 计算残差函数e
                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;

                // 计算像素点雅可比 J1
                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;

                // 计算图像梯度I
                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))
                );

                // 误差的雅可比J = -I*J1
                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<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

2.2.2 单层直接法

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

    const int iterations = 10;
    double cost = 0, lastCost = 0;    
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);  // 调用类，计算J

    for (int iter = 0; iter < iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),  // 给0-2000个点，并行计算
            std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // ΔT = H^(-1)*b
        Vector6d update = H.ldlt().solve(b);;
        T21 = Sophus::SE3d::exp(update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            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;

    // plot the projected pixels here
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
    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();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

2.2.3 多层直接法

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

    // 4层金字塔，缩放因子0.5
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = { 1.0, 0.5, 0.25, 0.125 };

    // 1. 创建金字塔，和光流法处一致
    vector<cv::Mat> pyr1, pyr2; // img1、img2
    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);
        }
    }

    // 2. 构建每层金字塔的（随机）关键点
    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // 相机内参
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; 
        for (auto& px : px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        // 3. 调用单层直接法
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

2.2.4 讨论

  如果想要得到正确的优化结果，就必须保证大部分像素梯度能够把优化引导到正确的方向。

  假设我们的初值比较差，在这个初值下，空间点P投影后的像素灰度值是126。于是，此像素的误差为229一126=103。为了减小这个误差，我们希望微调相机的位姿，使像素更亮一些。

  怎么知道往哪里微调像素会更亮呢？这就需要用到局部的像素梯度。我们在图像中发现，沿u轴往前走一步，该处的灰度值变成了123，即减去了3。同样地，沿v轴往前走一步，灰度值减了18，变成108。在这个像素周围，我们看到梯度是「-3，-181]，为了提高亮度，我们会建议优化算法微调相机，使P的像往左上方移动。在这个过程中，我们用像素的局部梯度近似了它附近的灰度分布，不过请注意，真实图像并不是光滑的，所以这个梯度在远处就不成立了。

  直接法的梯度是直接由图像梯度确定的，因此我们必须保证沿着图像梯度走时，灰度误差会不断下降。然而，图像通常是一个很强烈的非凸函数,实际中，如果我们沿着图像梯度前进，很容易由于图像本身的非凸性（或噪声)落进一个局部极小值中，无法继续优化。只有当相机运动很小，图像中的梯度不会有很强的非凸性时，直接法才能成立。

2.3 光流法与直接法的区别

  光流法一般用于特征追踪、匹配（只不过这里不是用描述子），直接法通常用来优化位姿。我们其实可以从残差构建和优化变量来分析这个问题
残差：两者都是基于灰度不变假设，求解最小光度误差
优化变量：光流法优化的是各个特征点的运动，而直接法是优化两帧间的位姿。

3 程序编译问题

这一章仍然不需要安装新的库，直接编译即可

mkdir build
cd build
cmake ..   # 这里会报错，因为我们安装的是3.4.16，但是CMakeLists.txt中find的是version 4 把4删掉即可编译成功
1
2
3

报错：CMake Error at CMakeLists.txt:8 (find_package):
Could not find a configuration file for package "OpenCV" that is compatible
with requested version "4".

The following configuration files were considered but not accepted:

/usr/local/share/OpenCV/OpenCVConfig.cmake, version: 3.4.16

make -j4   # 然后又会报错
1

报错如下：很明显，这是缺少了线程的头文件

error: ‘mutex’ in namespace ‘std’ does not name a type std::mutex hessian_mutex; ^~~~~ /home/p/SLAMBOOK2/slambook2/ch8/direct_method.cpp: In member function ‘void JacobianAccumulator::accumulate_jacobian(const cv::Range&)’: /home/p/SLAMBOOK2/slambook2/ch8/direct_method.cpp:288:9: error: ‘unique_lock’ was not declared in this scope unique_lock<mutex> lck(hessian_mutex); ^~~~~~~~~~~ /home/p/SLAMBOOK2/slambook2/ch8/direct_method.cpp:288:21: error: ‘mutex’ was not declared in this scope unique_lock<mutex> lck(hessian_mutex); ^~~~~ /home/p/SLAMBOOK2/slambook2/ch8/direct_method.cpp:288:32: error: ‘hessian_mutex’ was not declared in this scope unique_lock<mutex> lck(hessian_mutex); ^~~~~~~~~~~~~ /home/p/SLAMBOOK2/slambook2/ch8/direct_method.cpp:288:32: note: suggested alternative: ‘hessian’ unique_lock<mutex> lck(hessian_mutex); ^~~~~~~~~~~~~ hessian /home/p/SLAMBOOK2/slambook2/ch8/direct_method.cpp:288:28: error: ‘lck’ was not declared in this scope unique_lock<mutex> lck(hessian_mutex); ^~~ CMakeFiles/direct_method.dir/build.make:62: recipe for target 'CMakeFiles/direct_method.dir/direct_method.cpp.o' failed

解决办法：上面报错中提出了错误的源头是direct_method .cpp，所以在其中加上头文件

#include <thread>
#include <mutex>
#include <unistd.h>
1
2
3

make -j4  # 这一次就编译成功
1

报错补充：后面一台机器opencv版本比较低，可能会发生下面的错误。最好还是使用上面提到版本

error: invalid initialization of reference of type ‘const cv::ParallelLoopBody&’ from expression of type ‘std::_Bind_helper<false, void (OpticalFlowTracker::*)(const cv::Range&), OpticalFlowTracker*, const std::_Placeholder<1>&>::type {aka std::_Bind<void (OpticalFlowTracker::*(OpticalFlowTracker*, std::_Placeholder<1>))(const cv::Range&)>}’

1
2