(01)ORB-SLAM2源码无死角解析-(45) EPnP 源代码分析(5)→ICP 求解相机位姿

讲解关于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→官方认证

一、前言

在前面的博客 (01)ORB-SLAM2源码无死角解析-(37) EPnP 算法原理详解→理论基础一:控制点选取、透视投影约束的(09)式，如下(记3D点在世界坐标系下的坐标及对应相机坐标系下的坐标分别是 p i w , p i c , i = 1 , … , n \mathbf{p}_{i}^{w}, \mathbf{p}_{i}^{c}, \quad i=1, \ldots, n piw,pic,i=1,…,n)： [ p i c 1 ] = T c w [ p i w 1 ] = T c w [ ∑ j = 1 4 α i j c j w ∑ j = 1 4 α i j ] = ∑ j = 1 4 α i j T c w [ c j w 1 ] = ∑ j = 1 4 α i j [ c j c 1 ] (01) \color{Green} \tag{01} \begin{aligned} {\left[\begin{array}{c} \mathbf{p}_{i}^{c} \\ 1 \end{array}\right] } &=\mathbf T_{cw} \left[\begin{array}{c} \mathbf{p}_{i}^{w} \\ 1 \end{array}\right] \\ &=\mathbf T_{cw} \left[\begin{array}{c} \sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w} \\ \\ \sum_{j=1}^{4} \alpha_{i j} \end{array}\right] \\ &=\sum_{j=1}^{4} \alpha_{i j} \mathbf T_{cw} \left[\begin{array}{c} \mathbf{c}_{j}^{w} \\ 1 \end{array}\right] \\ &=\sum_{j=1}^{4} \alpha_{i j}\left[\begin{array}{c} \mathbf{c}_{j}^{c} \\ 1 \end{array}\right] \end{aligned} [pic1]=Tcw[piw1]=Tcw⎣ ⎡∑j=14αijcjw∑j=14αij⎦ ⎤=j=1∑4αijTcw[cjw1]=j=1∑4αij[cjc1](01) 可以很明显的知道 3D点在相机坐标系下的坐标 p i c \mathbf{p}_{i}^{c} pic 可以使用相机坐标系下的四个控制点 c j c \mathbf{c}_{j}^{c} cjc 进行表示。宁外 3D点在世界坐标下的坐标 p i w \mathbf{p}_{i}^{w} piw 可以使用相机坐标系下的四个控制点 c j w \mathbf{c}_{j}^{w} cjw 进行表示。这里注意一个点，他们的坐标 α i j \alpha_{i j} αij 是一样的。

可以简单的理解成: 保持坐标不变，进行基底变换，其由 c j w \mathbf{c}_{j}^{w} cjw 构建的基底 A \mathbf A A 到 c c w \mathbf{c}_{c}^{w} ccw 的构建基底 B \mathbf B B 的变换矩阵就是要求解的相机位姿，具体推导过程如下。

二、理论推导

世界坐标系的控制点，与世界坐标系的控制点表示如下(注意，此时还没有去质心，或者说去中心化) p i w ( i = 1 , 2 , ⋯ , n ) p i c ( i = 1 , 2 , ⋯ , n ) (02) \color{Green} \tag{02} \begin{array}{l} \mathbf{p}_{i}^{w}(i=1,2,\cdots,n)\\ \mathbf{p}_{i}^{c}~(i=1,2,\cdots,n) \end{array} piw(i=1,2,⋯,n)pic (i=1,2,⋯,n)(02)

