源于计算机视觉life的LiDAR+视觉+IMU多传感器融合SLAM：原理推导+源码逐行详解+项目实战

SVD求解Ax=0

首先，我们需要了解四元数的基本概念。四元数是由三个虚部和一个实部组成的复数扩展，可以用来表示三维空间中的旋转。四元数的乘法规则如下：

$$q_1\otimes q_2=[q_1{]}_L{q}_2=[q_2{]}_R{q}_1$$

其中，

• $[q_1{]}_L$ 是左乘矩阵：
  $$\left[\begin{array}{cccc}{p}_w& -p_x& -p_y& -p_z\\ p_x& p_w& -p_z& p_y\\ p_y& p_z& p_w& -p_x\\ p_z& -p_y& p_x& p_w\end{array}\right]$$
• $[q_1{]}_R$ 是右乘矩阵：
  $$\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right]$$

化简一下，即：

• $[q_1{]}_L=q_{w}I+\left[\begin{array}{cc}0& -\overrightarrow{q_v}\\ \overrightarrow{q_v}& [\overrightarrow{q_v}{]}_{\times }\end{array}\right]$，
• $[q_1{]}_R=q_{w}I+\left[\begin{array}{cc}0& -\overrightarrow{q_v}\\ \overrightarrow{q_v}& -[\overrightarrow{q_v}{]}_{\times }\end{array}\right]$

其中，

• $[\mathbf{a}{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]$

对于SVD求解Ax=0，举个例子，如lidar和IMU的外参标定，有以下关系

$$Q_{b_h+1}^{b_{k}L}{q}_i^l=Q_{l_{h+1}}^{l_{i}R}{q}_l^b$$

合并之后为：

$$(Q_{b_h+1}^{b_{k}L}-Q_{l_{h+1}}^{l_{i}R})q_l^b=0$$

通常来说，我们会收集若干个IMU和lidar的相对旋转和平移，则可以联立如下：

$$\left[\begin{array}{c}{Q}_{b_{n-1}}^{b_{n}L}-Q_{l_{n-1}}^{l_{n}R}\\ Q_{b_{n-2}}^{b_{n-1}L}-Q_{l_{n-2}}^{l_{n-1}R}\\ ⋮\\ Q_{b_0}^{b_{1}L}-Q_{l_0}^{l_{1}R}\end{array}\right]q_l^b=A_{4n\times 4}{q}_l^b=0\text{\hspace{1em}(1)}$$

相当于已知一个$4n\times 4$的小矩阵，求出一个$4\times 1$向量的最优解，通常$n>4$，因此，这是一个基本的超定方程组求解问题，通常使用SVD方法求解。

即将A矩阵进行SVD分解，得

$$UD{V}^{T}x=0$$

这里U矩阵和V矩阵都是正交矩阵（类比旋转矩阵，不改变大小），D是奇异值由大到小的对角矩阵，因此等价求解

$$D{V}^{T}x=0$$

然后，我们令$V^{T}x=y$，y仍然是一个四元数，所以$||y||=1$

当仅当$y=[0,0,0,1{]}^T$时，$Dy$取得最小值，则$x=V\ast [0,0,0,1{]}^T$

此时对应的$x$即为V矩阵中最小奇异值对应的列向量，然后将其转换成四元数即为所求的旋转。