视觉SLAM理论与实践进阶-手写VIO-6

1) 证明公式(15)

2) 完成特征点三角化代码
代码如下：

// 遍历所有的观测数据，并三角化
Eigen::Vector3d P_est; // 结果保存到这个变量
P_est.setZero();
/* your code begin */
auto loop_times = camera_pose.size()-start_frame_id;
Eigen::MatrixXd D((loop_times)*2,4);
for(int j=0;j<loop_times;++j)
{
Eigen::MatrixXd T_tmp(3,4);
T_tmp.block<3,3>(0,0)=camera_pose[j+3].Rwc.transpose();
T_tmp.block<3,1>(0,3)=-camera_pose[j+3].Rwc.transpose()*camera_pose[j+3].twc;
auto P_r1 = T_tmp.block<1,4>(0,0);
auto P_r2 = T_tmp.block<1,4>(1,0);
auto P_r3 = T_tmp.block<1,4>(2,0);
D.block<1,4>(2*j,0)=camera_pose[j+3].uv[0] * P_r3-P_r1;
D.block<1,4>(2*j+1,0)=camera_pose[j+3].uv[1] * P_r3-P_r2;
}
Eigen::Matrix4d D_res=D.transpose()*D;
Eigen::JacobiSVD<Eigen::Matrix4d> svd(D_res,Eigen::ComputeFullU|Eigen::ComputeFullV);
auto res_U = svd.matrixU();
auto res_V = svd.matrixV();
std::cout << "U=" << res_U << std::endl;
auto tmp = res_U.rightCols(1);
P_est=(tmp / tmp(3)).transpose().leftCols(3);

友情提示：代码下载需要C币，请事先判断是否对您有帮助，谨慎下载哦！！！