习题一:基础题-证明题

证明

$$ \mathbf{D}^{\top} \mathbf{D}=\sum_{i=1}^{4} \sigma_{i}^{2} \mathbf{u}_{i} \mathbf{u}_{j}^{\top} $$

式子中$\mathbf{y} = \mathbf{u}_ {4}$ 是该问题的最优解。提示:设 $\mathbf{y}^{\prime}=\mathbf{u}_ {4}+\mathbf{v}$,其中$\mathbf{v}$正交于$\mathbf{u}_ {4}$,证明

$$ \mathbf{y}^{\prime T} \mathbf{D}^{\top} \mathbf{D} \mathbf{y}^{\prime} \geq \mathbf{y}^{\top} \mathbf{D}^{\top} \mathbf{D} \mathbf{y} $$

该方法基于奇异值构造矩阵零空间的理论。

答案

换一种思路,没有采用提示的相关方法。 我们是需要寻找最小二乘解:

$$ \min _{\mathbf{y}}\|\mathbf{D} \mathbf{y}\|_{2}^{2}, \quad \text { s.t. }\|\mathbf{y}\|=1 $$

将上述式子展开:

$$ \|\mathrm{D} \mathrm{y} \|_{2}^{2} = \mathbf{y}^{\top} \mathbf{D}^{\top} \mathbf{D} \mathbf{y} $$

增加一个微小量$ \lambda $,则:

$$ \begin{aligned} \|\mathrm{D} \mathrm{y} \|_{2}^{2} &= \mathbf{y}^{\top} \mathbf{D}^{\top} \mathbf{D} \mathbf{y} \\ &= \mathbf{y}^{\top} \mathbf{D}^{\top} \mathbf{D} \mathbf{y} - \lambda(\mathbf{y}^{\top}\mathbf{y} - 1) \end{aligned} \tag 1 $$

对$\mathbf{y}$求导可得(因为求最小值,令导数为零):

$$ \mathbf{D}^{\top} \mathbf{D} \mathbf{y} - \lambda\mathbf{y} = 0 \tag 2 $$

此时$\mathbf{y}$为$\mathbf{D}^{\top} \mathbf{D}$矩阵的特征向量,$\lambda$为其特征值,将(2)式代入(1)式可得:

$$ \begin{aligned} \|\mathrm{D} \mathrm{y} \|_{2}^{2} &= \mathbf{y}^{\top} \mathbf{D}^{\top} \mathbf{D} \mathbf{y} \\ &= \mathbf{y}^{\top} \mathbf{D}^{\top} \mathbf{D} \mathbf{y} - \lambda(\mathbf{y}^{\top}\mathbf{y} - 1) \\ &= \lambda\mathbf{y}^{\top}\mathbf{y} - \lambda\mathbf{y}^{\top}\mathbf{y} + \lambda \\ &= \lambda \end{aligned} $$

所以,取$\mathbf{D}^{\top} \mathbf{D}$最小的特征值对应的特征向量,即$\mathbf{y}$取矩阵$\mathbf{D}^{\top} \mathbf{D}$最小特征值对应的特征向量时,即为上述最小二乘的解。

习题二:提升题-代码编写

完成代码部分-三角化估计深度的代码。

答案

代码编写,按照相应的公示,需要注意一下几点:

  • 需要注意投影矩阵是世界到相机的,而pose结构体中是相机到世界的,需要做一下转换,旋转矩阵问题不大,关键是平移,这个还得需要乘以旋转矩阵的转置,符号也需要变化。
  • 最后计算特征点坐标的时候,需要注意前三维需要除以最后一维。

代码如下:

    Eigen::Matrix<double, 14, 4> matrixD;
    matrixD.setZero();
    int num = 0;
    for (int i = start_frame_id; i < end_frame_id; ++i) 
    {
        
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d tcw = -camera_pose[i].Rwc.transpose()*camera_pose[i].twc;
        Eigen::Vector4d P11(Rcw(0,0), Rcw(0,1),Rcw(0,2), tcw.x());
        Eigen::Vector4d P12(Rcw(1,0), Rcw(1,1),Rcw(1,2), tcw.y());
        Eigen::Vector4d P13(Rcw(2,0), Rcw(2,1),Rcw(2,2), tcw.z());

        matrixD.block(num,0,1,4) = camera_pose[i].uv.x()*P13.transpose() -P11.transpose();
        matrixD.block(num+1,0,1,4) = camera_pose[i].uv.y()*P13.transpose() -P12.transpose();
        num = num+2;
    }
    Eigen::Matrix<double, 4, 4> matrixA;
    matrixA = matrixD.transpose()*matrixD;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrixA,Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix4d U = svd.matrixU();
    Eigen::Matrix4d V = svd.matrixV();
    Eigen::Matrix4d Sigma = U.inverse()*matrixA*V.transpose().inverse();
    // std::cout<<"U:\n"<<U<<"\nV:\n"<<V<<"\nSigma:\n"<<Sigma<<std::endl;
    P_est(0) = V(0,3)/ V(3,3);
    P_est(1) = V(1,3)/ V(3,3);
    P_est(2) = V(2,3)/ V(3,3);

最终结果如下,从矩阵结果中看出$\sigma_ {4} \ll \sigma_ {3}$,说明满足判解有效性。