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

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