basalt视觉因子构建

basalt视觉因子构建

​ 在视觉因子的雅克比求导过程中主要进行了视觉残差对相对位姿的导数与特征点参数uv和逆深度(这里的uv不是像素坐标而是特征点在赤平面的投影坐标)。

​ 在优化函数中(optimize函数)中会构建视觉因子，里面会调用linearizeHelper函数里面会调用linearizeHelperStatic函数对视觉因子的雅克比矩阵构建。

流程

• 遍历所有的观测，把所有主帧的id放到obs_tcid_vec中，并为返回数据初始化大小
• 对主帧序列进行并行循环计算（每一个主帧对所有的观测进行计算）
  • 计算相对位姿对绝对位姿的导数
  • 计算视觉残差以及计算计算视觉残差对相对位姿的导数、视觉残差对三维点参数的导数
  • 构建H和b矩阵进行保存

对视觉残差对相对位姿以及三维点参数进行说明

​ 视觉残差定义为在像素残差$r_{it}=z_{it}-{\pi }_c((T_i^c(T_{ti}^w{)}^{-1}\ast T_{hi}^w)T_c^i\ast p_{hc}(u,v,d))$过程就是把三维点参数反投影成三维点，再把三维点从host的相机坐标系转换到target的相机坐标系再进行按投影模型进行投影做残差的过程。

​ 注意问题：1、3d点参数是(u, v, invD) 但是上边式子$p_{hc}$是齐次坐标(x, y, z, 1/d)的形式的(虽然差一个d的比例但是没关系的)。

链式法则：残差先对${\pi }_c(P_{tc})$导数，再有${\pi }_c(p)$对相对位姿和三维点参数导数

• 残差对$P_{tc}$导数

  前面basalt前端中分析了其投影模型，在投影模型中$u=\frac{X}{d+Z}$ $v=\frac{Y}{d+Z}$ 其中$d=\sqrt{X^2+Y^2+Z^2}$分析一个比如v的残差对x的导数

  $\frac{\partial er{r}_v}{\partial X}=\frac{\partial \frac{Y}{\sqrt{X^2+Y^2+Z^2}+Z}}{\partial X}=Y\ast (-1)\ast (\sqrt{X^2+Y^2+Z^2}+Z{)}^{-2}\ast (1/2)\ast (X^2+Y^2+Z^2{)}^{-1/2}\ast 2X$

  化简得到$XY\ast 1/(\sqrt{X^2+Y^2+Z^2}+Z{)}^2\ast \sqrt{X^2+Y^2+Z^2}$ 这也是与代码对应的，其余的只是展开就好，过程稍微麻烦一些而已。但这里对最后一维度(逆深度)的导数是0的，因为最后一维度并没有参与运算过程。

• 残差对相对位姿导数

  对相对位姿，也就是对$(T_i^c(T_{ti}^w{)}^{-1}\ast T_{hi}^w)T_c^i$的导数, 把$p_{hc}(u,v,d))$用符号$p_{hc}$替代，把$(T_i^c(T_{ti}^w{)}^{-1}\ast T_{hi}^w)T_c^i$用符号$T_{hc}^{tc}$替代。采用$SE3$左扰动形式进行证明。$err=\left[\begin{array}{cc}(I+\delta {\varphi }^{\times })R_{hc}^{tc}& (I+\delta {\varphi }^{\times })t+\delta \rho \\ 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1/d\end{array}\right]-\left[\begin{array}{cc}{R}_{hc}^{tc}& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1/d\end{array}\right]$

  化简得到$\left[\begin{array}{cc}\delta {\varphi }^{\times }{R}_{hc}^{tc}& \delta {\varphi }^{\times }t+\delta \rho \\ 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1/d\end{array}\right]$因此很容易推倒出来和basalt一样的形式：

  $\frac{\partial P_{tc}}{\partial T_{hc}^{tc}}=\left[\begin{array}{cc}{I}_{3\times 3}{P}_{hc}[3]& -[R_{hc}^{tc}{P}_{hc}[0:2]+t_{hc}^{tc}{]}^{\times }\\ 0& 0\end{array}\right]$

• 残差对u、v、invd的导数

  这部分的导数要分两步看对uv的导数在进行unproject的过程算出来的Jup代表[x,y,z,1/d]对uv的导数，根据前面的投影模型$[x,y,z]=[\eta u,\eta v,\eta -1]$以及$\eta =\frac{2}{1+u^2+v^2}$两个式子，这里在证明的时候使用复合求导，对$\frac{\partial x}{\partial u}=\frac{\partial \eta }{\partial u}u+\eta =-2\ast (1+u^2+v^2{)}^{-2}\ast 2\ast u\ast u+\eta$。残差对其他的导数计算也是这样的，因为在反投影过程深度与uv是没有关系的，因此这里深度对uv的导数是0。残差中与逆深度相关的只有位姿矩阵的最后一列。

basalt源码

// 特征点残差的导数
template <class Scalar, class CamT>
inline bool linearizePoint(
    const Eigen::Matrix<Scalar, 2, 1>& kpt_obs, const Keypoint<Scalar>& kpt_pos,
    const Eigen::Matrix<Scalar, 4, 4>& T_t_h, const CamT& cam,
    Eigen::Matrix<Scalar, 2, 1>& res,
    Eigen::Matrix<Scalar, 2, POSE_SIZE>* d_res_d_xi = nullptr,
    Eigen::Matrix<Scalar, 2, 3>* d_res_d_p = nullptr,
    Eigen::Matrix<Scalar, 4, 1>* proj = nullptr) {
  static_assert(std::is_same_v<typename CamT::Scalar, Scalar>);
  // Todo implement without jacobians
  Eigen::Matrix<Scalar, 4, 2> Jup;
  Eigen::Matrix<Scalar, 4, 1> p_h_3d;
  p_h_3d = StereographicParam<Scalar>::unproject(kpt_pos.direction, &Jup);
  p_h_3d[3] = kpt_pos.inv_dist;
  const Eigen::Matrix<Scalar, 4, 1> p_t_3d = T_t_h * p_h_3d;// R 
  Eigen::Matrix<Scalar, 2, 4> Jp;
  bool valid = cam.project(p_t_3d, res, &Jp);
  valid &= res.array().isFinite().all();
  if (!valid) {
    return false;
  }

  if (proj) {
    proj->template head<2>() = res;// 前维度是重投影残差
    (*proj)[2] = p_t_3d[3] / p_t_3d.template head<3>().norm();// 1 / d
  }
  res -= kpt_obs;

  if (d_res_d_xi) {
    Eigen::Matrix<Scalar, 4, POSE_SIZE> d_point_d_xi;
    d_point_d_xi.template topLeftCorner<3, 3>() =
        Eigen::Matrix<Scalar, 3, 3>::Identity() * kpt_pos.inv_dist;
    d_point_d_xi.template topRightCorner<3, 3>() =
        -Sophus::SO3<Scalar>::hat(p_t_3d.template head<3>());
    d_point_d_xi.row(3).setZero();

    *d_res_d_xi = Jp * d_point_d_xi;
  }

  if (d_res_d_p) {
    Eigen::Matrix<Scalar, 4, 3> Jpp;
    Jpp.setZero();
    Jpp.template block<3, 2>(0, 0) = T_t_h.template topLeftCorner<3, 4>() * Jup;
    Jpp.col(2) = T_t_h.col(3);
    *d_res_d_p = Jp * Jpp;
  }
  return true;
}