basalt中的相对位姿和绝对位姿导数

basalt中的相对位姿和绝对位姿导数

​ 在作者的这部分代码是相对难以和代码对应的，使用相对位姿的导数目的：为了节约很多计算量,假设两帧之间有很多特征点，vins等的做法是特征点残差直接对绝对位姿求导数，basalt是残差对相对位姿求导数×相对位姿对绝对位姿求导数，这样在计算过程中虽然左半部分残差对相对位姿求导不一样但是相对位姿对绝对位姿求导数是一样的因此这部分就节省了很多的计算量。这部分代码要注意的要点：

• 作者使用$SO3+trans$的形式表述位姿
• 作者采用了巧妙的坐标准换，使得式子更加相对

$so3+trans$和$se3$导数的差别

basalt中定义的位姿的加法，可以看出其是先进行平移加再进行旋转的左乘，但更新方式和$SE(3)$是有区别的。做这个步骤的操作目的:在推倒公式的时候SE形式具有很好的公式和库的支持，so+trans的形式就需要自己分开写表达式再写，麻烦！因此可以对SE整体求导再转换回来。

inline static void incPose(const Vec6& inc, SE3& T) {
    T.translation() += inc.template head<3>();// 位移更新
    T.so3() = SO3::exp(inc.template tail<3>()) * T.so3();// 位姿进行左扰动更新
}

