视觉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);