《视觉SLAM十四讲》-- 视觉里程计 1（下）

6.4.1 直接线性变换（DLT）

已知空间中某点 $\mathbf{P}=[X,Y,Z,1{]}^T$，在图像中的归一化坐标 为 $[u,v,1{]}^T$，求解相机运动 $\mathbf{R}$、$\mathbf{t}$。定义增广矩阵 $[\mathbf{R}|\mathbf{t}]$ 为一个 $3\times 4$ 矩阵，包含了旋转和平移信息（区别于变换矩阵）。满足

s [ u 1 v 1 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 Y Z 1 ] (6-25) s\left[\begin{array}{l} u_{1} \\ v_{1} \\ 1 \end{array}\right]=\left[\begin{array}{llll} 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{array}\right]\left[\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right] \tag{6-25} s ​u1​v1​1​ ​= ​t1​t5​t9​​t2​t6​t10​​t3​t7​t11​​t4​t8​t12​​ ​ ​XYZ1​ ​(6-25)

用最后一行消去 $s$，得到

$$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$$
$$v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}\text{\hspace{1em}(6-26)}$$

为简化表示，定义

$${\mathbf{t}}_1=[t_1,t_2,t_3,t_4{]}^T,{\mathbf{t}}_2=[t_5,t_6,t_7,t_8{]}^T,{\mathbf{t}}_3=[t_9,t_{10},t_{11},t_{12}{]}^T$$

代入式（6-26），得

$${\mathbf{t}}_1^{\mathbf{T}}\mathbf{P}-{\mathbf{t}}_3^{\mathbf{T}}\mathbf{P}{u}_1=0$$
$${\mathbf{t}}_2^{\mathbf{T}}\mathbf{P}-{\mathbf{t}}_3^{\mathbf{T}}\mathbf{P}{v}_1=0\text{\hspace{1em}(6-27)}$$

每对特征点提供了 2 个约束，故至少需要 6 对特征点，可得到以下线性方程组（相当于取转置）：

( P 1 T 0 − u 1 P 1 T 0 P 1 T − v 1 P 1 T ⋮ ⋮ ⋮ P N T 0 − u N P N T 0 P N T − v N P N T ) ( t 1 t 2 t 3 ) = 0 (6-28) \left(\begin{array}{ccc} \boldsymbol{P}_{1}^{\mathrm{T}} & 0 & -u_{1} \boldsymbol{P}_{1}^{\mathrm{T}} \\ 0 & \boldsymbol{P}_{1}^{\mathrm{T}} & -v_{1} \boldsymbol{P}_{1}^{\mathrm{T}} \\ \vdots & \vdots & \vdots \\ \boldsymbol{P}_{N}^{\mathrm{T}} & 0 & -u_{N} \boldsymbol{P}_{N}^{\mathrm{T}} \\ 0 & \boldsymbol{P}_{N}^{\mathrm{T}} & -v_{N} \boldsymbol{P}_{N}^{\mathrm{T}} \end{array}\right)\left(\begin{array}{l} \boldsymbol{t}_{1} \\ \boldsymbol{t}_{2} \\ \boldsymbol{t}_{3} \end{array}\right)=0 \tag{6-28} ​P1T​0⋮PNT​0​0P1T​⋮0PNT​​−u1​P1T​−v1​P1T​⋮−uN​PNT​−vN​PNT​​ ​ ​t1​t2​t3​​ ​=0(6-28)

当匹配点大于 6 对时，可用 SVD 等方法对超定方程求最小二乘解。

注意，这里我们直接将变换矩阵 $\mathbf{T}$ 看成 12 个未知数，而忽略了其内部各元素之间的约束关系，因此我们还需要对求解结果进行处理。对于旋转矩阵 $\mathbf{R}$，将上述结果左边 $3\times 3$ 矩阵进行近似，可以采用 QR 分解或下面的方法

$$\mathbf{R}\leftarrow {\left(\mathbf{R}{\mathbf{R}}^{\mathrm{T}}\right)}^{-\frac{1}{2}}\mathbf{R}$$

6.4.2 P3P

P3P 是另一种解 PnP 的方法，它仅使用 3 对匹配点， 对数据要求较少。

（1）P3P 需要利用给定的 3 个点的几何关系。它的输入数据为 3 对 3D-2D 匹配点。

如图，A、B、C 为 3D 点， a、b、c 为三个点在成像平面上的投影。此外，还需要一对验证点 D-d（类似对极几何，选出正确解）。

根据三角形相似

$$\Delta Oab\sim \Delta OAB,\Delta Oac\sim \Delta OAC,\Delta Obc\sim \Delta OBC$$

