本文主要是介绍SLAM ORB-SLAM2(29)PnP估计姿态,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
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. 标题
谢谢
这篇关于SLAM ORB-SLAM2(29)PnP估计姿态的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!