## OKVIS 笔记：位姿变换及其局部参数类

2018-01-23

OKVIS 系列文章：

### 位姿变换类

okvis_kinematics/include/okvis/kinematics/Transformation.hpp


OKVIS 的 Trawnsformation 类实现了 SE3 变换。

OKVIS 的变换类虽然也采用了类似预积分论文中用最小表示的局部小量来做更新的 local parameterization（预积分论文称之为 lift-solve-retract），但不同于预积分论文的 $\rm (R\,Exp(\delta\alpha), p+R\delta p)$，更不同于常规的 Exp(se3)，OKVIS 的更新策略是将旋转量和平移量完全解耦，对旋转量使用左扰动，对平移量则直接加法：

$\rm x = \bar{x}\boxplus \Delta x: \, q \leftarrow \delta q \otimes \bar{q}, \, p \leftarrow \delta p+\bar{p}.$

// apply small update:
template<typename Derived_delta>
inline bool Transformation::oplus(
const Eigen::MatrixBase<Derived_delta> & delta) {
Eigen::Vector4d dq;
double halfnorm = 0.5 * delta.template tail<3>().norm();
dq.template head<3>() = sinc(halfnorm) * 0.5 * delta.template tail<3>();
dq[3] = cos(halfnorm);
q_ = (Eigen::Quaterniond(dq) * q_);
return true;
}


\begin{aligned} &\rm \frac{\partial x}{\partial \Delta x}:\\ &\rm \frac{\partial q}{\partial \delta \alpha} = \frac{\partial Q_{\bar{q}}\,\delta q}{\partial \delta \alpha} = \frac{1}{2}Q_{\bar{q}}I_{4\times 3}\\ &\rm \frac{\partial p}{\partial \delta p} = I_{3}\\ &\rm \frac{\partial q}{\partial \delta p} = \frac{\partial p}{\partial \delta \alpha} = 0 \end{aligned}

template<typename Derived_jacobian>
inline bool Transformation::oplusJacobian(
const Eigen::MatrixBase<Derived_jacobian> & jacobian) const
{
Eigen::Matrix<double, 4, 3> S = Eigen::Matrix<double, 4, 3>::Zero();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
.template topLeftCorner<3, 3>().setIdentity();
S(0, 0) = 0.5;
S(1, 1) = 0.5;
S(2, 2) = 0.5;
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
.template bottomRightCorner<4, 3>() = okvis::kinematics::oplus(q_) * S;
return true;
}


\begin{aligned} &\rm \frac{\partial \Delta x}{\partial x}:\\ &\rm \frac{\partial \delta \alpha}{\partial q}=\frac{\partial\delta\alpha}{\partial\delta q} \frac{\partial q\otimes\bar{q}^{-1}}{\partial q} = 2I_{3\times 4}\,Q_{\bar{q}^{-1}}\\ &\rm \frac{\partial \delta p}{\partial p} = I_3\\ &\rm \frac{\partial \delta \alpha}{\partial p} = \frac{\partial \delta p}{\partial q} = 0 \end{aligned}

template <typename Derived_jacobian>
inline bool Transformation::liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const
{
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template topLeftCorner<3,3>()
= Eigen::Matrix3d::Identity();
const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template bottomRightCorner<3,4>()
= 2*okvis::kinematics::oplus(q_.inverse()).template topLeftCorner<3,4>();
return true;
}


### 位姿局部参数类

okvis_ceres/include/okvis/ceres/PoseLocalParameterization.hpp
okvis_ceres/src/PoseLocalParameterization.cpp


x y z qx qy qz qw


dx dy dz dalphax dalphay dalphaz