简介面向机器人视觉惯性定位方向开发者与高校研究者的 ROVIO 源码包聚焦资源受限嵌入式平台上的实时 SLAM/VIO 实现。算法以扩展卡尔曼滤波融合单目或双目图像与 IMU 数据涵盖特征跟踪、运动与观测模型、误差状态估计及重定位优化等环节可用于无人机、移动机器人等场景的学习与二次开发。压缩包共 56 个文件、约 166KB以 hpp 头文件和 cpp 源文件为主体辅以 yaml 参数、launch 启动配置及 srv、shader 等支撑文件源码组织紧凑便于梳理滤波框架与 ROS 集成方式。已有 667 人学习或下载。读者可据此理解 ROVIO 的整体架构、状态更新流程和参数配置思路并以此为基准开展算法复现、性能分析与定制改造适合具备概率论、滤波理论和计算机视觉基础的中高级学习者。1. 一块算力吃紧的板子上为什么还会有人翻出 ROVIO 这套 SLAM 源码无人机、手持扫描、小型机器人这类场景常见的尴尬是工况要求三十帧以上的实时位姿板子却只给得起一个 ARM 核内存还得留一半给别的模块。这时候把 SLAM 里那套「提特征、算描述子、匹配、RANSAC、后端优化」的经典链路搬上去帧率掉到个位数是常事。ROVIO 走的是另一条路——单目相机加 IMU 的紧耦合 EKF把图像块的亮度误差直接当成观测残差丢进滤波不建关键帧、不做回环、不跑 BA状态量里连地图点坐标都换成了方向向量加逆深度。这套源码适合谁想在低成本硬件上跑视觉惯性里程计、又打算亲手改滤波器的人只想拿个黑盒里程计输出的装现成的包更省事。2. 模块分层一帧图像进来之后状态在哪些类之间流转读这套代码的第一道坎不是数学是找不到入口在哪、状态存在哪。它的组织方式跟 ORB-SLAM 那类关键帧架构完全不同没有 Map、没有 KeyFrame、没有 LocalMapping 线程整个系统就是一个滤波器加两个更新器IMU 更新、图像更新。所以先把类的角色分清楚后面看公式才不至于对着代码发愣。2.1 代码目录与四个核心角色工程本身是个 ROS 包加一个基于 Eigen 的位姿代数模板库作为依赖。目录大致是这么分的路径内容阅读优先级include/rovio/全部核心头文件逻辑基本都在头文件里实现高src/节点入口、ROS 消息收发、main中cfg/参数文件.info噪声、外参、金字塔层数都在这高launch/启动文件绑话题、加载参数中test/一些单元测试与标定相关的小工具低include/rovio/里角色可以归成四类理解了这四类剩下的都是细节节点层RovioNode负责订阅图像和 IMU、把 ROS 消息转成内部类型、把滤波结果再打包成nav_msgs/Odometry和可视化 Marker 发出去。它不做任何估计。滤波层FilterState有的版本叫State持有整个 EKF 的状态向量和协方差矩阵对外提供predict和update两类接口。所有数学最终都收敛到这里。更新层ImgUpdate管图像这条观测链路IMUUpdate或预测器管 IMU 的传播。两者都把残差 雅可比算好交给滤波层。几何层Camera内参、畸变、投影、Feature一个路标、Patch一个路标在某层金字塔上的图像块、Observation路标在某帧上的一次观测。这一层是纯几何和像素操作最容易单独拿出来改。2.2 状态向量怎么拼为什么路标存的是方向加逆深度这套滤波器的状态不是一个大而全的向量而是分成若干区块每个区块自己带协方差块和与其他区块的互协方差。这么做的好处是增删特征时不用重排整个协方差矩阵只动对应行列。// 状态区块的组织方式按项目结构抽取的骨架非逐字照抄 class FilterState { public: // 机器人自身状态位姿、速度、IMU 零偏 PoseBlock pose_; // IMU 在世界系下的位姿 VelocityBlock vel_; // 世界系线速度 BiasBlock biasAcc_; // 加速度计零偏 BiasBlock biasGyro_; // 陀螺仪零偏 // 外参相机相对 IMU 的变换在线估计还是固定由参数决定 ExtrinsicsBlock ext_; // q_CI b_CI // 路标每个 Feature 贡献 3 维bearing 的 2 维 逆深度 rho 1 维 std::vectorFeatureBlock features_; // 全局协方差按上面区块顺序分块存储 Eigen::MatrixXd P_; };关键点在那个rho。如果用三维坐标表示路标深度方向的方差会随着观测次数剧烈变化数值上很难收敛换成方向向量加逆深度深度不确定时rho的初值给 0对应无穷远方差给一个很大的数滤波器在前几帧几乎不动它等视差攒够了自然收敛。这也是为什么这套代码在纯旋转或静止初始化时会漂——没有视差rho就没法被约束。FeatureBlock里除了rho还有一组Patch每个Patch对应金字塔的一层存着参考块的像素灰度、像素坐标和法向。多层的意义在于高层分辨率低但收敛域大低层精度高但容易掉进局部极小。实际更新时是从粗到精逐层更新滤波器粗层先把位姿拉到大致的坑里细层再精修。2.3 一条更新链路的完整时序节点收到一帧图像后的调用顺序是理解这份代码最划算的一条线// 节点层只做数据转换和转发 void RovioNode::imgCallback(const sensor_msgs::ImageConstPtr msg) { // 1) ROS 图像转内部 cv::Mat统一成单通道灰度 cv::Mat img toGrayMat(msg); // 2) 时间戳必须落在 IMU 缓冲的时间范围内 // 否则这一帧的 IMU 预测无从做起只能丢帧 double t msg-header.stamp.toSec(); if (t imuBuffer_.getStartTime() || t imuBuffer_.getEndTime()) return; // 3) 交给图像更新器内部完成预测 观测更新 imgUpdate_.processImage(img, t); }// 更新层粗到精的逐层更新 void ImgUpdate::processImage(const cv::Mat img, double t) { // a. 先按 IMU 数据把状态传播到图像时刻得到先验与先验协方差 filter_.predictTo(t); // b. 构建图像金字塔层数由参数 nLevels 决定一般 3 到 4 层 buildPyramid(img, pyramid_, nLevels_); // c. 从最粗一层往最细一层走每层做一次 EKF 更新 for (int lvl nLevels_ - 1; lvl 0; --lvl) { // 遍历所有特征在该层的观测累加光度残差的 H 和 b for (auto feat : filter_.features()) { for (auto obs : feat.observations(lvl)) { // 投影参考块到当前帧逐像素算亮度差得到残差 e 与雅可比 J // J 再经链式法则映射到误差状态空间 accumulate(obs, lvl, H_, b_); } } // d. 一层更新完状态和协方差都变了下一层从新的先验继续 filter_.updateFromImage(H_, b_, sigmaImg_); } }这段流程里有两个容易被忽略的设计。一是每层更新完就立刻修改状态而不是把四层的残差一次性堆起来做一次大更新——这样粗层的粗结果会作为细层的先验收敛域更大但如果粗层本身被错误观测带偏细层是拉不回来的所以噪声参数的重要性远超你的直觉。二是整个更新过程可以迭代多轮代码里通常有iterations之类的计数每轮重新线性化一次本质上是在做高斯牛顿只是被包在 EKF 的框架里。2.4 时间轴与坐标系三个必须先在纸上画清的东西坐标系只有三个世界系 W、IMU 系 I、相机系 C。状态里的位姿是 I 在 W 下的位姿观测发生在 C 系所以每次算残差都要走一次T_WI * T_IC。外参T_IC要么从参数文件读固定值要么当成滤波器状态的一部分在线估。后者听起来更好实际调试时非常容易散——外参和 IMU 零偏的耦合很强标定不准的时候在线估会把误差摊到姿态上。时间轴上有三条IMU 采样时刻、图像曝光时刻、滤波器当前时刻。代码里用一个环形缓冲区保存最近一段 IMU 数据图像到来时从缓冲区里取到该时刻为止的测量做积分。这里最容易翻车的是时间戳没对齐——相机和 IMU 不是同一个时钟源时哪怕差 20 毫秒在有一定角速度的运动下也能让残差系统整体偏置表现出来就是跑着跑着轨迹往下掉。3. 编译与跑通在 catkin 工作空间里拉起一个最小节点拿到源码第一件事不是读FilterState是先让它跑出一帧轨迹。这套工程是标准的 ROS catkin 包编译本身不难难的是依赖版本和参数文件的匹配。3.1 依赖清单与编译顺序需要准备的东西ROS 基础环境roscpp、rosbag、tf、sensor_msgs 这些、OpenCV图像金字塔和插值用、Eigen3、Boost以及一个位姿代数的模板库。最后这个库是这个工程里最容易被忽略的依赖它的作用是给旋转、平移、位姿提供强类型封装避免四元数实部虚部顺序搞反这类玄学问题。# 建工作空间 mkdir -p ~/rovio_ws/src cd ~/rovio_ws/src # 位姿代数库和算法包都放在 src 下一起编译 # 注意位姿库要先于算法包被 catkin 找到 catkin_init_workspace cd ~/rovio_ws # Release 是必须的Debug 下每帧的光度残差计算会慢十倍以上 catkin_make -DCMAKE_BUILD_TYPERelease -j4 source devel/setup.bash编译时最先报错的地方通常是位姿库的头文件路径。它如果不是以 ROS 包的形式提供就得手工find_package或者在 CMakeLists 里加include_directories。另一种常见报错是 OpenCV 版本冲突系统里装了 OpenCV 4而 ROS 自带的 cv_bridge 链接的是 OpenCV 3两边模板实例化对不上报一堆undefined reference to cv::Mat::...。我一般的处理是把 cv_bridge 相关的转换代码收窄到节点层那一个文件其余部分完全不用 cv_bridge用裸cv::Mat传冲突面立刻小一半。3.2 参数文件逐项过一遍参数文件是整个系统的旋钮面板读一遍比读一千行代码更快建立直觉。核心参数分四组参数组典型命名作用调大的后果图像噪声imgUpdateNoise*光度残差的观测噪声滤波器更信 IMU图像更新变软收敛慢但抗外点IMU 噪声imuNoiseAcc/imuNoiseGyr加速度计、陀螺仪的连续时间噪声密度更信图像动态响应快但容易抖零偏随机游走imuNoiseBiasAcc/imuNoiseBiasGyr零偏自身的漂移速率零偏估计更活跃标定不准时反而更容易散结构参数nLevels/nMaxFeatures金字塔层数、最大特征数层数多收敛域大但耗时线性增长还有一组不显眼但很致命的相机内参和T_IC外参。内参错 2%视觉尺度就整体偏 2%IMU 那边会一直试图把这个偏差解释成零偏最后两个都错。外参里的旋转尤其敏感相机光轴和 IMU 某根轴不对齐时转弯过程中的残差会出现有规律的正负交替轨迹表现为周期性摆动而不是单调漂移——这个特征很好认看到就知道是外参而不是噪声参数的问题。提示nMaxFeatures不是越大越好。这套方法每个特征每层都要做 patch 对齐耗时是特征数的线性函数而精度在特征数超过某个值之后基本不涨。板子上跑不动的时候先砍这个数再砍层数。3.3 跑数据集与看什么启动节点和回放数据是两条命令的事关键在回放速度。# 终端一启动算法节点参数文件路径在 launch 里指定 roslaunch rovio rovio.launch # 终端二回放数据。-r 0.5 表示半速 # 第一次跑一定要降速否则 CPU 跟不上会丢帧 # 丢帧的表现是轨迹突然跳一段很容易被误判成算法发散 rosbag play your_dataset.bag --clock -r 0.5节点起来之后值得盯的输出就三样话题内容用途odometry位姿 速度 协方差主输出看轨迹是否连续特征可视化当前跟踪的特征点与 patch判断特征是不是成片丢失IMU 预测位姿只有 IMU 传播、没有图像更新的位姿和最终输出对比差值就是图像更新的贡献第三样是最有价值的调试信息。如果两个位姿几乎重合说明图像更新在摸鱼——可能是噪声参数给得太大也可能是特征根本没跟上如果两者差得离谱说明某一路的时间戳或者外参有问题。把这两条轨迹同时扔进可视化工具里对比比看任何日志都直观。4. 两块核心实现IMU 预测和图像块光度更新整套算法真正的计算量集中在这两处理解了它们改代码时才知道边界在哪。4.1 IMU 预测把状态推到图像时刻预测步做两件事名义状态按 IMU 测量积分往前推协方差按线性化模型传播。名义状态是最可能的值协方差是有多不确定两者在更新步里都要用到。// 预测步骨架积分 协方差传播 void FilterState::predict(double dt, const Eigen::Vector3d acc, const Eigen::Vector3d gyr) { // 1) 去偏。零偏本身也是状态这里用的是当前估计值 Eigen::Vector3d a acc - biasAcc_; Eigen::Vector3d w gyr - biasGyro_; // 2) 名义状态积分姿态用旋转矩阵或四元数更新 // 速度受重力影响位置由速度积分 pose_.q pose_.q * expSO3(w * dt); // 右乘表示体坐标系增量 vel_ (pose_.q * a gravity_) * dt; pose_.p vel_ * dt; // 3) 协方差传播 P F P F^T Q // F 是误差状态的一阶转移矩阵Q 由噪声密度和 dt 决定 Eigen::MatrixXd F buildTransition(dt, a, w); Eigen::MatrixXd Q buildProcessNoise(dt); P_ F * P_ * F.transpose() Q; }这段代码里参数的含义值得掰开讲。dt是两次 IMU 采样之间的间隔不是两帧图像之间的间隔——图像到来时状态可能已经预测了几十次每次都用当时最新的 IMU 测量。姿态更新那里用的是右乘增量含义是在体坐标系下转了一个小角度改成左乘会导致误差状态的雅可比对不上表现出来是旋转时估计发散而平移正常很有迷惑性。Q的构造是另一个关键。它由加速度计和陀螺仪的噪声密度乘以dt离散化后是乘dt还是dt²取决于采用的离散模型组成量纲必须和状态对应。零偏的随机游走单独占一块。有人把Q全设成同一个常数跑静止数据时看着挺好一动起来就散——因为不同状态的噪声量级差了好几个数量级位置噪声和角度噪声根本不能共用一个数。buildTransition里的F是个稀疏矩阵绝大部分元素是零。手写的时候按区块填别用稠密矩阵硬乘特征是几十上百个的时候稠密乘一次的代价能把实时性吃掉。工程里常见的做法是只对非零区块做运算或者用稀疏存储。4.2 图像块对齐光度残差怎么变成滤波更新量这是整套方法区别于特征点法的核心。不比较描述子不计算重投影误差而是把参考帧上的一小块像素按当前位姿投影到当前帧逐像素比亮度。// 单个 patch 的残差计算投影 双线性插值 亮度差 double Patch::computeResidual(const Eigen::Matrix3d R_cur_ref, const Eigen::Vector3d t_cur_ref, const cv::Mat img) const { double e 0.0; for (int i 0; i N; i) { // 1) 参考块里的第 i 个像素转到归一化坐标 Eigen::Vector3d p_ref(refPix_[i].x(), refPix_[i].y(), 1.0); // 2) 变换到当前帧再投影到像素平面 Eigen::Vector3d p_cur R_cur_ref * p_ref t_cur_ref; if (p_cur.z() 0) return kInvalid; // 投到相机后面判为无效 Eigen::Vector2d px(p_cur.x() / p_cur.z(), p_cur.y() / p_cur.z()); Eigen::Vector2d uv projectWithDistortion(px); // 3) 双线性插值取当前帧亮度和参考块里存的灰度作差 double I bilinearInterp(img, uv); e I - refIntensity_[i]; } return e / N; // 取均值避免 patch 越大残差量级越大 }N是 patch 的像素数常见取 4×4 到 8×8。太小的话对面内纹理不够敏感位姿的 roll 和 pitch 估不准太大的话跨越深度不连续区域时残差会被污染边缘处的特征点尤其明显。bilinearInterp不能省用最近邻取值会让残差变成阶梯函数雅可比全是零或者跳变更新直接失效。一个 patch 只给出一个标量残差但状态是几十维的。所以还要算残差对误差状态的雅可比先对像素坐标求导图像梯度再对归一化坐标求导投影几何再对位姿求导旋转和平移最后链到误差状态。这一串链式法则在代码里是几个小函数的组合看的时候对着注释一段一段追比整体硬啃强。注意图像梯度在纹理弱的地方接近零此时雅可比整行接近零滤波器对这条观测几乎不响应。看起来像是滤波器没更新实际是数据质量的问题。判断方法很简单——把参与更新的特征点位置画出来如果全挤在纯色区域那就是特征提取的问题不是滤波的问题。4.3 特征管理什么时候加新点、什么时候删旧点特征的增删直接影响协方差矩阵的尺寸是长期运行的稳定性关键。// 特征补充的典型策略 void FeatureManager::refill(const cv::Mat img, int lvlMax) { // 1) 只在图像中心区域找新点边缘畸变大、光照差、且更容易移出视野 cv::Rect roi centerRegion(img.size(), 0.6); // 2) 角点检测阈值由参数控制 std::vectorcv::KeyPoint kps; detector_-detect(img(roi), kps); // 3) 最小间距抑制避免两个 patch 大面积重叠导致信息重复 suppressByDistance(kps, minDist_); // 4) 按当前特征在图像中的分布做配额缺哪儿补哪儿 // 新特征逆深度初值取 0方差给一个大数表示完全不确定 for (auto kp : kps) { if (count_ 1 nMaxFeatures_) break; filter_.addFeature(bearingOf(kp), /*rho*/0.0, /*sigmaRho*/1.0); } }删除的判据通常有三条连续多帧匹配失败、逆深度收敛到负值说明在相机后面几何上不成立、或者移出视野太久。前两条是硬条件第三条要小心——特征暂时移出视野但可能马上回来删早了等于把已经收敛得很好的深度信息扔了那个路标再出现时又得从头收敛。我见过最省事的做法是给每个特征一个失联计数器超过阈值才删阈值取帧率的几倍。5. 避坑与排查编译、标定、发散这三类翻车现场这套代码的报错信息普遍不友好很多问题不会直接抛异常而是表现为轨迹慢慢变差。下面几条是踩过的、且能在现象上快速定位的。5.1 编译与链接环节现象链接时报一堆undefined reference to cv::...或者编译到一半模板实例化失败。原因基本是 OpenCV 版本冲突。系统里同时存在两个大版本ROS 的桥接包链接的是一个工程里find_package找到的是另一个头文件能过链接期对不上。解决办法是把图像格式转换的代码集中到节点层一个文件里其余所有模块只用裸图像类型CMake 里显式指定 OpenCV 路径别让它自己找。现象编译通过但一启动就报参数读取失败节点直接退出。原因通常是参数文件的路径写成了相对路径而 launch 文件没设置工作目录或者参数名拼写和代码里读的键不匹配。这套工程的参数名比较长改参数文件时最稳妥的办法是把键名从代码里复制过去别手打。另一个坑是参数文件里的数值单位——噪声密度用的是连续时间形式直接填标定出来的离散方差会大一个量级表现为滤波器极度不信任图像。5.2 标定与时间同步环节现象静止时位置慢慢往下沉运动时轨迹有明显周期性摆动。下沉一般是加速度计零偏没标好重力方向被错误吸收进零偏里摆动则几乎可以锁定外参旋转不准。判断方法把 IMU 单独积分出来的轨迹和最终输出画在一起如果纯 IMU 的轨迹在几十秒内就明显漂走说明零偏或者噪声参数有问题如果纯 IMU 还行、融合之后反而变差那是外参或者时间同步的问题。解决顺序是先静态标零偏再标外参旋转最后才动噪声参数。现象完全跑不动一帧都出不来日志里没有任何报错。很多时候是时间戳问题。回放数据时如果没有加--clock参数或者节点的use_sim_time没打开图像时间戳和 IMU 时间戳对不上代码里那个时间戳必须落在 IMU 缓冲区内的判断会把每一帧都静默丢掉。这类问题最坑的地方在于它是静默的没有异常、没有警告。我的习惯是第一次跑的时候在回调里加一句日志把收到的帧数和实际处理的帧数打出来两个数字对不上就是这个问题。5.3 状态发散环节现象跑了十几秒之后轨迹突然跳一大截协方差没有明显异常。大概率是有错误观测被当成了有效观测。这套方法没有 RANSAC一个纹理重复区域的特征点被匹配到错误位置残差虽然大但因为噪声参数给得宽松还是会影响更新。短期解决办法是把图像噪声参数调小让滤波器更保守长期办法是在残差计算里加一个鲁棒核或者简单的阈值剔除——残差超过若干倍标准差直接跳过这个观测。加阈值的时候注意不要加在平均值上要加在单像素残差上否则大 patch 里有一个坏点就会把整个观测废掉。现象静止时一切正常一开始快速旋转就发散恢复静止后也回不来。旋转过程中图像模糊patch 对齐的残差曲面变平雅可比变小滤波器得到的信息是错的。同时陀螺仪在这段的高动态下如果是饱和的预测也会偏。这种情况没有好办法只能限制角速度或者提高 IMU 采样率。检查方法很直接把陀螺仪原始数据画出来看有没有削顶。提示发散之后不要立刻去调噪声参数。先把每一帧的残差均值和特征数打出来看是残差突然变大还是特征数突然变少。前者是观测质量问题后者是跟踪丢失问题两条路线的处理完全不同。6. 二次开发改参数之外第一刀该切在哪真正要在这套代码上做扩展我的经验是先从加观测入手别动预测和状态定义。原因很实际状态一改所有已经调好的噪声参数全部失效前面几天的调试成果归零这个后悔药不好吃。具体一点的路线是这样的。第一步把ImgUpdate里残差累积的那段单独抽出来做成一个策略接口输入是特征和图像输出是H、b这样你可以在旁边并联一个新的观测源——比如轮速计、比如第二个相机——而完全不碰滤波器核心。第二步新观测的雅可比先不追求精确用数值差分验证符号和量级对不对确认方向和现有观测一致之后再写解析式。第三步新增状态块的时候Q和初始协方差先给保守值大方差跑一段静止和一段匀速看新状态是不是能收敛到合理范围再逐步收紧。改动风险的分布大致是这样改动位置风险验证方式噪声参数低可回退同一段数据跑三遍看轨迹终点漂移量特征提取策略中统计每帧参与更新的特征数是否稳定新增观测残差中高先关闭原观测单独跑确认新残差本身收敛新增状态块高静态数据下新状态的估计值应收敛到标定值改预测模型极高只能靠长距离数据对比几乎等于重做有一个技巧值得单独说给滤波器加一个影子模式。主滤波器照常跑另外复制一份状态只做预测不做更新两条轨迹同时发出来。任何一次改动之后如果影子轨迹和主轨迹的差值曲线形状变了说明你的改动确实影响到了估计如果曲线完全没变那多半是改动没生效或者被别的模块吃掉了。这个方法帮我省过好几次以为改了其实没编译进去的时间。还有一点跑长数据之前先跑短的回环数据。短数据能快速暴露初始化问题——这套方法的初始化阶段对 IMU 激励有要求纯静止启动时尺度是估不出来的一跑远就开始飘。我给自己的规矩是每次改完代码先跑一段三十秒、包含一次明显平移和一次明显旋转的小数据确认轨迹闭合误差在合理范围再去碰长序列。从那以后我每次改这块代码都强制走一遍这个短序列验证宁可多花十分钟也不想在长数据跑到一半时怀疑人生。希望帮到你。本文还有配套的精品资源点击获取 ROVIO SLAM源码解析：EKF紧耦合VIO工程实践与避坑
简介面向机器人视觉惯性定位方向开发者与高校研究者的 ROVIO 源码包聚焦资源受限嵌入式平台上的实时 SLAM/VIO 实现。算法以扩展卡尔曼滤波融合单目或双目图像与 IMU 数据涵盖特征跟踪、运动与观测模型、误差状态估计及重定位优化等环节可用于无人机、移动机器人等场景的学习与二次开发。压缩包共 56 个文件、约 166KB以 hpp 头文件和 cpp 源文件为主体辅以 yaml 参数、launch 启动配置及 srv、shader 等支撑文件源码组织紧凑便于梳理滤波框架与 ROS 集成方式。已有 667 人学习或下载。读者可据此理解 ROVIO 的整体架构、状态更新流程和参数配置思路并以此为基准开展算法复现、性能分析与定制改造适合具备概率论、滤波理论和计算机视觉基础的中高级学习者。1. 一块算力吃紧的板子上为什么还会有人翻出 ROVIO 这套 SLAM 源码无人机、手持扫描、小型机器人这类场景常见的尴尬是工况要求三十帧以上的实时位姿板子却只给得起一个 ARM 核内存还得留一半给别的模块。这时候把 SLAM 里那套「提特征、算描述子、匹配、RANSAC、后端优化」的经典链路搬上去帧率掉到个位数是常事。ROVIO 走的是另一条路——单目相机加 IMU 的紧耦合 EKF把图像块的亮度误差直接当成观测残差丢进滤波不建关键帧、不做回环、不跑 BA状态量里连地图点坐标都换成了方向向量加逆深度。这套源码适合谁想在低成本硬件上跑视觉惯性里程计、又打算亲手改滤波器的人只想拿个黑盒里程计输出的装现成的包更省事。2. 模块分层一帧图像进来之后状态在哪些类之间流转读这套代码的第一道坎不是数学是找不到入口在哪、状态存在哪。它的组织方式跟 ORB-SLAM 那类关键帧架构完全不同没有 Map、没有 KeyFrame、没有 LocalMapping 线程整个系统就是一个滤波器加两个更新器IMU 更新、图像更新。所以先把类的角色分清楚后面看公式才不至于对着代码发愣。2.1 代码目录与四个核心角色工程本身是个 ROS 包加一个基于 Eigen 的位姿代数模板库作为依赖。目录大致是这么分的路径内容阅读优先级include/rovio/全部核心头文件逻辑基本都在头文件里实现高src/节点入口、ROS 消息收发、main中cfg/参数文件.info噪声、外参、金字塔层数都在这高launch/启动文件绑话题、加载参数中test/一些单元测试与标定相关的小工具低include/rovio/里角色可以归成四类理解了这四类剩下的都是细节节点层RovioNode负责订阅图像和 IMU、把 ROS 消息转成内部类型、把滤波结果再打包成nav_msgs/Odometry和可视化 Marker 发出去。它不做任何估计。滤波层FilterState有的版本叫State持有整个 EKF 的状态向量和协方差矩阵对外提供predict和update两类接口。所有数学最终都收敛到这里。更新层ImgUpdate管图像这条观测链路IMUUpdate或预测器管 IMU 的传播。两者都把残差 雅可比算好交给滤波层。几何层Camera内参、畸变、投影、Feature一个路标、Patch一个路标在某层金字塔上的图像块、Observation路标在某帧上的一次观测。这一层是纯几何和像素操作最容易单独拿出来改。2.2 状态向量怎么拼为什么路标存的是方向加逆深度这套滤波器的状态不是一个大而全的向量而是分成若干区块每个区块自己带协方差块和与其他区块的互协方差。这么做的好处是增删特征时不用重排整个协方差矩阵只动对应行列。// 状态区块的组织方式按项目结构抽取的骨架非逐字照抄 class FilterState { public: // 机器人自身状态位姿、速度、IMU 零偏 PoseBlock pose_; // IMU 在世界系下的位姿 VelocityBlock vel_; // 世界系线速度 BiasBlock biasAcc_; // 加速度计零偏 BiasBlock biasGyro_; // 陀螺仪零偏 // 外参相机相对 IMU 的变换在线估计还是固定由参数决定 ExtrinsicsBlock ext_; // q_CI b_CI // 路标每个 Feature 贡献 3 维bearing 的 2 维 逆深度 rho 1 维 std::vectorFeatureBlock features_; // 全局协方差按上面区块顺序分块存储 Eigen::MatrixXd P_; };关键点在那个rho。如果用三维坐标表示路标深度方向的方差会随着观测次数剧烈变化数值上很难收敛换成方向向量加逆深度深度不确定时rho的初值给 0对应无穷远方差给一个很大的数滤波器在前几帧几乎不动它等视差攒够了自然收敛。这也是为什么这套代码在纯旋转或静止初始化时会漂——没有视差rho就没法被约束。FeatureBlock里除了rho还有一组Patch每个Patch对应金字塔的一层存着参考块的像素灰度、像素坐标和法向。多层的意义在于高层分辨率低但收敛域大低层精度高但容易掉进局部极小。实际更新时是从粗到精逐层更新滤波器粗层先把位姿拉到大致的坑里细层再精修。2.3 一条更新链路的完整时序节点收到一帧图像后的调用顺序是理解这份代码最划算的一条线// 节点层只做数据转换和转发 void RovioNode::imgCallback(const sensor_msgs::ImageConstPtr msg) { // 1) ROS 图像转内部 cv::Mat统一成单通道灰度 cv::Mat img toGrayMat(msg); // 2) 时间戳必须落在 IMU 缓冲的时间范围内 // 否则这一帧的 IMU 预测无从做起只能丢帧 double t msg-header.stamp.toSec(); if (t imuBuffer_.getStartTime() || t imuBuffer_.getEndTime()) return; // 3) 交给图像更新器内部完成预测 观测更新 imgUpdate_.processImage(img, t); }// 更新层粗到精的逐层更新 void ImgUpdate::processImage(const cv::Mat img, double t) { // a. 先按 IMU 数据把状态传播到图像时刻得到先验与先验协方差 filter_.predictTo(t); // b. 构建图像金字塔层数由参数 nLevels 决定一般 3 到 4 层 buildPyramid(img, pyramid_, nLevels_); // c. 从最粗一层往最细一层走每层做一次 EKF 更新 for (int lvl nLevels_ - 1; lvl 0; --lvl) { // 遍历所有特征在该层的观测累加光度残差的 H 和 b for (auto feat : filter_.features()) { for (auto obs : feat.observations(lvl)) { // 投影参考块到当前帧逐像素算亮度差得到残差 e 与雅可比 J // J 再经链式法则映射到误差状态空间 accumulate(obs, lvl, H_, b_); } } // d. 一层更新完状态和协方差都变了下一层从新的先验继续 filter_.updateFromImage(H_, b_, sigmaImg_); } }这段流程里有两个容易被忽略的设计。一是每层更新完就立刻修改状态而不是把四层的残差一次性堆起来做一次大更新——这样粗层的粗结果会作为细层的先验收敛域更大但如果粗层本身被错误观测带偏细层是拉不回来的所以噪声参数的重要性远超你的直觉。二是整个更新过程可以迭代多轮代码里通常有iterations之类的计数每轮重新线性化一次本质上是在做高斯牛顿只是被包在 EKF 的框架里。2.4 时间轴与坐标系三个必须先在纸上画清的东西坐标系只有三个世界系 W、IMU 系 I、相机系 C。状态里的位姿是 I 在 W 下的位姿观测发生在 C 系所以每次算残差都要走一次T_WI * T_IC。外参T_IC要么从参数文件读固定值要么当成滤波器状态的一部分在线估。后者听起来更好实际调试时非常容易散——外参和 IMU 零偏的耦合很强标定不准的时候在线估会把误差摊到姿态上。时间轴上有三条IMU 采样时刻、图像曝光时刻、滤波器当前时刻。代码里用一个环形缓冲区保存最近一段 IMU 数据图像到来时从缓冲区里取到该时刻为止的测量做积分。这里最容易翻车的是时间戳没对齐——相机和 IMU 不是同一个时钟源时哪怕差 20 毫秒在有一定角速度的运动下也能让残差系统整体偏置表现出来就是跑着跑着轨迹往下掉。3. 编译与跑通在 catkin 工作空间里拉起一个最小节点拿到源码第一件事不是读FilterState是先让它跑出一帧轨迹。这套工程是标准的 ROS catkin 包编译本身不难难的是依赖版本和参数文件的匹配。3.1 依赖清单与编译顺序需要准备的东西ROS 基础环境roscpp、rosbag、tf、sensor_msgs 这些、OpenCV图像金字塔和插值用、Eigen3、Boost以及一个位姿代数的模板库。最后这个库是这个工程里最容易被忽略的依赖它的作用是给旋转、平移、位姿提供强类型封装避免四元数实部虚部顺序搞反这类玄学问题。# 建工作空间 mkdir -p ~/rovio_ws/src cd ~/rovio_ws/src # 位姿代数库和算法包都放在 src 下一起编译 # 注意位姿库要先于算法包被 catkin 找到 catkin_init_workspace cd ~/rovio_ws # Release 是必须的Debug 下每帧的光度残差计算会慢十倍以上 catkin_make -DCMAKE_BUILD_TYPERelease -j4 source devel/setup.bash编译时最先报错的地方通常是位姿库的头文件路径。它如果不是以 ROS 包的形式提供就得手工find_package或者在 CMakeLists 里加include_directories。另一种常见报错是 OpenCV 版本冲突系统里装了 OpenCV 4而 ROS 自带的 cv_bridge 链接的是 OpenCV 3两边模板实例化对不上报一堆undefined reference to cv::Mat::...。我一般的处理是把 cv_bridge 相关的转换代码收窄到节点层那一个文件其余部分完全不用 cv_bridge用裸cv::Mat传冲突面立刻小一半。3.2 参数文件逐项过一遍参数文件是整个系统的旋钮面板读一遍比读一千行代码更快建立直觉。核心参数分四组参数组典型命名作用调大的后果图像噪声imgUpdateNoise*光度残差的观测噪声滤波器更信 IMU图像更新变软收敛慢但抗外点IMU 噪声imuNoiseAcc/imuNoiseGyr加速度计、陀螺仪的连续时间噪声密度更信图像动态响应快但容易抖零偏随机游走imuNoiseBiasAcc/imuNoiseBiasGyr零偏自身的漂移速率零偏估计更活跃标定不准时反而更容易散结构参数nLevels/nMaxFeatures金字塔层数、最大特征数层数多收敛域大但耗时线性增长还有一组不显眼但很致命的相机内参和T_IC外参。内参错 2%视觉尺度就整体偏 2%IMU 那边会一直试图把这个偏差解释成零偏最后两个都错。外参里的旋转尤其敏感相机光轴和 IMU 某根轴不对齐时转弯过程中的残差会出现有规律的正负交替轨迹表现为周期性摆动而不是单调漂移——这个特征很好认看到就知道是外参而不是噪声参数的问题。提示nMaxFeatures不是越大越好。这套方法每个特征每层都要做 patch 对齐耗时是特征数的线性函数而精度在特征数超过某个值之后基本不涨。板子上跑不动的时候先砍这个数再砍层数。3.3 跑数据集与看什么启动节点和回放数据是两条命令的事关键在回放速度。# 终端一启动算法节点参数文件路径在 launch 里指定 roslaunch rovio rovio.launch # 终端二回放数据。-r 0.5 表示半速 # 第一次跑一定要降速否则 CPU 跟不上会丢帧 # 丢帧的表现是轨迹突然跳一段很容易被误判成算法发散 rosbag play your_dataset.bag --clock -r 0.5节点起来之后值得盯的输出就三样话题内容用途odometry位姿 速度 协方差主输出看轨迹是否连续特征可视化当前跟踪的特征点与 patch判断特征是不是成片丢失IMU 预测位姿只有 IMU 传播、没有图像更新的位姿和最终输出对比差值就是图像更新的贡献第三样是最有价值的调试信息。如果两个位姿几乎重合说明图像更新在摸鱼——可能是噪声参数给得太大也可能是特征根本没跟上如果两者差得离谱说明某一路的时间戳或者外参有问题。把这两条轨迹同时扔进可视化工具里对比比看任何日志都直观。4. 两块核心实现IMU 预测和图像块光度更新整套算法真正的计算量集中在这两处理解了它们改代码时才知道边界在哪。4.1 IMU 预测把状态推到图像时刻预测步做两件事名义状态按 IMU 测量积分往前推协方差按线性化模型传播。名义状态是最可能的值协方差是有多不确定两者在更新步里都要用到。// 预测步骨架积分 协方差传播 void FilterState::predict(double dt, const Eigen::Vector3d acc, const Eigen::Vector3d gyr) { // 1) 去偏。零偏本身也是状态这里用的是当前估计值 Eigen::Vector3d a acc - biasAcc_; Eigen::Vector3d w gyr - biasGyro_; // 2) 名义状态积分姿态用旋转矩阵或四元数更新 // 速度受重力影响位置由速度积分 pose_.q pose_.q * expSO3(w * dt); // 右乘表示体坐标系增量 vel_ (pose_.q * a gravity_) * dt; pose_.p vel_ * dt; // 3) 协方差传播 P F P F^T Q // F 是误差状态的一阶转移矩阵Q 由噪声密度和 dt 决定 Eigen::MatrixXd F buildTransition(dt, a, w); Eigen::MatrixXd Q buildProcessNoise(dt); P_ F * P_ * F.transpose() Q; }这段代码里参数的含义值得掰开讲。dt是两次 IMU 采样之间的间隔不是两帧图像之间的间隔——图像到来时状态可能已经预测了几十次每次都用当时最新的 IMU 测量。姿态更新那里用的是右乘增量含义是在体坐标系下转了一个小角度改成左乘会导致误差状态的雅可比对不上表现出来是旋转时估计发散而平移正常很有迷惑性。Q的构造是另一个关键。它由加速度计和陀螺仪的噪声密度乘以dt离散化后是乘dt还是dt²取决于采用的离散模型组成量纲必须和状态对应。零偏的随机游走单独占一块。有人把Q全设成同一个常数跑静止数据时看着挺好一动起来就散——因为不同状态的噪声量级差了好几个数量级位置噪声和角度噪声根本不能共用一个数。buildTransition里的F是个稀疏矩阵绝大部分元素是零。手写的时候按区块填别用稠密矩阵硬乘特征是几十上百个的时候稠密乘一次的代价能把实时性吃掉。工程里常见的做法是只对非零区块做运算或者用稀疏存储。4.2 图像块对齐光度残差怎么变成滤波更新量这是整套方法区别于特征点法的核心。不比较描述子不计算重投影误差而是把参考帧上的一小块像素按当前位姿投影到当前帧逐像素比亮度。// 单个 patch 的残差计算投影 双线性插值 亮度差 double Patch::computeResidual(const Eigen::Matrix3d R_cur_ref, const Eigen::Vector3d t_cur_ref, const cv::Mat img) const { double e 0.0; for (int i 0; i N; i) { // 1) 参考块里的第 i 个像素转到归一化坐标 Eigen::Vector3d p_ref(refPix_[i].x(), refPix_[i].y(), 1.0); // 2) 变换到当前帧再投影到像素平面 Eigen::Vector3d p_cur R_cur_ref * p_ref t_cur_ref; if (p_cur.z() 0) return kInvalid; // 投到相机后面判为无效 Eigen::Vector2d px(p_cur.x() / p_cur.z(), p_cur.y() / p_cur.z()); Eigen::Vector2d uv projectWithDistortion(px); // 3) 双线性插值取当前帧亮度和参考块里存的灰度作差 double I bilinearInterp(img, uv); e I - refIntensity_[i]; } return e / N; // 取均值避免 patch 越大残差量级越大 }N是 patch 的像素数常见取 4×4 到 8×8。太小的话对面内纹理不够敏感位姿的 roll 和 pitch 估不准太大的话跨越深度不连续区域时残差会被污染边缘处的特征点尤其明显。bilinearInterp不能省用最近邻取值会让残差变成阶梯函数雅可比全是零或者跳变更新直接失效。一个 patch 只给出一个标量残差但状态是几十维的。所以还要算残差对误差状态的雅可比先对像素坐标求导图像梯度再对归一化坐标求导投影几何再对位姿求导旋转和平移最后链到误差状态。这一串链式法则在代码里是几个小函数的组合看的时候对着注释一段一段追比整体硬啃强。注意图像梯度在纹理弱的地方接近零此时雅可比整行接近零滤波器对这条观测几乎不响应。看起来像是滤波器没更新实际是数据质量的问题。判断方法很简单——把参与更新的特征点位置画出来如果全挤在纯色区域那就是特征提取的问题不是滤波的问题。4.3 特征管理什么时候加新点、什么时候删旧点特征的增删直接影响协方差矩阵的尺寸是长期运行的稳定性关键。// 特征补充的典型策略 void FeatureManager::refill(const cv::Mat img, int lvlMax) { // 1) 只在图像中心区域找新点边缘畸变大、光照差、且更容易移出视野 cv::Rect roi centerRegion(img.size(), 0.6); // 2) 角点检测阈值由参数控制 std::vectorcv::KeyPoint kps; detector_-detect(img(roi), kps); // 3) 最小间距抑制避免两个 patch 大面积重叠导致信息重复 suppressByDistance(kps, minDist_); // 4) 按当前特征在图像中的分布做配额缺哪儿补哪儿 // 新特征逆深度初值取 0方差给一个大数表示完全不确定 for (auto kp : kps) { if (count_ 1 nMaxFeatures_) break; filter_.addFeature(bearingOf(kp), /*rho*/0.0, /*sigmaRho*/1.0); } }删除的判据通常有三条连续多帧匹配失败、逆深度收敛到负值说明在相机后面几何上不成立、或者移出视野太久。前两条是硬条件第三条要小心——特征暂时移出视野但可能马上回来删早了等于把已经收敛得很好的深度信息扔了那个路标再出现时又得从头收敛。我见过最省事的做法是给每个特征一个失联计数器超过阈值才删阈值取帧率的几倍。5. 避坑与排查编译、标定、发散这三类翻车现场这套代码的报错信息普遍不友好很多问题不会直接抛异常而是表现为轨迹慢慢变差。下面几条是踩过的、且能在现象上快速定位的。5.1 编译与链接环节现象链接时报一堆undefined reference to cv::...或者编译到一半模板实例化失败。原因基本是 OpenCV 版本冲突。系统里同时存在两个大版本ROS 的桥接包链接的是一个工程里find_package找到的是另一个头文件能过链接期对不上。解决办法是把图像格式转换的代码集中到节点层一个文件里其余所有模块只用裸图像类型CMake 里显式指定 OpenCV 路径别让它自己找。现象编译通过但一启动就报参数读取失败节点直接退出。原因通常是参数文件的路径写成了相对路径而 launch 文件没设置工作目录或者参数名拼写和代码里读的键不匹配。这套工程的参数名比较长改参数文件时最稳妥的办法是把键名从代码里复制过去别手打。另一个坑是参数文件里的数值单位——噪声密度用的是连续时间形式直接填标定出来的离散方差会大一个量级表现为滤波器极度不信任图像。5.2 标定与时间同步环节现象静止时位置慢慢往下沉运动时轨迹有明显周期性摆动。下沉一般是加速度计零偏没标好重力方向被错误吸收进零偏里摆动则几乎可以锁定外参旋转不准。判断方法把 IMU 单独积分出来的轨迹和最终输出画在一起如果纯 IMU 的轨迹在几十秒内就明显漂走说明零偏或者噪声参数有问题如果纯 IMU 还行、融合之后反而变差那是外参或者时间同步的问题。解决顺序是先静态标零偏再标外参旋转最后才动噪声参数。现象完全跑不动一帧都出不来日志里没有任何报错。很多时候是时间戳问题。回放数据时如果没有加--clock参数或者节点的use_sim_time没打开图像时间戳和 IMU 时间戳对不上代码里那个时间戳必须落在 IMU 缓冲区内的判断会把每一帧都静默丢掉。这类问题最坑的地方在于它是静默的没有异常、没有警告。我的习惯是第一次跑的时候在回调里加一句日志把收到的帧数和实际处理的帧数打出来两个数字对不上就是这个问题。5.3 状态发散环节现象跑了十几秒之后轨迹突然跳一大截协方差没有明显异常。大概率是有错误观测被当成了有效观测。这套方法没有 RANSAC一个纹理重复区域的特征点被匹配到错误位置残差虽然大但因为噪声参数给得宽松还是会影响更新。短期解决办法是把图像噪声参数调小让滤波器更保守长期办法是在残差计算里加一个鲁棒核或者简单的阈值剔除——残差超过若干倍标准差直接跳过这个观测。加阈值的时候注意不要加在平均值上要加在单像素残差上否则大 patch 里有一个坏点就会把整个观测废掉。现象静止时一切正常一开始快速旋转就发散恢复静止后也回不来。旋转过程中图像模糊patch 对齐的残差曲面变平雅可比变小滤波器得到的信息是错的。同时陀螺仪在这段的高动态下如果是饱和的预测也会偏。这种情况没有好办法只能限制角速度或者提高 IMU 采样率。检查方法很直接把陀螺仪原始数据画出来看有没有削顶。提示发散之后不要立刻去调噪声参数。先把每一帧的残差均值和特征数打出来看是残差突然变大还是特征数突然变少。前者是观测质量问题后者是跟踪丢失问题两条路线的处理完全不同。6. 二次开发改参数之外第一刀该切在哪真正要在这套代码上做扩展我的经验是先从加观测入手别动预测和状态定义。原因很实际状态一改所有已经调好的噪声参数全部失效前面几天的调试成果归零这个后悔药不好吃。具体一点的路线是这样的。第一步把ImgUpdate里残差累积的那段单独抽出来做成一个策略接口输入是特征和图像输出是H、b这样你可以在旁边并联一个新的观测源——比如轮速计、比如第二个相机——而完全不碰滤波器核心。第二步新观测的雅可比先不追求精确用数值差分验证符号和量级对不对确认方向和现有观测一致之后再写解析式。第三步新增状态块的时候Q和初始协方差先给保守值大方差跑一段静止和一段匀速看新状态是不是能收敛到合理范围再逐步收紧。改动风险的分布大致是这样改动位置风险验证方式噪声参数低可回退同一段数据跑三遍看轨迹终点漂移量特征提取策略中统计每帧参与更新的特征数是否稳定新增观测残差中高先关闭原观测单独跑确认新残差本身收敛新增状态块高静态数据下新状态的估计值应收敛到标定值改预测模型极高只能靠长距离数据对比几乎等于重做有一个技巧值得单独说给滤波器加一个影子模式。主滤波器照常跑另外复制一份状态只做预测不做更新两条轨迹同时发出来。任何一次改动之后如果影子轨迹和主轨迹的差值曲线形状变了说明你的改动确实影响到了估计如果曲线完全没变那多半是改动没生效或者被别的模块吃掉了。这个方法帮我省过好几次以为改了其实没编译进去的时间。还有一点跑长数据之前先跑短的回环数据。短数据能快速暴露初始化问题——这套方法的初始化阶段对 IMU 激励有要求纯静止启动时尺度是估不出来的一跑远就开始飘。我给自己的规矩是每次改完代码先跑一段三十秒、包含一次明显平移和一次明显旋转的小数据确认轨迹闭合误差在合理范围再去碰长序列。从那以后我每次改这块代码都强制走一遍这个短序列验证宁可多花十分钟也不想在长数据跑到一半时怀疑人生。希望帮到你。本文还有配套的精品资源点击获取