vSLAMNet（九）-运动估计-三角量测

发表于 | 分类于 ，

1. 概述

已知相机参数和匹配点，恢复三维点的坐标。如下图所示：

image-20210704150900562

第i个相机投影矩阵如下所示：

image-20210704151015769

给定检测和匹配的三个特征点，$x_1$, $x_2$和$x_3$，空间三维点坐标为$\bf{X} = [x, y, z, 1]^T$，对应在第i个视角中投影的图像坐标为$\bf{x}_i = [x_i, y_i, 1]^T$。那么理想情况下，这三条射线相较于同一个点。但是，由于误差的存在，这三个点的射线并不能够准确相较于一点。此时，可采用最小二乘来求解。

2. 算法详解

根据投影方程$\bf{d}_{i}\bf{x}_{i} = \bf{P}_{i}\bf{X}$，等式两侧同时叉乘$\bf{x}_i$，得到：

$\bf{x}_i \times (\bf{P}_{i}\bf{X}) = \bf{0}$,

其中，$\bf{x}_i = [x_i, y_i, 1]^T$，那么，我们得到：

image-20210704151824642

该矩阵为3行1列的。根据向量叉乘的运算，展开的：

image-20210704151903750

由于最后一行可以通过前两行计算得到，因此，化简得到：

image-20210704151948651

1个观测点可以提供2个约束，$\bf{X}$有3个自由度，求解该方程至少需要2对匹配点，即$\bf{A}\bf{X} = \bf{0}$，其中$\bf{A}$为：

image-20210704152150930

此时可对$\bf{A}$进行SVD分解，求得$\bf{X}$。

宽基线求解更加稳定，在匹配成功的点中，选择宽基线中的匹配对进行三角化求解。对track进行排序，来找到宽基线的匹配点。如果存在外点，采用RANSAC算法来进行精确求解。流程如下所示：

  • 计算RANSAC采样次数，设置内点阈值（重投影误差）；
  • 随机采样一对视角，计算三维点坐标；
  • 计算每个视角中的重投影误差，统计内点个数；
  • 重复上述步骤，直到满足采样次数，选择内点最多的视角；
  • 利用所有内点重新计算三维点坐标。

3. 代码实现

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
#include <math/matrix_svd.h>
#include "math/vector.h"
#include "math/matrix.h"

int main(int argc, char* argv[]) {
    math::Vec2f p1;
    p1[0] = 0.289986; p1[1] = -0.0355493;
    math::Vec2f p2;
    p2[0] = 0.316154; p2[1] =  0.0898488;

    math::Matrix<double, 3, 4> P1, P2;
    P1(0, 0) = 0.919653;    P1(0, 1)=-0.000621866; P1(0, 2)= -0.00124006; P1(0, 3) = 0.00255933;
    P1(1, 0) = 0.000609954; P1(1, 1)=0.919607    ; P1(1, 2)= -0.00957316; P1(1, 3) = 0.0540753;
    P1(2, 0) = 0.00135482;  P1(2, 1) =0.0104087  ; P1(2, 2)= 0.999949;    P1(2, 3) = -0.127624;

    P2(0, 0) = 0.920039;    P2(0, 1)=-0.0117214;  P2(0, 2) = 0.0144298;   P2(0, 3)   = 0.0749395;
    P2(1, 0) = 0.0118301;   P2(1, 1)=0.920129  ;  P2(1, 2) = -0.00678373; P2(1, 3) = 0.862711;
    P2(2, 0) = -0.0155846;  P2(2, 1) =0.00757181; P2(2, 2) = 0.999854 ;   P2(2, 3)   = -0.0887441;

    math::Matrix<double, 4, 4> A;
    A(0, 0) = p1[0] * P1(2, 0) - P1(0, 0);
    A(0, 1) = p1[0] * P1(2, 1) - P1(0, 1);
    A(0, 2) = p1[0] * P1(2, 2) - P1(0, 2);
    A(0, 3) = p1[0] * P1(2, 3) - P1(0, 3);

    A(1, 0) = p1[1] * P1(2, 0) - P1(1, 0);
    A(1, 1) = p1[1] * P1(2, 1) - P1(1, 1);
    A(1, 2) = p1[1] * P1(2, 2) - P1(1, 2);
    A(1, 3) = p1[1] * P1(2, 3) - P1(1, 3);

    A(2, 0) = p2[0] * P2(2, 0) - P2(0, 0);
    A(2, 1) = p2[0] * P2(2, 1) - P2(0, 1);
    A(2, 2) = p2[0] * P2(2, 2) - P2(0, 2);
    A(2, 3) = p2[0] * P2(2, 3) - P2(0, 3);

    A(3, 0) = p2[1] * P2(2, 0) - P2(1, 0);
    A(3, 1) = p2[1] * P2(2, 1) - P2(1, 1);
    A(3, 2) = p2[1] * P2(2, 2) - P2(1, 2);
    A(3, 3) = p2[1] * P2(2, 3) - P2(1, 3);

    math::Matrix<double, 4, 4> V;
    math::matrix_svd<double, 4, 4> (A, nullptr, nullptr, &V);
    math::Vec3f X;
    X[0] = V(0, 3)/V(3, 3);
    X[1] = V(1, 3)/V(3, 3);
    X[2] = V(2, 3)/V(3, 3);

    std::cout<<" trianglede point is :"<<X[0]<<" "<<X[1]<<" "<<X[2]<<std::endl;
    std::cout<<" the result should be "<<"2.14598 -0.250569 6.92321\n"<<std::endl;

    return 0;
}

4. 总结和讨论

  • 三角量测的矛盾点
    • 在同样的相机分辨率下，平移越大，三角化测量就会越精确；但是平移越大，将会导致图像的外观发生明确变化，这会使得特征的提取和匹配变得更困难

5. 参考