OKVIS 笔记：边缘化实现

2018-03-23

OKVIS 系列文章：

okvis_ceres/include/okvis/ceres/MarginalizationError.hpp
okvis_ceres/src/MarginalizationError.cpp
okvis_ceres/src/Estimator.cpp


封装结构

OKVIS 的边缘化在 MarginalizationError 类中实现，在 Estimator 类的 applyMarginalizationStrategy() 函数中调用。

/// @name The internal storage of the linearised system.
/// lhs and rhs: (left hand side / right hand side)
/// H_*delta_Chi = _b - H_*Delta_Chi .
/// the lhs Hessian matrix is decomposed as _H = J^T*J = _U*S*_U^T ,
/// the rhs is decomposed as _b - _H*Delta_Chi = -J^T * (-pinv(J^T) * _b + J*Delta_Chi) ,
/// i.e. we have the ceres standard form with weighted Jacobians _J,
/// an identity information matrix, and an error
/// _e = -pinv(J^T) * _b + J*Delta_Chi .
/// _e = _e0 + J*Delta_Chi .
/// @{
Eigen::MatrixXd H_;  ///< lhs - Hessian
Eigen::VectorXd b0_;  ///<  rhs constant part
Eigen::VectorXd e0_;  ///<  _e0 := pinv(J^T) * _b0
Eigen::MatrixXd J_;  ///<  Jacobian such that _J^T * J == _H
Eigen::MatrixXd U_;  ///<  H_ = _U*_S*_U^T lhs Eigen decomposition
Eigen::VectorXd S_;  ///<  singular values
Eigen::VectorXd S_sqrt_;  ///<  cwise sqrt of _S, i.e. _S_sqrt*_S_sqrt=_S; _J=_U^T*_S_sqrt
Eigen::VectorXd S_pinv_;  ///<  pseudo inverse of _S
Eigen::VectorXd S_pinv_sqrt_;  ///<  cwise sqrt of _S_pinv, i.e. pinv(J^T)=_U^T*_S_pinv_sqrt


$\rm H \delta \chi = b - H \Delta\chi$

$\rm J^TJ \delta \chi = -J^T(-J^{T+}b+J \Delta\chi) \tag{1.1}$ $\rm e:=-J^{T+}b+J \Delta\chi \tag{1.2}$

主要接口和函数

MarginalizationError::addResidualBlock() 把一个 Residual 及其对应的 ParameterBlock 加入现有的 Marginalization 系统（即 MarginalizationError 类中封装的 H-b 系统）中，同时将其从 Map 中剔除。 此后 Marginalization 系统会一直维持该 Residual 在加入 Marginalization 系统时的线性点（即 FEJ），直至其被重新线性化或从当前窗口中剔除。

Map::ParameterBlockCollection parameters = mapPtr_->parameters(
residualBlockId);


| H_00         H_01 |      |  b_0  |
|       H_new       |      | b_new |
| H_01         H_11 |      |  b_1  |


| H_00  H_01        |      |  b_0  |
| H_01  H_11        |      |  b_1  |
|             H_new |      | b_new |


if(additionalSize>0) {
if (!isLandmark) {
// insert
// lhs
Eigen::MatrixXd H01 = H_.topRightCorner(denseSize, origSize - denseSize);
Eigen::MatrixXd H10 = H_.bottomLeftCorner(origSize - denseSize, denseSize);
Eigen::MatrixXd H11 = H_.bottomRightCorner(origSize - denseSize, origSize - denseSize);
// rhs
Eigen::VectorXd b1 = b0_.tail(origSize - denseSize);

conservativeResize(b0_, origSize + additionalSize);  // rhs

H_.topRightCorner(denseSize, origSize - denseSize) = H01;
H_.bottomLeftCorner(origSize - denseSize, denseSize) = H10;
H_.bottomRightCorner(origSize - denseSize, origSize - denseSize) = H11;

b0_.tail(origSize - denseSize) = b1;
} else {
conservativeResize(b0_, origSize + additionalSize);  // rhs
// just append
}
}


errorInterfacePtr->EvaluateWithMinimalJacobians(parametersRaw, residualsRaw,
jacobiansRaw,
jacobiansMinimalRaw);


