MATLAB实现SOFT立体视觉里程计：从特征跟踪到局部地图优化

简介：面向机器人技术、立体视觉与视觉里程计研究者的MATLAB实现，基于SOFT算法完成特征选择与跟踪，并估计相机运动轨迹。代码已在MATLAB R2018a上测试，依赖并行处理与计算机视觉工具箱，同时给出特征处理、匹配、选择及运动估计各阶段耗时，便于评估算法性能。压缩包共42个文件，包含27个MATLAB脚本、测试图像与说明文档，核心函数visualSOFT.m和主程序main.m可直接运行，配置、授权说明及示例图片一应俱全，整体仅3.88MB，轻量易部署。已有707人学习，适合具备基础MATLAB经验、希望深入理解特征提取与运动估计流程的开发者，也可作为算法对比或课程设计的参考实现。

1. 为什么视觉里程计选了 SOFT 而不是端到端网络

做机器人定位的人都有个共识：在室内结构光、室外弱纹理这类场景里，基于学习的视觉里程计往往比传统几何方法更“脆”。SOFT（Stereo Odometry based on robust Feature Tracking）是 2017 年发表在 ICRA 上的一个立体视觉里程计算法，它用四叉树均匀化特征、两步位姿估计和局部地图边缘化，拿到了 KITTI 视觉里程计榜单上传统方法里非常靠前的位置。更关键的是，SOFT 的全部逻辑都可以拆成离散的步骤，这正好适合在 MATLAB 里复现——毕竟 MATLAB 的矩阵运算和工具箱能省掉一大半底层实现。这篇文章面向做机器人导航、自动驾驶感知或 SLAM 入门的学生和工程师，带你从立体匹配一直推到局部地图优化，每一步给出可运行的 MATLAB 代码和参数依据。顺带说一句，就算以后 AI 编码工具能像执行 Python 一样操作 MATLAB 任务，SOFT 这种可解释的几何流程依然是调试和验证的好底子。

2. 立体匹配与视差图：SOFT 算法的输入质量决定一切

2.1 SOFT 为什么依赖立体视觉而不是单目

单目视觉里程计面临尺度漂移问题，因为从两帧图像里解出的平移向量缺少绝对尺度信息。立体视觉通过左右相机的固定基线直接恢复深度，把尺度问题变成标定问题——只要基线长度和相机内参正确，深度就是物理单位。SOFT 选择立体视觉的另一个原因是鲁棒性：右目图像提供了额外的几何约束，在特征匹配阶段可以用左右一致性检查剔除误匹配，这是单目无法做到的。

在 MATLAB 里做立体视觉里程计，首先要确认相机标定参数。常见做法是用estimateCameraParameters配合棋盘格完成标定，得到左右相机的内参矩阵K1、K2，以及右目相对左目的旋转矩阵R和平移向量T。SOFT 论文里假设双目已经做过极线校正，也就是说左右图像对应点只存在水平视差。实际工程中这一步容易出问题：如果标定板图像拍少了，校正后的图像会有垂直偏差，直接影响后续的深度估计精度。

2.2 用 MATLAB 的 disparitySGM 计算稠密视差

MATLAB 的 Computer Vision Toolbox 提供了半全局匹配（SGM）函数disparitySGM，它是 SOFT 预处理阶段的可靠选择。下面这段代码展示了从校正后的左右灰度图计算视差图的最小流程：

% 读取校正后的左右图像 I_left = im2gray(imread('left_000000.png')); I_right = im2gray(imread('right_000000.png')); % 设置 SGM 参数 disparityRange = [0 64]; % 视差搜索范围，单位像素 blockSize = 9; % 匹配窗口大小，奇数 contrastThreshold = 0.5; % 对比度阈值，过滤低纹理区域 % 计算视差图 disparityMap = disparitySGM(I_left, I_right, ... 'DisparityRange', disparityRange, ... 'UniquenessThreshold', 15, ... 'BlockSize', blockSize, ... 'ContrastThreshold', contrastThreshold); % 将视差图转换为深度图 baseline = 0.54; % 双目基线，单位米，根据标定结果修改 focalLength = 718.856; % 焦距，单位像素，来自内参矩阵 depthMap = zeros(size(disparityMap)); validIdx = disparityMap > 0; depthMap(validIdx) = baseline * focalLength ./ disparityMap(validIdx);