由余弦定理
$$\begin{gathered} O{A}^2+O{B}^2-2OA\cdot OB\cdot \cos <Oa,Ob>=A{B}^2 \\ O{A}^2+O{C}^2-2OA\cdot OC\cdot \cos <Oa,Oc>=A{C}^2 \\ O{B}^2+O{C}^2-2OB\cdot OC\cdot \cos <Ob,Oc>=B{C}^2\text{\hspace{1em}(6-29)} \end{gathered}$$

两边同除 $O{C}^2$，记 $x=OA/OC$， $y=OB/OC$，得

$$\begin{gathered} x^2+y^2-2xy\cos <Oa,Ob>=A{B}^2/O{C}^2 \\ {x}^2+1-2x\cos <Oa,Oc>=A{C}^2/O{C}^2 \\ {y}^2+1-2y\cos <Ob,Oc>=B{C}^2/O{C}^2\text{\hspace{1em}(6-30)} \end{gathered}$$

记 $v=A{B}^2/O{C}^2,uv=B{C}^2/O{C}^2,wv=A{C}^2/O{C}^2$，则

$$\begin{gathered} x^2+y^2-2xy\cos <Oa,Ob>-v=0 \\ {x}^2+1-2x\cos <Oa,Oc>-wv=0 \\ {y}^2+1-2y\cos <Ob,Oc>-uv=0\text{\hspace{1em}(6-31)} \end{gathered}$$

将第一个式子中的 $v$ 代入后面两式，整理得

$$(1-u)y^2-u{x}^2-2y\cos <Ob,Oc>+2uxy\cos <Oa,Ob>+1=0$$
$$(1-w)x^2-w{x}^2-2x\cos <Oa,Oc>+2wxy\cos <Oa,Ob>+1=0\text{\hspace{1em}(6-32)}$$

注意，我们已知三个点的空间坐标及其相机投影，也就是说 $u=B{C}^2/A{B}^2,w=A{C}^2/A{B}^2$ 已知，三个余弦值也已知。这样，可以将上式看成关于 $x$、$y$ 的二元二次方程。

该方程最多有四个解，用验证点计算最可能的解，从而得到三个点此时在相机坐标系下的 3D 坐标，之后再用 3D-3D 的点对，计算出相机运动（见 7.5 节）。

6.4.3 最小化重投影误差求解 PnP

（1）除了上面的线性变换方法，我们还可以把 PnP 问题构建成一个关于重投影误差的非线性最小二乘问题。线性方法往往是 先求相机位姿，再求空间点位置，而非线性优化则是把他们同时看成优化变量。这种把 相机和三维点放在一起进行最小化 的问题，统称为 Bundle Adjustment（BA）。

（2）

假设我们已知 $n$ 个三维空间点 $P$ 和他们的投影 $p$，希望求出相机位姿 $\mathbf{R}$、 $\mathbf{t}$，它的李代数为 $\mathbf{\xi }$。假设其中某点的空间坐标为 ${\mathbf{P}}_i=[X_i,Y_i,Z_i{]}^T$，其投影的像素坐标为 ${\mathbf{u}}_i=[u_i,v_i{]}^T$，两者关系如下：

s i [ u i v i 1 ] = K exp ⁡ ( ξ ∧ ) [ X i Y i Z i 1 ] (6-32) s_{i}\left[\begin{array}{l} u_{i} \\ v_{i} \\ 1 \end{array}\right]=\boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right)\left[\begin{array}{c} X_{i} \\ Y_{i} \\ Z_{i} \\ 1 \end{array}\right] \tag{6-32} si​ ​ui​vi​1​ ​=Kexp(ξ∧) ​Xi​Yi​Zi​1​ ​(6-32)

其中 $s_i$ 为深度。将上式写成矩阵形式

$$s_i{\mathbf{u}}_i=\mathbf{K}\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}$$

但是，由于噪声的存在，等式往往不会成立，而是会有偏差。因此，我们将误差求和，构建一个最小二乘问题，寻找最优的位姿，使得误差最小：