for (size_t i = 0; i < parameters.size(); ++i) {

ParameterBlockInfo parameterBlockInfo_i = parameterBlockInfos_.at(
parameterBlockId2parameterBlockInfoIdx_[parameters[i].first]);

/// 计算主对角
H_.block(parameterBlockInfo_i.orderingIdx, parameterBlockInfo_i.orderingIdx,
parameterBlockInfo_i.minimalDimension,
parameterBlockInfo_i.minimalDimension) += jacobiansMinimalEigen.at(
i).transpose().eval() * jacobiansMinimalEigen.at(i);
b0_.segment(parameterBlockInfo_i.orderingIdx,
parameterBlockInfo_i.minimalDimension) -= jacobiansMinimalEigen
.at(i).transpose().eval() * residualsEigen;

/// 计算右上和左下角
for (size_t j = 0; j < i; ++j) {
ParameterBlockInfo parameterBlockInfo_j = parameterBlockInfos_.at(
parameterBlockId2parameterBlockInfoIdx_[parameters[j].first]);

// upper triangular:
H_.block(parameterBlockInfo_i.orderingIdx,
parameterBlockInfo_j.orderingIdx,
parameterBlockInfo_i.minimalDimension,
parameterBlockInfo_j.minimalDimension) += jacobiansMinimalEigen
.at(i).transpose().eval() * jacobiansMinimalEigen.at(j);
// lower triangular:
H_.block(parameterBlockInfo_j.orderingIdx,
parameterBlockInfo_i.orderingIdx,
parameterBlockInfo_j.minimalDimension,
parameterBlockInfo_i.minimalDimension) += jacobiansMinimalEigen
.at(j).transpose().eval() * jacobiansMinimalEigen.at(i);
}
}


MarginalizationError::marginalizeOut()

MarginalizationError::marginalizeOut() 实现了根据 Schur Complement 来修改 H-b 系统，将给定 ParameterBlock 边缘化。

• 先对 H 和 b0 （b0 即初始的 b）进行 scale，保持计算尺度正规化
• H 分解出 U 和 V，b0 分解出 b_a 和 b_b；U 和 b_a 对应不被边缘化的部分，V 和 b_b 对应将被边缘化的部分
• 计算 delta_b 和 delta_H（代码有精简）：

for (size_t i = 0; int(i) < V.cols(); i += sdim) {
Eigen::MatrixXd M = W.block(0, i, W.rows(), sdim) * V_inv_sqrt;
Eigen::MatrixXd M1 = W.block(0, i, W.rows(), sdim) * V_inv_sqrt*V_inv_sqrt.transpose();

delta_H.at(idx) = delta_H.at(idx - 1) + M * M.transpose();
delta_b.at(idx) = delta_b.at(idx - 1) + M1 * b_b.segment<sdim>(i);
}


我们知道在舒尔补的计算中，$\rm H_{11}$ 需要减去的量为 $\rm H_{12} H_{22}^{-1} H_{21}$，$\rm b_1$ 需要减去的量为 $\rm H_{12} H_{22}^{-1} b_{2}$。 在上面的代码中，W 对应 $\rm H_{12}$，V 对应 $\rm H_{22}$，V_inv_sqrt 可以近似地理解为 $\rm H_{22}^{-1/2}$，即满足 V_inv_sqrt*V_inv_sqrt'=$\rm H_{22}^{-1}$。 于是，M 就是 $\rm H_{12}H_{22}^{-1/2}$，M1 就是 $\rm H_{12}H_{22}^{-1}$。

• 完成舒尔补的更新：

b0_ -= delta_b.at(idx - 1);
H_ -= delta_H.at(idx - 1);


此处 b0 的更新仅对应论文中 eq(26) 的左半步。 右半步则是在 EvaluateWithMinimalJacobians() 函数中完成。

• 最后对 H 和 b0 进行 unscale

mapPtr_->removeParameterBlock(parameterBlockIdsCopy[i]);


MarginalizationError::updateErrorComputation()

MarginalizationError::updateErrorComputation() 这个函数比较简单，就是计算上面 (1.1) 式中的 $\rm J$, 和 (1.2) 式中的 $\rm e$ 的初始值（$\rm -J^{T+}b_0$）。 这个函数需要在添加状态量（AddResidualBlock）和边缘化状态量（marginalizeOut）之后、执行优化之前调用。

S_ = Eigen::VectorXd(
(saes.eigenvalues().array() > tolerance).select(
saes.eigenvalues().array(), 0));
S_pinv_ = Eigen::VectorXd(
(saes.eigenvalues().array() > tolerance).select(
saes.eigenvalues().array().inverse(), 0));
S_sqrt_ = S_.cwiseSqrt();
S_pinv_sqrt_ = S_pinv_.cwiseSqrt();


