深蓝学院《从零开始手写VIO》作业六

深蓝学院《从零开始手写VIO》作业六

1. 证明题

证明$Dy=0$的最优解$y$等于$D^{T}D$的最小奇异值对应的奇异值向量

矩阵$D^{T}D$的奇异值分解如下：
$${\mathbf{D}}^{\top }\mathbf{D}=\sum _{i=1}^4{\sigma }_i^2{\mathbf{u}}_i{\mathbf{u}}_j^{\top }$$
回顾我们的原始问题如下：
求解$D_{2n\times 4}{y}_{4\times 1}=0$，这是一个超定方程，本质上求解得到的是一个最小二乘解，使用如下表示：
$$\underset{y}{\min }||Dy|{|}^2=(Dy{)}^T(Dy)=y^T{D}^{T}Dy$$
其中$||y||=1$

$y$可以由$D^{T}D$的奇异值向量线性组合得到，也就是可以表示成如下形式
$$y=\sum _{i=1}^4{k}_i{u}_i=k_i{u}_i+v$$
其中$$v=\sum _{j=1,j\ne i}^4{k}_j{u}_j$$
$$k_i,k_j\in R$$
容易知道$u_i$和$v$正交。
将$y$代入$y^T{D}^{T}Dy$得到
$$\underset{y}{\min }||Dy|{|}^2=(k_i{u}_i+v{)}^T{D}^{T}D(k_i{u}_i+v)=k_i^2{u}_i^T{D}^{T}D{u}_i+v^T{D}^{T}Dv+k_i{u}_i^T{D}^{T}Dv+k_i{v}^T{D}^{T}D{u}_i$$

由于$u_i$和$v$正交，所以后两项为0；并且$D{u}_i={\sigma }_i{u}_i$，带入得到
$$\underset{y}{\min }||Dy|{|}^2=(k_i{u}_i+v{)}^T{D}^{T}D(k_i{u}_i+v)=k_i^2{u}_i^T{D}^{T}D{u}_i+v^T{D}^{T}Dv=k_i^2{\sigma }_i^2||u_i|{|}^2+v^T{D}^{T}Dv>=k_i^2{\sigma }_i^2||u_i|{|}^2$$
当且仅当$v=0$的时候等号成立。
如果想取得最小值，则${\sigma }_i={\sigma }_4$,也就是取得最小奇异值的时候，目标函数取得最小值，此时
$$y=k_4{u}_4+v=k_4{u}_4$$
由于$||y||=1$,所以$k_4=1$,所以
$$y=u_4$$
证明完毕。

2. 代码题

代码如下：

    /// TODO::homework; 请完成三角化估计深度的代码
    // 遍历所有的观测数据，并三角化
    Eigen::Vector3d P_est;           // 结果保存到这个变量
    P_est.setZero();
    /* your code begin */
    Eigen::MatrixXd matD = ConstructMatrixD(camera_pose);
    Eigen::BDCSVD bcdsvd(matD.transpose()*matD, Eigen::ComputeFullV | Eigen::ComputeFullU);
    auto singular_values = bcdsvd.singularValues();
    auto matU = bcdsvd.matrixU();
    auto matV = bcdsvd.matrixV();


    // result 1
    auto tmp_y = matU.rightCols(1);
    auto real_y = tmp_y/tmp_y(3);

    // result2
    double s = matD.maxCoeff();
    Eigen::Matrix4d scale_mat = Eigen::Matrix4d::Identity()/s;
    auto matD_new = matD*scale_mat;
    Eigen::BDCSVD bcdsvd_new(matD_new.transpose()*matD_new, Eigen::ComputeFullV | Eigen::ComputeFullU);
    auto matU_new = bcdsvd_new.matrixU();
    auto tmp_y_new = scale_mat*matU_new.rightCols(1); //结果一样
    auto real_y_new = tmp_y_new/tmp_y_new(3);
    /* your code end */

结果如下：

ground truth: 
  -2.9477 -0.330799   8.43792
[normal solve] your result1: 
  -2.9477 -0.330799   8.43792         1
[using Scale solve] your result2: 
  -2.9477 -0.330799   8.43792         1