ξ ∗ = arg ⁡ min ⁡ ξ 1 2 ∑ i = 1 n ∥ e i ∥ 2 2 = arg ⁡ min ⁡ ξ 1 2 ∑ i = 1 n ∥ u i − 1 s i K exp ⁡ ( ξ ∧ ) P i ∥ 2 2 (6-33) \boldsymbol{\xi}^{*}=\arg \min _{\boldsymbol{\xi}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{e}_i\right\|_{2}^{2}=\arg \min _{\boldsymbol{\xi}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{u}_{i}-\frac{1}{s_{i}} \boldsymbol{K} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}_{i}\right\|_{2}^{2} \tag{6-33} ξ∗=argξmin​21​i=1∑n​∥ei​∥22​=argξmin​21​i=1∑n​ ​ui​−si​1​Kexp(ξ∧)Pi​ ​22​(6-33)

注意，这里的误差采用非齐次坐标，即只有两维（最后一维误差一直为零）。

（3）前面我们已经讲过非线性优化方法，关键在于梯度的确定。这里将误差 $\mathbf{e}$ 线性化：

$$\mathbf{e}(\mathbf{x}+\Delta \mathbf{x})\approx \mathbf{e}(\mathbf{x})+{\mathbf{J}}^{\mathrm{T}}\Delta \mathbf{x}\text{\hspace{1em}(6-34)}$$

其中，误差 $\mathbf{e}$ 为 2 维，$\mathbf{x}$ 为相机位姿（也就是变换矩阵的李代数）6 维，那么 ${\mathbf{J}}^{\mathrm{T}}$ 就是 $2\times 6$ 维矩阵。这里的 ${\mathbf{J}}^{\mathrm{T}}$ 是误差 $\mathbf{e}$ 关于李代数的一阶导数，下面进行推导。

记点 $P$ 变换到相机坐标系下的空间点坐标为 ${\mathbf{P}}^{\prime }$，并且将前三维取出来（注意维度）：

$${\mathbf{P}}^{\prime }=(\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}{)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T\text{\hspace{1em}(6-35)}$$

相机坐标再到像素坐标：

$$s\mathbf{u}=\mathbf{K}{\mathbf{P}}^{\prime }\text{\hspace{1em}(6-36)}$$

展开

s [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X ′ Y ′ Z ′ ] (6-37) s\left[\begin{array}{l} u \\ v \\ 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}{l} X^{\prime} \\ Y^{\prime} \\ Z^{\prime} \end{array}\right] \tag{6-37} s ​uv1​ ​= ​fx​00​0fy​0​cx​cy​1​ ​ ​X′Y′Z′​ ​(6-37)

利用第 3 行消去 $s$（也就是深度），得

$$u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y\text{\hspace{1em}(6-38)}$$

我们对 ${\mathbf{\xi }}^{\wedge }$ 左乘扰动变量 $\delta \mathbf{\xi }$，然后考虑 $\mathbf{e}$ 关于扰动量的导数（也就是 ${\mathbf{J}}^T$）

$${\mathbf{J}}^T=\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\mathbf{e}(\delta \mathbf{\xi }\oplus \mathbf{\xi })-\mathbf{e}(\mathbf{\xi })}{\delta \mathbf{\xi }}=\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \mathbf{\xi }}\text{\hspace{1em}(6-39)}$$

这里 $\oplus$ 表示左乘。先求第一项

$$\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\text{\hspace{1em}(6-40)}$$

第二项为变换后的空间点关于李代数的导数

$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \mathbf{\xi }}=\frac{\partial (\mathbf{T}\mathbf{P})}{\partial \delta \mathbf{\xi }}=(\mathbf{T}\mathbf{P}{)}^{\odot }=\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}\right]\text{\hspace{1em}(6-41)}$$

取出前三维，即

∂ P ′ ∂ δ ξ = [ I − P ′ ∧ ] = [ 1 0 0 0 Z − Y 0 1 0 − Z 0 X 0 0 1 Y − X 0 ] (6-42) \frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}=\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{P}^{\prime \wedge} \end{array}\right]=\left[\begin{array}{cccccc} 1 & 0 & 0 & 0 & Z & -Y \\ 0 & 1 & 0 & -Z & 0 & X \\ 0 & 0 & 1 & Y & -X & 0 \\ \end{array}\right] \tag{6-42} ∂δξ∂P′​=[I​−P′∧​]= ​100​010​001​0−ZY​Z0−X​−YX0​ ​(6-42)

两项相乘，得到 $2\times 6$ 的雅克比矩阵

$${\mathbf{J}}^T=\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]\text{\hspace{1em}(6-43)}$$

（4）除了优化位姿，我们还希望优化特征点的空间位置，也就是需要求误差 $\mathbf{e}$ 关于空间点 $\mathbf{P}$ 的导数。由链式求导法则：

$$\frac{\partial \mathbf{e}}{\partial \mathbf{P}}=\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}\text{\hspace{1em}(6-44)}$$
第一项已由式（6-40）求出。关于第二项，有

