(01)ORB-SLAM2源码无死角解析-(43) EPnP 源代码分析(3)→find_betas_approx(),gauss_newton()
讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
文末正下方中心提供了本人
联系方式，
点击本人照片即可显示
W
X
→
官方认证
{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}
文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
上一篇博客对 src/PnPsolver.cc 文件中的 compute_pose() 函数分析，并且对其中调用的 choose_control_points()，compute_barycentric_coordinates() 进行了讲解，知道了代码中是如何求得 四个控制点，以及它们对应的系数，并且进一步求得了 相机坐标系下的控制点。有了这些信息，根据前面博客 (01)ORB-SLAM2源码无死角解析-(38) EPnP 算法原理详解→理论基础二:分情况求得beta初始解 可以建立矩阵方程：
N
=
4
的情况
:
\color{blue}{N=4的情况}:
N=4的情况:
L
6
×
4
⋅
β
4
×
1
=
ρ
6
×
1
(16)
\color{Green} \tag{16} \mathbf{L}_{6 \times 4} \cdot \boldsymbol{\beta}_{4 \times 1}=\boldsymbol{\rho}_{6 \times 1}
L6×4⋅β4×1=ρ6×1(16)
N
=
3
的情况
:
\color{blue}{N=3的情况}:
N=3的情况:
L
6
×
5
⋅
β
5
×
1
=
ρ
6
×
1
(19)
\color{Green} \tag{19} \mathbf{L}_{6 \times 5} \cdot \boldsymbol{\beta}_{5 \times 1}=\boldsymbol{\rho}_{6 \times 1}
L6×5⋅β5×1=ρ6×1(19)
N
=
2
的情况
:
\color{blue}{N=2的情况}:
N=2的情况:
L
6
×
3
⋅
β
3
×
1
=
ρ
6
×
1
(22)
\color{Green} \tag{22} \mathbf{L}_{6 \times 3} \cdot \boldsymbol{\beta}_{3 \times 1}=\boldsymbol{\rho}_{6 \times 1}
L6×3⋅β3×1=ρ6×1(22)
N
=
1
的情况
(
存在闭解
,
直接求解即可
)
:
\color{blue}{N=1的情况(存在闭解,直接求解即可)}:
N=1的情况(存在闭解,直接求解即可):
β
=
∑
{
i
,
j
}
∈
[
1
:
4
]
∥
v
[
i
]
−
v
[
j
]
∥
⋅
∥
c
i
w
−
c
j
w
∥
∑
{
i
,
j
}
∈
[
1
:
4
]
∥
v
[
i
]
−
v
[
j
]
∥
2
(06)
\color{Green} \tag{06} \beta=\frac{\sum_{\{i, j\} \in[1: 4]}\left\|\mathbf{v}^{[i]}-\mathbf{v}^{[j]}\right\| \cdot\left\|\mathbf{c}_{i}^{w}-\mathbf{c}_{j}^{w}\right\|}{\sum_{\{i, j\} \in[1: 4]}\left\|\mathbf{v}^{[i]}-\mathbf{v}^{[j]}\right\|^{2}}
β=∑{i,j}∈[1:4]∥
∥v[i]−v[j]∥
∥2∑{i,j}∈[1:4]∥
∥v[i]−v[j]∥
∥⋅∥
∥ciw−cjw∥
∥(06)
通过如上方程即可求得
β
\boldsymbol{\beta}
β 的初始解，进一步通过高斯牛顿迭代对其进行优化。下面我们就来看看源码中的实现，其代码都位于src/PnPsolver.cc 文件中，在 compute_pose() 函数中可以看到如下代码:
// 求解近似解：N=4的情况
find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
// 高斯牛顿法迭代优化得到 beta
gauss_newton(&L_6x10, &Rho, Betas[1]);
// 求解近似解：N=2的情况
find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
gauss_newton(&L_6x10, &Rho, Betas[2]);
// 求解近似解：N=3的情况
find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
gauss_newton(&L_6x10, &Rho, Betas[3]);
二、find_betas_approx_1
/**
* @brief 计算N=4时候的粗糙近似解，暴力将其他量置为0
*
* @param[in] L_6x10 矩阵L
* @param[in] Rho 非齐次项 \rho, 列向量
* @param[out] betas 计算得到的beta
*/
void PnPsolver::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
{
// 计算N=4时候的粗糙近似解，暴力将其他量置为0
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] -- L_6x10中每一行的内容
// betas_approx_1 = [B11 B12 B13 B14 ] -- L_6x4 中一行提取出来的内容
double l_6x4[6 * 4], b4[4];
CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
CvMat B4 = cvMat(4, 1, CV_64F, b4);
// 提取L_6x10矩阵中每行的第0,1,3,6个元素，得到L_6x4
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0)); //将L_6x10的第i行的第0个元素设置为L_6x4的第i行的第0个元素
cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3));
cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));
}
// SVD方式求解方程组 L_6x4 * B4 = Rho
cvSolve(&L_6x4, Rho, &B4, CV_SVD);
// 得到的解是 b00 b01 b02 b03 因此解出来b00即可
if (b4[0] < 0) {
betas[0] = sqrt(-b4[0]);
betas[1] = -b4[1] / betas[0];
betas[2] = -b4[2] / betas[0];
betas[3] = -b4[3] / betas[0];
} else {
betas[0] = sqrt(b4[0]);
betas[1] = b4[1] / betas[0];
betas[2] = b4[2] / betas[0];
betas[3] = b4[3] / betas[0];
}
}
三、find_betas_approx_2
/**
* @brief 计算N=2时候的粗糙近似解，暴力将其他量置为0
*
* @param[in] L_6x10 矩阵L
* @param[in] Rho 非齐次项 \rho, 列向量
* @param[out] betas 计算得到的beta
*/
void PnPsolver::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
{
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_2 = [B11 B12 B22 ]
double l_6x3[6 * 3], b3[3];
CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3);
CvMat B3 = cvMat(3, 1, CV_64F, b3);
// 提取
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2));
}
// 求解方程组
cvSolve(&L_6x3, Rho, &B3, CV_SVD);
// 从b11 b12 b22 中恢复 b1 b2
if (b3[0] < 0) {
betas[0] = sqrt(-b3[0]);
betas[1] = (b3[2] < 0) ? sqrt(-b3[2]) : 0.0;
} else {
betas[0] = sqrt(b3[0]);
betas[1] = (b3[2] > 0) ? sqrt(b3[2]) : 0.0;
}
if (b3[1] < 0) betas[0] = -betas[0];
// 这俩没有使用到
betas[2] = 0.0;
betas[3] = 0.0;
}
四、find_betas_approx_3
/**
* @brief 计算N=3时候的粗糙近似解，暴力将其他量置为0
*
* @param[in] L_6x10 矩阵L
* @param[in] Rho 非齐次项 \rho, 列向量
* @param[out] betas 计算得到的beta
*/
void PnPsolver::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
double * betas)
{
// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
// betas_approx_3 = [B11 B12 B22 B13 B23 ]
double l_6x5[6 * 5], b5[5];
CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
CvMat B5 = cvMat(5, 1, CV_64F, b5);
// 获取并构造矩阵
for(int i = 0; i < 6; i++) {
cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0));
cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1));
cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2));
cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3));
cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4));
}
// 求解这个方程组
cvSolve(&L_6x5, Rho, &B5, CV_SVD);
// 从 B11 B12 B22 B13 B23 中恢复出 B1 B2 B3
if (b5[0] < 0) {
betas[0] = sqrt(-b5[0]);
betas[1] = (b5[2] < 0) ? sqrt(-b5[2]) : 0.0;
} else {
betas[0] = sqrt(b5[0]);
betas[1] = (b5[2] > 0) ? sqrt(b5[2]) : 0.0;
}
if (b5[1] < 0) betas[0] = -betas[0];
betas[2] = b5[3] / betas[0];
// N=3的时候没有B4
betas[3] = 0.0;
}
六、gauss_newton
通过上面的代码注释，很详细的展示出了分情况求得beta初始解的过程，其原理也比较简单，就是先建立缩小版本的 L β = ρ \mathbf L \boldsymbol {\beta}=\boldsymbol{\rho} Lβ=ρ 矩阵方程，然后通过 SVD奇异值分解获得最优解。进一步通过高斯牛顿迭代对其进行优化，代码注释如下：
/**
* @brief 对计算出来的Beta结果进行高斯牛顿法优化,求精. 过程参考EPnP论文中式(15)
*
* @param[in] L_6x10
* @param[in] Rho
* @param[in] betas
*/
void PnPsolver::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,
double betas[4])
{
// 只进行5次迭代
const int iterations_number = 5;
/** 这里是求解增量方程组Ax=B,其中的x就是增量. 根据论文中的式15,可以得到优化的目标函数为:
* \f$ f(\mathbf{\beta})=\sum_{(i,j \ s.t. \ i<j)}
* \left( ||\mathbf{c}^c_i-\mathbf{c}^c_j ||^2 - ||\mathbf{c}^w_i-\mathbf{c}^w_j ||^2 \right) \f$
* 而根据高斯牛顿法,增量方程为:
* \f$ \mathbf{H}\mathbf{\Delta x}=\mathbf{g} \f$
* 也就是:(参考视觉SLAM十四讲第一版P112式6.21 6.22)
* \f$ \mathbf{J}^T\mathbf{J}\mathbf{\Delta x}=-\mathbf{J}^T f(x) \f$
* 不过这里在计算的时候将等式左右两边的雅克比 \f$ \mathbf{J}^T \f$ 都给约去了,得到精简后的增量方程:
* \f$ \mathbf{J}\mathbf{\Delta x}=-f(x) \f$
* 然后分别对应为程序代码中的系数矩阵A和非齐次项B.
*/
double a[6*4], b[6], x[4];
CvMat A = cvMat(6, 4, CV_64F, a); // 系数矩阵
CvMat B = cvMat(6, 1, CV_64F, b); // 非齐次项
CvMat X = cvMat(4, 1, CV_64F, x); // 增量，待求量
// 对于每次迭代过程
for(int k = 0; k < iterations_number; k++) {
// 计算增量方程的系数矩阵和非齐次项
compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db,
betas, &A, &B);
// 使用QR分解来求解增量方程,解得当前次迭代的增量X
qr_solve(&A, &B, &X);
// 应用增量,对估计值进行更新;估计值是beta1~beta4组成的向量
for(int i = 0; i < 4; i++)
betas[i] += x[i];
}
}
从源码上看，也就是比较建档的，就是求得增量方程的系数矩阵和非齐次项,然后建立增量方程，求得增量
Δ
β
\Delta \boldsymbol{\beta}
Δβ。其具体理论过程可以参考前面的博客: (01)ORB-SLAM2源码无死角解析-(39) EPnP 算法原理详解→理论基础三:高斯牛顿迭代。其核心就是求得一阶雅克比矩阵:
J
=
∂
f
(
β
)
β
=
[
∂
f
(
β
)
∂
β
1
∂
f
(
β
)
∂
β
2
∂
f
(
β
)
∂
β
3
∂
f
(
β
)
∂
β
4
]
=
[
∂
(
L
β
)
∂
β
1
∂
(
L
β
)
∂
β
2
∂
(
L
β
)
∂
β
3
∂
(
L
β
)
∂
β
4
]
(15)
\color{Green} \tag{15} \begin{aligned} \mathbf{J}=\frac{\partial f(\boldsymbol{\beta})}{\boldsymbol{\beta}} &=\left[\begin{array}{llll} \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{1}} & \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{2}} & \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{3}} & \frac{\partial f(\boldsymbol{\beta})}{\partial \beta_{4}} \end{array}\right] \\ &=\left[\begin{array}{llll} \frac{\partial(\mathbf{L} \boldsymbol{\beta})}{\partial \beta_{1}} & \frac{\partial(\mathbf{L} \boldsymbol{\beta})}{\partial \beta_{2}} & \frac{\partial(\mathbf{L} \beta)}{\partial \beta_{3}} & \frac{\partial(\mathbf{L} \boldsymbol{\beta})}{\partial \beta_{4}} \end{array}\right] \end{aligned}
J=β∂f(β)=[∂β1∂f(β)∂β2∂f(β)∂β3∂f(β)∂β4∂f(β)]=[∂β1∂(Lβ)∂β2∂(Lβ)∂β3∂(Lβ)∂β4∂(Lβ)](15)
以及
Δ
y
=
f
(
β
)
=
L
β
−
ρ
(19)
\color{Green} \tag{19} \Delta \mathbf{y}=f(\boldsymbol{\beta})=\mathbf{L} \boldsymbol{\beta}-\boldsymbol{\rho}
Δy=f(β)=Lβ−ρ(19)其上的
J
\mathbf{J}
J 对应源码中的
A
A
A,
Δ
y
\Delta \mathbf{y}
Δy 对应源码中的
B
B
B。这样能构建增量方程：
J
Δ
β
=
Δ
y
\mathbf{J} \Delta \boldsymbol{\beta}= \Delta \mathbf{y}
JΔβ=Δy，源码构建的方程为：
A
Δ
β
=
Δ
B
(30)
\color{Green} \tag{30} \mathbf{A} \Delta \boldsymbol{\beta}= \Delta \mathbf{B}
AΔβ=ΔB(30)
compute_A_and_b_gauss_newton 代码的注释如下：
/**
* @brief 计算高斯牛顿法优化时,增量方程中的系数矩阵和非齐次项
* @param[in] l_6x10 L矩阵
* @param[in] rho Rho矩向量
* @param[in] cb 当前次迭代得到的beta1~beta4
* @param[out] A 计算得到的增量方程中的系数矩阵
* @param[out] b 计算得到的增量方程中的非齐次项
*/
void PnPsolver::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
double betas[4], CvMat * A, CvMat * b)
{
// 以下推导就是求解一阶雅克比矩阵
// /* 根据前面函数 gauss_newton 中的一些工作,可以发现这里的系数矩阵其实就是目标函数雅克比的转置. 原目标函数:
// * \f$ f(\mathbf{\beta})=\sum_{(i,j \ s.t. \ i<j)} \left( ||\mathbf{c}^c_i-\mathbf{c}^c_j ||^2 - ||\mathbf{c}^w_i-\mathbf{c}^w_j ||^2 \right) \f$
// * 然后观察一下每一项的组成: \f$ ||\mathbf{c}^c_i-\mathbf{c}^c_j ||^2 \f$ 由论文中式12可以发现就对应着矩阵 L 中的一行,
// * 同样地对于 \f$ ||\mathbf{c}^w_i-\mathbf{c}^w_j ||^2 \f$ 则对应着式(13)中向量 \f$ \mathbf{\rho} \f$ 的每一行.所以目标函数完全可以写成矩阵的形式:
// * \f$ f(\mathbf{\beta})=\mathbf{L}\mathbf{\bar{\beta}}-\mathbf{\rho} \f$
// * 注意这里使用的符号:
// * \f$ \mathbf{\bar{\beta}}= \begin{bmatrix} \beta_{11}\\\beta_{12}\\\beta_{22}\\\beta_{13}\\\beta_{23}\\\beta_{33}\\\beta_{14}\\\beta_{24}\\\beta_{34}\\\beta_{44} \end{bmatrix} \f$
// * 为了方便求导,计算得到一个中间结果先:
// * \f$ \begin{split}
// * \mathbf{\bar{L}}&=\mathbf{L}\mathbf{\bar{\beta}}\\
// * &=
// * \begin{bmatrix}
// * L_{11}\beta_{11}+L_{12}\beta_{12}+L_{13}\beta_{22}+\cdots+L_{1A}\beta_{44}\\
// * L_{21}\beta_{11}+L_{22}\beta_{22}+L_{13}\beta_{22}+\cdots+L_{2A}\beta_{44}\\
// * \cdots\\
// * L_{61}\beta_{11}+L_{62}\beta_{22}+L_{63}\beta_{22}+\cdots+L_{6A}\beta_{44}\\
// * \end{bmatrix}
// * &=
// * \begin{bmatrix}
// * \mathbf{L_1}\\\mathbf{L_2}\\\cdots\\\mathbf{L_3}
// * \end{bmatrix}
// * \end{split} \f$
// * 然后原来的目标函数矩阵表示变成为了:
// * \f$ f(\mathbf{\beta})=\mathbf{\bar{L}}-\mathbf{\rho} \f$
// * 接下来准备求目标函数的雅克比.注意到只有矩阵 \f$ \mathbf{\bar{L}} \f$ 和优化变量 \f$ \mathbf{\beta} \f$ 有关系,因此有:
// * \f$ \begin{split}
// * \frac{\partial f(\mathbf{\beta})}{\partial \mathbf{\beta}}&=\frac{\partial \mathbf{L}}{\partial \mathbf{\beta}}\\
// * &=
// * \begin{bmatrix}
// * \frac{\partial \mathbf{L}}{\partial \beta_1}&\frac{\partial \mathbf{L}}{\partial \beta_2}&
// * \frac{\partial \mathbf{L}}{\partial \beta_3}&\frac{\partial \mathbf{L}}{\partial \beta_4}
// * \end{bmatrix} \\
// * &=
// * \begin{bmatrix}
// * \frac{\partial \mathbf{L}_1}{\partial \beta_1}&\frac{\partial \mathbf{L}_1}{\partial \beta_2}&
// * \frac{\partial \mathbf{L}_1}{\partial \beta_3}&\frac{\partial \mathbf{L}_1}{\partial \beta_4}\\
// * \frac{\partial \mathbf{L}_2}{\partial \beta_1}&\frac{\partial \mathbf{L}_2}{\partial \beta_2}&
// * \frac{\partial \mathbf{L}_2}{\partial \beta_3}&\frac{\partial \mathbf{L}_2}{\partial \beta_4}\\
// * \cdots&\cdots&\cdots&\cdots\\
// * \frac{\partial \mathbf{L}_6}{\partial \beta_1}&\frac{\partial \mathbf{L}_6}{\partial \beta_2}&
// * \frac{\partial \mathbf{L}_6}{\partial \beta_3}&\frac{\partial \mathbf{L}_6}{\partial \beta_4}
// * \end{bmatrix}
// * \end{split} \f$
// * 从优化目标函数的概念触发,其中的每一行的约束均由一对点来提供,因此不同行之间其实并无关系,可以相互独立地计算,因此对于其中的每一行:(以第一行为例)
// * \f$ \mathbf{L}_1=
// * \beta_{11}L_{11}+\beta_{12}L_{12}+\beta_{22}L_{13}+\beta_{13}L_{14}+\beta_{23}L_{15}+
// * \beta_{33}L_{16}+\beta_{14}L_{17}+\beta_{24}L_{18}+\beta_{34}L_{19}+\beta_{44}L_{1A} \f$
// * 分别对beat进行求导:(注意为了方便这里把L的下标从1开始变成了从0开始)
// * \f$ \frac{\partial \mathbf{L}_1}{\partial \beta_1}=2\beta_1L_{10}+\beta_2L_{11}+\beta_3L_{13}+\beta_4L_{16} \\
// * \frac{\partial \mathbf{L}_1}{\partial \beta_2}=\beta_1L_{11}+2\beta_2L_{12}+\beta_3L_{14}+\beta_4L_{17} \\
// * \frac{\partial \mathbf{L}_1}{\partial \beta_3}=\beta_1L_{13}+\beta_2L_{14}+2\beta_3L_{15}+\beta_4L_{18} \\
// * \frac{\partial \mathbf{L}_1}{\partial \beta_4}=\beta_1L_{16}+\beta_2L_{17}+\beta_3L_{18}+2\beta_4L_{19} \f$
// * 就是下面计算每一行的雅克比的式子.
// *
// * 另外对于当前行的非齐次项, 在 gauss_newton 中简化后得到的结果为 -f(x), 也就是:
// * \f$ ||\mathbf{c}^w_i-\mathbf{c}^w_j ||^2 - ||\mathbf{c}^c_i-\mathbf{c}^c_j ||^2 \f$
// * 每一行都会有一个特定的i和j.上式中的前者可以直接由 \f$ \mathbf{\rho} \f$ 的对应行给定,而后者则要根据论文公式(12)给出了:
// * \f$ ||\mathbf{c}^c_i-\mathbf{c}^c_j ||^2 = \mathbf{L}_k\mathbf{\bar{\beta}} \f$
// * 这个也就是非齐次项部分的计算过程
// */
// 一共有六个方程组, 对每一行(也就是每一个方程展开遍历);
// 从优化目标函数的概念出发,其中的每一行的约束均由一对点来提供,因此不同行之间其实并无关系,可以相互独立地计算
for(int i = 0; i < 6; i++) {
// 获得矩阵L中的行指针
const double * rowL = l_6x10 + i * 10;
double * rowA = A->data.db + i * 4;
// Step 1: 计算当前行的雅克比
rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3];
rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3];
rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3];
rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
// Step 2: 计算当前行的非齐次项
cvmSet(b, i, 0, rho[i] -
( // 从0开始的下标 | 从1开始的下标
rowL[0] * betas[0] * betas[0] + //b00 b11
rowL[1] * betas[0] * betas[1] + //b01 b12
rowL[2] * betas[1] * betas[1] + //b11 b22
rowL[3] * betas[0] * betas[2] + //b02 b13
rowL[4] * betas[1] * betas[2] + //b12 b23
rowL[5] * betas[2] * betas[2] + //b22 b33
rowL[6] * betas[0] * betas[3] + //b03 b14
rowL[7] * betas[1] * betas[3] + //b13 b24
rowL[8] * betas[2] * betas[3] + //b23 b34
rowL[9] * betas[3] * betas[3] //b33 b44
));
}
}
六、结语
通过上面博客的介绍，最终在高斯牛顿(gauss_newton)函数中建立了增量方程 A Δ β = Δ B \mathbf{A} \Delta \boldsymbol{\beta}= \Delta \mathbf{B} AΔβ=ΔB，那么下一步就是求解增量 Δ B \Delta \mathbf{B} ΔB。源码中是通过 QR分解(豪斯霍尔德Householder变换) 实现的，对应于上面的函数 qr_solve(&A, &B, &X)，在下一篇博客中会进行详细的讲解。
本文内容来自计算机视觉life ORB-SLAM2 课程课件
相关文章
- 【Redis源码】setnx、setex、psetex命令
- 【Flutter&Flame 游戏 - 贰伍】pinball 源码分析 - 资源加载与 Loading
- Redisson 高性能 Redis 分布式锁源码分析
- Postgresql源码（69）常规锁简单分析
- Android之rild进程启动源码分析「建议收藏」
- 微服务组件-----Spring Cloud Alibaba 注册中心 Nacos源码（1.4.x版本）分析
- flutter:一个bug的源码分析
- Android 时钟TextClock 使用及源码分析
- minHook源码阅读分析
- 爱说JDK集合-List源码剖析
- 重申dubbo源码阅读顺序
- react源码分析：babel如何解析jsx
- react的useState源码分析2
- ruoyi-vue版本框架（二）源码目录结构的讲解，与底层子项目的讲解
- 3. 「snabbdom@3.5.1 源码分析」patch(如何打补丁？)
- React源码分析(一)Fiber_2023-03-15
- 【Android 电量优化】JobScheduler 相关源码分析 ( ConnectivityController 底层源码分析 | 构造函数 | 追踪任务更新 | 注册接收者监听连接变化 )
- 【Android 异步操作】Handler 机制 ( Handler 常用用法 | HandlerThread 简介 | HandlerThread 源码注释分析 )
- 【Android 事件分发】事件分发源码分析 ( ViewGroup 事件传递机制 三 )
- YARN源码分析(一)—–ApplicationMaster详解大数据
- 一份精辟的俄罗斯方块Java源码（335行）详解编程语言
- TreeMap实现原理及源码分析详解编程语言
- Linux之父开发分支：基于源码编译系统建立（linux的分支）
- 如何才能顺利下载Linux源码包？（下载linux源码包）
- 分析48小时解析MySQL源码，实现自己的研发梦想（48节mysql源码）
- 深度分析TP6中Redis源码解析（tp redis 源码）
- MySQL源码包下载与使用教程详解（mysql下载源码包教程）