【SLAM】直接法应用_slam直接法中的转移矩阵初值

1.直接法的引入

在光流的计算中，他的计算量比起特征匹配的计算量要小很多，但是依然很大；在BA优化问题中，我们通过最小化重投影误差，可以获得较优化的转移矩阵（包括旋转矩阵和平移矩阵）。

直接法与光流法类似，对于不同视角的2个相机位姿，依然可以用一个旋转矩阵和一个平移矩阵表示三维点，只不过在直接法中，假设图像具有光照不变性，即灰度不变性，就是我们假设一系列的图像的灰度是不变的。

> 关键点的提取与描述子的计算非常耗时。实践当中，SIFT目前在CPU上是无法实时计算的，而ORB也需要近20毫秒的计算。如果整个SLAM以30毫秒/帧的速度运行，那么一大半时间都花在计算特征点上。
使用特征点时，忽略了除特征点以外的所有信息。一张图像有几十万个像素，而特征点只有几百个。只使用特征点丢弃了大部分可能有用的图像信息。
相机有时会运动到特征缺失的地方，往往这些地方都没有什么明显的纹理信息。例如，有时我们会面对一堵白墙，或者一个空荡荡的走廓。这些场景下特征点数量会明显减少，我们可能找不到足够的匹配点来计算相机运动。

2.直接法

引入直接法，这里对于按照旋转矩阵和平移矩阵计算出的最小化的误差就不是投影误差了，而是光度误差。

具体来说，在之前我们使用特征点法估计相机运动位姿时，我们把特征点看作固定在三维空间的三维不动点。根据三维点它们在相机中的投影位置，通过最小化重投影误差（Reprojection error）来优化相机运动估计位姿。在此，我们需要知道三维空间点在两个视角相机中投影后的像素二维位置———这也就是我们为何要对特征进行特征点匹配或特征点跟踪的理由。而在直接法中，我们最小化的不再是重投影误差，而是光度（测量）误差（Phometric error）。

3.数学优化

当然，这里变成了优化问题，可以使用之前介绍的优化求解方式求解，如高斯牛顿法等等。

通过李群李代数，左给扰动，计算相应的导数，通过类似梯度法等求解最优化的解.

4.实践

这里，我们就讲完了相关的理论部分，接下来，我来介绍一下实践的应用部分。对于直接法的相关三个分类：

> 此时我们又得到了一张新的图像，需要估计它的相机位姿。这个位姿是由一个初值再上不断地优化迭代得到的。假设我们的初值比较差，在这个初值下，空间点P投影后的像素灰度值是126。于是，这个像素的误差为229−126=103，我们希望微调相机的位姿，使像素更亮一些。
　　怎么知道往哪里微调，像素会更亮呢？这就需要用到像素梯度。我们在图像中发现，沿u轴往前走一步，该处的灰度值变成了123，即减去了3。同样地，沿v轴往前走一步，灰度值减18，变成108。在这个像素周围，我们看到梯度是[−3,−18]，为了提高亮度，我们会建议优化算法微调相机，使P的像往左上方移动。由于这个梯度是在局部求解的，这个移动量不能太大。
　　但是，优化算法不能只听这个像素的一面之词，还需要听取其他像素的建议。综合听取了许多像素的意见之后，优化算法选择了一个和我们建议的方向偏离不远的地方，计算出一个更新量exp(ξ∧)。加上更新量后，图像从I2移动到了I′2，像素的投影位置也变到了一个更亮的地方。我们看到，通过这次更新，误差变小了。在理想情况下，我们期望误差会不断下降，最后收敛。

/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
    JacobianAccumulator(
        const cv::Mat &img1_,
        const cv::Mat &img2_,
        const VecVector2d &px_ref_,
        const vector<double> depth_ref_,
        Sophus::SE3d &amp;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 &amp;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 &amp;img1;
    const cv::Mat &amp;img2;
    const VecVector2d &amp;px_ref;
    const vector<double> depth_ref;
    Sophus::SE3d &amp;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 &amp;img1,
    const cv::Mat &amp;img2,
    const VecVector2d &amp;px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &amp;T21
);

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

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


void DirectPoseEstimationSingleLayer(
    const cv::Mat &amp;img1,
    const cv::Mat &amp;img2,
    const VecVector2d &amp;px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &amp;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 &lt; iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&amp;JacobianAccumulator::accumulate_jacobian, &amp;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 &lt;&lt; "update is nan" &lt;&lt; endl;
            break;
        }
        if (iter &gt; 0 &amp;&amp; cost &gt; lastCost) {
            cout &lt;&lt; "cost increased: " &lt;&lt; cost &lt;&lt; ", " &lt;&lt; lastCost &lt;&lt; endl;
            break;
        }
        if (update.norm() &lt; 1e-3) {
            // converge
            break;
        }

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

    cout &lt;&lt; "T21 = \n" &lt;&lt; T21.matrix() &lt;&lt; endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>&gt;(t2 - t1);
    cout &lt;&lt; "direct method for single layer: " &lt;&lt; time_used.count() &lt;&lt; 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 &lt; px_ref.size(); ++i) {
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] &gt; 0 &amp;&amp; p_cur[1] &gt; 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
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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166

主函数如下：

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<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i &lt; 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<uchar>(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 &lt; 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;
}

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

5.小结

通常情况下，直接法会比光流法、特征匹配法快一些。但是，由于直接法计算的是图像光度误差以及梯度，所以通常情况下，我们不希望图像的梯度非常大，这会导致图像梯度非凸，换句话说，不好收敛，所以需要图像的有凸。除此之外，相机变化的曝光参数也会改变算法的性能。

> 参考资料：视觉slam十四讲