$SE(3）增量$ $\delta {\xi }_1=\left[\begin{array}{c}\delta {\rho }_1\\ \delta {\varphi }_1\end{array}\right]$ $SO(3)+trans增量$ $\delta {\xi }_2=\left[\begin{array}{c}\delta {\rho }_2\\ \delta {\varphi }_2\end{array}\right]$

$SE(3)更新$ $Exp(\delta {\xi }_1)T=\left[\begin{array}{cc}(I+[\delta {\varphi }_1{]}^{\times })& \delta {\rho }_1\\ 0& I\end{array}\right]\left[\begin{array}{cc}(R& t\\ 0& I\end{array}\right]$ 式子1

$SO(3)+trans更新$ $Exp(\delta {\xi }_2)T=\left[\begin{array}{cc}(I+[\delta {\varphi }_2{]}^{\times })R& t+\delta t_2\\ 0& I\end{array}\right]$ 式子2

将式子1和式子2进行对比得到

$\delta {\xi }_1=\left[\begin{array}{c}\delta {\rho }_1\\ \delta {\varphi }_1\end{array}\right]=\left[\begin{array}{cc}I& [t{]}^{\times }\\ 0& I\end{array}\right]\left[\begin{array}{c}\delta {\rho }_2\\ \delta {\varphi }_2\end{array}\right]=\left[\begin{array}{cc}I& [t{]}^{\times }\\ 0& I\end{array}\right]\delta {\xi }_2$

因此得到的一个关系是:$\frac{\partial \delta {\xi }_1}{\partial \delta {\xi }_2}=\left[\begin{array}{cc}I& [t{]}^{\times }\\ 0& I\end{array}\right]$

一个变换

​ 其实相对位姿对位姿的导数在slam14的位姿图优化部分已经介绍了，但是和代码还是有一点对应不上的地方，对应不上的就是一个小转换。把$T_i^w$和$T_j^w$都乘以一个$T^{\prime }=\left[\begin{array}{cc}I& -t_{wi}^w\\ 0& 1\end{array}\right]$的变换 其作用相当于转换到在i位置新建一个和世界坐标系姿态一样但原点在i位置处的坐标系中。感性的认识是在这个坐标系下的旋转和平移增量如果是定义在解耦的so3+trans的形式下就是在世界坐标系下的增量

​ 感性认识理解:原来basalt定义的残差是$err=ln[T_j^i((T_j^w{)}^T\ast T_i^w)]$ 加上这个变换后 $er{r}_{new}=ln[T_j^i((T^{\prime }{T}_j^w{)}^T\ast T^{\prime }{T}_i^w)]=ln[T_j^i((T_j^w{)}^T{T}^{\prime T}{T}^{\prime }{T}_i^w)]=ln[T_j^i((T_j^w{)}^T\ast T_i^w)]$ 即这两个误差是一样的，但此时再对其进行求雅克比的时候则对$T^{\prime }{T}_x^w$一起求导得到的都是，再带入se3对so3+trans的导数关系，这样确实对i的导数trans部分和j的导数在Adj上的区别了。用人话来说就是，变换之前世界坐标系下的导数和当前转换后世界坐标系下的导数及残差应该是一样的，因此选择转换后的坐标系为基准，对转换后的坐标内求相对对绝对位姿的导数是和代码的表述是一致的。

​ 证明。首先对于导数表达式子中对se3的导数只是正负号有差别，带入$\frac{\partial se3}{\partial so3+trans}=\left[\begin{array}{cc}I& [t{]}^{\times }\\ 0& I\end{array}\right]$ 到雅克比矩阵中进行简写为(…为$J_r$的那一部分)$..Adj(T{i}^T)\left[\begin{array}{cc}I& [t^w{]}^{\times }\\ 0& I\end{array}\right]=...\left[\begin{array}{cc}{R}_i^T& [-R_i^T\ast t_i^w{]}^{\times }\\ 0& R_i^T\end{array}\right]\left[\begin{array}{cc}I& [t^w{]}^{\times }\\ 0& I\end{array}\right]$ 对式子进行打开得到：

$\left[\begin{array}{cc}{R}_i^T& R_i^T[t^w{]}^{\times }+(-R_i^T{t}_i^w{)}^{\times }{R}_i^T\\ 0& R_i^T\end{array}\right]=\left[\begin{array}{cc}{R}_i^T& R_i^T[t^w{]}^{\times }\ast R_i\ast R_i^T+(-R_i^T{t}_i^w{)}^{\times }{R}_i^T\\ 0& R_i^T\end{array}\right]$

$=\left[\begin{array}{cc}{R}_i^T& (R_i^T{t}^w{)}^{\times }\ast R_i^T+(-R_i^T{t}_i^w{)}^{\times }{R}_i^T\\ 0& R_i^T\end{array}\right]$

$=\left[\begin{array}{cc}{R}_i^T& (R_i^T{t}^w-R_i^T{t}_i^w{)}^{\times }\ast R_i^T\\ 0& R_i^T\end{array}\right]$

$=\left[\begin{array}{cc}{R}_i^T& (R_i^T(t^w-t_i^w){)}^{\times }\ast R_i^T\\ 0& R_i^T\end{array}\right]$

当对$i$求导数的时候，$t^w=t_i^w$此时矩阵为$=\left[\begin{array}{cc}{R}_i^T& 0\\ 0& R_i^T\end{array}\right]$

当对$j$求导数的时候，$t_w=t_j^w$此时矩阵为$=\left[\begin{array}{cc}{R}_i^T& (R_i^T(t_{ij}^w){)}^{\times }\ast R_i^T\\ 0& R_i^T\end{array}\right]=\left[\begin{array}{cc}{R}_i^T& (t_{ij}^i{)}^{\times }\ast R_i^T\\ 0& R_i^T\end{array}\right]$

总结

导数为：

​ $\frac{\partial err}{T_i^w}=J_r^{-1}(res)\ast \left[\begin{array}{cc}{R}_i^T& 0\\ 0& R_i^T\end{array}\right]$

​ $\frac{\partial err}{T_j^w}=J_r^{-1}(res)\ast \left[\begin{array}{cc}{R}_i^T& (t_{ij}^i{)}^{\times }\ast R_i^T\\ 0& R_i^T\end{array}\right]$

对应代码为：

inline Sophus::Vector6d relPoseError(
    const Sophus::SE3d& T_i_j, const Sophus::SE3d& T_w_i,
    const Sophus::SE3d& T_w_j, Sophus::Matrix6d* d_res_d_T_w_i = nullptr,
    Sophus::Matrix6d* d_res_d_T_w_j = nullptr) {
  Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i;// 相对位姿
  Sophus::Vector6d res = Sophus::se3_logd(T_i_j * T_j_i);// 计算残差

  if (d_res_d_T_w_i || d_res_d_T_w_j) {// 如果需要计算jacobian
    Sophus::Matrix6d J;
    Sophus::rightJacobianInvSE3Decoupled(res, J);// 算jr的逆
    Eigen::Matrix3d R = T_w_i.so3().inverse().matrix();
    Sophus::Matrix6d Adj;
    Adj.setZero();
    Adj.topLeftCorner<3, 3>() = R;
    Adj.bottomRightCorner<3, 3>() = R;
    if (d_res_d_T_w_i) {// 计算左边的导数
      *d_res_d_T_w_i = J * Adj;
    }
    if (d_res_d_T_w_j) {// 如果计算残差对右边的导数
      // 在右上角填充
      Adj.topRightCorner<3, 3>() = Sophus::SO3d::hat(T_j_i.inverse().translation()) * R;
      *d_res_d_T_w_j = -J * Adj;
    }
  }
  return res;
}