VIO导航--第 03 篇 · 特征点提取与光流跟踪

· 本篇你将学到

一、前端视觉的作用与挑战

VIO 后端优化的视觉残差，需要的是「同一个物理点在多帧间的观测对应」。前端（feature tracker）的职责就是建立这种对应：输入一串灰度图，输出每帧中若干特征点的归一化坐标与唯一 ID，让后端知道「第 5 帧的 37 号点」和「第 6 帧的 37 号点」是同一个世界点。

挑战有三：①提取–哪些像素适合长期跟踪（角点优于平坦区域）；②跟踪–已知上一帧角点，如何在当前帧找到对应（光流）；③管理–特征要分布均匀、丢失要及时补充、外点要剔除。VINS 的前端用 Shi-Tomasi 角点 + KLT 光流 + Mask 均匀化三件套解决。

架构差异提醒：原版 VINS-Mono 的前端入口叫 readImage（返回 void，featureFrame 在 ROS 层构建）；VINS-Fusion 重构为 trackImage（feature_tracker.cpp:94），直接返回 featureFrame。本系列以 VINS-Fusion 代码为准，精读 trackImage。

二、Shi-Tomasi 角点提取

2.1 为什么是角点？

特征跟踪依赖「邻域纹理」。平坦区域（如白墙）各方向梯度都小，无法定位；边缘（如桌沿）只有一个方向有梯度，沿边缘方向存在孔径问题（光流只能求出一个分量）；只有角点（如墙角）两个方向都有强梯度，才能被唯一、稳定地跟踪。

2.2 结构张量与角点响应

对每个像素取邻域窗口，计算图像梯度 $I_x,I_y$，构造结构张量（二阶矩矩阵）：

$$M=\sum _{(x,y)\in W}\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]$$

$M$ 的两个特征值 ${\lambda }_1,{\lambda }_2$ 描述了窗口在两个主方向上的灰度变化强度：

Harris 角点响应为 $R={\lambda }_1{\lambda }_2-k({\lambda }_1+{\lambda }_2{)}^2$，依赖经验参数 $k$。Shi-Tomasi（即 goodFeaturesToTrack 用的 cv::GOODS_FROM_EIGEN）直接取更稳健的判据：

$$R=\min ({\lambda }_1,{\lambda }_2)>\tau$$

无需调 $k$，且与「最小特征值越大越稳定」的直觉一致。代码里：

// feature_tracker.cpp:184

cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT – cur_pts.size(), 0.01, MIN_DIST, mask);

参数依次：图像、输出点、最大数量（MAX_CNT=150 减去已跟踪数）、质量等级 0.01、点间最小距离 MIN_DIST=30、掩膜 mask。掩膜的作用下一节讲。

三、KLT 稀疏光流跟踪

3.1 亮度恒定假设

Lucas-Kanade 光流的核心假设：同一个点在相邻帧间亮度不变，且邻域内所有点运动一致：

$$I(x,y,t)=I(x+u,y+v,t+1)$$

对右边做一阶泰勒展开：

$$I(x+u,y+v,t+1)\approx I(x,y,t)+I_{x}u+I_{y}v+I_t$$

代入恒等式得 Lucas-Kanade 方程：

$$\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-I_t$$

邻域内 $n$ 个点叠加（每个点一个方程），得到超定方程 $A\mathbf{d}=\mathbf{b}$，其中 $\mathbf{d}=[u,v{]}^T$，$A$ 的每行是 $[I_x,I_y]$，最小二乘解为：

$$\mathbf{d}=(A^{T}A{)}^{-1}{A}^T\mathbf{b},A^{T}A=\sum \left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]=M$$

注意 $A^{T}A$ 正是上一节的结构张量 $M$–角点处 $M$ 可逆，光流可解；边缘/平坦处 $M$ 奇异，光流不可靠。这就是「跟踪要选角点」的数学根因。

3.2 图像金字塔：解决大位移

单层光流假设位移小（一阶泰勒有效）。相机快速运动时特征位移几十像素，单层直接失败。金字塔光流（calcOpticalFlowPyrLK）从最粗层开始估计大位移，逐层细化到原图，把「大位移」拆成多次「小位移」。

代码里第三个金字塔层数参数是关键：

// feature_tracker.cpp:133 常规跟踪：3 层金字塔
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21,21), 3);

窗口 21×21，金字塔 3 层。

3.3 正反向校验：剔除外点

光流可能跟丢或跟错（如运动模糊、遮挡）。VINS 用前向+反向双向光流校验（FLOW_BACK=1，yaml:41）：

