《视觉SLAM十四讲》公式推导

数学表达式：

运动：从$k$时刻到$k+1$时刻，机器人的位置$x$的变化
观测：在$k$时刻于$x_k$出检测到了某一路标$y_j$
\[ \begin{cases} x_{k+1}=f(x_{k},u_k)+w_k, &k=1,\cdots,K \\ z_{k,j}=h(x_k,y_j)+v_{k,j}, &(k,j)\in \mathcal{O} \end{cases} \]
其中$\mathcal{O}$是一个集合，记录着在哪个时刻观察到了哪个路标，$z$为传感器读数。
$z_{k,j}$表示在$k$时刻观察到的$j$路标的像素坐标(图片)，$x_k$表示为用外参$R,t$描述相机的旋转、平移，$y_j$表示路标点在世界坐标系下的坐标$P_w$，$h(x_k,y_j)$表示在$x_k$处观察到$y_j$

对于变换矩阵$T$来说
\[T=\left [ \begin{matrix}R&t\\0^t&1\end{matrix} \right ]\]求逆：
由于$T$ 为分块矩阵，
$T^{-1}=\left [ \begin{matrix} R^{-1}&A \ 0^T&1^{-1} \end{matrix}\right] $ $A=-R^{-1}*t*(1^{-1})$(负号且左乘同行，右乘同列)
$T^{-1}=\left [ \begin{matrix} R^{-1}&-R^{-1}*t*(1^{-1}） \\ 0^T&1^{-1} \end{matrix}\right] =\left [ \begin{matrix}R^{T}&-R^{T}t \\ 0^T&1 \end{matrix}\right]$

矩阵的奇异值分解（Singular Value Decomposition）：

定义：设矩阵$A \in C_r^{m\times n}$,$\lambda _i$是$AA^H(A^HA)$的非零特征值，则称$\sigma _i=\sqrt{\lambda _i}$为$A$的奇异值，$i=1,2,\cdots,r$
定理：设矩阵$A \in C_r^{m\times n}$,则存在$U \in U^{m\times m}$，$V \in U^{n\times n}$，使得$A=U \left [ \begin{matrix} \Delta &0\\ 0&0\end{matrix} \right]V^H$，其中$\Delta=diag[\sigma _1,\sigma _2,\cdots,\sigma _r]$，$\sigma _1 \geq \sigma _2 \geq \cdots \geq \sigma _r$为$A$的奇异值。
证明：因为$AA^H$是正规阵，所以存在$U\in U^{m\times n}$ ，使得\[U^HAA^HU=diag[\sigma _1^2,\sigma _2^2, \cdots,\sigma _r^2,0,\cdots,0]=\left[\begin{matrix}\Delta \Delta ^H&0\\0&0\end{matrix}\right ]\]
且$\sigma _1^2 \geq \sigma _2^2 \geq \cdots \geq\sigma _r^2$
其中$\Delta=diag[\sigma _1,\sigma _2,\cdots,\sigma _r]$。设$U=[\begin{array}{cc}U_1&U_2\end{array}]$，则\[U^HAA^HU=\left[ \begin{array} {c}U_1^H\\U_2^H\end{array}\right]AA^H\left[\begin{array}{cc}U_1&U_2\end{array}\right]\\=\left[ \begin{array}{c} U_1^H\\U_2^H\end{array}\right]\left[\begin{array}{cc}AA^HU_1&AA^HU_2\end{array}\right]\\=\left[\begin{matrix}U_1^HAA^HU_1&U_1^HAA^HU_2\\U_2^HAA^HU_1&U_2^HAA^HU_2\end{matrix}\right]\\=\left[\begin{matrix}\Delta \Delta ^H&0\\0&0\end{matrix}\right]\]故有\[U_1^HAA^HU_1=\Delta \Delta ^H \\U_1^HAA^HU_2=0\\U_2^HAA^HU_1=0\\U_2^HAA^HU_2=0\]令$V_1=A^HU_1\Delta ^{-H}$，则$V_1^HV_1=\Delta ^{-1}U_1^HAA^HU_1\Delta ^{-H}$，由$U_1^HAA^HU_1=\Delta \Delta ^H$得\[V_1^HV_1=E_r\]所以$V_1$为次酉阵，即$V_1 \in U_r^{n\times r}$，故存在$V_2 \in U_{n-r}^{n\times (n-r)}$，使得$V=[\begin{array}{cc}V_1&V_2\end{array}]\in U^{n\times n}$，所以\[U^HAV=\left [ \begin{array}{c}U_1^H\\U_2^H\end{array}\right]A[\begin{array}{cc}V_1&V_2\end{array}]=\left [ \begin{matrix} U_1^HAV_1&U_1^HAV_2\\U_2^HAV_1&U_2^HAV_2\end{matrix}\right]\]由$U_1^HAA^HU_1=\Delta \Delta ^H$得\[U_1^HAV_1=U_1^HAA^HU_1\Delta ^{-H}=\Delta\]由$U_2^HAA^HU_2=(A^HU_2)^H(A^HU_2)=0$得\[A^HU_2=0，U_2^HA=0\]所以$U_2^HAV_1=0，U_2^HAV_2=0$。又因为\[V_1=A^HU_1\Delta ^{-H}\Rightarrow V_1\Delta ^H=A^HU_1 \Rightarrow U_1^HA=\Delta V_1^H\]所以$U_1^HAV_2=\Delta V_1^HV_2=0$。故$U^HAV=\left[\begin{matrix} \Delta&0\\0&0\end{matrix}\right]$，即$A=U\left[\begin{matrix} \Delta&0\\0&0\end{matrix}\right]V^H$。

分解步骤：

步骤1：求出$AA^H(A^HA)$全部非零特征值$\lambda _i$，记$\Delta=diag[\sigma _1,\sigma _2,\cdots,\sigma _r]$，且$\sigma _1 \geq \sigma _2 \geq \cdots \geq \sigma _r$为$A$的正奇异值；
步骤2：求酉矩阵$U \in U^{m\times m}(V \in V^{n\times n})$，使得\[U^HAA^HU=diag[\sigma _1^2,\sigma _2^2, \cdots,\sigma _r^2,0,\cdots,0]\$V^HA^HAV=diag[\sigma _1^2,\sigma _2^2, \cdots,\sigma _r^2,0,\cdots,0])\]
步骤3：设\(U=[\begin{array}{cc}U_1&U_2\end{array}](V=[\begin{array}{cc}V_1&V_2\end{array}])$，其中$U_1(V_1)$为$U(V)$的前$r$ 列，令$V_1=A^HU_1\Delta ^{-H}(U_1=AV_1\Delta ^{-1})$，则$V_1(U_1)$为次酉阵，求$V_2\in U_{n-r}^{n\times (n-r)}(U_2\in U_{m-r}^{m\times (m-r)})$，使得$V=[\begin{array}{cc}V_1&V_2\end{array}]\in U^{n\times n}(U=[\begin{array}{cc}U_1&U_2\end{array}]\in U^{m\times m})$；
步骤4：$A=U\left[\begin{matrix} \Delta&0\\0&0\end{matrix}\right]V^H$

几何意义：

$\Delta$视为放缩矩阵，$U，V$视为旋转矩阵。
因此，$M=AN，A=U\Delta V^H$可以直观的解释为一个图像$N$经过$A$变换成另一个图像$M$的过程，首先经过$V^H$的旋转，再经过$\Delta$的放缩，最后在经过$U$的旋转得到最终的图像。