VINS

奔跑的瓦力 2019-09-16

展開全文

VINS-Mono源碼解析（五）后端: 緊耦合優(yōu)化

1. 優(yōu)化原理

a) 優(yōu)化變量

\begin{aligned} χ & = [x_{0}, x_{1}, . . ., x_{n}, x_{c}^{b}, λ_{0}, λ_{1}, . . ., λ_{m}] \\ 第 k 帧 状 态 : x_{k} & = [p_{b_{k}}^{w}, v_{b_{k}}^{w}, q_{b_{k}}^{w}, b_{a}, b_{g}] 15 维 \\ 相 机 外 参 : x_{c}^{b} & = [p_{c}^{b}, q_{c}^{b}] 6 维 \end{aligned}

λ_{i}

是第i個特征點的第一個觀測對應(yīng)的逆深度. 所以總的狀態(tài)的長度為

15 \times N + 6 + M

N

為滑動窗中frame的個數(shù),

M

為特征點的個數(shù).

b) 優(yōu)化函數(shù)

min_{χ} {\underset{m a r g i n a l i z a t i o n r e s i d u a l}{\underset{⏟}{{| | r_{p} - H_{p} χ | |}^{2}}} + \underset{I M U r e s i d u a l}{\underset{⏟}{\sum_{k \in B} {| | r_{B} ({\hat{z}}_{b_{k + 1}}^{b_{k}}, χ) | |}^{2}}} + \underset{v i s u a l r e s i d u a l}{\underset{⏟}{\sum_{(l, j) \in C} {| | r_{c} ({\hat{z}}_{l}^{c_{j}}, χ) | |}_{P_{l}^{c_{i}}}^{2}}}}

優(yōu)化函數(shù)有三部分,
- 第一部分是那些已經(jīng)從sliding windows中去掉(marginalize)的節(jié)點和特征點構(gòu)成的約束, 暫且簡單的理解為marginalization得到的歷史約束的prior, 是一個關(guān)于

χ

的等式約束.
- 第二部分是IMU 運動模型的誤差, 每相鄰的兩幀IMU之間產(chǎn)生一個residual.
- 第三部分是視覺的誤差, 單個特征點

l

在相機

c_{j}

下的投影會產(chǎn)生一個residual.

注意: 下面暫時忽略掉loop closure部分的優(yōu)化, 只考慮VIO的部分, 這樣便于理解些.

2. ceres優(yōu)化: Estimator::optimization()

整個slidingwindows優(yōu)化在optimization()函數(shù)中求解, 該函數(shù)具體步驟如下:

a) 初始化ceres

創(chuàng)建一個ceres Problem實例, loss_function定義為CauchyLoss.

ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);

b) 加入待優(yōu)化參數(shù)項

　　先添加優(yōu)化參數(shù)量, ceres中參數(shù)用ParameterBlock來表示,類似于g2o中的vertex, 這里的參數(shù)塊有sliding windows中所有幀的para_Pose(7維) 和 para_SpeedBias(9維).

/*parameters.h*/
enum SIZE_PARAMETERIZATION
{
    SIZE_POSE = 7,        //7 DoF(x,y,z,qx,qy,qz,qw)
    SIZE_SPEEDBIAS = 9,   //9 DoF(vx,vy,vz, bas_x,bas_y,bas_z, bgs_x,bgs_y,bgs_z)
    SIZE_FEATURE = 1      //1 DoF(inv_depth)
};

/*estimator.cpp*/
/*add vertex of: 1)pose, 2)speed and 3)bias of acc and gyro */
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
    problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}
/*add vertex of: camera extrinsic */
for (int i = 0; i < NUM_OF_CAM; i++)
{
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
    if (!ESTIMATE_EXTRINSIC)
    {
        ROS_DEBUG("fix extinsic param");
        problem.SetParameterBlockConstant(para_Ex_Pose[i]);
    }
    else
        ROS_DEBUG("estimate extinsic param");
}

c) 加入誤差項

　　代碼如下, 依次加入margin項,IMU項和視覺feature項. 每一項都是一個factor, 這是ceres的使用方法, 創(chuàng)建一個類繼承ceres::CostFunction類, 重寫Evaluate()函數(shù)定義residual的計算形式. 分別對應(yīng)marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三個類.

// add residual for prior from last marginalization
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);
}
// add residual for IMU
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]);
}
// add residual for per feature to per frame
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, 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;
        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]);
        f_m_cnt++;
    }
}

