Basal前端梳理

Basalt前端逻辑梳理

TBB安装参考
https://zhuanlan.zhihu.com/p/480823197
代码注释参考
https://blog.csdn.net/qq_39266065/article/details/106175701#t7
光流追踪参考
https://blog.csdn.net/weixin_41738773/article/details/130282527

VI Odometry

KLT tracking

原理

为了实现快速、鲁棒和准确的光流追踪，为了强度尺度不变性，将逆合成方法和强度缩放不变的patch dissimilarity 范数相结合。一些作者建议对光照不变光流使用零均值归一化互相关(ZNCC)，但我们使用[21]中定义的局部缩放平方差和（LSSD），其计算成本少于备选方案。

vio的视觉前端我们采用基于像素块的inverse compositional光流方法, 并且采用**locally-scaled sum of squared differences (LSSD)**作为衡量像素块光度一致性的误差计算方法。

这里把像素块跟踪问题建模为求解图像$I_t$和$I_{t+1}$上对应像素块的2维仿射矩阵$T\in SE(2)$

代价函数为：

$I_t(x)$表示像素x处的强度值，$\Omega $表示图像区域,$\overline{I_{t}}$表示区域$\Omega $内的平均像素强度值。

为了剔除错误的像素匹配，这里采用了交叉跟踪$I_t⟺I_{t+1}$的方法。原文就说不想用阈值，而是用这种双向检验的方式来剔除外点。

processFrame()→addPoint()

每个50size的cell提取一个特征点

输入图像金字塔构建

金字塔构建+图像下采样

  inline void setFromImage(const ManagedImage<T>& other, size_t num_levels) {
    orig_w = other.w;
    image.Reinitialise(other.w + other.w / 2, other.h);
    image.Fill(0);
    lvl_internal(0).CopyFrom(other);

    for (size_t i = 0; i < num_levels; i++) {
      const Image<const T> l = lvl(i);
      Image<T> lp1 = lvl_internal(i + 1);
      subsample(l, lp1);
    }
  }

得到三层拼接得到的图像金字塔

插值取图像里的像素灰度值

双线性插值：

当有一点M位于内部时利用线性插值

$$\begin{gathered} E=\frac{dy}{1}(b-a)+a=dy\ast b+ddy\ast a \\ F=\frac{dy}{1}(d-c)+c=dy\ast c+ddy\ast d \\ M=\frac{dx}{1}(F-E)+E=dx\ast dy\ast c+dx\ast ddy\ast d+ddx\ast dy\ast b+ddx\ast ddy\ast a \end{gathered}$$

template <typename S>
  inline S interp(S x, S y) const {
    static_assert(std::is_floating_point_v<S>,
                  "interpolation / gradient only makes sense "
                  "for floating point result type");

    BASALT_BOUNDS_ASSERT(InBounds(x, y, 0));
    // 下采样后的(int)
    int ix = x;
    int iy = y;
    S dx = x - ix;// 小数的部分
    S dy = y - iy;
    S ddx = S(1.0) - dx;// 负的小数字部分
    S ddy = S(1.0) - dy;
    // 双线性插值
    return ddx * ddy * (*this)(ix, iy) + ddx * dy * (*this)(ix, iy + 1) +
           dx * ddy * (*this)(ix + 1, iy) + dx * dy * (*this)(ix + 1, iy + 1);
  }

光度残差的计算

inline bool residual(const Image<const uint16_t> &img,
                       const Matrix2P &transformed_pattern,
                       VectorP &residual) const {
    Scalar sum = 0;
    int num_valid_points = 0;
    // 对pattern的每一个数据进行计算 这里还没有做差，只是求取了每个pattern在像素处的值
    for (int i = 0; i < PATTERN_SIZE; i++) {
      if (img.InBounds(transformed_pattern.col(i), 2)) {// 在图像边界里面
        residual[i] = img.interp<Scalar>(transformed_pattern.col(i));
        sum += residual[i];// 求总和值
        num_valid_points++;
      } else {
        residual[i] = -1;// 不存在图像的就是-1
      }
    }
    // all-black patch cannot be normalized
    if (sum < std::numeric_limits<Scalar>::epsilon()) {// 小于优化的值了 return
      residual.setZero();
      return false;
    }
    int num_residuals = 0;
    // 对于pattern的每个点进行计算
    for (int i = 0; i < PATTERN_SIZE; i++) {
      if (residual[i] >= 0 && data[i] >= 0) {// 有数的
        const Scalar val = residual[i];// 这地方相当于做类型转换
        residual[i] = num_valid_points * val / sum - data[i];// 归一化后再相减
        num_residuals++;
      } else {
        residual[i] = 0;
      }
    }
    return num_residuals > PATTERN_SIZE / 2;// 超过一半的值才是符合的
  }

