视觉SLAM-7-2

第七章 视觉里程计

三 3D-2D: PnP

在之前建图的基础上，已知了3D点的空间位置信息，和点投影到图像上的信息，求相机的旋转和平移。

1. DLT 直接线性变换

已知空间点P和投影点x：(其中[R|t]是3×4的增广矩阵，在这里省略了相机内参K，因为K是已知的，所以没有带入)
$$\begin{gathered} P=(X,Y,Z,1{)}^T \\ x=(u,v,1) \\ sx=[R|t]P \end{gathered}$$
展开得到：

用最后一行消掉s，得到：

其中：
$${t_1\text{t1}{t}_1}^T=(t_1,t_2,t_3,t_4),{t_2\text{t2}{t}_2}^T=(t_5,t_6,t_7,t_8),{t_3\text{t3}{t}_3}^T=(t_9,t_{10},t_{11},t_{12})$$
因为一共12个未知数，一对点可以有两个方程，所以需要6对点。给的点多于6对时，求最小二乘解。得到[R|t]后用QR分解投影到SO(3)，然后得到R和t。

2. P3P

利用3对点求解和t，下图中是相机成像的，存在很多的相似三角形。

看三角形OAB得到：(余弦定理)
$$\begin{gathered} O{A}^2+O{B}^2-2OA\ast OB\ast cos(\angle AOB)=A{B}^2 \\ 三角形AOC：O{A}^2+O{C}^2-2OA\ast OC\ast cos(\angle AOC)=A{C}^2 \\ 三角形BOC：O{B}^2+O{C}^2-2OB\ast OC\ast cos(\angle BOC)=B{C}^2 \end{gathered}$$
上式全部除以 OC^2，记x=OA/OC，y=OB/OC，得到：
$$\begin{gathered} x^2+y^2-2xy\ast cos(\angle AOB)=A{B}^2/O{C}^2 \\ {x}^2+1-2x\ast cos(\angle AOC)=A{C}^2/O{C}^2 \\ {y}^2+1-2y\ast cos(\angle BOC)=B{C}^2/O{C}^2 \end{gathered}$$
然后：
$$\begin{gathered} v=A{B}^2/O{C}^2,u=B{C}^2/A{B}^2,w=A{C}^2/A{B}^2 \\ uv=B{C}^2/O{C}^2,wv=A{C}^2/O{C}^2 \end{gathered}$$
所以得到：
$$\begin{gathered} x^2+y^2-2xy\ast cos(\angle AOB)=v \\ {x}^2+1-2x\ast cos(\angle AOC)=wv \\ {y}^2+1-2y\ast cos(\angle BOC)=uv \end{gathered}$$
消掉v得到：
$$\begin{gathered} (1-u)y^2-u{x}^2-2y\ast cos(\angle BOC)+2uxy\ast cos(\angle AOB)+1=0 \\ (1-w)x^2-w{y}^2-2x\ast cos(\angle AOC)+2wxy\ast cos(\angle AOB)+1=0 \end{gathered}$$
由于知道2D点图像位置，三个余弦角可以得到，同时w,u也是可以得到的，所以可以求得x,y值。从而得到ABC在相机坐标系下的3D坐标，然后根据3D-3D的点对，计算相机运动的R,t。

3. Bundle Adjustment

优化解法，最小化重投影误差，投影关系为：
$$\begin{gathered} s_i{u}_i=Kexp({\xi }^{\wedge })P_i \\ 所以:u_i=\frac{1}{s_i}Kexp({\xi }^{\wedge })P_i \end{gathered}$$
u的真实值和计算值存在误差，要求出使该误差最小的T，就是相机的平移和旋转。
$$\begin{gathered} {\xi }^{\ast }=argmi{n}_{\xi }\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}Kexp({\xi }^{\wedge })P_i|{|}_2^2 \\ 令：e_i=u_i-\frac{1}{s_i}Kexp({\xi }^{\wedge })P_i \end{gathered}$$
对于优化这个目标函数，用的是李代数扰动（因为不方便直接对位姿求导为0），所以是对扰动的小量求导，也就是：

