Vins-Mono 论文 && Coding 一 6(1). VIO: solveOdometry

最新推荐文章于 2022-03-06 15:19:19 发布

scofield_ly

最新推荐文章于 2022-03-06 15:19:19 发布

阅读量200

点赞数

分类专栏： # vio 文章标签： slam

本文链接：https://blog.csdn.net/weixin_40072161/article/details/110749671

版权

vio 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

一、流程

1. 三角化

估计 features 初始深度

2. 优化求解

构造非线性最小二乘问题

3. 边缘化

求解边缘化先验约束

二、三角化

1. 细节

(1). 用所有帧的 pose 三角化，待求解量：启始帧相机坐标系下的齐次坐标

$\bar{x}=\begin{bmatrix} k\cdot x_c\\ k\cdot y_c\\ k\cdot z_c\\ k \end{bmatrix}$ (1)

(2). 通过相机坐标系下的齐次坐标，计算深度

$d=z_c=\bar{x}[2]/\bar{x}[3]$ (2)

2. 代码实现

void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{
    for (auto &it_per_id : feature) {

        ......
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;  // 起始帧

        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
        int svd_idx = 0;

        Eigen::Matrix<double, 3, 4> P0;
        Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
        Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
        P0.leftCols<3>() = Eigen::Matrix3d::Identity();
        P0.rightCols<1>() = Eigen::Vector3d::Zero();

        // 用所有帧的 pose 三角化
        for (auto &it_per_frame : it_per_id.feature_per_frame) {
            imu_j++;

            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            Eigen::Vector3d t = R0.transpose() * (t1 - t0);
            Eigen::Matrix3d R = R0.transpose() * R1;
            Eigen::Matrix<double, 3, 4> P;
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            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);
            if (imu_i == imu_j)
                continue;
        }
        // SVD 求解
        Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
        double svd_method = svd_V[2] / svd_V[3];
        // 估计初始深度
        it_per_id.estimated_depth = svd_method;
        if (it_per_id.estimated_depth < 0.1) {
            it_per_id.estimated_depth = INIT_DEPTH;
        }
    }
}

三、优化

1. marginlization_factor

引入边缘化先验约束

if (last_marginalization_info){
   // construct new marginlization_factor
  MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
  problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
}

具体细节 todo

2. imu factor

for (int i = 0; i < WINDOW_SIZE; i++) {
   int j = i + 1;
   if (pre_integrations[j]->sum_dt > 10.0)
        continue;
   IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
   problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}

具体细节参考：Vins-Mono 论文 && Coding 一 4(2). imu factor

3. reprojection factor

int f_m_cnt = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature) {
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
        continue;
    
    ++feature_index;
    int imu_i = it_per_id.start_frame；
    int imu_j = imu_i - 1;
    Vector3d pts_i = it_per_id.feature_per_frame[0].point;

    for (auto &it_per_frame : it_per_id.feature_per_frame) {
        imu_j++;
        if (imu_i == imu_j) {
            continue;
        }
        Vector3d pts_j = it_per_frame.point;
        if (ESTIMATE_TD) {  // 考虑 timeoffset
            ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,                                                      it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,                                                      it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
            problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
        } else {  
            ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);  // 观测：在两帧之间的像素坐标
            problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);   // 影响的状态变量：第 i,j 帧的 pose，外参，特征点位置信息
       }
       f_m_cnt++;  // factor 计数加上一
    }
}

4. relocalization factor

if(relocalization_info) {
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);  // 引入的额外状态变量：double relo_Pose[SIZE_POSE];

    int retrive_feature_index = 0;
    int feature_index = -1;
    for (auto &it_per_id : f_manager.feature) {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
        ++feature_index;
        int start = it_per_id.start_frame;
        if(start <= relo_frame_local_index)  {   
            while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id) {
                retrive_feature_index++;
            }
            if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id) {
                // 该 feature 同时在 frame_i(窗口内) 和 relo_pose(窗口外) 被观测
                Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
                Vector3d pts_i = it_per_id.feature_per_frame[0].point;
                ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
                retrive_feature_index++;
            }     
        }
    }
}

具体细节 todo

三、marginalization

1. marginalization_flag == MARGIN_OLD

<1>. last margin block

ResidualBlockInfo *residual_block_info = new 
ResidualBlockInfo(marginalization_factor, NULL, 
                  last_marginalization_parameter_blocks, drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);

<2>. imu factor block

ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL, vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]}, vector<int>{0, 1});
marginalization_info->addResidualBlockInfo(residual_block_info);

<3>. reprojection error

if (ESTIMATE_TD) {
    ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,       it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,            it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
   ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,                                                                        vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},                                              vector<int>{0, 3});
  marginalization_info->addResidualBlockInfo(residual_block_info);
} else {
  ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
  ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,                                                                  vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},                                                        vector<int>{0, 3});
 marginalization_info->addResidualBlockInfo(residual_block_info);
}

<4>. marginalization_info->preMarginalize();

<5>. marginalization_info->marginalize();

2. marginalization_flag == MARGIN_SECOND_NEW

<1>. last margin block

ResidualBlockInfo *residual_block_info = new 
ResidualBlockInfo(marginalization_factor, NULL, 
                  last_marginalization_parameter_blocks, drop_set);
marginalization_info->addResidualBlockInfo(residual_block_info);

<2>. marginalization_info->preMarginalize();

<3>. marginalization_info->marginalize();