$${\mathbf{P}}^{\prime }=\mathbf{RP}+\mathbf{t}$$

则

$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}=\mathbf{R}$$

合并第一、二两项，得

$$\frac{\partial \mathbf{e}}{\partial \mathbf{P}}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\mathbf{R}\text{\hspace{1em}(6-45)}$$

至此，我们推导出了观测相机方程关于相机位姿和特征点的两个导数矩阵，后续进行优化迭代。

6.5 3D-3D：ICP

假设有一组匹配好的 3D 点（例如对两幅 RGB-D 图像进行匹配）：

$$\mathbf{P}=\left\{{\mathbf{p}}_1,\cdots ,{\mathbf{p}}_n\right\},{\mathbf{P}}^{\prime }=\left\{{\mathbf{p}}_1^{\prime },\cdots ,{\mathbf{p}}_n^{\prime }\right\}$$

现在，想要找一个欧式变换 $\mathbf{R}$、$\mathbf{t}$，使得

$$\forall i,{\mathbf{p}}_i=\mathbf{R}{\mathbf{p}}_i^{\prime }+\mathbf{t}$$

采用迭代最近点（ICP）求解：一种是利用线性代数的求解，一种是利用非线性优化方式的求解。

6.5.1 SVD 方法

（1）定义第 $i$ 对点的误差项为：

$${\mathbf{e}}_i={\mathbf{p}}_i-({\mathbf{Rp}}_i^{\prime }+\mathbf{t})\text{\hspace{1em}(6-46)}$$

构建最小二乘问题，求出使误差平方和最小的 $\mathbf{R}$、$\mathbf{t}$：

$$\underset{\mathbf{R},\mathbf{t}}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert \left({\mathbf{p}}_i-\left(\mathbf{R}{\mathbf{p}}_i^{\prime }+\mathbf{t}\right)\right)\Vert }_2^2\text{\hspace{1em}(6-47)}$$

定义两组点的质心：

$$\mathbf{p}=\frac{1}{n}\sum _{i=1}^n({\mathbf{p}}_{\mathbf{i}})，{\mathbf{p}}^{\prime }=\frac{1}{n}\sum _{i=1}^n({\mathbf{p}}_{\mathbf{i}}^{\prime })\text{\hspace{1em}(6-48)}$$

则式（4-47）可写为

$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{p}}_i-\left(\mathbf{R}{\mathbf{p}}_i{}^{\prime }+\mathbf{t}\right)\Vert }^2=& \frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{p}}_i-\mathbf{R}{\mathbf{p}}_i{}^{\prime }-\mathbf{t}-\mathbf{p}+\mathbf{R}{\mathbf{p}}^{\prime }+\mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }\Vert }^2\\ =& \frac{1}{2}\sum _{i=1}^n{\Vert \left({\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i{}^{\prime }-{\mathbf{p}}^{\prime }\right)\right)+\left(\mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t}\right)\Vert }^2\\ =& \frac{1}{2}\sum _{i=1}^n({\Vert {\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i{}^{\prime }-{\mathbf{p}}^{\prime }\right)\Vert }^2+{\Vert \mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t}\Vert }^2+\\ & 2{\left({\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i{}^{\prime }-{\mathbf{p}}^{\prime }\right)\right)}^{\mathrm{T}}\left(\mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t}\right))\end{array}\text{\hspace{1em}(6-49)}$$
注意到，后一项 $\left({\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i{}^{\prime }-{\mathbf{p}}^{\prime }\right)\right)$ 求和后等于零。则目标函数变为

$$\underset{\mathbf{R},\mathbf{t}}{\min }J=\frac{1}{2}\sum _{i=1}^n({\Vert {\mathbf{p}}_i-\mathbf{p}-\mathbf{R}\left({\mathbf{p}}_i{}^{\prime }-{\mathbf{p}}^{\prime }\right)\Vert }^2+{\Vert \mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }-\mathbf{t}\Vert }^2\text{\hspace{1em}(6-50)}$$

观察左右两项，发现左边只和旋转 $\mathbf{R}$ 有关，而右边和 $\mathbf{R}$、$\mathbf{t}$ 都有关。因此，只要我们获得了 $\mathbf{R}$，再令第二项等于零就可求出 $\mathbf{t}$。于是，ICP 可分为三个步骤求解：

——————————————————————————————————————————————————————————