// assign Jacobian
J_ = (p.asDiagonal() * saes.eigenvectors() * (S_sqrt_.asDiagonal())).transpose();


$\rm J^{T+}=S^{-1/2}U^{-1}=S^{-1/2}U^T$：

Eigen::MatrixXd J_pinv_T = (S_pinv_sqrt_.asDiagonal())
* saes.eigenvectors().transpose()  *p_inv.asDiagonal();


e0_ = (-J_pinv_T * b0_);


MarginalizationError::EvaluateWithMinimalJacobians()

MarginalizationError::EvaluateWithMinimalJacobians() 这个函数在 Evaluate() 中调用， Evaluate() 是 Ceres 内置虚函数，用于自定义 contraint， 根据现有状态量计算 residual 误差量和 Jacobian，由 Ceres 在求解过程中自动调用。

Jacobians 我们在 updateErrorComputation() 中已经算好，绑定到输出数据即可，不提。

residual 误差量根据上面 (1.2) 式计算。 首先计算估计值更新量 $\Delta \rm \chi$：

Eigen::VectorXd Delta_Chi;
computeDeltaChi(parameters, Delta_Chi);


e = e0_ + J_ * Delta_Chi;


if (jacobians != NULL) {
if (jacobians[i] != NULL) {
/// ....
J_i = Jmin_i * J_lift; // J_err_group = J_err_algebra * J_algebra_group
}
}


Estimator::applyMarginalizationStrategy()

Estimator::applyMarginalizationStrategy() 实现了边缘化策略，哪些 states 和 residuals 要 marg 掉，哪些要保留等等。 整个逻辑比较烦，文字描述已经无力，下面直接贴伪代码。 看起来可能有点乱，实际代码更乱。 一旦真搞起这种工程逻辑，OKVIS 作者也就顾不上 OOP 了 :)

～～～～ 伪代码开始 ～～～～

Declare:

• removeFrames: states to remove (frame id, or oldest keyframe id)

• removeAllButPose: all in marg window (frame id)

• allLinearizeFrames: all in marg window (frame id)

removeFrames contains $\rm x^{c-S}$ or $\rm x^{k_1}$. The newest one in allLinearizeFrames / removeAllButPose is $\rm x^{c-S}$. Check the strategy.

For each in removeAllButPose:

• Add SpeedAndBias states to parameterBlockToBeMarg.
• Add residuals concerning SpeedAndBias states to marginalizationErrorPtr. They are never ReprojectionError.

end For

For each in removeFrames:

• Add Pose state to parameterBlockToBeMarg.
• Get residuals concerning Pose state.
• If a residual is PoseError:

• it is an initial pose prior, remove it from mapPtr_;
• set reDoFixation true

Else:

• it must be a ReprojectionError.

End If

• Add Extrinsics state to parameterBlockToBeMarg.
• Residuals concerning Extrinsics state must be ReprojectionError.
• Now let’s handle the landmarks

For each in landmarksMaps:

• Get related residuals

Declare:

• marginalize = true, indicating if this landmark should be marg
• errorTermAdded = false

If this landmark is connected to $\rm x^{c-S}$ or newer Frames:

• set marginalize false

End If

If this landmark has no connected Frame in removedFrames:

• Continue

End If

If residuals is empty:

• Remove this landmark
• Continue

End If

If marginalize is false:

• Remove residuals whose related Frames are in removedFrames

End if

If marginalize:

• Remove residuals whose related Frames are not in marg window
• If this landmark has < 2 connected Frames in the marg window, remove residuals even if their related Frames are in marg window
• If this landmark has >= 2 connected Frames in the marg window, and if a residual’s related Frame is in the marg window, add the ReprojectionError to marginalizationErrorPtr, and set errorTermAdded true

End if

If after residual removal, residuals is empty:

• Remove this landmark
• Continue

End If

If marginalize And errorTermAdded:

• Add this landmark to parameterBlockToBeMarg
• Remove this landmark from landmarksMaps

End If

End For

• Remove this Frame from statesMap_

End For

If parameterBlockToBeMarg is not empty:

• marginalizationErrorPtr->marginalizeOut()
• marginalizationErrorPtr->updateErrorComputation()

End If

Add marginalizationErrorPtr together with parameterBlockToBeMarg as a residual to mapPtr_

If reDoFixation:

• Re-add a PoseError prior to mapPtr_

End If

～～～～ 伪代码结束 ～～～～