// feature_tracker.cpp:135-151
if(FLOW_BACK) {
vector<cv::Point2f> reverse_pts = prev_pts;
cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, ...); // 反向
for(size_t i = 0; i < status.size(); i++)
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
status[i] = 1; // 正向到当前帧，再反向回上一帧，若回到原位则可信
else
status[i] = 0; // 否则判为外点剔除
}

直觉：从 A 跟到 B，再从 B 反向跟回，若落在 A 附近（阈值 0.5 像素）则该匹配自洽可信。这是一个零成本的外点剔除技巧，比 RANSAC 轻量。

代码细节：feature_tracker.cpp:169 的 rejectWithF()（基础矩阵 RANSAC）被注释掉了，VINS-Fusion 默认只靠双向光流剔点，不再做 F 矩阵 RANSAC。rejectWithF 函数体（:308）仍保留，需要时可打开。这是 VINS-Fusion 相对原版的简化。

四、特征均匀化：Mask 掩膜法

如果不加约束，goodFeaturesToTrack 会在纹理密集处扎堆提取，导致特征分布不均、某些区域无观测。VINS 用掩膜解决，且做得更聪明–优先保留跟踪寿命长的点。

【函数】feature_tracker.cpp:55 void setMask()

作用：构建掩膜，让新检测的角点既不与已有点过近，又优先保留长寿命点。

核心代码：

// feature_tracker.cpp:57

mask = cv::Mat(row, col, CV_8UC1, cv::Scalar(255)); // 全 255=允许检测

// :60-68 按 track_cnt 降序排列 (跟踪次数, (点坐标, id))

vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;

for (unsigned int i = 0; i < cur_pts.size(); i++)

cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i])));

sort(cnt_pts_id.begin(), cnt_pts_id.end(),

[](const auto &a, const auto &b){ return a.first > b.first; });

// :70-83 清空后按寿命从长到短重新放回，每个点周围 MIN_DIST 范围内置 0

cur_pts.clear(); ids.clear(); track_cnt.clear();

for (auto &it : cnt_pts_id) {

if (mask.at<uchar>(it.second.first) == 255) { // 该位置仍允许

cur_pts.push_back(it.second.first);

ids.push_back(it.second.second);

track_cnt.push_back(it.first);

cv::circle(mask, it.second.first, MIN_DIST, 0, –1); // 周围画黑，禁止再挤入

}

}

逐行注释：

• 掩膜初始全白（255），表示「全图允许检测新点」；

• 按 track_cnt 降序排序是关键设计：跟踪次数多 = 寿命长 = 质量高，优先占位。寿命短的点若落在长寿命点的 MIN_DIST 邻域内，会被挤掉；

• cv::circle(mask, pt, MIN_DIST, 0, -1) 在已保留点周围画半径 MIN_DIST=30 的黑圆（0=禁止），保证后续新点与之至少相距 30 像素，实现空间均匀。

这样 goodFeaturesToTrack 传入该 mask 后，只会在「未被占用」的区域补新点，且补的点天然远离老点。

五、代码精读：trackImage 全流程

trackImage（feature_tracker.cpp:94）是前端唯一对外接口，输入一帧（双目可选第二帧）+ 时间戳，输出 featureFrame。整体流程如下：

图 1 · trackImage 全流程

前端唯一对外接口 trackImage 的端到端流程：从读入图像出发，经光流跟踪、反向校验剔外点、掩膜均匀化、补充新点，再到去畸变归一化、计算速度并构建 7 维 featureFrame 输出。