雅可比计算（对应pattern原图位置中的雅可比）

目的：减小计算量，反向光流是在上一帧上计算，因此只需要计算一遍

理论:

对应单个像素的光流$\frac{\partial I}{\partial se2}=\frac{\partial I}{\partial p}*\frac{\partial p}{\partial se2}$

其中$\frac{\partial I}{\partial p}$这部分是在特征点处的图像梯度，图像是离散表达，因此实际上是采用定义计算的，$f^{\prime }(x)=\frac{f(x+\Delta x-f(x))}{\Delta x}$，简单来说，取相邻像素差作为图像梯度。但是为了保证精度，Basalt做了线性插值。如图所示，求T点的图像梯度的时候对利用上下插值出的点求取出其图像梯度，再利用其上下插值出的计算竖直方向的梯度。

另一部分$\frac{\partial p}{\partial se2}$是像素位移对特征点在图像坐标系中位姿se2的导数，对于第i个pattern在图像坐标系下的位置$p=pos+se2\ast pattern$其中se(2)由so(2)和一个二维位置组成$\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]$

p对pos的导数是单位阵，对s e 2 se2se2中的旋转求导数需要稍微推倒一下对矩阵中的θ求导为$\left[\begin{array}{cc}-sin\theta & -cos\theta \\ cos\theta & -sin\theta \end{array}\right]$，可以看到其正好与原矩阵是反过来的。（哪反过来了？）

图像梯度$\frac{\partial I}{\partial p}$计算

template <typename S>
  inline Eigen::Matrix<S, 3, 1> interpGrad(S x, S y) const {
  ......

    Eigen::Matrix<S, 3, 1> res;
    const T& px0y0 = (*this)(ix, iy);
    const T& px1y0 = (*this)(ix + 1, iy);
    const T& px0y1 = (*this)(ix, iy + 1);
    const T& px1y1 = (*this)(ix + 1, iy + 1);
    // 插值的像素  
    res[0] = ddx * ddy * px0y0 + ddx * dy * px0y1 + dx * ddy * px1y0 +
             dx * dy * px1y1;

    const T& pxm1y0 = (*this)(ix - 1, iy);
    const T& pxm1y1 = (*this)(ix - 1, iy + 1);

    S res_mx = ddx * ddy * pxm1y0 + ddx * dy * pxm1y1 + dx * ddy * px0y0 +
               dx * dy * px0y1;

    const T& px2y0 = (*this)(ix + 2, iy);
    const T& px2y1 = (*this)(ix + 2, iy + 1);

    S res_px = ddx * ddy * px1y0 + ddx * dy * px1y1 + dx * ddy * px2y0 +
               dx * dy * px2y1;
    // x 方向梯度
    res[1] = S(0.5) * (res_px - res_mx);

    const T& px0ym1 = (*this)(ix, iy - 1);
    const T& px1ym1 = (*this)(ix + 1, iy - 1);

    S res_my = ddx * ddy * px0ym1 + ddx * dy * px0y0 + dx * ddy * px1ym1 +
               dx * dy * px1y0;

    const T& px0y2 = (*this)(ix, iy + 2);
    const T& px1y2 = (*this)(ix + 1, iy + 2);

    S res_py = ddx * ddy * px0y1 + ddx * dy * px0y2 + dx * ddy * px1y1 +
               dx * dy * px1y2;
    // y 方向梯度
    res[2] = S(0.5) * (res_py - res_my);

    return res;
  }

se2及整体梯度

template <typename ImgT>
  static void setDataJacSe2(const ImgT &img, const Vector2 &pos, Scalar &mean,
                            VectorP &data, MatrixP3 &J_se2) {
  ......
    Jw_se2.template topLeftCorner<2, 2>().setIdentity();
    // 对于每个pattern内部的点进行计算
    for (int i = 0; i < PATTERN_SIZE; i++) {
      Vector2 p = pos + pattern2.col(i);// 位于图像的位置

      // Fill jacobians with respect to SE2 warp
      Jw_se2(0, 2) = -pattern2(1, i);
      Jw_se2(1, 2) = pattern2(0, i);

      if (img.InBounds(p, 2)) {
        Vector3 valGrad = img.template interpGrad<Scalar>(p);
        J_se2.row(i) = valGrad.template tail<2>().transpose() * Jw_se2;// 链式法则
        grad_sum_se2 += J_se2.row(i);
        num_valid_points++;
      } else {
        data[i] = -1;
      }
    }

detectKeyPoints

只对左目提点，右目的特征点靠前后帧追踪和左右目光流。

降阈值的思路可以参考

trackPoint

inline bool trackPoint(const basalt::ManagedImagePyr &old_pyr,

const basalt::ManagedImagePyr &pyr,

const Eigen::AffineCompact2f &old_transform,

Eigen::AffineCompact2f &transform) const

点的描述