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 normalizedif (sum < std::numeric_limits<Scalar>::epsilon()) {// 小于优化的值了 returnresidual.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 warpJw_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<uint16_t> &old_pyr,

const basalt::ManagedImagePyr<uint16_t> &pyr,

const Eigen::AffineCompact2f &old_transform,

Eigen::AffineCompact2f &transform) const

点的描述