预处理共轭梯度法(PCG)

开始读研了，研究方向是SLAM。SLAM中很重要的一部分是位姿估计，需要用到一些优化算法对位姿进行优化。预处理共轭梯度法PCG(Preconditioned Conjugate Gradient)是一种SLAM中常用的位姿优化算法，从共轭梯度法（Conjugate Gradient）衍生而来，用于快速计算最优值。

假设有一个线性方程： Ax = b \textbf{A}\textbf{x} = \textbf{b} Ax=b

其中 A A A是一个已知的对称且正定的矩阵，b也已知。记 x ∗ \textbf{x}_* x∗​为方程的唯一解。如果两个非零向量 u,v \textbf{u,v} u,v满足： u T Av = 0 \textbf{u}^{\rm T}\textbf{A}\textbf{v} = 0 uTAv=0

则称 u \textbf{u} u和 v \textbf{v} v共轭（相对于 A \textbf A A)。共轭是一种对称的关系，如果 u \textbf u u对于 v \textbf v v共轭，那么 v \textbf v v对于 u \textbf u u共轭。 由于 A \textbf{A} A对称且正定，于是可以定义內积： ⟨ u , v ⟩ A : = ⟨ Au , v ⟩ = ⟨ u , A T v ⟩ = ⟨ u , Av ⟩ = u T Av \langle \textbf{u},\textbf{v} \rangle_{\textbf{A}} := \langle \textbf{A}\textbf{u},\textbf{v} \rangle = \langle \textbf{u},\textbf{A}^{\rm T}\textbf{v} \rangle = \langle \textbf{u},\textbf{A}\textbf{v} \rangle = \textbf{u}^{\rm T}\textbf{A}\textbf{v} ⟨u,v⟩A​:=⟨Au,v⟩=⟨u,ATv⟩=⟨u,Av⟩=uTAv

如果有一个矩阵 P \textbf{P} P： P = { P 1 , P 2 , . . . . , P n } \textbf{P} = \left\{\textbf{P}_{1},\textbf{P}_{2},....,\textbf{P}_{n}\right\} P={P1​,P2​,....,Pn​}

且 P \textbf{P} P中的列向量两两共轭，那么 Ax = b \textbf{Ax} = \textbf{b} Ax=b的解 x ∗ \textbf{x}_{*} x∗​可以写成： x ∗ = ∑ i = 1 n α i P i \textbf{x}_{*} = \sum\limits_{i=1}^{n}\alpha_{i}\textbf{P}_{i} x∗​=i=1∑n​αi​Pi​

于是， A x ∗ = ∑ i = 1 n α i A P i \textbf{A}\textbf{x}_{*} = \sum\limits_{i=1}^{n}\alpha_{i}\textbf{A}\textbf{P}_{i} Ax∗​=i=1∑n​αi​APi​

左乘 P k T \textbf{P}_{k}^{\rm T} PkT​: P k T A x ∗ = ∑ i = 1 n α i P k T A P i \textbf{P}_{k}^{\rm T}\textbf{A}\textbf{x}_{*} = \sum\limits_{i=1}^{n}\alpha_{i}\textbf{P}_{k}^{\rm T}\textbf{A}\textbf{P}_{i} PkT​Ax∗​=i=1∑n​αi​PkT​APi​

将 Ax = b \textbf{A}\textbf{x} = \textbf{b} Ax=b和 u T Av = ⟨ u , v ⟩ A \textbf{u}^{\rm T}\textbf{A}\textbf{v} = \langle \textbf{u},\textbf{v} \rangle_{\textbf{A}} uTAv=⟨u,v⟩A​代入得： P k T b = ∑ i = 1 n α i ⟨ P k ， P i ⟩ A \textbf{P}_{k}^{\rm T}\textbf{b} = \sum\limits_{i=1}^{n}\alpha_{i}\langle\textbf{P}_k，\textbf{P}_i\rangle_{\textbf{A}} PkT​b=i=1∑n​αi​⟨Pk​，Pi​⟩A​