誤差項總結(jié):
- Marginalization residual: 1個
- IMU residual: WINDOW_SIZE個(總長度WINDOW_SIZE+1), 每相鄰兩個Pose之間一個IMU residual項.
- feature residual: 觀測數(shù)大于2的特征, 首次觀測與后面的每次觀測之間各一個residual項.

d) ceres求解

創(chuàng)建一個求解配置參數(shù)Option, 定義成DENSE_SCHUR(為什么不是SPARSE_SCHUR?), 優(yōu)化算法用的”dog leg”, 設(shè)置最大迭代次數(shù)和最大求解時間. 創(chuàng)建一個求解描述Summary, 調(diào)用ceres::Solve()進行求解.

// ceres solve problem
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
    options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
    options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

求解完成之后, 還有做marginalization, 這個后面再單獨討論吧. 這里的話, 下面主要分析一下兩個Factor類中的Evaluate()函數(shù), 也就是residual定義.

3. IMU Residual

IMUFactor類的聲明繼承如下:

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>

模板參數(shù)的含義如下:

15: 殘差向量的長度(包括p,v,q,ba,bg)

7: 第1個優(yōu)化參數(shù)的長度(para_Pose[i])

9: 第2個優(yōu)化參數(shù)的長度(para_SpeedBias[i])

7: 第3個優(yōu)化參數(shù)的長度(para_Pose[j])

9: 第4個優(yōu)化參數(shù)的長度(para_SpeedBias[j])

對于Evaluate輸入double const *const *parameters, parameters[0], parameters[1], parameters[2], parameters[3]分別對應(yīng)4個輸入?yún)?shù), 它們的長度依次是7,9,7,9

IMUFactor類重寫Evaluate()函數(shù)輸入parameter計算residual, 實際是調(diào)用IntegrationBase::evaluate()來真正計算殘差.

Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();
residual = sqrt_info * residual;

真正IMU殘差計算IntegrationBase::evaluate()的代碼如下:

Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi, const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
    Eigen::Matrix<double, 15, 1> residuals;

    Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
    Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

    Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);

    Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
    Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);

    Eigen::Vector3d dba = Bai - linearized_ba;
    Eigen::Vector3d dbg = Bgi - linearized_bg;

    // IMU預(yù)積分的結(jié)果,消除掉acc bias和gyro bias的影響, 對應(yīng)IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
    Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
    Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
    Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

    // IMU項residual計算,輸入?yún)?shù)是狀態(tài)的估計值, 上面correct_delta_*是預(yù)積分值, 二者求'diff'得到residual.
    residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
    residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
    residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
    residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
    residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
    return residuals;
}

該函數(shù)輸入前后兩個時刻的 $P, Q, V, B a, B g$ , 根據(jù)預(yù)積分結(jié)果delta_q, delta_v, delta_p求IMU殘差, 殘差是一個長度為15的向量, 包括 $(p, v, q, b a, b g)$ 共5個3維殘差向量, 代碼對應(yīng)計算公式如下:

r_{I M U} = [\begin{matrix} R_{w}^{b_{k}} (p_{b_{k + 1}}^{w} - p_{b_{k}}^{w} + \frac{1}{2} g^{w} Δ t_{k}^{2} - v_{b_{k}}^{w} Δ t_{k}) - {\hat{α}}_{b_{k + 1}}^{b_{k}} \\ R_{w}^{b_{k}} (v_{b_{k + 1}}^{w} + g^{w} Δ t_{k} - v_{b_{k}}^{w}) - {\hat{β}}_{b_{k + 1}}^{b_{k}} \\ 2 {[{q_{b_{k}}^{w}}^{- 1} ⨂ q_{b_{k + 1}}^{w} ⨂ ({\hat{γ}}_{b_{k + 1}}^{b_{k}})^{- 1}]}_{x y z} \\ b_{a b_{k + 1}} - b_{a b_{k}} \\ b_{w b_{k + 1}} - b_{w b_{k}} \end{matrix}]

值得注意的是,這里的

{\hat{α}}_{b_{k + 1}}^{b_{k}}, {\hat{β}}_{b_{k + 1}}^{b_{k}}, {\hat{γ}}_{b_{k + 1}}^{b_{k}}

是經(jīng)過校正過bias的(根據(jù)

p, q, v

對bias的jacobian以及bias的差對預(yù)積分量進行修正,具體見上面代碼,), 只有acc和gyro的噪聲, 是跟bias相關(guān)的量, 跟初始時刻的速度及姿態(tài)都無關(guān). 在優(yōu)化迭代的過程中, 預(yù)積分值是不變的, 輸入的狀態(tài)值會被不斷的更新, 然后不斷的調(diào)用evaluate()計算更新后的IMU殘差.