第一项为：
$$\begin{gathered} 假设：P^{\prime }=(exp({\xi }^{\wedge })P{)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }] \\ 所以：su=K{P}^{\prime }变为： \\ \left[\begin{array}{c}su\\ sv\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right] \end{gathered}$$

化简得到：(用第三行消掉s)
$$u=f_x\frac{X^{"}}{Z^{"}}+c_x,v=f_y\frac{Y^{"}}{Z^{"}}+c_y$$
所以得到导数为：

第二项为：在第四章中可以知道扰动模型的导数为：
$$\frac{ð(TP)}{ð\delta \xi }=\left[\begin{array}{cc}I& -P^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (Tp{)}^{\odot }$$
所以非其次的形式为：（3×6的矩阵）
$$\frac{ð(P^{\prime })}{ð\delta \xi }=\left[\begin{array}{cc}I& -P^{\prime \wedge }\end{array}\right]$$
两项的乘积为：

为优化提供了方向。不仅可以优化相机位移和旋转，还可以优化3D点位置，关于3D点位置的导数为：
$$\frac{ðe}{ðP}=\frac{ðe}{ðP^{\prime }}\ast R$$
代码 pose_estimation_3d2d.cpp

四 3D-3D ICP

1. 用SVD解法：

给配对好的两组3D点，求其旋转和平移。可以用迭代最近点(ICP)求解。设：
P = { p 1 , . . . , p n } , P ′ = { p 1 ′ , . . . , p n ′ } ∀ i , p i = R p i ′ + t P=\{p_1,...,p_n\},P'=\{p'_1,...,p'_n\} \\ \forall i,p_i=Rp'_i+t P={p1​,...,pn​},P′={p1′​,...,pn′​}∀i,pi​=Rpi′​+t
pi的真实值与计算值之间存在误差：
$$e_i=p_i-(R{p}_i^{\prime }+t)$$
所以就可以定义为最小二乘问题：
$$\underset{R,t}{\min }J=\frac{1}{2}\sum _{i=1}^n||(p_i-(R{p}_i^{\prime }+t))|{|}_2^2$$
定义质心为：

得到：

其中交叉项 (pi - p - R ( pi’ - p’ ) ) 为0，因为 pi 和 p 的相对位置与 pi‘ 和 p’ 的相对位置大小是相同的，在乘上R之后，就是一样的向量，所以做差为0。所以优化目标函数可以简化为：
$$\underset{R,t}{\min }J=\frac{1}{2}\sum _{i=1}^n||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2+||p-R{p}^{\prime }-t|{|}^2$$
前面一项只和R有关，所以使前面一项最小得到R，然后取t使得后一项为0。

如何求前面一项最小呢，分为这么几步：

定义去质心坐标：
$$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$$
所以变为最小化：
$$R^{\ast }=arg\underset{R}{\min }\frac{1}{2}\sum _{i=1}^n||q_i-R{q}_i^{\prime }|{|}^2$$

因为R^T * R 等于 I ，所以前两项都和R 没有关系，所以实际上优化目标函数变为：

然后：

将第二项变为0，可以得到如下公式，求出t：
$$t=p-R{p}^{\prime }$$

2. 非线性优化

非线性优化的思想是用迭代的方式去寻找最优值。当以李代数表达位姿时，目标函数改写为：
$$\underset{\xi }{\min }=\frac{1}{2}\sum _{i=1}^n\Vert (p_i-exp({\xi }^{\wedge })p{\prime }_i){\Vert }_2^2$$
它关于位姿导数已推导过，使用李代数扰动模型：
$$\frac{\partial e}{\partial \delta \xi }=-(exp({\xi }^{\wedge })\ast p_i^{\prime }{)}^{\odot }$$

• ICP问题存在唯一解和无穷多解的情况
• 若存在唯一解，极小值点即为全局最优解
• 在匹配已知的情况下，最小二乘实际上具有解析解，没有必要用迭代优化
• 可以混合使用PnP和ICP优化
  • 深度值已知的点，用建模它们的3D-3D误差
  • 深度值未知的点，用建模3D-2D的重投影误差

代码：pose_estimation_3d3d.cpp