BASALT前端

basalt

basalt相机投影模型

​ basalt采用极射赤平投影表示 $\frac{1}{d}[x,y,z]=\frac{1}{d}[\eta u,\eta v,n-1]$, $\eta =\frac{2}{1+u^2+v^2}$ , $d=\frac{1}{\sqrt{X^2+Y^2+Z^2}}$ 因为平方项目总是大于等于0的，因此此种形式是没有办法表示$[0,0,-1]$方向上的点的。采用图进行说明

投影公式

​ 三个点共线，即绿色三角型的斜边直线表达式子$\frac{X/d-0}{u-0}=\frac{Y/d-0}{v-0}=\frac{Z/d+1}{0+1}$(其中d是三维点的距离XYZ代表三维点坐标)

​ 解得：

​ $u=\frac{X}{d+Z}$ $v=\frac{Y}{d+Z}$ 令$[x,y,z]=[\frac{X}{d},\frac{Y}{d},\frac{Z}{d}]$

​ $u=\frac{x}{1+z}$ $v=\frac{y}{1+z}$

反投影公式（平面到单位向量）

​ $[x,y,z]=[\eta u,\eta v,\eta -1]$ , $\eta =\frac{2}{1+u^2+v^2}$

​ 待定系数法 $[x,y,z]=[(1+z)u,(1+z)v,z]$ （投影公式直接得到的）

​ $(\eta u{)}^2+(\eta v{)}^2+(\eta -1{)}^2=1$ （这个公式是点在圆球面上+绿色和黄色三角型是相似的）

basalt的特征点

​ basalt的特征点也是和投影模型相关的，其Keypoint结构体主要包括:在赤平面的投影、逆深度、主帧id、观测map(key是观测的camera id value是 赤平面向量)，同样其中重载了backup和restore函数用于将数据进行备份和恢复(这里可能是为fej准备的)。

template <class Scalar_>
struct Keypoint {
  using Scalar = Scalar_;
  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
  using ObsMap = Eigen::aligned_map<TimeCamId, Vec2>;
  using MapIter = typename ObsMap::iterator;
  // 3D position parameters
  Vec2 direction;
  Scalar inv_dist;
  // Observations
  TimeCamId host_kf_id;
  ObsMap obs;
  inline void backup() {
    backup_direction = direction;
    backup_inv_dist = inv_dist;
  }
  inline void restore() {
    direction = backup_direction;
    inv_dist = backup_inv_dist;
  }
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 private:
  Vec2 backup_direction;
  Scalar backup_inv_dist;
};

​ 同时还存在一个特征点数据库:LandmarkDatabase，在滑动窗口中一直维护的是这个数据库的内容，这个内容主要是kpts和observations这两个成员变量

// key是特征点id value是特征点 
Eigen::aligned_unordered_map<KeypointId, Keypoint<Scalar>> kpts;
// key是帧率id+相机id value是 map 这个map是 key是帧率+相机id value是特征点列表
std::unordered_map<TimeCamId, std::map<TimeCamId, std::set<KeypointId>>> observations;

basalt构建图像金字塔

basalt的构建图像金字塔是自己实现的，其形成的图像金字塔图像数据是保存在同一张图里的（在默认的配置参数中，basalt默认图像金字塔构建三层-事实上他构建多了就容易出问题，会超出图像的索引，所以这个参数不能随便调整）。构建成如图所示的图像金字塔。

缩放后的图像块所在的位置代码分析：

inline Image<T> lvl_internal(size_t lvl) {
    // 图像应该放的位置上
    size_t x = (lvl == 0) ? 0 : orig_w;// orig_w = 第0层图像的宽度
    size_t y = (lvl <= 1) ? 0 : (image.h - (image.h >> (lvl - 1)));
    size_t width = (orig_w >> lvl);// 缩放
    size_t height = (image.h >> lvl);// 缩放

    return image.SubImage(x, y, width, height);
  }

x = 0, orig_w， orig_w

y = 0， 0， h/2

因此这里可以看到索引是与图像进行对应上的

缩放图像代码分析：

