1. 下面这个代码一直有问题
for (int i = frame_count; i >= 0; i--)
Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
2. 你觉得这里上面那一行才是对的???VINS
// Eigen::Vector3d t0 = Ps[imu_i]; Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
3.
Eigen::Vector3d f = it_per_frame.point.normalized(); svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0); svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);
4.
SLAM轨迹精度评估工具evo使用方法_wongHome的博客-CSDN博客_evo评估slam
evo对于tum格式需要的是 (时间戳,tx,ty,tz,qx,qy,qz,qw)
但是实际tum数据格式是(时间戳,tx,ty,tz,qw,qx,qy,qz)
因此对于实际的tum数据集,还需要调整顺序。