这段代码里有几个参数值得细说。DisparityRange的选取取决于场景深度范围和基线长度：基线越长，同样的深度变化对应的视差变化越大，搜索范围就要相应扩大。UniquenessThreshold控制匹配的唯一性，值越大越严格，能减少误匹配但也会增加空洞。BlockSize影响视差图的平滑度，太小容易产生噪声，太大会丢失深度突变处的细节。

深度图的计算公式是depth = baseline * focalLength / disparity，注意这里假设了校正后的图像主点已经对齐到同一水平线。MATLAB 的disparitySGM返回的是单精度浮点视差图，对无效匹配返回-realmax，所以上面代码里用disparityMap > 0做了掩码过滤。

2.3 深度误差的放大效应与滤波策略

立体视觉的深度误差与时差平方成正比。把深度公式对视差求导，得到∂depth/∂d = -baseline * focal / d²，这意味着近处的物体深度精度高，远处的物体深度误差急剧放大。SOFT 的特征点大多分布在中等距离范围，这正是工程上的无奈：太近的特征容易出视野，太远的特征深度不可信。

实际处理中，我一般会在计算完深度图后加一步中值滤波，消除 SGM 在纹理稀疏区域的“飞点”。MATLAB 里用medfilt2(depthMap, [5 5])即可，但要注意滤波窗口不能太大，否则会磨掉深度边缘。另外，左图和右图各自计算一遍视差再做左右一致性检查（LRC）能更彻底地剔除遮挡区域的错误匹配，MATLAB 的disparitySGM不直接提供 LRC 选项，需要自己写。一个简单实现是分别以左图和右图为基准计算两张视差图，然后比较映射后的差是否小于 1 像素。

3. 四叉树均匀化特征提取与 SOFT 的特征轨迹管理

3.1 为什么普通角点检测不够用

FAST 或 Harris 角点检测的响应值在纹理密集区域会扎堆，导致位姿估计时特征点分布不均。如果图像上半部分是天空、下半部分是路面，所有角点都集中在路面，那么帧间估计的旋转分量会严重偏向图像坐标系的某一轴。SOFT 的核心改进之一是采用四叉树（Quadtree）对特征点进行空间均匀化，保证每个网格区域里保留最强的特征，而不是全局取响应值最高的 N 个点。

四叉树的思想很直接：把图像按四等分递归分裂，直到每个叶子节点里的特征点数量不超过设定阈值，然后从每个叶子节点里挑出响应值最高的点作为最终特征。MATLAB 里没有现成的四叉树特征提取函数，但用递归结构可以轻松实现。下面是一个精简版的四叉树分裂逻辑：

