从零开始手写vio 第六节 视觉前端 任务7.4代码实践题

 /* your code begin */
    Eigen::Matrix<double,14,4> D;
    D.setZero();
    Vector4d truth ;
    truth<<tx,ty,tz,1;
    for(int i=start_frame_id; i< end_frame_id;++i)
    {
        Eigen::Matrix<double,3,4> T;
        T.setZero();
        T.block(0,0,3,3)=camera_pose[i].Rwc.inverse();
        T.block(0,3,3,1)=-camera_pose[i].Rwc.inverse()*camera_pose[i].twc;

        D.block(2*(i-start_frame_id),0,2,4) = camera_pose[i].uv*T.block(2,0,1,4)-T.block(0,0,2,4);
    }
    cout<<"D*y="<<endl<<D*truth<<endl;//每一项误差
//
    Eigen::SelfAdjointEigenSolver < Eigen::Matrix<double, 4, 4> > eigen_sovler(D.transpose()*D);  // 特征值求解器
    cout << "Eigen Values : " << endl;
    cout << eigen_sovler.eigenvalues().transpose() << endl;    // 输出特征值
    cout << "Eigen Vector :" << endl;
    cout << eigen_sovler.eigenvectors() << endl;               // 输出特征向量

    double v[4];
    v[0] = eigen_sovler.eigenvectors()(0, 0);                  // 提取第一列特征向量
    v[1] = eigen_sovler.eigenvectors()(1, 0);
    v[2] = eigen_sovler.eigenvectors()(2, 0);
    v[3] = eigen_sovler.eigenvectors()(3, 0);

    v[0] /= v[3];
    v[1] /= v[3];
    v[2] /= v[3];
    v[3] /= v[3];
    P_est<<v[0],v[1],v[2];

    /* your code end */