因为 u T v = ⟨ u , v ⟩ \textbf{u}^{\rm T}\textbf{v} = \langle \textbf{u},\textbf{v} \rangle uTv=⟨u,v⟩,且对于 ∀ i ̸ = k : ⟨ u , v ⟩ A = 0 \forall i\not= k:\langle \textbf{u},\textbf{v}\rangle_{\textbf{A}} = 0 ∀i̸​=k:⟨u,v⟩A​=0,所以：

⟨ P k , b ⟩ = α k ⟨ P k , P k ⟩ A \langle \textbf{P}_{k},\textbf{b}\rangle = \alpha_{k}\langle \textbf{P}_{k},\textbf{P}_{k}\rangle_{\textbf{A}} ⟨Pk​,b⟩=αk​⟨Pk​,Pk​⟩A​

由此可以得到 α k \alpha_{k} αk​的表达式： α k = ⟨ b k , b ⟩ ⟨ P k , P k ⟩ A \alpha_{k} = \frac{\langle \textbf{b}_{k},\textbf{b}\rangle}{\langle \textbf{P}_{k},\textbf{P}_{k}\rangle_{\textbf{A}}} αk​=⟨Pk​,Pk​⟩A​⟨bk​,b⟩​

结合 x ∗ = ∑ i = 1 n α i A P i \textbf{x}_{*} = \sum\limits_{i=1}^{n}\alpha_{i}\textbf{A}\textbf{P}_{i} x∗​=i=1∑n​αi​APi​就可以求解 x ∗ \textbf{x}_{*} x∗​。 当 n n n很大时，用直接法求解就会非常耗时，于是引出迭代法。

为了求得 x ∗ \textbf{x}_{*} x∗​的一个很好的近似，我们并不需要全部的 P k \textbf{P}_{k} Pk​，当 n n n很大且 A \textbf{A} A又具有一定的稀疏性时，可以用迭代法来求得一个近似的结果。令 x ∗ \textbf{x}_{*} x∗​的初始估计值为 x 0 \textbf{x}_{0} x0​( x 0 = 0 \textbf{x}_{0}=\textbf{0} x0​=0,etc.)。 构建二次函数：

f ( x ) = 1 2 x T Ax − x T b f(x) = \frac{1}{2}\textbf{x}^{\rm T}\textbf{A}\textbf{x} - \textbf{x}^{\rm T}\textbf{b} f(x)=21​xTAx−xTb

该函数一、二阶导为:

D f ( x ) = Ax − b ， Df(x) = \textbf{A}\textbf{x} - \textbf{b}， Df(x)=Ax−b，

D 2 f ( x ) = A , D^2f(x) = \textbf{A}, D2f(x)=A,

由于 A \textbf{A} A对称且正定， f ( x ) f(x) f(x)有唯一最小值。取 P 0 \textbf{P}_{0} P0​为 f ( x ) f(x) f(x)在 x 0 \textbf{x}_{0} x0​处的负梯度，即 P 0 = b − Ax 0 \textbf{P}_{0} = \textbf{b} - \textbf{Ax}_{0} P0​=b−Ax0​， P 0 \textbf{P}_{0} P0​同时也是算法初始步骤的残差项。 令 r k \textbf{r}_{k} rk​为第 k k k步的残差： r k = b − Ax k \textbf{r}_{k} = \textbf{b} - \textbf{Ax}_{k} rk​=b−Axk​

r k \textbf{r}_{k} rk​也是梯度下降法中的下降方向，在共轭梯度法中，为保证当前下降方向与之前步骤中的下降方向共轭，取： P k = r k − ∑ i ; k P i T A r k P i T A P i P i \textbf{P}_{k} = \textbf{r}_{k} - \sum\limits_{i;k}{}\frac{\textbf{P}_{i}^{\rm T}\textbf{A}\textbf{r}_{k}}{\textbf{P}_{i}^{\rm T}\textbf{A}\textbf{P}_{i}}\textbf{P}_{i} Pk​=rk​−i