function features = quadtreeUniformFeatures(points, scores, minLeafSize) % points: Nx2 矩阵，每行是 [x, y] % scores: Nx1 向量，每个点的响应得分 % minLeafSize: 叶子节点允许的最小区域边长 % 初始包围盒：整幅图像范围 root = struct('x', 0, 'y', 0, 'w', 640, 'h', 480, ... 'indices', (1:size(points,1))', 'children', []); leaves = {}; stack = root; while ~isempty(stack) node = stack(end); stack(end) = []; % 如果节点内点数很少或区域足够小，停止分裂 if length(node.indices) <= 1 || node.w < 2*minLeafSize leaves{end+1} = node; continue; end % 四等分 halfW = node.w / 2; halfH = node.h / 2; for quadrant = 1:4 switch quadrant case 1 % 左上 mask = points(node.indices,1) < node.x + halfW & ... points(node.indices,2) < node.y + halfH; case 2 % 右上 mask = points(node.indices,1) >= node.x + halfW & ... points(node.indices,2) < node.y + halfH; case 3 % 左下 mask = points(node.indices,1) < node.x + halfW & ... points(node.indices,2) >= node.y + halfH; case 4 % 右下 mask = points(node.indices,1) >= node.x + halfW & ... points(node.indices,2) >= node.y + halfH; end childIdx = node.indices(mask); if ~isempty(childIdx) child = struct('x', node.x + mod(quadrant-1,2)*halfW, ... 'y', node.y + floor((quadrant-1)/2)*halfH, ... 'w', halfW, 'h', halfH, ... 'indices', childIdx, 'children', []); stack(end+1) = child; end end end % 从每个叶子节点选得分最高的点 features = []; for i = 1:length(leaves) idx = leaves{i}.indices; if isempty(idx), continue; end [~, bestIdx] = max(scores(idx)); features = [features; points(idx(bestIdx), :)]; end end

这段代码的关键参数是minLeafSize，它决定特征空间分布的最细粒度。取值太小会导致特征在纹理密集区域依然扎堆，太大则特征总数过少。一般建议minLeafSize设为图像短边的 1/100 左右，例如 480p 图像取 5~8 像素。

3.2 特征轨迹的年龄与生命力

SOFT 区别于传统帧间匹配的核心在于管理特征轨迹的生命周期，而不是仅仅做相邻两帧的匹配。每个特征点携带age和activeTime两个属性：age表示该特征从第一次出现到现在经过的帧数，activeTime表示它连续被成功跟踪的帧数。当一个特征的activeTime超过阈值时，它会被标记为“成熟特征（mature）”，只有成熟特征才参与位姿估计和局部地图优化；反之，如果特征在某一帧丢失，SOFT 不会立即删除它，而是给它一段宽限期，在宽限期内如果重新匹配成功，则恢复为活跃状态。

这种策略在 MATLAB 里可以这样组织：维护一个结构体数组tracks，每个元素包含当前图像坐标、三维坐标、轨迹年龄和最近丢失帧数。前端跟踪使用 KLT 光流法（vision.PointTracker），因为它的计算效率高于每帧重新做描述子匹配。MATLAB 的vision.PointTracker内部使用金字塔 Lucas-Kanade，对小幅帧间运动足够鲁棒。

tracker = vision.PointTracker('MaxBidirectionalError', 1.5); initialize(tracker, prevPoints, prevFrame); [currPoints, validIdx] = tracker(currFrame);

MaxBidirectionalError是正反向光流误差阈值，默认值 1.5 对大多数室内场景可用。如果机器人运动速度较快或图像分辨率较低，可以适当放宽到 2.5，但要接受更多误匹配进入后续几何验证环节。

3.3 特征数量与分布的自适应控制

SOFT 每帧提取特征数量并不是固定的。它根据上一帧的成熟特征数量动态调整本帧需要补充的新特征数量。如果上一帧的成熟特征不足，比如低于 200，就会在图像中重新运行角点检测和四叉树均匀化，补齐到目标数量。这个目标数量一般设置在 1000~1500 之间——太少精度不够，太多会让后端优化耗时急剧上升。

MATLAB 里可以用detectFASTFeatures配合自定义得分（如cornernessMetric）来实现特征候选提取。注意 FAST 的响应值在高纹理区域会重复，所以提取数量要多于最终需求，例如设定'MinQuality', 0.01, 'MinContrast', 0.1先拿到 5000 个候选点，再用四叉树压缩到 1200 个。

4. 两步法帧间位姿估计：RANSAC 粗估计与非线性精化

4.1 为什么不能只做一次优化

帧间位姿估计是视觉里程计的心脏。SOFT 采用两步策略：第一步用 P3P 或本质矩阵配合 RANSAC 获得初始位姿估计，第二步把所有内点和对应的三维地图点代入，用重投影误差构建非线性最小二乘问题精化位姿。这种做法的主流原因有两点：RANSAC 能剔除匹配外点，但它输出的位姿只有代数精度，没有最小化几何误差；而直接做非线性优化又容易被外点拉偏。只有先把外点清干净，再让优化器发挥最大似然估计的作用，才能在速度和精度之间取得平衡。

4.2 粗估计：estimateWorldCameraPose 与 RANSAC

MATLAB 的estimateWorldCameraPose函数直接封装了 P3P + RANSAC，输入匹配好的三维点和二维像素点，输出相机在世界坐标系中的位姿。注意这个函数的坐标系约定：世界点在相机坐标系中的表示通过R * worldPoint + t转换。

% 三维点是上一帧三角化得到的，二维点是当前帧的匹配点 [worldOrientation, worldLocation, inlierIdx] = estimateWorldCameraPose(... matchedPoints3D, matchedPoints2D, cameraParams, ... 'MaxNumTrials', 2000, 'Confidence', 99, 'MaxReprojectionError', 2.0);

这里的MaxNumTrials和Confidence共同决定 RANSAC 的迭代次数。MaxReprojectionError是内点判定阈值，单位像素，2.0 是 SOFT 论文中的常用值；如果图像噪声大，可以放宽到 3.0，但这时后续精化阶段的权重设计要更保守。

从estimateWorldCameraPose得到的位姿还不是最终答案。它用的是 P3P 最小解，只有三个点的约束，即使经过 RANSAC 投票，最终的位姿也远没有利用全部内点的信息。下一步要把所有内点送入优化器。

4.3 精化：用 MATLAB 优化工具箱最小化重投影误差

精化阶段的目标函数是重投影误差的平方和。设相机的旋转和平移为R和t，第i个三维地图点Pi在当前帧的观测像素为ui，则误差项为：

ei = ui - K * (R * Pi + t) / (R * Pi + t)_z

这是一个典型的非线性最小二乘问题。MATLAB 的lsqnonlin在optimtool工具箱里，能处理中小规模的此类问题。但直接lsqnonlin效率不够高，因为位姿的自由度只有 6，而观测往往有几百个点。更好的做法是手写高斯牛顿法，利用 JtJ 结构的稀疏性。

function [R_refined, t_refined] = refinePoseGN(R0, t0, points3D, points2D, K) % 将 R, t 转为 6 维李代数向量（旋转向量 + 平移） xi = [rotationMatrixToVector(R0); t0(:)]; for iter = 1:10 [J, e, r, t] = computeJacobian(xi, points3D, points2D, K); delta = - (J'*J + 1e-6*eye(6)) \ (J'*e); xi = [rotationMatrixToVector(r * rotationVectorToMatrix(xi(1:3))) ... + delta(1:3); t + delta(4:6)]; if norm(delta) < 1e-8 break; end end R_refined = rotationVectorToMatrix(xi(1:3)); t_refined = xi(4:6); end

这里有一个隐含的数学点：李代数扰动模型把误差对位姿的导数写成[I, -skew(Pi)]的形式，skew是反对称矩阵。computeJacobian需要自己实现，核心公式如下：

function [J, e, R, t] = computeJacobian(xi, points3D, points2D, K) R = rotationVectorToMatrix(xi(1:3)); t = xi(4:6); n = size(points3D, 1); J = zeros(2*n, 6); e = zeros(2*n, 1); for i = 1:n P = points3D(i,:)'; Pc = R * P + t; u = K * Pc; u(1) = u(1) / u(3); u(2) = u(2) / u(3); e(2*i-1:2*i) = u(1:2) - points2D(i,1:2)'; % 链式法则的雅可比 du_dPc = [K(1,1)/Pc(3), 0, -K(1,1)*Pc(1)/Pc(3)^2; 0, K(2,2)/Pc(3), -K(2,2)*Pc(2)/Pc(3)^2]; dPc_dxi = [eye(3), -skew(Pc)]; J(2*i-1:2*i, :) = du_dPc * dPc_dxi; end end

修正量 δ 的计算用到了阻尼最小二乘(J'J + λI)δ = -J'e，λ 取1e-6是为了防止 J'J 奇异。注意位姿更新时需要把旋转向量先转成旋转矩阵再复合，不能直接做向量加法——这是新手最容易踩的坑。

4.4 退化场景：纯平移与低视差

两步法在特定运动模式下会退化。当机器人做纯平移（尤其是沿光轴方向）时，旋转分量不可观，J 矩阵的条件数会迅速增大。检测退化的一种方法是观察 J'J 的最小特征值：如果它小于某个阈值（比如1e-6），说明问题病态，此时应该减少优化自由度，固定旋转只优化平移，或者引入惯性测量数据。SOFT 算法本身没有显式处理退化，但工程实现中这个检查非常值得保留。

5. 局部地图与边缘化：SOFT 的长时程一致性保障

5.1 滑窗优化的必要性

帧间估计只约束相邻两帧，误差会随轨迹增长而累积漂移。SOFT 借鉴视觉惯性导航系统的做法，维护一个固定大小的局部窗口（通常是最近 10~15 帧），窗口内的所有关键帧共同参与联合优化，目标是最小化所有地图点在窗口内所有帧上的重投影误差之和。窗口滑动时，最老的关键帧被边缘化（marginalize），而不是简单丢弃——丢弃是最粗暴的近似，等于扔掉约束信息；边缘化则把被移除帧的信息转化为先验约束，保留在优化问题里。

5.2 信息矩阵与 Schur Complement

边缘化的数学本质是高斯消元。设整个优化问题的信息矩阵（Hessian）为H，我们把要被移除的状态量记为xm，保留的状态量记为xr，则先验约束通过 Schur Complement 计算：

H_prior = H_rr - H_rm * H_mm^{-1} * H_mr

MATLAB 里实现这一过程的关键是正确地组装 H 矩阵。SOFT 的局部地图包含两类状态：窗口内关键帧的位姿（6 自由度/帧）和地图点的三维坐标。由于地图点的观测只出现在它被三角化之后的帧中，H 矩阵天然是稀疏的。用 MATLAB 的稀疏矩阵类型sparse来存储 H 可以显著降低内存占用和计算时间。

function [H_prior, b_prior] = marginalize(H, b, idx_m) % H: 稀疏信息矩阵, b: 信息向量 % idx_m: 被边缘化的状态索引 n = size(H, 1); idx_r = setdiff(1:n, idx_m); H_rr = H(idx_r, idx_r); H_rm = H(idx_r, idx_m); H_mm = H(idx_m, idx_m); b_r = b(idx_r); b_m = b(idx_m); H_prior = H_rr - H_rm * (H_mm \ H_rm'); b_prior = b_r - H_rm * (H_mm \ b_m); end

这里H_mm \ H_rm'用稀疏矩阵求解器完成。实际运行时如果 H_mm 不可逆（比如被边缘化帧的观测太少），需要加正则化项1e-6 * eye(size(H_mm))。边缘化的另一个坑是状态重参数化：被移除帧的位姿和地图点之间存在关联，如果边缘化后保留的状态参数化方式改变了（例如旋转向量变成四元数），先验约束与新增残差之间会出现不一致。

5.3 地图点的三角化与筛选

新特征点变成地图点需要满足两个条件：被连续跟踪超过 3 帧，且在这 3 帧里的视差角大于一定阈值。视差角太小会导致三角化出的深度不可靠。MATLAB 里可以用triangulate函数配合左右目或双帧位姿完成三角化，但要注意它返回的是齐次坐标，需要手动归一化。

筛选地图点的过程中，最重要的指标是重投影误差的均值。如果一个地图点在窗口内的平均重投影误差大于 3 像素，它多半是误三角化的点，应该直接剔除。SOFT 策略里还会计算地图点的“被观帧数”，被观帧数过少的点也要删掉——它们对约束的贡献微乎其微，反而拖慢优化速度。

5.4 窗口大小与计算负载平衡

滑窗大小的选择直接影响实时性。10 帧窗口在 MATLAB 里单次优化大约需要 30~50 毫秒，15 帧窗口就可能涨到 80 毫秒以上，这还不包括特征提取和匹配的时间。SOFT 论文的实验在 C++ 上能做到 20 毫秒/帧，MATLAB 由于解释执行和内存管理开销，达到 30~50 毫秒/帧已经算不错。如果你的机器人运动较慢、场景纹理丰富，窗口可以减小到 8 帧；反之则要加大，否则漂移速度会明显加快。

6. 在 MATLAB 中验证 SOFT 效果：KITTI 序列与参数调优技巧

6.1 用 KITTI odometry 数据跑通全过程

KITTI 视觉里程计基准提供了 11 个带真实轨迹的训练序列，是验证 SOFT 复现效果的标准数据集。下载序列后，需要读入左右灰度图像和时间戳。一个关键的工程细节是：KITTI 的图像已经做了校正，但并没有去畸变，所以读图后要用undistortImage先处理，否则特征匹配在图像边缘处会系统性地偏差半个像素以上。

跑通一个序列的最小脚本结构大致如下：

% 设置序列路径 seqPath = 'dataset/sequences/00'; leftFiles = dir(fullfile(seqPath, 'image_0/*.png')); numFrames = length(leftFiles); % 初始化轨迹矩阵 trajectory = zeros(numFrames, 3); prevPose = eye(4); for k = 2:numFrames % 读图、匹配、估计位姿（省略细节） [R, t] = estimatePoseBetweenFrames(k-1, k); % 累积变换 currPose = prevPose * [R, t; 0 0 0 1]; trajectory(k,:) = currPose(1:3,4)'; prevPose = currPose; end

注意这里prevPose * [R, t; 0 0 0 1]的相乘顺序。如果R, t表示的是从上一帧到当前帧的运动，那么累积位姿应该是左乘，也就是currPose = prevPose * deltaPose。很多人会把顺序弄反，导致轨迹方向完全错误。

6.2 轨迹精度评价：ATE 与 RPE

MATLAB 里没有内置的视觉里程计评价函数，但计算绝对轨迹误差（ATE）和相对位姿误差（RPE）只需要几十行代码。ATE 的计算思路是用 Umeyama 算法将估计轨迹与真实轨迹对齐（消除参考系差异），然后计算每个时间戳上的位置误差均值。Umeyama 算法在 MATLAB 里可以用absOrientation（在 Aerospace Toolbox）或手写奇异值分解实现。

function [ate, rpe] = evaluateTrajectory(estTraj, gtTraj) % 用 Umeyama 对齐：求 [s, R, t] 使 est 到 gt 的均方误差最小 n = size(estTraj, 1); mu_e = mean(estTraj); mu_g = mean(gtTraj); estCentered = estTraj - mu_e; gtCentered = gtTraj - mu_g; H = estCentered' * gtCentered; [U, ~, V] = svd(H); R = V * U'; if det(R) < 0 V(:,end) = -V(:,end); R = V * U'; end s = trace(R' * H) / sum(sum(estCentered.^2)); t = mu_g' - s * R * mu_e'; aligned = (s * estTraj * R' + t'); errors = vecnorm(aligned - gtTraj, 2, 2); ate = mean(errors); % RPE 计算：按固定间隔比较相对运动 delta = 10; % 每 10 帧比较一次 rpeErrors = []; for i = 1:(n-delta) estDelta = inv(estTraj(i,:)) * estTraj(i+delta,:); gtDelta = inv(gtTraj(i,:)) * gtTraj(i+delta,:); rel = inv(gtDelta) * estDelta; rpeErrors(end+1) = norm(rel(1:3,4)); %#ok<AGROW> end rpe = mean(rpeErrors); end

6.3 参数调优的推荐顺序

参数调整的顺序会影响调试效率。我最常用的顺序是：先固定特征提取参数，把帧间跟踪调稳；再调 RANSAC 阈值和重投影阈值，把粗估计的内点率做到 90% 以上；最后才调滑窗大小和边缘化参数。SGM 的DisparityRange放在最后调，因为它的改动会影响整个深度分布，代价最大。

参数	推荐起点	调节方向	影响
BlockSize(SGM)	9	纹理稀疏时增大到 15	深度平滑度 vs 边缘精度
UniquenessThreshold	15	误匹配多时增大到 25	内点率 vs 特征密度
MaxBidirectionalError	1.5	运动快时放宽到 2.5	跟踪稳定性 vs 误匹配率
MaxReprojectionError	2.0	图像噪声大时放宽到 3.0	位姿鲁棒性 vs 优化精度
窗口大小	10 帧	场景简单时减小到 8	计算量 vs 漂移速度

6.4 一个常被忽略的 MATLAB 性能陷阱

MATLAB 里的vision.PointTracker默认使用双精度浮点做光流计算，在 1080p 图像上每帧跟踪 1000 个点需要约 15 毫秒。如果发现跟踪耗时异常，先检查是否把原始图像直接传入而没有转灰度。RGB 输入会触发内部类型转换，耗时翻倍。另一个陷阱是循环内频繁调用estimateWorldCameraPose，该函数包含随机采样，每次调用会有 5~10 毫秒的初始化开销，建议在外部预分配相机参数对象，并且用'UseRandomSeed', true保证可重复实验。

本文还有配套的精品资源，点击获取