static void subsample(const Image<const T>& img, Image<T>& img_sub) {
	......
    constexpr int kernel[5] = {1, 4, 6, 4, 1};// 5的卷积核
    ManagedImage<int> tmp(img_sub.h, img.w);// 中间变量 高度缩小一半  宽度先不变
    {
      for (int r = 0; r < int(img_sub.h); r++) {// 遍历高度
        const T* row_m2 = img.RowPtr(std::abs(2 * r - 2));
        const T* row_m1 = img.RowPtr(std::abs(2 * r - 1));
        const T* row = img.RowPtr(2 * r);
        const T* row_p1 = img.RowPtr(border101(2 * r + 1, img.h));
        const T* row_p2 = img.RowPtr(border101(2 * r + 2, img.h));
        for (int c = 0; c < int(img.w); c++) {// 遍历宽度
          tmp(r, c) = kernel[0] * int(row_m2[c]) + kernel[1] * int(row_m1[c]) +
                      kernel[2] * int(row[c]) + kernel[3] * int(row_p1[c]) +
                      kernel[4] * int(row_p2[c]);
        }
      }
    }
    // Horizontal convolution  再垂直搞一遍这个事情就成了 这样的运算是等效的
    {
      for (int c = 0; c < int(img_sub.w); c++) {
        const int* row_m2 = tmp.RowPtr(std::abs(2 * c - 2));
        const int* row_m1 = tmp.RowPtr(std::abs(2 * c - 1));
        const int* row = tmp.RowPtr(2 * c);
        const int* row_p1 = tmp.RowPtr(border101(2 * c + 1, tmp.h));
        const int* row_p2 = tmp.RowPtr(border101(2 * c + 2, tmp.h));

        for (int r = 0; r < int(tmp.w); r++) {
          int val_int = kernel[0] * row_m2[r] + kernel[1] * row_m1[r] +
                        kernel[2] * row[r] + kernel[3] * row_p1[r] +
                        kernel[4] * row_p2[r];
          T val = ((val_int + (1 << 7)) >> 8);// 这个位置是处以256   这里采用四舍五入的思想了
          img_sub(c, r) = val;// 最后再赋值一下
        }
      }
    }
  }

下采样的本质是，当前需要计算的位置乘以比例系数来找到在原图像中的位置，然后使用卷积核进行计算一个加权均值，然后得到该位置处的像素值，需要注意的是这个地方是进行了优化了的。

basalt前端流程

前端的一些数据结构体

光流跟踪输入结构体

struct OpticalFlowInput {
  using Ptr = std::shared_ptr<OpticalFlowInput>;// 定义ptr类型
  int64_t t_ns;// 图像的时间戳
  std::vector<ImageData> img_data;// 图像
};

光流跟踪输出结构体

struct OpticalFlowResult {
  using Ptr = std::shared_ptr<OpticalFlowResult>;// 定义指针
  int64_t t_ns;// 时间戳
  // vector的key是相机  value是map map的 key是特征点id value是特征点的平移和旋转参数
  std::vector<Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>>
      observations;
  // key是相机 value是map map的key是特征点id value是特征点放在哪层金字塔
  std::vector<std::map<KeypointId, size_t>> pyramid_levels;
  OpticalFlowInput::Ptr input_images;// 输入的数据
};

1、读取图像并喂给光流追踪

​ 在vio.cpp的main中开辟了t1线程运行了feed_images函数

step1、 构建光流跟踪结构体并把时间戳图像数据放进去

step2、塞入到输入队列中

step3、如果运行完塞入一个空的数据，告诉后面的数据结束

2、光流追踪线程

​ 在vio.cpp的main中根据pattern的格式不同重新新建了光流追踪的线程

opt_flow_ptr = basalt::OpticalFlowFactory::getOpticalFlow(vio_config, calib);
// 调用了
res.reset(new PatchOpticalFlow<float, Pattern24>(config, cam));
// 调用了构造函数
PatchOpticalFlow(const VioConfig& config,
                   const basalt::Calibration<double>& calib)
      : initSome {
    patches.reserve(3000);
    // 输入队列设置容量
    input_queue.set_capacity(10);
    patch_coord = PatchT::pattern2.template cast<float>();
    // 如果是双目相机 计算两者的E矩阵
    if (calib.intrinsics.size() > 1) {
      Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
      computeEssential(T_i_j, E);
    }
    // 开启处理线程
    processing_thread.reset(
        new std::thread(&PatchOpticalFlow::processingLoop, this));
  }

