basalt
basalt相机投影模型
basalt采用极射赤平投影表示 1 d [ x , y , z ] = 1 d [ η u , η v , n − 1 ] \frac{1}{d}[x, y,z] = \frac{1}{d}[ \eta u, \eta v, n-1] d1[x,y,z]=d1[ηu,ηv,n−1], η = 2 1 + u 2 + v 2 \eta = \frac{2}{1+u^2 + v^2} η=1+u2+v22 , d = 1 X 2 + Y 2 + Z 2 d = \frac{1}{\sqrt{X^2+ Y^2 + Z^2}} d=X2+Y2+Z21 因为平方项目总是大于等于0的,因此此种形式是没有办法表示 [ 0 , 0 , − 1 ] [0, 0, -1] [0,0,−1]方向上的点的。采用图进行说明
投影公式
三个点共线,即绿色三角型的斜边直线表达式子 X / d − 0 u − 0 = Y / d − 0 v − 0 = Z / d + 1 0 + 1 \frac{X/d-0}{u-0} = \frac{Y/d-0}{v-0} = \frac{Z/d+1}{0+1} u−0X/d−0=v−0Y/d−0=0+1Z/d+1(其中d是三维点的距离XYZ代表三维点坐标)
解得:
u = X d + Z u = \frac{X}{d+Z} u=d+ZX v = Y d + Z v = \frac{Y}{d+Z} v=d+ZY 令 [ x , y , z ] = [ X d , Y d , Z d ] [x,y,z] = [\frac{X}{d}, \frac{Y}{d} , \frac{Z}{d}] [x,y,z]=[dX,dY,dZ]
u = x 1 + z u = \frac{x}{1+z} u=1+zx v = y 1 + z v = \frac{y}{1+z} v=1+zy
反投影公式(平面到单位向量)
[ x , y , z ] = [ η u , η v , η − 1 ] [x, y, z] = [\eta u, \eta v, \eta-1] [x,y,z]=[ηu,ηv,η−1] , η = 2 1 + u 2 + v 2 \eta = \frac{2}{1+u^2 + v^2} η=1+u2+v22
待定系数法 [ x , y , z ] = [ ( 1 + z ) u , ( 1 + z ) v , z ] [x, y, z] = [(1+z)u, (1+z)v, z] [x,y,z]=[(1+z)u,(1+z)v,z] (投影公式直接得到的)
( η u ) 2 + ( η v ) 2 + ( η − 1 ) 2 = 1 (\eta u)^2 + (\eta v)^2 + (\eta -1)^2 = 1 (ηu)2+(ηv)2+(η−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 = d y 1 ( b − a ) + a = d y ∗ b + d d y ∗ a E = \frac{dy}{1}(b-a)+a = dy * b + ddy * a E=1dy(b−a)+a=dy∗b+ddy∗a
F = d y 1 ( c − d ) + d = d y ∗ c + d d y ∗ d F = \frac{dy}{1}(c-d)+d = dy*c + ddy*d F=1dy(c−d)+d=dy∗c+ddy∗d
再根据EF对M点进行差值 M = d x 1 ( F − E ) + E = d x ∗ d y ∗ c + d x ∗ d d y ∗ d + d d x ∗ d y ∗ b + d d x ∗ d d y ∗ a M = \frac{dx}{1}(F-E)+E = dx*dy*c + dx * ddy * d + ddx * dy * b + ddx* ddy * a M=1dx(F−E)+E=dx∗dy∗c+dx∗ddy∗d+ddx∗dy∗b+ddx∗ddy∗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只用反向光流。
目的:减小计算量反向光流是在上一帧上计算,因此只需要计算一遍
理论部分:
对应单个像素的光流 ∂ I ∂ s e 2 = ∂ I ∂ p i x ∗ ∂ p i x ∂ s e 2 \frac{\partial I}{\partial se2} = \frac{\partial I}{\partial pix} * \frac{\partial pix}{ \partial se2} ∂se2∂I=∂pix∂I∗∂se2∂pix
其中 ∂ I ∂ p i x \frac{\partial I}{\partial pix} ∂pix∂I 这一部分是在特征点处的图像梯度,因为图像是个离散的表达,因此这部分其实是采用 f ′ ( x ) = f ( x + Δ x ) − f ( x ) Δ x f'(x) = \frac{f(x+\Delta x) - f(x)}{\Delta x} f′(x)=Δxf(x+Δx)−f(x)进行计算的,简单说,就是相邻像素差就是图像梯度了,但是为了保证精度,basalt做了线性插值。如图所示,求T点的图像梯度的时候对利用上下插值出的点求取出其图像梯度,再利用其上下插值出的计算竖直方向的梯度
另外一部分 ∂ p i x ∂ s e 2 \frac{\partial pix}{ \partial se2} ∂se2∂pix这一部分是像素位移对特征点在图像坐标系中位姿的导数对于第i个pattern在图像坐标系下的位置 p i x = p o s + s e 2 ∗ p a t t e r n i pix = pos + se2 * pattern_i pix=pos+se2∗patterni 其中 s e 2 se2 se2由 s o 2 so2 so2和一个二维位置组成 [ c o s ( θ ) − s i n ( θ ) s i n ( θ ) c o s ( θ ) ] \begin{bmatrix}cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{bmatrix} [cos(θ)sin(θ)−sin(θ)cos(θ)]
p i x pix pix对 p o s pos pos的导数是单位阵,对 s e 2 se2 se2中的旋转求导数需要稍微推倒一下对矩阵中的 θ \theta θ求导为 [ − s i n ( θ ) − c o s ( θ ) c o s ( θ ) − s i n ( θ ) ] \begin{bmatrix} -sin(\theta) & -cos(\theta) \\ cos(\theta) & -sin(\theta) \\ \end{bmatrix} [−sin(θ)cos(θ)−cos(θ)−sin(θ)] 可以看到其正好与原矩阵是反过来的。
代码部分:
图像梯度
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;
}
s e 2 se2 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类型数据(写在光流跟踪输出结构体中)