代碼中residual還乘以了一個sqrt_info,這是為什么呢?
這是因為真正的優(yōu)化項其實是Mahalanobis 距離: $d = r^{T} P^{- 1} r$ , $P$ 是協(xié)方差, 而ceres只接受最小二乘優(yōu)化, 也就是 $min e^{T} e$ , 所以把 $P^{- 1}$ 做LLT分解即 $L L^{T} = P^{- 1}$ , 那么 $d = r^{T} L L^{T} r = (L^{T} r)^{T} (L^{T} r)$ , 令 $r^{'} = L^{T} r$ 作為新的優(yōu)化誤差, 這樣就能用ceres求解了. $L^{T} r$ 就是代碼中的sqrt_info.

Mahalanobis距離其實相當(dāng)于一個殘差加權(quán), 協(xié)方差大的加權(quán)小, 協(xié)方差小的加權(quán)大, 著重優(yōu)化那些比較確定的殘差. 注掉的那行//sqrt_info.setIdentity();相當(dāng)于不加權(quán), 這個加權(quán)策略的效果需要對比測試才知道.

最后更新jacobian并進行numerical unstable判斷, 這一塊暫時沒看懂, 求大神點撥~

4. Visual Residual

ProjectionFactor類的聲明繼承如下

class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>

2: 殘差長度(err_x, err_y)

7: 第1個優(yōu)化參數(shù)pose_i的長度(para_Pose[imu_i]=(px,py,pz,qx,qy,qz,qw) )

7: 第2個優(yōu)化參數(shù)pose_j的長度(para_Pose[imu_j])

7: 第3個優(yōu)化參數(shù)外參的長度(para_Ex_Pose[0])

1: 第4個優(yōu)化參數(shù)feature_inverse_depth的長度(para_Feature[feature_index])

關(guān)鍵計算代碼如下:

double inv_dep_i = parameters[3][0];

Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;       // pt in ith camera frame
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;   // pt in ith body frame
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;            // pt in world frame    
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);// pt in jth body frame
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);  // pt in jth camera frame
Eigen::Map<Eigen::Vector2d> residual(residuals);

//reprojection error
#ifdef UNIT_SPHERE_ERROR 
residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

residual = sqrt_info * residual;

計算過程很簡單,就是重投影過程:

i幀中圖像中的點=>i幀相機系=>i幀body系=>world系=>j幀body系=>j幀相機系

重投影誤差計算有兩種, 如果是用SPHERE相機模型, 就取tangent平面上的誤差, 否則就用常規(guī)的圖像平面誤差. 然后后面也同樣乘以了一個sqrt_info轉(zhuǎn)成馬氏距離. 最后也是更新jacobian.

4. Marginalization

sliding windows bounding了優(yōu)化問題中pose的個數(shù), 從而防止pose和特征的個數(shù)隨時間不斷增加, 使得優(yōu)化問題始終在一個有限的復(fù)雜度內(nèi), 不會隨時間不斷增長.

然而, 將pose移出windows時, 有些約束會被丟棄掉, 這樣勢必會導(dǎo)致求解的精度下降, 而且當(dāng)MAV進行一些退化運動(如: 勻速運動)時, 沒有歷史信息做約束的話是無法求解的. 所以, 在移出位姿或特征的時候, 需要將相關(guān)聯(lián)的約束轉(zhuǎn)變成一個約束項作為prior放到優(yōu)化問題中. 這就是marginalization要做的事情.

Marginalization數(shù)學(xué)上是用schur complement來實現(xiàn), 暫時還沒弄太懂, 有一些參考的博文:
1. SLAM中的marginalization 和 Schur complement
2. DSO 中的Windowed Optimization

VINS-MONO中,為了處理一些懸停的case, 引入了一個two-way marginalization, 簡單來說就是:
- 如果倒數(shù)第二幀是關(guān)鍵幀, 則將最舊的pose移出sliding window, 也就是MARGIN_OLD,
- 如果倒數(shù)第二幀不是關(guān)鍵幀, 則將倒數(shù)第二幀pose移出sliding window, 也就是MARGIN_NEW.
選取關(guān)鍵幀的策略是視差足夠大

在懸停等運動較小的情況下, 會頻繁的MARGIN_NEW, 這樣也就保留了那些比較舊但是視差比較大的pose. 這種情況如果一直MARGIN_OLD的話, 視覺約束不夠強, 狀態(tài)估計會受IMU積分誤差影響, 具有較大的累積誤差.

代碼中對應(yīng)marginalization_factor.cpp文件, 這塊還沒太看懂.