​ processingLoop循环处理部分：拿到一帧的数据指针、处理一帧processFrame

​ processFrame这里进行图像金字塔创建+跟踪+剔除特征点

void processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec){
if(...){// 第一次进入
	......
}
else {// 非第一次进入
      t_ns = curr_t_ns;// 拷贝时间戳
      old_pyramid = pyramid;// 图像金字塔传递
      pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);// 重新设置新指针
      pyramid->resize(calib.intrinsics.size());
      for (size_t i = 0; i < calib.intrinsics.size(); i++) {
        pyramid->at(i).setFromImage(*new_img_vec->img_data[i].img,
                                    config.optical_flow_levels);
      }
      // 新的返回参数
      OpticalFlowResult::Ptr new_transforms;
      new_transforms.reset(new OpticalFlowResult);
      new_transforms->observations.resize(new_img_vec->img_data.size());
      new_transforms->t_ns = t_ns;
      // 对当前帧的和上一帧进行跟踪（左目与左目 右目与右目）
      for (size_t i = 0; i < calib.intrinsics.size(); i++) {
        trackPoints(old_pyramid->at(i), pyramid->at(i),
                    transforms->observations[i],
                    new_transforms->observations[i]);
      }

      transforms = new_transforms;
      transforms->input_images = new_img_vec;
      // 增加点
      addPoints();
      // 使用双目E剔除点
      filterPoints();
    }
    // 喂到队列里
    if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) {
      output_queue->push(transforms);
    }
    // 跟踪数量增加
    frame_counter++;

增加点函数addpoint:

• step1 在当前帧第0层检测特征(划分网格，在网格中只保存响应比较好的和以前追踪的)
• step2 遍历特征点个每个特征点利用特征点 初始化光流金字塔的初值
• step3 如果是有双目的化左右目追踪右目保留和左目追踪上的特征点

3、追踪两个帧里的点

​ trackPoints函数是用来追踪两幅图像的特征点的输入是 金字塔1 金字塔2 1中的点 输出的是2追踪1的点

​ trackPoint函数是追踪一个点输入是金字塔2 特征点在《目标图像金字塔金字塔+在金字塔层级下的特征点的位置》形成的vector 返回在特征点在2中应该是在哪个位置

残差计算(对应pattern位置处的像素差值)

step1 取图像里的像素值

双线性插值公式：

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

$$E=\frac{dy}{1}(b-a)+a=dy\ast b+ddy\ast a$$

$$F=\frac{dy}{1}(c-d)+d=dy\ast c+ddy\ast d$$

再根据EF对M点进行差值$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$

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);
  }

step2 整体做减法

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对应原图位置中的雅克比)

​ 这部分在slam14光流小节中也实现了，只不过slam14是算一下正向的再算一下反向的光流，而basalt只用反向光流。

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

理论部分:

​ 对应单个像素的光流$\frac{\partial I}{\partial se2}=\frac{\partial I}{\partial pix}\ast \frac{\partial pix}{\partial se2}$

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

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

$pix$对$pos$的导数是单位阵，对$se2$中的旋转求导数需要稍微推倒一下对矩阵中的$\theta$求导为$\left[\begin{array}{cc}-sin(\theta )& -cos(\theta )\\ cos(\theta )& -sin(\theta )\end{array}\right]$ 可以看到其正好与原矩阵是反过来的。

​ 代码部分:

图像梯度

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;
      }
    }

追踪简要流程

• 对每个特征点在金字塔高层级到低层级进行追踪(由粗到精)
• 追踪当前帧的所有特征点
• 反向追踪上的才算真正成功的

4、梳理追踪数据

​ 输入数据：processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) 时间戳 + 图像数据

​ 输出数据：output_queue->push(transforms); OpticalFlowResult类型数据(写在光流跟踪输出结构体中)