SLAM ORB-SLAM2(29)PnP估计姿态
- 1. PnP问题
- 2. EPnP算法
- 2.1. 计算4对控制点的世界坐标
- 2.2. 计算齐次质心坐标
- 2.3. 计算4对控制点的相机坐标
- 2.3.1. 构造M矩阵
- 2.3.2. 计算 M T M M^TM MTM的0特征值对应的特征向量
- 2.3.3. 计算零空间的秩
- 2.3.4. 计算线性组合的系数
- 2.4. 选择最小重投影误差
- 3. 标题
1. PnP问题
在 《SLAM ORB-SLAM2(26)重定位过程》 提到的重定位过程中
先计算当前帧特征点的词袋向量ComputeBoW
接着就是用词袋找到与当前帧相似的候选关键帧DetectRelocalizationCandidates
然后通过词袋模型进行初步匹配,紧接着查询较匹配的关键帧,此时先通过PnP投影估计姿态
根据匹配的 3D地图点 和当前帧特征点的 2D像素坐标 估计相机位姿
这种根据3D-2D映射关系求解相机位姿的问题,就是所谓的PnP问题(Perspective-n-Point Problem)
在机器视觉领域中,这是一个非常常见的问题
OpenCV 库还专门提供了函数 solvePnP进行求解
PnP 问题有很多种解法,ORB-SLAM2 采用了一种称为 EPnP 的算法,求解效率还不错
通常,EPnP的解都会拿来作为高斯牛顿之类迭代优化算法的初值, 经过迭代之后得到一个更优的解
从 OpenCV的文档 中找到上图所示
已知相机内参矩阵
K
K
K 和
n
n
n 个 3D 空间点
{
c
1
,
c
2
,
⋯
,
c
n
}
\{ \boldsymbol{c_1, c_2, \cdots, c_n} \}
{c1,c2,⋯,cn}
及其到图像上 2D 的投影点
{
μ
1
,
μ
2
,
⋯
,
μ
n
}
\{ \boldsymbol{\mu_1, \mu_2, \cdots, \mu_n} \}
{μ1,μ2,⋯,μn},求解相机的位置和姿态
记第
i
i
i 个 3D 空间点的齐次坐标为
c
i
=
[
x
i
y
i
z
i
1
]
T
\boldsymbol{c_i} = \begin{bmatrix} x_i & y_i & z_i & 1\end{bmatrix}^T
ci=[xiyizi1]T
其在图像上投影的 2D 像素坐标为
μ
i
=
[
u
i
v
i
1
]
T
\boldsymbol{\mu_i} = \begin{bmatrix} u_i & v_i & 1 \end{bmatrix}^T
μi=[uivi1]T
其投影过程,可以分解为两步:
- 根据相机的位姿,将空间点 c i \boldsymbol{c_i} ci 从世界坐标系下变换到相机坐标系下 [ R ∣ t ] c i \left[ \boldsymbol{R} \big | \boldsymbol{t} \right]\boldsymbol{c_i} [R t]ci
- 将相机坐标系下的点,根据相机内参矩阵 K \boldsymbol{K} K,投影到图像 μ i \boldsymbol{\mu_i} μi 上
其整个过程相当于连续乘了两个矩阵:
s
[
u
i
v
i
1
]
=
K
[
R
∣
t
]
[
x
i
y
i
z
i
1
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
t
1
t
2
t
3
t
4
t
5
t
6
t
7
t
8
t
9
t
10
t
11
t
12
]
[
x
i
y
i
z
i
1
]
s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \boldsymbol{K} \left[ \boldsymbol{R} \big | \boldsymbol{t} \right] \begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} t_1 & t_2 & t_3 & t_4 \\ t_5 & t_6 & t_7 & t_8 \\ t_9 & t_{10} & t_{11} & t_{12} \end{bmatrix} \begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix}
s
uivi1
=K[R
t]
xiyizi1
=
fx000fy0cxcy1
t1t5t9t2t6t10t3t7t11t4t8t12
xiyizi1
其中,
s
s
s 是一个尺度系数,在计算时通常通过叉乘或者归一化将之消除掉
K
,
R
,
t
K,R,t
K,R,t 分别是相机的内参矩阵、姿态矩阵和位置向量
参照 单应矩阵和基础矩阵的求解过程, 用矩阵
A
=
K
[
R
∣
t
]
A=K[R∣t]
A=K[R∣t] 将上式改写为:
s
[
u
i
v
i
1
]
=
[
a
1
a
2
a
3
a
4
a
5
a
6
a
7
a
8
a
9
a
10
a
11
a
12
]
⏟
A
[
x
i
y
i
z
i
1
]
⇒
{
u
i
=
a
1
x
i
+
a
2
y
i
+
a
3
z
i
+
a
4
a
9
x
i
+
a
10
y
i
+
a
11
z
i
+
a
12
v
i
=
a
5
x
i
+
a
6
y
i
+
a
7
z
i
+
a
8
a
9
x
i
+
a
10
y
i
+
a
11
z
i
+
a
12
⇒
[
x
i
y
i
z
i
1
0
0
0
0
−
x
i
−
y
i
−
z
i
−
1
0
0
0
0
x
i
y
i
z
i
1
−
x
i
−
y
i
−
z
i
−
1
]
[
a
1
a
2
⋮
a
11
a
12
]
=
0
s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} a_1 & a_2 & a_3 & a_4 \\ a_5 & a_6 & a_7 & a_8 \\ a_9 & a_{10} & a_{11} & a_{12} \end{bmatrix}}_{\boldsymbol{A}} \begin{bmatrix} x_i \\ y_i \\ z_i \\ 1 \end{bmatrix} \Rightarrow \begin{cases} u_i = \frac{ a_1 x_i + a_2 y_i + a_3 z_i + a_4 }{ a_9 x_i + a_{10} y_i + a_{11}z_i + a_{12} } \\ v_i = \frac{ a_5 x_i + a_6 y_i + a_7 z_i + a_8 }{ a_9 x_i + a_{10} y_i + a_{11}z_i + a_{12} } \end{cases} \Rightarrow \begin{bmatrix} x_i & y_i & z_i & 1 & 0 & 0 & 0 & 0 & -x_i & -y_i & -z_i & -1 \\ 0 & 0 & 0 & 0 & x_i & y_i & z_i & 1 & -x_i & -y_i & -z_i & -1 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ \vdots \\ a_{11} \\ a_{12} \end{bmatrix} = \boldsymbol{0}
s
uivi1
=A
a1a5a9a2a6a10a3a7a11a4a8a12
xiyizi1
⇒{ui=a9xi+a10yi+a11zi+a12a1xi+a2yi+a3zi+a4vi=a9xi+a10yi+a11zi+a12a5xi+a6yi+a7zi+a8⇒[xi0yi0zi0100xi0yi0zi01−xi−xi−yi−yi−zi−zi−1−1]
a1a2⋮a11a12
=0
如此,对于
n
n
n 个匹配点对,就可以得到下面形式的线性方程组
SVD分解,解零空间,就可以解得矩阵A
最少6个匹配点对,就可以完成求解,这就是一个DLT(Direct Linear Transformation)的方法
[
x
0
y
0
z
0
1
0
0
0
0
−
x
0
−
y
0
−
z
0
−
1
0
0
0
0
x
0
y
0
z
0
1
−
x
0
−
y
0
−
z
0
−
1
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
⋮
x
n
−
1
y
n
−
1
z
n
−
1
1
0
0
0
0
−
x
n
−
1
−
y
n
−
1
−
z
n
−
1
−
1
0
0
0
0
x
n
−
1
y
n
−
1
z
n
−
1
1
−
x
n
−
1
−
y
n
−
1
−
z
n
−
1
−
1
]
[
a
1
a
2
⋮
a
11
a
12
]
=
0
\begin{bmatrix} x_0 & y_0 & z_0 & 1 & 0 & 0 & 0 & 0 & -x_0 & -y_0 & -z_0 & -1 \\ 0 & 0 & 0 & 0 & x_0 & y_0 & z_0 & 1 & -x_0 & -y_0 & -z_0 & -1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x_{n-1} & y_{n-1} & z_{n-1} & 1 & 0 & 0 & 0 & 0 & -x_{n-1} & -y_{n-1} & -z_{n-1} & -1 \\ 0 & 0 & 0 & 0 & x_{n-1} & y_{n-1} & z_{n-1} & 1 & -x_{n-1} & -y_{n-1} & -z_{n-1} & -1 \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ \vdots \\ a_{11} \\ a_{12} \end{bmatrix} = \boldsymbol{0}
x00⋮xn−10y00⋮yn−10z00⋮zn−1010⋮100x0⋮0xn−10y0⋮0yn−10z0⋮0zn−101⋮01−x0−x0⋮−xn−1−xn−1−y0−y0⋮−yn−1−yn−1−z0−z0⋮−zn−1−zn−1−1−1⋮−1−1
a1a2⋮a11a12
=0
当然上述DLT算法解得的是矩阵 A A A,它包含了相机内参 K K K、姿态矩阵 R R R和平移向量 t t t
进一步的,通过QR分解,可以从矩阵
A
A
A中把这三个都给分解出来
看起来这一过程还附带算出了相机的内参,这也正是相机的内参标定的求解过程
DLT算法简单直接,但是它忽略了太多的约束,所以结果一般都不会很好
后来人们还研究出了很多求解 PnP 问题的算法,有只需要3个点就可以求解的P3P算法
ORB-SLAM2 用的就是EPnP算法,效率高而且稳定,据说其算法复杂度是 O(n) 的
2. EPnP算法
在 《SLAM ORB-SLAM2(28)PnP估计姿态过程》 中
提到的估计相机位姿函数 compute_pose
及其调用的子函数是 EPnP 算法的核心
下面结合论文中关于算法原理的介绍,分析函数 compute_pose
的实现过程
根据论文中的表述,EPnP 算法的时间复杂度是
O
(
n
)
O(n)
O(n) 的, 相比于其它
O
(
n
5
)
O(n^5)
O(n5),
O
(
n
8
)
O(n^8)
O(n8)的算法而言,它要高效很多了
其核心思想体现在三个方面:
- 将世界坐标系下的 n n n 个 3D 点,用 4 个虚拟的控制点通过加权和的形式表达
- 这种虚拟控制点的加权和关系,经过相机位姿 [ R ∣ ∣ t ] [R∣∣t] [R∣∣t] 变换到相机坐标系下仍然成立,既对应点的加权系数不变
- 根据2D像素坐标估计出 4 个虚拟的控制点在相机坐标系的坐标,可以将 3D-2D 的映射问题转换为 3D-3D 点映射问题
总体上,EPnP 算法可以分为四步:
- 计算世界坐标系下的虚拟控制点
- 计算表达各个3D点的控制点权重
- 求解相机坐标系下的控制点坐标
- ICP 求解相机位姿
2.1. 计算4对控制点的世界坐标
下面是其具体实现的函数 compute_pose
的代码片段
它有两个参数 R, t 分别输出相机的旋转矩阵和平移向量
用于求解相机位姿的匹配点信息已经保存在成员变量 pws
和 us
中
/**
* @brief 估计相机位姿函数
* @param R 相机的旋转矩阵
* @param t 相机的平移向量
* @return rep_errors 重投影误差
*/
double PnPsolver::compute_pose(double R[3][3], double t[3])
{
/* 计算世界坐标系下的四个虚拟控制点 */
choose_control_points();
该函数一开始,先调用函数 choose_control_points
计算了世界坐标系下的四个虚拟控制点
2.2. 计算齐次质心坐标
理论上控制点的选取可以是任意的
但论文中说,以 3D点的质心以及PCA分解后的三个主轴上选取的三个点
所构成的控制点有助于提高算法的稳定性
/* 计算齐次质心坐标 */
compute_barycentric_coordinates();
把计算得到的 4 个控制点的世界坐标记为
c
j
w
,
j
=
1
,
⋯
,
4
\boldsymbol{c_j^w}, j = 1, \cdots, 4
cjw,j=1,⋯,4
那么世界坐标系下的第
i
i
i 个 3D 点坐标都可以写成这四个控制点的加权和形式
p
i
w
=
∑
j
=
1
4
α
i
j
c
j
w
,
with
∑
j
=
1
4
α
i
j
=
1
\boldsymbol{p_i^w} = \sum_{j=1}^4 \alpha_{ij} \boldsymbol{c_j^w}, \text{with} \sum_{j=1}^4 \alpha_{ij} = 1
piw=j=1∑4αijcjw,withj=1∑4αij=1
α
i
j
\alpha_{ij}
αij 齐次质心坐标,可以将上式写成齐次矩阵的形式,求解得到
[
p
i
w
1
]
=
[
c
1
w
c
2
w
c
3
w
c
4
w
1
1
1
1
]
⏟
C
[
α
i
1
α
i
2
α
i
3
α
i
4
]
⇒
[
α
i
1
α
i
2
α
i
3
α
i
4
]
=
C
−
1
[
p
i
w
1
]
\begin{bmatrix} \boldsymbol{p_i^w} \\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} \boldsymbol{c_1^w} & \boldsymbol{c_2^w} & \boldsymbol{c_3^w} & \boldsymbol{c_4^w} \\ 1 & 1 & 1 & 1 \end{bmatrix}}_{C} \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix} \Rightarrow \begin{bmatrix} \alpha_{i1} \\ \alpha_{i2} \\ \alpha_{i3} \\ \alpha_{i4} \end{bmatrix} = C^{-1} \begin{bmatrix} \boldsymbol{p_i^w} \\ 1 \end{bmatrix}
[piw1]=C
[c1w1c2w1c3w1c4w1]
αi1αi2αi3αi4
⇒
αi1αi2αi3αi4
=C−1[piw1]
通过函数 compute_barycentric_coordinates
计算得到
结果将被保存在成员变量
a
a
a 中
该函数并没有直接构建 矩阵
C
C
C 然后对其求逆
而是将 3D 坐标和四个控制点都减去质心,再进行求解
2.3. 计算4对控制点的相机坐标
2.3.1. 构造M矩阵
世界坐标系下的点
p
i
w
\boldsymbol{p_i^w}
piw 经过变换
[
R
∣
t
]
[R∣t]
[R∣t] 之后得到相机坐标系下的坐标
p
i
c
\boldsymbol{p_i^c}
pic
对控制点
c
j
w
\boldsymbol{c_j^w}
cjw 做相同的变换得到
c
j
j
\boldsymbol{c_j^j}
cjj,仍然满足相同的加权关系,即:
p
i
c
=
∑
j
=
1
4
α
i
j
c
j
c
,
with
∑
j
=
1
4
α
i
j
=
1
\boldsymbol{p_i^c} = \sum_{j=1}^4 \alpha_{ij} \boldsymbol{c_j^c}, \text{with} \sum_{j=1}^4 \alpha_{ij} = 1
pic=j=1∑4αijcjc,withj=1∑4αij=1
再考虑到针孔相机模型,对于每一个 3D-2D 匹配点关系,都有下式成立:
∀
i
,
w
i
[
u
i
v
i
1
]
=
[
f
u
0
u
c
0
f
v
v
c
0
0
1
]
(
∑
j
=
1
4
α
i
j
[
x
j
c
y
j
c
z
j
c
]
)
\forall i, w_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \begin{bmatrix} f_u & 0 & u_c \\ 0 & f_v & v_c \\ 0 & 0 & 1 \end{bmatrix}\left( \sum_{j=1}^4 \alpha_{ij} \begin{bmatrix} x_j^c \\ y_j^c \\ z_j^c \end{bmatrix}\right)
∀i,wi
uivi1
=
fu000fv0ucvc1
j=1∑4αij
xjcyjczjc
其中,
c
j
c
T
=
[
x
j
c
y
j
c
z
j
c
]
\boldsymbol{{c_j^c}}^T = \begin{bmatrix} x_j^c & y_j^c & z_j^c \end{bmatrix}
cjcT=[xjcyjczjc]是第
j
j
j个控制点在相机坐标系下的坐标
u
i
,
v
i
u_i, v_i
ui,vi是2D像素坐标,
f
u
,
f
v
,
u
c
,
v
c
f_u, f_v, u_c, v_c
fu,fv,uc,vc分别是相机的焦距和光心
w
i
w_i
wi是一个尺度因子,将在后续计算中约去
参照 DLT 算法的套路,每个点都可以写出两个约束,
n
n
n个点可以构造如下的
M
x
=
0
\boldsymbol{Mx} = \boldsymbol{0}
Mx=0 形式的线性方程组:
[
α
11
f
x
0
α
11
(
c
x
−
u
1
)
⋯
α
14
f
x
0
α
14
(
c
x
−
u
1
)
0
α
11
f
y
α
11
(
c
y
−
v
1
)
⋯
0
α
14
f
y
α
14
(
c
y
−
v
1
)
⋮
⋮
⋮
⋮
⋮
⋮
α
n
1
f
x
0
α
n
1
(
c
x
−
u
1
)
⋯
α
n
4
f
x
0
α
n
4
(
c
x
−
u
1
)
0
α
n
1
f
y
α
n
1
(
c
y
−
v
1
)
⋯
0
α
n
4
f
y
α
n
4
(
c
y
−
v
1
)
]
[
c
1
c
⋮
c
4
c
]
=
0
\begin{bmatrix} \alpha_{11} f_x & 0 & \alpha_{11}(c_x - u_1) & \cdots & \alpha_{14} f_x & 0 & \alpha_{14}(c_x - u_1) \\ 0 & \alpha_{11} f_y & \alpha_{11}(c_y - v_1) & \cdots & 0 & \alpha_{14} f_y & \alpha_{14}(c_y - v_1) \\ \vdots & \vdots & \vdots & & \vdots & \vdots & \vdots \\ \alpha_{n1} f_x & 0 & \alpha_{n1}(c_x - u_1) & \cdots & \alpha_{n4} f_x & 0 & \alpha_{n4}(c_x - u_1) \\ 0 & \alpha_{n1} f_y & \alpha_{n1}(c_y - v_1) & \cdots & 0 & \alpha_{n4} f_y & \alpha_{n4}(c_y - v_1) \\ \end{bmatrix} \begin{bmatrix} \boldsymbol{c_1^c} \\ \vdots \\ \boldsymbol{c_4^c} \end{bmatrix} = \boldsymbol{0}
α11fx0⋮αn1fx00α11fy⋮0αn1fyα11(cx−u1)α11(cy−v1)⋮αn1(cx−u1)αn1(cy−v1)⋯⋯⋯⋯α14fx0⋮αn4fx00α14fy⋮0αn4fyα14(cx−u1)α14(cy−v1)⋮αn4(cx−u1)αn4(cy−v1)
c1c⋮c4c
=0
其中, x = [ c 1 c T c 2 c T c 3 c T c 4 c T ] T \boldsymbol{x} = \begin{bmatrix} \boldsymbol{{c_1^c}}^T & \boldsymbol{{c_2^c}}^T & \boldsymbol{{c_3^c}}^T & \boldsymbol{{c_4^c}}^T \end{bmatrix}^T x=[c1cTc2cTc3cTc4cT]T
求解出 x x x 就得到了相机坐标系下的控制点坐标
在下面的代码片段中,先通过 OpenCV 的接口创建了一个
2
n
×
12
2n×12
2n×12 的矩阵
再通过函数 fill_M
完成上式中矩阵
M
M
M 的构建工作
/* 构造M矩阵 */
CvMat *M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);
for (int i = 0; i < number_of_correspondences; i++)
fill_M(M, 2 * i, alphas + 4 * i, us[2 * i], us[2 * i + 1]);
2.3.2. 计算 M T M M^TM MTM的0特征值对应的特征向量
根据线性代数的说法,这个方程组的解一定属于
M
M
M 的零空间
既
x
x
x 可以写成
M
M
M 的零空间正交基的线性组合
x
=
∑
i
=
1
N
β
i
v
i
\boldsymbol{x} = \sum_{i = 1}^N { \beta_i \boldsymbol{v}_i }
x=i=1∑Nβivi
其中,
v
i
,
i
∈
[
1
,
N
]
\boldsymbol{v}_i, i \in [1, N]
vi,i∈[1,N] 是
M
M
M 的 零空间正交基
既该矩阵的
N
N
N 个为 0 的奇异值所对应的列向量
β
i
β_i
βi 是线性组合的系数
下面的代码中, 先构建了局部变量 MtM, D, Ut
分别用来记录矩阵 MTM,以及它分解的特征值和特征向量
OpenCV 的接口 cvMulTransposed
根据 M
生成 MTM
,cvSVD
完成实际的SVD分解操作
/* 定义矩阵MTM,分解的特征值和特征向量 */
double mtm[12 * 12], d[12], ut[12 * 12];
CvMat MtM = cvMat(12, 12, CV_64F, mtm);
CvMat D = cvMat(12, 1, CV_64F, d);
CvMat Ut = cvMat(12, 12, CV_64F, ut);
/* 通过SVD分解,计算MTM的0特征值对应的特征向量 */
cvMulTransposed(M, &MtM, 1);
cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
cvReleaseMat(&M);
通过对 12×12 的矩阵 M T M \boldsymbol{M}^T \boldsymbol{M} MTM 进行奇异值分解可以得到 v i , i ∈ [ 1 , N ] \boldsymbol{v}_i, i \in [1, N] vi,i∈[1,N]
2.3.3. 计算零空间的秩
现在,还需要确定零空间的秩
N
N
N 和线性组合系数
β
i
β_i
βi
论文中通过一些实验发现,
N
N
N 跟相机的焦距之间呈现一种正相关的关系
下面是从原文中抠出来的一张图,图中横坐标是MTM的12个奇异值, 纵轴是奇异值的大小
不同的曲线对应着不同的焦距,随着焦距的增大,曲线不断下移
从右侧放大的图中可以看到,
f
=
10000
f=10000
f=10000 时也就只有4个奇异值接近为0
于是乎,作者就针对
N
∈
[
1
,
4
]
N∈[1,4]
N∈[1,4] 的四种情况分别讨论了系数
β
i
β_i
βi的计算方法
2.3.4. 计算线性组合的系数
控制点坐标是经过旋转和平移,从世界坐标系转换到相机坐标系的
这两种变换都不会改变控制点之间的距离,因此存在如下的关系式:
∥
c
i
c
−
c
i
c
∥
=
∥
c
i
w
−
c
j
w
∥
⇒
∑
k
=
1
N
∥
β
k
v
k
[
i
]
−
β
k
v
k
[
j
]
∥
=
∥
c
i
w
−
c
j
w
∥
\begin{array}{rc} & \| \boldsymbol{c_i^c} - \boldsymbol{c_i^c} \| = \| \boldsymbol{c_i^w} - \boldsymbol{c_j^w} \| \\ \Rightarrow & \sum_{k = 1}^N \| { \beta_k \boldsymbol{v}_k^{[i]} } - { \beta_k \boldsymbol{v}_k^{[j]} } \| = \| \boldsymbol{c_i^w} - \boldsymbol{c_j^w} \| \\ \end{array}
⇒∥cic−cic∥=∥ciw−cjw∥∑k=1N∥βkvk[i]−βkvk[j]∥=∥ciw−cjw∥
其中,
v
k
[
i
]
\boldsymbol{v}_k^{[i]}
vk[i] 是零空间中特征向量
v
k
\boldsymbol{v}_k
vk 与
c
i
c
\boldsymbol{c_i^c}
cic 对应的 3×1 的子向量
EPnP 的论文中说是针对 N=1,⋯4 四种情况分别计算了部分 β 的取值
然后通过高斯-牛顿的方法求解所有的 β值,构建相机坐标系下的控制点
c
i
c
\boldsymbol{c_i^c}
cic
接着,求解 ICP 问题计算相机的姿态矩阵和平移向量,
实际看代码,好像只依次讨论了
N
=
4
,
2
,
3
N=4,2,3
N=4,2,3 三种情况
再根据路标点在世界坐标系中的坐标和相机坐标系中的坐标,使用ICP估计相机位姿
double l_6x10[6 * 10], rho[6];
CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
CvMat Rho = cvMat(6, 1, CV_64F, rho);
compute_L_6x10(ut, l_6x10); /* SVD分解出的U矩阵和L矩阵 */
compute_rho(rho); /* 计算世界坐标系下的四个控制点两两之间的距离 */
double Betas[4][4], rep_errors[4];
double Rs[4][3][3], ts[4][3];
find_betas_approx_1(&L_6x10, &Rho, Betas[1]); /* 线性组合的系数 N = 4所有的β值 */
gauss_newton(&L_6x10, &Rho, Betas[1]); /* 通过高斯-牛顿的方法求解所有的β值 */
rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]); /* 计算相机位姿变换并返回重投影误差 */
/* 线性组合的系数 N = 2 */
find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
gauss_newton(&L_6x10, &Rho, Betas[2]);
rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
/* 线性组合的系数 N = 3 */
find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
gauss_newton(&L_6x10, &Rho, Betas[3]);
rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]);
2.4. 选择最小重投影误差
选取使得重投影误差最小的那组作为最后的解
/* 选取最小重投影误差的那个作为最后的解 */
int N = 1;
if (rep_errors[2] < rep_errors[1])
N = 2;
if (rep_errors[3] < rep_errors[N])
N = 3;
copy_R_and_t(Rs[N], ts[N], R, t);
return rep_errors[N];
3. 标题
谢谢