#mermaid-svg-ATUGTkoJK9CZT3Dk{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-ATUGTkoJK9CZT3Dk .error-icon{fill:#552222;}#mermaid-svg-ATUGTkoJK9CZT3Dk .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-ATUGTkoJK9CZT3Dk .marker{fill:#333333;stroke:#333333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .marker.cross{stroke:#333333;}#mermaid-svg-ATUGTkoJK9CZT3Dk svg{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-ATUGTkoJK9CZT3Dk p{margin:0;}#mermaid-svg-ATUGTkoJK9CZT3Dk .label{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;color:#333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .cluster-label text{fill:#333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .cluster-label span{color:#333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .cluster-label span p{background-color:transparent;}#mermaid-svg-ATUGTkoJK9CZT3Dk .label text,#mermaid-svg-ATUGTkoJK9CZT3Dk span{fill:#333;color:#333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .node rect,#mermaid-svg-ATUGTkoJK9CZT3Dk .node circle,#mermaid-svg-ATUGTkoJK9CZT3Dk .node ellipse,#mermaid-svg-ATUGTkoJK9CZT3Dk .node polygon,#mermaid-svg-ATUGTkoJK9CZT3Dk .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-ATUGTkoJK9CZT3Dk .rough-node .label text,#mermaid-svg-ATUGTkoJK9CZT3Dk .node .label text,#mermaid-svg-ATUGTkoJK9CZT3Dk .image-shape .label,#mermaid-svg-ATUGTkoJK9CZT3Dk .icon-shape .label{text-anchor:middle;}#mermaid-svg-ATUGTkoJK9CZT3Dk .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-ATUGTkoJK9CZT3Dk .rough-node .label,#mermaid-svg-ATUGTkoJK9CZT3Dk .node .label,#mermaid-svg-ATUGTkoJK9CZT3Dk .image-shape .label,#mermaid-svg-ATUGTkoJK9CZT3Dk .icon-shape .label{text-align:center;}#mermaid-svg-ATUGTkoJK9CZT3Dk .node.clickable{cursor:pointer;}#mermaid-svg-ATUGTkoJK9CZT3Dk .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .arrowheadPath{fill:#333333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-ATUGTkoJK9CZT3Dk .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-ATUGTkoJK9CZT3Dk .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-ATUGTkoJK9CZT3Dk .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-ATUGTkoJK9CZT3Dk .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-ATUGTkoJK9CZT3Dk .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-ATUGTkoJK9CZT3Dk .cluster text{fill:#333;}#mermaid-svg-ATUGTkoJK9CZT3Dk .cluster span{color:#333;}#mermaid-svg-ATUGTkoJK9CZT3Dk div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-ATUGTkoJK9CZT3Dk .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-ATUGTkoJK9CZT3Dk rect.text{fill:none;stroke-width:0;}#mermaid-svg-ATUGTkoJK9CZT3Dk .icon-shape,#mermaid-svg-ATUGTkoJK9CZT3Dk .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-ATUGTkoJK9CZT3Dk .icon-shape p,#mermaid-svg-ATUGTkoJK9CZT3Dk .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-ATUGTkoJK9CZT3Dk .icon-shape .label rect,#mermaid-svg-ATUGTkoJK9CZT3Dk .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-ATUGTkoJK9CZT3Dk .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-ATUGTkoJK9CZT3Dk .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-ATUGTkoJK9CZT3Dk :root{–mermaid-font-family:\”trebuchet ms\”,verdana,arial,sans-serif;}#mermaid-svg-ATUGTkoJK9CZT3Dk .input>*{fill:#eef!important;stroke:#88f!important;}#mermaid-svg-ATUGTkoJK9CZT3Dk .input span{fill:#eef!important;stroke:#88f!important;}#mermaid-svg-ATUGTkoJK9CZT3Dk .branch>*{fill:#ffe!important;stroke:#fc6!important;}#mermaid-svg-ATUGTkoJK9CZT3Dk .branch span{fill:#ffe!important;stroke:#fc6!important;}#mermaid-svg-ATUGTkoJK9CZT3Dk .out>*{fill:#efe!important;stroke:#3c3!important;}#mermaid-svg-ATUGTkoJK9CZT3Dk .out span{fill:#efe!important;stroke:#3c3!important;}

是

否

是

否

是

否

输入图像 + 时间戳

IMU 预测?hasPrediction

IMU 辅助预测1 层金字塔初值

光流跟踪calcOpticalFlowPyrLK 3 层

FLOW_BACK?

正反向校验反向光流阈值 0.5

剔除失败点/外点reduceVector 同步剔

边界检查剔除越界点

寿命加一track_cnt++

掩膜均匀化 setMask按寿命降序 + MIN_DIST

需补新点?

补充新点goodFeaturesToTrack 分配 ID

去畸变归一化undistortedPts / liftProjective

计算归一化平面速度ptsVelocity

构建特征帧 featureFrame7 维 x,y,z,u,v,vx,vy

输出 featureFrame交给后端 processMeasurements

5.1 跟踪与剔除（:112-162）

// feature_tracker.cpp:164-165 跟踪成功者寿命 +1

for (auto &n : track_cnt) n++;

跟踪失败的点已在前面 reduceVector 中与 prev_pts/cur_pts/ids/track_cnt 同步剔除（:156-159）–四个 vector 用同一个 status，保证下标对齐。这是 VINS 前端管理多组并行数据的核心约定，后续 feature_manager 也沿用。

5.2 补充新点（:167-197）

// feature_tracker.cpp:177-195

int n_max_cnt = MAX_CNT – static_cast<int>(cur_pts.size());

if (n_max_cnt > 0)

