VINS-Mono源碼解析（五）后端: 緊耦合優(yōu)化
1. 優(yōu)化原理
a) 優(yōu)化變量
χ第k幀狀態(tài):xk相機(jī)外參:xbc=[x0,x1,...,xn, xbc, λ0,λ1,...,λm]=[pwbk,vwbk,qwbk,ba,bg] 15維=[pbc,qbc] 6維
λi是第i個(gè)特征點(diǎn)的第一個(gè)觀測(cè)對(duì)應(yīng)的逆深度. 所以總的狀態(tài)的長(zhǎng)度為15×N+6+M, N為滑動(dòng)窗中frame的個(gè)數(shù), M為特征點(diǎn)的個(gè)數(shù).
b) 優(yōu)化函數(shù)
minχ?????????????||rp?Hpχ||2marginalization residual+∑k∈B∣∣∣∣rB(z^bkbk+1,χ)∣∣∣∣2IMU residual+∑(l,j)∈C∣∣∣∣rc(z^cjl,χ)∣∣∣∣2Pcilvisual residual?????????????
優(yōu)化函數(shù)有三部分,
- 第一部分是那些已經(jīng)從sliding windows中去掉(marginalize)的節(jié)點(diǎn)和特征點(diǎn)構(gòu)成的約束, 暫且簡(jiǎn)單的理解為marginalization得到的歷史約束的prior, 是一個(gè)關(guān)于χ的等式約束.
- 第二部分是IMU 運(yùn)動(dòng)模型的誤差, 每相鄰的兩幀IMU之間產(chǎn)生一個(gè)residual.
- 第三部分是視覺的誤差, 單個(gè)特征點(diǎn)l在相機(jī)cj下的投影會(huì)產(chǎn)生一個(gè)residual.
注意: 下面暫時(shí)忽略掉loop closure部分的優(yōu)化, 只考慮VIO的部分, 這樣便于理解些.
2. ceres優(yōu)化: Estimator::optimization()
整個(gè)slidingwindows優(yōu)化在optimization()函數(shù)中求解, 該函數(shù)具體步驟如下:
a) 初始化ceres
創(chuàng)建一個(gè)ceres Problem實(shí)例, 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ù)項(xiàng)
先添加優(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) 加入誤差項(xiàng)
代碼如下, 依次加入margin項(xiàng),IMU項(xiàng)和視覺feature項(xiàng). 每一項(xiàng)都是一個(gè)factor, 這是ceres的使用方法, 創(chuàng)建一個(gè)類繼承ceres::CostFunction類, 重寫Evaluate()函數(shù)定義residual的計(jì)算形式. 分別對(duì)應(yīng)marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三個(gè)類.
// 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++;
}
}
誤差項(xiàng)總結(jié):
- Marginalization residual: 1個(gè)
- IMU residual: WINDOW_SIZE個(gè)(總長(zhǎng)度WINDOW_SIZE+1), 每相鄰兩個(gè)Pose之間一個(gè)IMU residual項(xiàng).
- feature residual: 觀測(cè)數(shù)大于2的特征, 首次觀測(cè)與后面的每次觀測(cè)之間各一個(gè)residual項(xiàng).
d) ceres求解
創(chuàng)建一個(gè)求解配置參數(shù)Option, 定義成DENSE_SCHUR(為什么不是SPARSE_SCHUR?), 優(yōu)化算法用的”dog leg”, 設(shè)置最大迭代次數(shù)和最大求解時(shí)間. 創(chuàng)建一個(gè)求解描述Summary, 調(diào)用ceres::Solve()進(jìn)行求解.
// 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, 這個(gè)后面再單獨(dú)討論吧. 這里的話, 下面主要分析一下兩個(gè)Factor類中的Evaluate()函數(shù), 也就是residual定義.
3. IMU Residual
IMUFactor類的聲明繼承如下:
class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
模板參數(shù)的含義如下:
- 15: 殘差向量的長(zhǎng)度(包括p,v,q,ba,bg)
- 7: 第1個(gè)優(yōu)化參數(shù)的長(zhǎng)度(para_Pose[i])
- 9: 第2個(gè)優(yōu)化參數(shù)的長(zhǎng)度(para_SpeedBias[i])
- 7: 第3個(gè)優(yōu)化參數(shù)的長(zhǎng)度(para_Pose[j])
- 9: 第4個(gè)優(yōu)化參數(shù)的長(zhǎng)度(para_SpeedBias[j])
對(duì)于Evaluate輸入double const *const *parameters, parameters[0], parameters[1], parameters[2], parameters[3]分別對(duì)應(yīng)4個(gè)輸入?yún)?shù), 它們的長(zhǎng)度依次是7,9,7,9
IMUFactor類重寫Evaluate()函數(shù)輸入parameter計(jì)算residual, 實(shí)際是調(diào)用IntegrationBase::evaluate()來真正計(jì)算殘差.
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殘差計(jì)算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的影響, 對(duì)應(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項(xiàng)residual計(jì)算,輸入?yún)?shù)是狀態(tài)的估計(jì)值, 上面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ù)輸入前后兩個(gè)時(shí)刻的P,Q,V,Ba,Bg, 根據(jù)預(yù)積分結(jié)果delta_q, delta_v, delta_p求IMU殘差, 殘差是一個(gè)長(zhǎng)度為15的向量, 包括(p,v,q,ba,bg)共5個(gè)3維殘差向量, 代碼對(duì)應(yīng)計(jì)算公式如下:
rIMU=????????????Rbkw(pwbk+1?pwbk+12gwΔt2k?vwbkΔtk)?α^bkbk+1Rbkw(vwbk+1+gwΔtk?vwbk)?β^bkbk+12[qwbk?1?qwbk+1?(γ^bkbk+1)?1]xyzbabk+1?babkbwbk+1?bwbk????????????
值得注意的是,這里的α^bkbk+1,β^bkbk+1,γ^bkbk+1是經(jīng)過校正過bias的(根據(jù)p,q,v對(duì)bias的jacobian以及bias的差對(duì)預(yù)積分量進(jìn)行修正,具體見上面代碼,), 只有acc和gyro的噪聲, 是跟bias相關(guān)的量, 跟初始時(shí)刻的速度及姿態(tài)都無關(guān). 在優(yōu)化迭代的過程中, 預(yù)積分值是不變的, 輸入的狀態(tài)值會(huì)被不斷的更新, 然后不斷的調(diào)用evaluate()計(jì)算更新后的IMU殘差.
代碼中residual還乘以了一個(gè)sqrt_info,這是為什么呢?
這是因?yàn)檎嬲膬?yōu)化項(xiàng)其實(shí)是Mahalanobis 距離: d=rTP?1r, P是協(xié)方差, 而ceres只接受最小二乘優(yōu)化, 也就是mineTe, 所以把P?1做LLT分解即LLT=P?1, 那么d=rTLLTr=(LTr)T(LTr), 令r′=LTr作為新的優(yōu)化誤差, 這樣就能用ceres求解了.LTr就是代碼中的sqrt_info.
Mahalanobis距離其實(shí)相當(dāng)于一個(gè)殘差加權(quán), 協(xié)方差大的加權(quán)小, 協(xié)方差小的加權(quán)大, 著重優(yōu)化那些比較確定的殘差. 注掉的那行//sqrt_info.setIdentity();相當(dāng)于不加權(quán), 這個(gè)加權(quán)策略的效果需要對(duì)比測(cè)試才知道.
最后更新jacobian并進(jìn)行numerical unstable判斷, 這一塊暫時(shí)沒看懂, 求大神點(diǎn)撥~
4. Visual Residual
ProjectionFactor類的聲明繼承如下
class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>
- 2: 殘差長(zhǎng)度(err_x, err_y)
- 7: 第1個(gè)優(yōu)化參數(shù)pose_i的長(zhǎng)度(para_Pose[imu_i]=(px,py,pz,qx,qy,qz,qw) )
- 7: 第2個(gè)優(yōu)化參數(shù)pose_j的長(zhǎng)度(para_Pose[imu_j])
- 7: 第3個(gè)優(yōu)化參數(shù)外參的長(zhǎng)度(para_Ex_Pose[0])
- 1: 第4個(gè)優(yōu)化參數(shù)feature_inverse_depth的長(zhǎng)度(para_Feature[feature_index])
關(guān)鍵計(jì)算代碼如下:
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;
計(jì)算過程很簡(jiǎn)單,就是重投影過程:
i幀中圖像中的點(diǎn)=>i幀相機(jī)系=>i幀body系=>world系=>j幀body系=>j幀相機(jī)系
重投影誤差計(jì)算有兩種, 如果是用SPHERE相機(jī)模型, 就取tangent平面上的誤差, 否則就用常規(guī)的圖像平面誤差. 然后后面也同樣乘以了一個(gè)sqrt_info轉(zhuǎn)成馬氏距離. 最后也是更新jacobian.
4. Marginalization
sliding windows bounding了優(yōu)化問題中pose的個(gè)數(shù), 從而防止pose和特征的個(gè)數(shù)隨時(shí)間不斷增加, 使得優(yōu)化問題始終在一個(gè)有限的復(fù)雜度內(nèi), 不會(huì)隨時(shí)間不斷增長(zhǎng).
然而, 將pose移出windows時(shí), 有些約束會(huì)被丟棄掉, 這樣勢(shì)必會(huì)導(dǎo)致求解的精度下降, 而且當(dāng)MAV進(jìn)行一些退化運(yùn)動(dòng)(如: 勻速運(yùn)動(dòng))時(shí), 沒有歷史信息做約束的話是無法求解的. 所以, 在移出位姿或特征的時(shí)候, 需要將相關(guān)聯(lián)的約束轉(zhuǎn)變成一個(gè)約束項(xiàng)作為prior放到優(yōu)化問題中. 這就是marginalization要做的事情.
Marginalization數(shù)學(xué)上是用schur complement來實(shí)現(xiàn), 暫時(shí)還沒弄太懂, 有一些參考的博文:
1. SLAM中的marginalization 和 Schur complement
2. DSO 中的Windowed Optimization
VINS-MONO中,為了處理一些懸停的case, 引入了一個(gè)two-way marginalization, 簡(jiǎn)單來說就是:
- 如果倒數(shù)第二幀是關(guān)鍵幀, 則將最舊的pose移出sliding window, 也就是MARGIN_OLD,
- 如果倒數(shù)第二幀不是關(guān)鍵幀, 則將倒數(shù)第二幀pose移出sliding window, 也就是MARGIN_NEW.
選取關(guān)鍵幀的策略是視差足夠大
在懸停等運(yùn)動(dòng)較小的情況下, 會(huì)頻繁的MARGIN_NEW, 這樣也就保留了那些比較舊但是視差比較大的pose. 這種情況如果一直MARGIN_OLD的話, 視覺約束不夠強(qiáng), 狀態(tài)估計(jì)會(huì)受IMU積分誤差影響, 具有較大的累積誤差.
代碼中對(duì)應(yīng)marginalization_factor.cpp文件, 這塊還沒太看懂.
|
