简介面向机器人技术、立体视觉与视觉里程计研究者的MATLAB实现基于SOFT算法完成特征选择与跟踪并估计相机运动轨迹。代码已在MATLAB R2018a上测试依赖并行处理与计算机视觉工具箱同时给出特征处理、匹配、选择及运动估计各阶段耗时便于评估算法性能。压缩包共42个文件包含27个MATLAB脚本、测试图像与说明文档核心函数visualSOFT.m和主程序main.m可直接运行配置、授权说明及示例图片一应俱全整体仅3.88MB轻量易部署。已有707人学习适合具备基础MATLAB经验、希望深入理解特征提取与运动估计流程的开发者也可作为算法对比或课程设计的参考实现。1. 为什么视觉里程计选了 SOFT 而不是端到端网络做机器人定位的人都有个共识在室内结构光、室外弱纹理这类场景里基于学习的视觉里程计往往比传统几何方法更“脆”。SOFTStereo 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{end1} 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(end1) 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 与 RANSACMATLAB 的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修正量 δ 的计算用到了阻尼最小二乘(JJ λI)δ -Jeλ 取1e-6是为了防止 JJ 奇异。注意位姿更新时需要把旋转向量先转成旋转矩阵再复合不能直接做向量加法——这是新手最容易踩的坑。4.4 退化场景纯平移与低视差两步法在特定运动模式下会退化。当机器人做纯平移尤其是沿光轴方向时旋转分量不可观J 矩阵的条件数会迅速增大。检测退化的一种方法是观察 JJ 的最小特征值如果它小于某个阈值比如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_mrMATLAB 里实现这一过程的关键是正确地组装 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 与 RPEMATLAB 里没有内置的视觉里程计评价函数但计算绝对轨迹误差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(idelta,:); gtDelta inv(gtTraj(i,:)) * gtTraj(idelta,:); rel inv(gtDelta) * estDelta; rpeErrors(end1) norm(rel(1:3,4)); %#okAGROW end rpe mean(rpeErrors); end6.3 参数调优的推荐顺序参数调整的顺序会影响调试效率。我最常用的顺序是先固定特征提取参数把帧间跟踪调稳再调 RANSAC 阈值和重投影阈值把粗估计的内点率做到 90% 以上最后才调滑窗大小和边缘化参数。SGM 的DisparityRange放在最后调因为它的改动会影响整个深度分布代价最大。参数推荐起点调节方向影响BlockSize(SGM)9纹理稀疏时增大到 15深度平滑度 vs 边缘精度UniquenessThreshold15误匹配多时增大到 25内点率 vs 特征密度MaxBidirectionalError1.5运动快时放宽到 2.5跟踪稳定性 vs 误匹配率MaxReprojectionError2.0图像噪声大时放宽到 3.0位姿鲁棒性 vs 优化精度窗口大小10 帧场景简单时减小到 8计算量 vs 漂移速度6.4 一个常被忽略的 MATLAB 性能陷阱MATLAB 里的vision.PointTracker默认使用双精度浮点做光流计算在 1080p 图像上每帧跟踪 1000 个点需要约 15 毫秒。如果发现跟踪耗时异常先检查是否把原始图像直接传入而没有转灰度。RGB 输入会触发内部类型转换耗时翻倍。另一个陷阱是循环内频繁调用estimateWorldCameraPose该函数包含随机采样每次调用会有 5~10 毫秒的初始化开销建议在外部预分配相机参数对象并且用UseRandomSeed, true保证可重复实验。本文还有配套的精品资源点击获取