① 计算两组点的质心 $\mathbf{p}$`、${\mathbf{p}}^{\prime }$，然后计算每个点的去质心坐标：

$${\mathbf{q}}_{\mathbf{i}}={\mathbf{p}}_{\mathbf{i}}-\mathbf{p},{\mathbf{q}}_{\mathbf{i}}^{\prime }={\mathbf{p}}_{\mathbf{i}}^{\prime }-{\mathbf{p}}^{\prime }$$

② 先优化第一项，求出 $\mathbf{R}$

$${\mathbf{R}}^{\ast }=\arg \underset{\mathbf{R}}{\min }\frac{1}{2}\sum _{i=1}^n\Vert {\mathbf{q}}_{\mathbf{i}}-\mathbf{R}{\mathbf{q}}_{\mathbf{i}}^{\prime }{\Vert }^2$$

③ 令第二项等于零，求出 $\mathbf{t}$

$${\mathbf{t}}^{\ast }=\mathbf{p}-\mathbf{R}{\mathbf{p}}^{\prime }$$

——————————————————————————————————————————————————————————

下面主要进行第二步的推导，以求出 $\mathbf{R}$:

$$\frac{1}{2}\sum _{i=1}^n\Vert {\mathbf{q}}_{\mathbf{i}}-\mathbf{R}{\mathbf{q}}_{\mathbf{i}}^{\prime }{\Vert }^2=\frac{1}{2}\sum _{i=1}^n({{\mathbf{q}}_{\mathbf{i}}}^T{\mathbf{q}}_{\mathbf{i}}+{{\mathbf{q}}_{\mathbf{i}}^{\prime }}^T{\mathbf{R}}^T\mathbf{R}{\mathbf{q}}_{\mathbf{i}}^{\prime }-2{{\mathbf{q}}_{\mathbf{i}}}^T\mathbf{R}{\mathbf{q}}_{\mathbf{i}}^{\prime })\text{\hspace{1em}(6-51)}$$

注意到，第一项和 $\mathbf{R}$ 无关，第二项中 ${\mathbf{R}}^{\mathbf{T}}\mathbf{R}=\mathbf{I}$，亦与 $\mathbf{R}$ 无关。因此，目标优化函数变为

$$\sum _{i=1}^n-{{\mathbf{q}}_{\mathbf{i}}}^{\mathrm{T}}\mathbf{R}{\mathbf{q}}_{\mathbf{i}}^{\prime }=\sum _{i=1}^n-\mathrm{tr}(\mathbf{R}{\mathbf{q}}_{\mathbf{i}}^{\prime }{{\mathbf{q}}_{\mathbf{i}}}^T)=-\mathrm{tr}(\mathbf{R}\sum _{i=1}^n{\mathbf{q}}_{\mathbf{i}}^{\prime }{{\mathbf{q}}_{\mathbf{i}}}^{\mathrm{T}})\text{\hspace{1em}(6-52)}$$

注意，这里用了矩阵内积和迹的关系。

定义

$$\mathbf{W}={\mathbf{q}}_{\mathbf{i}}{{\mathbf{q}}_{\mathbf{i}}^{\prime }}^{\mathrm{T}}\text{\hspace{1em}(6-53)}$$

$\mathbf{W}$ 是一个 $3\times 3$ 的矩阵，对其进行 SVD 分解，得

$$\mathbf{W}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{\mathrm{T}}$$

当 $\mathbf{W}$ 满秩时，求得

$$\mathbf{R}=\mathbf{U}{\mathbf{V}}^{\mathrm{T}}\text{\hspace{1em}(6-54)}$$

若 $\mathbf{R}$ 的行列式为负，则取 $-\mathbf{R}$ 作为最优值。

6.5.2 非线性优化方法

用李代数表示位姿，则目标函数可写成

$$\underset{\mathbf{\xi }}{\min }=\frac{1}{2}\sum _{i=1}^n\Vert ({\mathbf{p}}_i-\exp ({\mathbf{\xi }}^{\wedge }){\mathbf{p}}_{\mathbf{i}}^{\prime }){\Vert }_2^2\text{\hspace{1em}(6-55)}$$

类似式（6-39）、（6-40）、（6-41），可推导单个误差项关于位姿的数，即

$$\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}=\frac{\partial \mathbf{e}}{\partial \mathbf{p}}\frac{\partial \mathbf{p}}{\partial \delta \mathbf{\xi }}=-1\cdot (\exp ({\mathbf{\xi }}^{\wedge }){\mathbf{p}}_{\mathbf{i}}^{\prime }{)}^{\odot }=-(\exp ({\mathbf{\xi }}^{\wedge }){\mathbf{p}}_{\mathbf{i}}^{\prime }{)}^{\odot }\text{\hspace{1em}(6-56)}$$

求出梯度后，便可不断迭代优化， 直至求出最优解。