( 1 ) : \color{blue}{(1)}: (1): 首先分别计算它们的质心： p 0 w = 1 n ∑ i = 1 n p i w p 0 c = 1 n ∑ i = 1 n p i c (03) \color{Green} \tag{03} \begin{aligned} \mathbf{p}_{0}^{w} &=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{w} \\ \mathbf{p}_{0}^{c} &=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{c} \end{aligned} p0wp0c=n1i=1∑npiw=n1i=1∑npic(03) ( 2 ) : \color{blue}{(2)}: (2): 核心关键是使得关于 R , t \mathbf{R},\mathbf{t} R,t 的误差函数 F ( R c w , t c w ) F(\mathbf R_{cw}, \mathbf t_{cw}) F(Rcw,tcw) 最小(注意，下面的)： F ( R c w , t c w ) = 1 n ∑ i = 1 n ∥ p i c − ( R p i w + t ) ∥ 2 (04) \color{Green} \tag{04} F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}\left\|\mathbf p_{i}^c-(\mathbf R \mathbf p_{i}^w+\mathbf t\right)\|^{2} F(Rcw,tcw)=n1i=1∑n∥pic−(Rpiw+t)∥2(04) ( 3 ) : \color{blue}{(3)}: (3): 下面引入质心 p 0 w \mathbf{p}_{0}^{w} p0w， p 0 c \mathbf{p}_{0}^{c} p0c；由于核心公式里 p i w \mathbf{p}_{i}^{w} piw 前乘了 R \mathbf R R，所以 p 0 w \mathbf{p}_{0}^{w} p0w 前也乘 R \mathbf R R 才好合并，下式小括号中的值为 0: F ( R c w , t c w ) = 1 n ∑ i = 1 n ∥ p i c − R p i w − t − ( p 0 c − R p 0 w − p 0 c + R p 0 w ) ∥ 2 (05) \color{Green} \tag{05} F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}\left\|\mathbf p_{i}^c-\mathbf R \mathbf p_{i}^w-\mathbf t-\color{red}{(\mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w-\mathbf p_{0}^c+\mathbf R \mathbf p_{0}^w)} \color{Green} \right\|^{2} F(Rcw,tcw)=n1i=1∑n∥pic−Rpiw−t−(p0c−Rp0w−p0c+Rp0w)∥2(05)整理得: F ( R c w , t c w ) = 1 n ∑ i = 1 n ∥ p i c − p 0 c − R ( p i w − p 0 w ) + ( p 0 c − R p 0 w − t ) ∥ 2 (06) \color{Green}\tag{06} F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}\left\|\mathbf p_{i}^c- \color{red} {\mathbf p_{0}^c}\color{Green}-\mathbf R(\mathbf p_{i}^w-\color{red} \mathbf p_{0}^w\color{Green} ) + (\color{red} \mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w\color{Green} -\mathbf t) \color{Green}\right\|^{2} F(Rcw,tcw)=n1i=1∑n∥pic−p0c−R(piw−p0w)+(p0c−Rp0w−t)∥2(06) ( 4 ) : \color{blue}{(4)}: (4): 对上式进行展开： F ( R c w , t c w ) = 1 n ∑ i = 1 n [ ∥ p i c − p 0 c − R ( p i w − p 0 w ) ∥ 2 + ∥ p 0 c − R p 0 w − t ∥ 2 + 2 ( p i c − p 0 c − R ( p i w − p 0 w ) ) T ( p 0 c − R p 0 w − t ) ] (07) \color{Green} \tag{07} F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}\left[\left\|\mathbf p_{i}^c-\mathbf p_{0}^c-\mathbf R\left(\mathbf p_{i}^w-\mathbf p_{0}^w\right)\right\|^{2}+\left\|\mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w-\mathbf t\right\|^{2}+2\left(\mathbf p_{i}^c-\mathbf p_{0}^c-\mathbf R\left(p_{i}^w-\mathbf p_{0}^w\right)\right)^{T}\left(\mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w-\mathbf t\right)\right] F(Rcw,tcw)=n1i=1∑n[∥pic−p0c−R(piw−p0w)∥2+∥p0c−Rp0w−t∥2+2(pic−p0c−R(piw−p0w))T(p0c−Rp0w−t)](07) 上式子也等价如下形式: F ( R c w , t c w ) = 1 n ∑ i = 1 n ∥ p i c − p 0 c − R ( p i w − p 0 w ) ∥ 2 + 1 n ∑ i = 1 n ∥ p 0 c − R p 0 w − t ∥ 2 + 1 n ∑ i = 1 n 2 ( p i c − p 0 c − R ( p i w − p 0 w ) ) T ( p 0 c − R p 0 w − t ) (08) \color{Green} \tag{08} F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}\left\|\mathbf p_{i}^c-\mathbf p_{0}^c-\mathbf R\left(\mathbf p_{i}^w-\mathbf p_{0}^w\right)\right\|^{2}\\ +\frac{1}{n} \sum_{i=1}^{n}\left\|\mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w-\mathbf t\right\|^{2}\\ +\frac{1}{n} \sum_{i=1}^{n}2\left(\mathbf p_{i}^c-\mathbf p_{0}^c-\mathbf R\left(p_{i}^w-\mathbf p_{0}^w\right)\right)^{T}\left(\mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w-\mathbf t\right) F(Rcw,tcw)=n1i=1∑n∥pic−p0c−R(piw−p0w)∥2+n1i=1∑n∥p0c−Rp0w−t∥2+n1i=1∑n2(pic−p0c−R(piw−p0w))T(p0c−Rp0w−t)(08)从上面可以看到，函数 F ( R c w , t c w ) F(\mathbf R_{cw}, \mathbf t_{cw}) F(Rcw,tcw) 可以被表示成三段子函数。首先可以看出第3段函数为0，另外第一段函数只与 R c w \mathbf R_{cw} Rcw 相关，第2段函数对于任意的 R c w \mathbf R_{cw} Rcw，总能得到 t = p 0 c − R p 0 w \mathbf t = \mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w t=p0c−Rp0w，使得其等于0。那么原核型公式可以转化为: m i n F ( R c w , t c w ) = 1 n ∑ i = 1 n ∥ p i c − p 0 c − R ( p i w − p 0 w ) ∥ 2 (09) \color{Green} \tag{09} min F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}\left\|\mathbf p_{i}^c-\mathbf p_{0}^c-\mathbf R\left(\mathbf p_{i}^w-\mathbf p_{0}^w\right)\right\|^{2} minF(Rcw,tcw)=n1i=1∑n∥pic−p0c−R(piw−p0w)∥2(09) ( 5 ) : \color{blue}{(5)}: (5):观察上式，可以把 p i w − p 0 w \mathbf{p}_{i}^{w}-\mathbf{p}_{0}^{w} piw−p0w 与 p i c − p 0 c \mathbf{p}_{i}^{c}~-\mathbf{p}_{0}^{c} pic −p0c 进行如下表示，也称去质心或者去中心化： p ′ i w = p i w − p 0 w ( i = 1 , 2 , ⋯ , n ) p ′ i c = p i c − p 0 c ( i = 1 , 2 , ⋯ , n ) (10) \color{Green} \tag{10} \begin{array}{l} \mathbf{p'}_{i}^{w}=\mathbf{p}_{i}^{w}-\mathbf{p}_{0}^{w}(i=1,2,\cdots,n)\\ \mathbf{p'}_{i}^{c}~=\mathbf{p}_{i}^{c}~-\mathbf{p}_{0}^{c}~(i=1,2,\cdots,n) \end{array} p′iw=piw−p0w(i=1,2,⋯,n)p′ic =pic −p0c (i=1,2,⋯,n)(10)带入之后，可以写成如下形式: m i n F ( R c w , t c w ) = 1 n ∑ i = 1 n ∥ p ′ i c − R p ′ i w ∥ 2 (11) \color{Green} \tag{11} min F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}\left\|\mathbf{p'}_{i}^{c}-\mathbf{R}\mathbf{p'}_{i}^{w}\right\|^{2} minF(Rcw,tcw)=n1i=1∑n∥ ∥p′ic−Rp′iw∥ ∥2(11) ( 6 ) : \color{blue}{(6)}: (6):对上式子进行展开，可以得到如下等式 m i n F ( R c w , t c w ) = 1 n ∑ i = 1 n [ ( p ′ i c ) T p ′ i c + ( p ′ i w ) T R T R p ′ i w − 2 ( p ′ i c ) T R p ′ i w ] (12) \color{Green} \tag{12} min F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}[(\mathbf{{p'}}_{i}^{c})^T\mathbf{{p'}}_{i}^{c}+(\mathbf{p'}_{i}^{w})^T\mathbf{R}^T\mathbf{R}\mathbf{p'}_{i}^{w}-2(\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w}] minF(Rcw,tcw)=n1i=1∑n[(p′ic)Tp′ic+(p′iw)TRTRp′iw−2(p′ic)TRp′iw](12)中间项的 R T R = E \mathbf{R}^T\mathbf{R}=\mathbf E RTR=E，也就是没有 R \mathbf{R} R 了，无 R \mathbf{R} R 的往后放，写成如下形式: m i n F ( R c w , t c w ) = 1 n ∑ i = 1 n [ − 2 ( p ′ i c ) T R p ′ i w ] + 1 n ∑ i = 1 n [ ( p ′ i c ) T p ′ i c + ( p ′ i w ) T R T R p ′ i w ] (13) \color{Green} \tag{13} min F(\mathbf R_{cw}, \mathbf t_{cw})=\frac{1}{n} \sum_{i=1}^{n}[-2(\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w}]+\frac{1}{n} \sum_{i=1}^{n}[(\mathbf{{p'}}_{i}^{c})^T\mathbf{{p'}}_{i}^{c}+(\mathbf{p'}_{i}^{w})^T\mathbf{R}^T\mathbf{R}\mathbf{p'}_{i}^{w}] minF(Rcw,tcw)=n1i=1∑n[−2(p′ic)TRp′iw]+n1i=1∑n[(p′ic)Tp′ic+(p′iw)TRTRp′iw](13)明显的的可以知道上式中右边的子式与 R \mathbf{R} R 无关，所以求第一项的最小值即可，等价于求 ( p ′ i c ) T R p ′ i w 的 \color{red} (\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w} 的 (p′ic)TRp′iw的最大值 \color{red}最大值最大值。 ( 7 ) : \color{blue}{(7)}: (7):对 ( p ′ i c ) T R p ′ i w (\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w} (p′ic)TRp′iw 展开得到如下

( p ′ i c ) T R p ′ i w = [ p ′ i 0 c p ′ i 0 c p ′ i 0 c ] [ R 0 R 1 R 2 ] p ′ i w (14) \color{Green} \tag{14} (\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w}= \begin{bmatrix} {{p'}}_{i0}^{c}& {{p'}}_{i0}^{c}& {{p'}}_{i0}^{c} \end{bmatrix} \begin{bmatrix} \mathbf R_0 \\ \mathbf R_1\\ \mathbf R_2 \end{bmatrix} \mathbf{p'}_{i}^{w} (p′ic)TRp′iw=[p′i0cp′i0cp′i0c]⎣ ⎡R0R1R2⎦ ⎤p′iw(14) 进一步写成如下形式: = p ′ i 0 c R 0 p ′ i w + p ′ i 1 c R 1 p ′ i w + p ′ i 2 c R 2 p ′ i w = R 0 p ′ i 0 c p ′ i w + R 1 p ′ i 1 c p ′ i w + R 2 p ′ i 2 c p ′ i w (15) \color{Green} \tag{15} ={{p'}}_{i0}^{c}\mathbf R_0\mathbf{p'}_{i}^{w}+{{p'}}_{i1}^{c}\mathbf R_1\mathbf{p'}_{i}^{w}+{{p'}}_{i2}^{c}\mathbf R_2\mathbf{p'}_{i}^{w}= \color{red} \mathbf R_0{{p'}}_{i0}^{c}\mathbf{p'}_{i}^{w}+\mathbf R_1{{p'}}_{i1}^{c}\mathbf{p'}_{i}^{w}+\mathbf R_2{{p'}}_{i2}^{c}\mathbf{p'}_{i}^{w} =p′i0cR0p′iw+p′i1cR1p′iw+p′i2cR2p′iw=R0p′i0cp′iw+R1p′i1cp′iw+R2p′i2cp′iw(15) ( 8 ) : \color{blue}{(8)}: (8):下面我们还需要进行如下推导 R p ′ i w ( p ′ i c ) T = [ R 0 R 1 R 2 ] [ p ′ i 0 c p ′ i w p ′ i 0 c p ′ i w p ′ i 0 c p ′ i w ] (16) \color{Green} \tag{16} \mathbf R\mathbf{p'}_{i}^{w}(\mathbf{p'}_{i}^{c})^T=\begin{bmatrix} \mathbf R_0 \\ \mathbf R_1\\ \mathbf R_2 \end{bmatrix} \begin{bmatrix} {{p'}}_{i0}^{c}{{p'}}_{i}^{w}& {{p'}}_{i0}^{c}{{p'}}_{i}^{w}& {{p'}}_{i0}^{c}{{p'}}_{i}^{w}\end{bmatrix} Rp′iw(p′ic)T=⎣ ⎡R0R1R2⎦ ⎤[p′i0cp′iwp′i0cp′iwp′i0cp′iw](16)再进行简化 = [ R 0 p ′ i 0 c p ′ i w R 0 p ′ i 1 c p ′ i w R 0 p ′ i 2 c p ′ i w R 1 p ′ i 0 c p ′ i w R 1 p ′ i 1 c p ′ i w R 1 p ′ i 2 c p ′ i w R 2 p ′ i 0 c p ′ i w R 2 p ′ i 1 c p ′ i w R 2 p ′ i 2 c p ′ i w ] (17) \color{Green} \tag{17} =\begin{bmatrix} \color{red} \mathbf R_0{{p'}}_{i0}^{c}{{p'}}_{i}^{w}&\mathbf R_0{{p'}}_{i1}^{c}{{p'}}_{i}^{w}&\mathbf R_0{{p'}}_{i2}^{c}{{p'}}_{i}^{w}\\ \mathbf R_1{{p'}}_{i0}^{c}{{p'}}_{i}^{w}&\color{red} \mathbf R_1{{p'}}_{i1}^{c}{{p'}}_{i}^{w}&\mathbf R_1{{p'}}_{i2}^{c}{{p'}}_{i}^{w}\\ \mathbf R_2{{p'}}_{i0}^{c}{{p'}}_{i}^{w}&\mathbf R_2{{p'}}_{i1}^{c}{{p'}}_{i}^{w}&\color{red} \mathbf R_2{{p'}}_{i2}^{c}{{p'}}_{i}^{w} \end{bmatrix} =⎣ ⎡R0p′i0cp′iwR1p′i0cp′iwR2p′i0cp′iwR0p′i1cp′iwR1p′i1cp′iwR2p′i1cp′iwR0p′i2cp′iwR1p′i2cp′iwR2p′i2cp′iw⎦ ⎤(17)

( 9 ) : \color{blue}{(9)}: (9):从上面可以观察到，式(15)红色部分为式(17)矩阵对角线元素之和，那么可以得到： ( p ′ i c ) T R p ′ i w = T r a c e ( R p ′ i w ( p ′ i c ) T ) (18) \color{Green} \tag{18} (\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w}=Trace(\mathbf R\mathbf{p'}_{i}^{w}(\mathbf{p'}_{i}^{c})^T) (p′ic)TRp′iw=Trace(Rp′iw(p′ic)T)(18)到现在核心公式转为了求下面的式子 m a x ∑ i = 1 n ( p ′ i c ) T R p ′ i w = m a x T r a c e ( R ∑ i = 1 n p ′ i w ( p ′ i c ) T ) 令 : H = p ′ i w ( p ′ i c ) T m a x ∑ i = 1 n ( p ′ i c ) T R p ′ i w = m a x T r a c e ( R H ) (19) \color{Green} \tag{19} max \sum_{i=1}^{n}(\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w}=maxTrace(\mathbf R \sum_{i=1}^{n}\mathbf{p'}_{i}^{w}(\mathbf{p'}_{i}^{c})^T)\\ 令:\mathbf{H}=\mathbf{p'}_{i}^{w}(\mathbf{p'}_{i}^{c})^T\\ max \sum_{i=1}^{n}(\mathbf{{p'}}_{i}^{c})^T\mathbf{R}\mathbf{p'}_{i}^{w} =maxTrace(\mathbf {RH}) maxi=1∑n(p′ic)TRp′iw=maxTrace(Ri=1∑np′iw(p′ic)T)令:H=p′iw(p′ic)Tmaxi=1∑n(p′ic)TRp′iw=maxTrace(RH)(19)

( 10 ) : \color{blue}{(10)}: (10): 现在对 H \mathbf H H 进行 SVD奇异值分解，即 H = U Λ V T 令 X = V U T X H = V U T U Λ V T = V Λ V T (20) \color{Green} \tag{20} \mathbf H=\mathbf U \Lambda \mathbf V^T~~~~~~~~~令 \mathbf X=\mathbf V \mathbf U^T\\ \mathbf X\mathbf H=\mathbf V \mathbf U^{T} \mathbf U \mathbf \Lambda \mathbf V^{T}=\mathbf V \mathbf \Lambda \mathbf V^{T} H=UΛVT 令X=VUTXH=VUTUΛVT=VΛVT(20)其中 X H \mathbf X\mathbf H XH 为正定对称矩阵，所以： T r a c e ( R H ) ≤ T r a c e ( X H ) (21) \color{Green} \tag{21} Trace (\mathbf R \mathbf H) \leq Trace(\mathbf X \mathbf H) Trace(RH)≤Trace(XH)(21) 当 R = X \mathbf R= \mathbf X R=X 时， R H \mathbf R \mathbf H RH 的值最大，所以 R \mathbf R R 的解为(结合前面(08)式的推导，可以求解出 t \mathbf t t)： R = X = V U T (22) \color{Green} \tag{22} \mathbf R= \mathbf X= \mathbf V \mathbf U^T R=X=VUT(22)

总结 : \color{blue}{总结}: 总结: 通过前面的一系列推导，总结如下： H = p ′ i w ( p ′ i c ) T = U Λ V T R = V U T t = p 0 c − R p 0 w (23) \color{Green} \tag{23} \mathbf{H}=\mathbf{p'}_{i}^{w}(\mathbf{p'}_{i}^{c})^T=\mathbf U \Lambda \mathbf V^T\\\mathbf R= \mathbf V \mathbf U^T\\ \mathbf t = \mathbf p_{0}^c-\mathbf R \mathbf p_{0}^w H=p′iw(p′ic)T=UΛVTR=VUTt=p0c−Rp0w(23)

三、代码注释

代码位于 src/PnPsolver.cc 文件中，主调函数如下：

/**
 * @brief 根据已经得到的控制点在当前相机坐标系下的坐标来恢复出相机的位姿
 * @param[in]  ut         vi
 * @param[in]  betas      betas
 * @param[out] R          计算得到的相机旋转R
 * @param[out] t          计算得到的相机位置t
 * @return double         使用这个位姿,所得到的重投影误差
 */
double PnPsolver::compute_R_and_t(const double * ut, const double * betas,
			     double R[3][3], double t[3])
{
  // Step 1 根据前面的计算结果来"组装"得到控制点在当前相机坐标系下的坐标
  compute_ccs(betas, ut);
  // Step 2 将世界坐标系下的3D点的坐标转换到控制点的坐标系下
  compute_pcs();
  // Step 3 调整点坐标的符号,来保证在相机坐标系下点的深度为正
  solve_for_sign();

  // Step 4 ICP计算R和t 
  estimate_R_and_t(R, t);

  // Step 5 计算使用这个位姿,所得到的每对点平均的重投影误差,作为返回值
  return reprojection_error(R, t);
}

estimate_R_and_t 的注释如下：

/**
 * @brief 用3D点在世界坐标系和相机坐标系下对应的坐标，用ICP求取R t
 * @param[out] R   旋转
 * @param[out] t   平移
 */
void PnPsolver::estimate_R_and_t(double R[3][3], double t[3])
{
  // Step 1 计算3D点的质心
  double pc0[3],                                              //3D点世界坐标系下坐标的质心
         pw0[3];                                              //3D点相机坐标系下坐标的质心
  // 初始化这两个质心
  pc0[0] = pc0[1] = pc0[2] = 0.0;
  pw0[0] = pw0[1] = pw0[2] = 0.0;

  // 然后累加求质心
  for(int i = 0; i

(01)ORB-SLAM2源码无死角解析-(45) EPnP 源代码分析(5)→ICP 求解相机位姿

最近更新

热门博客

[ 申请 ]友情链接：