cv::goodFeaturesToTrack(cur_img, n_pts, n_max_cnt, 0.01, MIN_DIST, mask);

for (auto &p : n_pts) {

cur_pts.push_back(p);

ids.push_back(n_id++); // 新点分配全局唯一递增 id

track_cnt.push_back(1); // 初始寿命=1

}

新点用 n_id++ 分配唯一 ID，这是特征在全生命周期的身份标识，后端靠它做数据关联。n_id 是 FeatureTracker 的成员（feature_tracker.h:82），单调递增不回收。

5.3 IMU 辅助预测（进阶，:117-131）

若 hasPrediction（后端优化后由 setPrediction 设置），前端用 IMU 预测的位姿把特征点投影到当前帧作为光流初值，仅用 1 层金字塔即可快速收敛；若预测成功数 <10（预测不可靠），降级回 3 层。这是 VIO「视觉-惯性耦合」在前端的体现–IMU 不是只在后端用，也提前帮视觉缩小搜索范围。该机制涉及后端 predictPtsInNextFrame，第 15 篇再展开。

六、featureFrame 的 7 维含义

我们说 featureFrame 装着「7 维观测」但没展开，现在补上。构建逻辑在 trackImage 末尾（:260-279）：

// feature_tracker.cpp:263-278

double x, y, z;

x = cur_un_pts[i].x; // 去畸变后的归一化坐标

y = cur_un_pts[i].y;

z = 1; // 归一化平面 z=1

double p_u, p_v;

p_u = cur_pts[i].x; // 原始像素坐标（含畸变）

p_v = cur_pts[i].y;

double velocity_x, velocity_y;

velocity_x = pts_velocity[i].x;

velocity_y = pts_velocity[i].y;

Eigen::Matrix<double, 7, 1> xyz_uv_velocity;

xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;

featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);

逐字段含义

去畸变：undistortedPts（:391-402）

// feature_tracker.cpp:396-399

Eigen::Vector2d a(pts[i].x, pts[i].y); // 像素

Eigen::Vector3d b;

cam->liftProjective(a, b); // 像素 -> 归一化射线 (去畸变)

un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z())); // 投影到 z=1 平面

liftProjective 是相机模型（针孔/MEI）的逆投影：把带畸变的像素 $(u,v)$ 还原成相机坐标系的归一化射线 $(X/Z,Y/Z,1)$。后端视觉残差用的是这组归一化坐标（第 12 篇），因为它与相机内参解耦，几何意义干净。EuRoC 用 MEI 模型（cam0_mei.yaml），liftProjective 会按 MEI 的全向模型去畸变。

速度：ptsVelocity（:404-442）–一个易误解点

// feature_tracker.cpp:425-426

double v_x = (pts[i].x – it->second.x) / dt; // pts 是 cur_un_pts（归一化坐标！）

double v_y = (pts[i].y – it->second.y) / dt;

注意 trackImage:200 传入 ptsVelocity 的是 cur_un_pts（归一化坐标），不是像素坐标。所以第 5、6 维虽常被称作「像素速度」，实际是归一化平面上的速度 $\Delta (x/z,y/z)/\Delta t$。它用于 setPrediction/fastPredictIMU 的特征运动预测。后续篇以本篇为准。

featureFrame 的外层结构是 map<feature_id, vector<pair<camera_id, 7维>>>：key 是特征 ID，value 是它在各相机的观测列表（单目只有 camera_id=0 一项，双目还有 camera_id=1）。这就是 featureBuf 里那串吓人模板的真实语义。

小结

本篇拆解了 VINS 前端三件套：

• Shi-Tomasi 角点：结构张量特征值判据，goodFeaturesToTrack + mask 提取；

• KLT 金字塔光流：亮度恒定假设下的 Lucas-Kanade 方程，金字塔解大位移，双向校验剔外点；

• Mask 均匀化：按寿命排序优先保留长寿命点，MIN_DIST 圆保证空间均匀。

trackImage 最终输出 featureFrame，每条观测是 7 维 [归一化x,y,z=1, 像素u,v, 归一化速度vx,vy]，其中归一化坐标供后端视觉残差、像素速度供 IMU 辅助预测。下一帧到来时，这些带 ID 的特征会被送进 FeatureManager 判断是否构成关键帧。

下一篇预告：第 04 篇《关键帧选择与特征点管理》–视差与时间阈值的关键帧判据、FeatureManager/FeaturePerId/FeaturePerFrame 的生命周期管理、逆深度参数化初探，精读 feature_manager.cpp 的 addFeatureCheckParallax。难度 ★★★☆☆。