SLAM中三角测量求解特征点世界坐标系下坐标Pw的方法

一.SVD分解法

1.SVD分解的介绍

   

SVD分解的本质就是将一个线性变换 M 分解为旋转 左乘拉伸 再左乘旋转U。

    U和V都表示旋转，均为单位正交矩阵，即满足

，

，

    有时也称作酉矩阵（Unitary Matrix）（矩阵的共轭转置（也称为厄米共轭或伴随矩阵）等于其逆矩阵。

        的性质如下图中所示：

   

求矩阵M的SVD分解的步骤如下：

 2.SVD分解法的推导

 注意事项

•  这里的pr1和pr2表示的含义如下，此时，对应的u和v为特征点在像素坐标系下的坐标

        当pr1和pr2表示的含义如下时（即不包括相机内参时），对应的u和v变为特征点在归一化坐标系下的坐标 

•  在下文的求解中，得到的解 x 实际上是 P1 的坐标，是 Pw 在相机1坐标系（即C1）下的坐标，即Pc1。在上面的推导中，我们考虑了相机1的位姿 T1w 和 相机2的位姿 T2w 的相对关系，假设了世界坐标系和相机1坐标系（即C1）重合（这样才有P1=Pw），当这两个坐标系不重合时，需使用如下公式得到真正的Pw：

3.SVD分解法的求解

    先给出结论：Ax=0的最小二乘解是最小特征值对应的特征向量

    取SVD分解得到的V矩阵最后一列作为 P 1的解，但我们还需要归一化以后才能使用。
    对于超定方程Ax=0，没有真正的非零解析解，因为约束太多，没法都满足(零向量除外)，求其最小二乘解近似。

理论推导部分
齐次方程组形如： A x = 0 。
在一些优化，拟合等问题中经常出现，我们常考虑方程多于未知数元数的情况------超定方程组。

首先对于平凡解x=0我们一般不感兴趣，一般我们会寻求方程组的非零解。

如果x是方程组的一个解，那么对于 V k ∈ R , kx也是齐次方程组的解，一个合理的假设是只求满足  的解。

假设A的维数是 m × n ，一般的 m > n (超定)，没有真正的非零解析解。当没有精确解的时候，我们通常求其最小二乘解，描述为：

求使||Ax||最小化并满足||x||=1的 x

先介绍一个引理，即对于一个酉阵 p（）和一个向量x(向量维数等于P列数)，有：

 

至此，求解完毕。

参考：

      【学长小课堂】什么是奇异值分解SVD--SVD如何分解时空矩阵

        奇异值分解（Singular Values Decomposition，SVD）

        SLAM--三角测量SVD分解法、最小二乘法及R t矩阵的判断

        奇异值分解（SVD）方法求解最小二乘问题