广元商城网站开发,鲲鹏建设集团有限公司网站,网站关键词进前三,网络推广都有哪些平台引言
已有研究在单目视觉方法上取得优异成果#xff0c;能够估计出准确的相机运动与比例模糊的环境结构。为解决比例模糊问题#xff0c;多传感器融合成为研究热点。尤其是将相机与IMU#xff08;惯性测量单元#xff09;结合#xff0c;在实现6自由度SLAM#xff08;同…引言
已有研究在单目视觉方法上取得优异成果能够估计出准确的相机运动与比例模糊的环境结构。为解决比例模糊问题多传感器融合成为研究热点。尤其是将相机与IMU惯性测量单元结合在实现6自由度SLAM同时定位与建图方面展现出卓越性能。 IMU可以提供俯仰角与比例信息还能在视觉跟踪失败时进行补偿从而增强系统稳定性。 IMU通常包含 三轴陀螺仪 和 三轴加速度计通过传感器融合如卡尔曼滤波可估算物体的 三维姿态 陀螺仪测量角速度通过积分可得到姿态变化但存在漂移误差。加速度计测量重力方向。当车辆静止或匀速运动时重力分量在车体坐标系中的分布可直接推算出俯仰角前后倾斜和横滚角左右倾斜。磁力计如有提供航向角偏航角参考辅助校准。 在视觉SLAM如VIO视觉惯性里程计中单目摄像头无法直接感知环境的实际尺度即距离的绝对值而IMU通过加速度计提供的加速度信息需二次积分得到位移可以帮助恢复尺度信息。例如 单目视觉SLAM的初始地图是“无尺度”的比如只能确定物体A比物体B远2倍但不知具体多少米而IMU通过重力加速度和运动加速度的测量可推断出实际位移的比例尺Scale Factor将地图从“相对比例”校正到“真实比例”。 为什么IMU能提供尺度 IMU的加速度计测量的是实际物理加速度单位m/s²通过对时间积分可得到实际位移米从而为视觉估计的相对运动提供尺度基准。 在进行多传感器融合时不同传感器的时间戳必须精确同步。但在实际中传感器时间戳常因触发延迟、传输延迟等问题而与采样时间不符从而导致时间不同步时间偏移。这种时间偏移会对融合系统造成致命影响尤其是在低成本或DIY的视觉-惯性系统中由于缺乏硬件同步机制时间偏移问题普遍存在。
为此本文提出一种视觉-惯性系统中在线标定时间偏移的方法。我们将时间偏移建模为一个常数但未知的变量并在SLAM系统中与相机与IMU状态、特征点位置一同进行在线估计。所提时间偏移标定方法是一个通用因子可方便嵌入各种基于特征的优化框架。虽然本文使用单目视觉-惯性系统进行展示但方法本身适用于多相机系统。主要贡献如下 提出一种用于视觉-惯性系统中相机与IMU时间偏移的在线标定方法 通过仿真与真实实验展示在线标定的重要性 标定方法已集成至开源项目中。
相关工作
大多数视觉-惯性算法使用稀疏特征而非密集图像处理。例如[9], [10], [18]使用了“无结构视觉因子”通过将视觉残差投影至空域中消除特征点从而专注于相机或IMU的运动估计而非特征位置。而[13], [14], [16]则保留关键帧与特征点并联合优化相机运动与特征位置。
已有若干研究关注时间偏移的标定。例如Mair等人[19]提出了一种初始化方法结合互相关与相位一致性实现时空标定将待标定变量从其他未知量中分离提供良好先验。Kelly等人[20]通过对齐相机与IMU的旋转曲线并结合ICP迭代最近点方法逐步匹配完成时间偏移标定。Kalibr工具箱[21]则在连续批量优化过程中估计时间偏移、相机运动与相机-IMU间外参是一种广泛使用的离线方法。该工具依赖平面标定图案如棋盘格可获得稳健的特征跟踪与三维位置。
此外Li等人[22]在多状态约束EKF框架中提出了一种具备在线标定能力的相机-IMU运动估计方法。该方法在计算复杂度上具备优势适用于移动设备。相比之下本文基于优化的算法在精度方面更具优势因为我们可在大型优化窗中迭代更新多个变量避免过早线性化误差固定。
算法 实验结果 数据集
EuRoC MAV 数据集是一个公开的、多场景、带地面真值的视觉-惯性导航评估数据集主要用于评估 SLAM / VIO 系统的性能。它包括三个不同环境的飞行记录每个环境下有多个子序列
环境代号含义子序列名示例MHMachine Hall工厂车间MH_01、MH_02、MH_03 等V1Vicon Room 1室内实验V1_01、V1_02、V1_03 等V2Vicon Room 2V2_01、V2_02
我们使用 EuRoC MAV 视觉-惯性数据集 [27] 评估所提时间标定方法对整体 VIO 性能的影响。该数据集由微型飞行器采集包含
立体图像Aptina MT9V034 全局快门WVGA单通道20 FPS
IMU 数据ADIS16448200 Hz
地面真值由 VICON 与 Leica MS50 提供。
本实验仅使用左侧相机图像。该数据集已知相机与 IMU 精确同步。
为了评估时间偏移的影响我们人为将 IMU 时间戳整体平移±5ms 到 ±40ms构造具有不同时间偏移的测试序列。
我们首先分析偏移对基础方法VINS-Mono [23]的影响。图示结果表明VINS-Mono 对时间偏移极为敏感RMSE 在 ±6ms 以外急剧上升。而加入所提时间标定方法后RMSE 在不同时间偏移下保持稳定表明本方法能有效消除时间偏移带来的误差。
随后我们与 OKVIS [16] 方法进行对比。表格 II 展示了在多个 EuRoC 序列中设置不同偏移5ms、15ms、30ms后OKVIS 与本文方法的 RMSE 对比 总结
本文提出了一种在线标定视觉-惯性系统中相机与 IMU 之间时间偏移的方法。该方法将时间偏移视作一个待估参数联合优化相机状态、IMU 状态与特征点位置以实现高精度的时间同步。
该方法具备以下优势
通用性强可嵌入多种基于优化的 VIO 框架中
无需额外标定图案如棋盘格即可在线运行适合实际部署
实验显示其标定精度与 Kalibr 等离线工具相当甚至在稳定性和实时性方面更具优势
在 EuRoC 与实际场景测试中表现出优异的鲁棒性与轨迹精度。
虽然本文使用的是单目视觉-惯性系统但所提出的方法也适用于多相机系统。其源码已集成于 VINS-Mono 项目中便于学术与工程实践应用。
我的反思
本文提出了一种在线时间偏移标定方法用于视觉-惯性系统VIO中**相机与 IMU 数据时间不同步即时间偏移**的场景。
该方法的核心思想是
将时间偏移 _d作为一个 待优化的变量与其他状态IMU、相机位姿、特征点联合建模并优化实现在线实时校正。 关键组成模块
滑动窗口优化结构Sliding Window BA 系统采用的是滑动窗口式局部非线性优化典型特征如下 窗口中保留固定数量如7~10个的相机帧与IMU状态 对窗口内的相机位姿、IMU偏置、速度、特征点位置及时间偏移进行联合优化 窗口外的历史信息通过**边缘化Marginalization**保留为先验残差避免信息损失。 时间偏移的动态更新策略 每轮优化完成后
系统将当前优化得到的 _d 应用至下一轮图像帧时间戳 并继续估计剩余时间差 _进行下一轮修正 即通过“边运行边补偿”逐步将大偏移收敛到零。 该机制保证即便初始偏移高达数百毫秒也可从粗到精逐步矫正。 所以说虽然视觉因子的公式看起来“只动了图像点”但优化目标函数中IMU 是等权参与者且正是因为 IMU 与视觉的张力才使得 _能被精确估计。
代码阅读
代码结构
VINS-Fusion-master/ ├── .gitignore ├── LICENCE ├── README.md ├── camera_models/ ├── config/ │ ├── A3_ptgrey/ │ ├── euroc/ │ ├── extrinsic_parameter_example.pdf │ ├── fisheye_mask.jpg │ ├── fisheye_mask_752x480.jpg │ ├── kitti_odom/ │ ├── kitti_raw/ │ ├── mynteye/ │ ├── realsense_d435i/ │ ├── simulation/ │ ├── vi_car/ │ └── vins_rviz_config.rviz ├── docker/ ├── global_fusion/ ├── loop_fusion/ ├── support_files/ └── vins_estimator/ ├── CMakeLists.txt ├── cmake/ ├── launch/ ├── package.xml └── src/ ├── KITTIGPSTest.cpp ├── KITTIOdomTest.cpp ├── estimator/ │ ├── estimator.cpp │ ├── estimator.h │ ├── feature_manager.cpp │ ├── feature_manager.h │ ├── parameters.cpp │ └── parameters.h ├── factor/ ├── featureTracker/ ├── initial/ ├── rosNodeTest.cpp └── utility/
camera_models
这个模块的主要功能是提供多种相机模型的实现包括
标准针孔相机模型带畸变的针孔相机模型鱼眼相机模型等距和 Scaramuzza 模型等距投影相机模型
提供相机标定功能
支持使用棋盘格进行相机标定提供标定示例代码使用 Google Ceres 求解器进行优化
提供相机模型的基本操作
3D点投影到图像平面图像平面点反投影到3D空间支持不同相机模型的参数优化 这个模块是 VINS-Fusion 中处理相机模型和标定的重要组件为整个系统提供了准确的相机模型支持这对于视觉SLAM系统的精度至关重要。 config
config/ 目录保存了 VINS-Fusion 运行时所需的全部配置文件主要分为三类 ① 各类相机硬件Point Grey、Mynt Eye、Realsense D435i 等的标定与运行参数 ② 针对公开数据集EuRoC、KITTI、仿真等的参数模板 ③ 通用辅助文件外参说明、鱼眼掩码、RViz 可视化配置。 config/ ├── A3_ptgrey/ # Point Grey 相机 │ ├── left.yaml / right.yaml # 左/右相机内参 │ ├── a3_ptgrey_mono_imu_config.yaml # 单目IMU 运行参数 │ ├── a3_ptgrey_stereo_config.yaml # 双目无 IMU │ ├── a3_ptgrey_stereo_imu_config.yaml # 双目IMU常用 │ └── a3_ptgrey_stereo_imu_config_backup.yaml # 备份 │ ├── euroc/ # EuRoC MAV 数据集 │ ├── cam0_mei.yaml / cam1_mei.yaml # Mei 鱼眼模型内参 │ ├── cam0_pinhole.yaml / cam1_pinhole.yaml # Pinhole 模型内参 │ ├── euroc_mono_imu_config.yaml # 单目IMU │ ├── euroc_stereo_config.yaml # 双目无 IMU │ └── euroc_stereo_imu_config.yaml # 双目IMU官方推荐 │ ├── kitti_odom/ # KITTI 里程计序列 (00-21) │ ├── cam00-02.yaml / cam03.yaml … # 不同序列相机内参 │ ├── kitti_config00-02.yaml # 00-02 号序列完整配置 │ ├── kitti_config03.yaml │ ├── kitti_config04-12.yaml │ └── kitti_config13-21.yaml # 针对每段序列微调的外参 │ ├── kitti_raw/ # KITTI 原始(raw)数据 │ ├── cam_09_30.yaml / cam_10_03.yaml # 不同采集日的相机内参 │ ├── kitti_09_30_config.yaml │ └── kitti_10_03_config.yaml # 对应完整运行配置 │ ├── mynteye/ # Mynt Eye S1030 │ ├── left_mei.yaml / right_mei.yaml # 左右鱼眼内参 │ ├── mynteye_mono_imu_config.yaml │ ├── mynteye_stereo_config.yaml │ └── mynteye_stereo_imu_config.yaml │ ├── realsense_d435i/ # Intel Realsense D435i │ ├── left.yaml / right.yaml # 将深度摄像头分左右使用 │ ├── realsense_stereo_imu_config.yaml # D435i 双目IMU │ └── rs_camera.launch # ROS 驱动启动文件 │ ├── simulation/ # Gazebo/Unity 等仿真场景 │ ├── cam0_mei.yaml / cam1_mei.yaml # 虚拟鱼眼相机内参 │ └── simulation_config.yaml # 仿真环境总体配置 │ ├── vi_car/ # 车载双目IMU自采集 │ ├── cam0_mei.yaml / cam1_mei.yaml │ ├── cam0_pinhole.yaml / cam1_pinhole.yaml # 同一套图像提供两种模型 │ └── vi_car.yaml # 系统运行参数 │ ├── extrinsic_parameter_example.pdf # 如何测量/书写外参的说明文档 ├── fisheye_mask.jpg # 3680×2760 鱼眼黑边掩码 ├── fisheye_mask_752x480.jpg # EuRoC 尺寸的鱼眼掩码 └── vins_rviz_config.rviz # RViz 预设布局话题、颜色等
global_fusion
global_fusion/ 实现了“GPS 辅助的全局位姿图优化”。 它将 VINS 局部里程计、回环信息与 GPS/RTK 绝对定位联合优化输出长时间无漂移的全局轨迹并自带可视化模型。
global_fusion/ ―― GPS-VIO 全局优化模块ROS 包 ├── CMakeLists.txt # 构建配置依赖 ceres、Eigen、roslib 等 ├── package.xml # ROS 元信息与依赖声明 ├── ThirdParty/ │ └── GeographicLib/ # 嵌入式地理坐标库精简版 │ ├── include/Config.h … # WGS-84 常量、坐标变换 │ └── src/Math.cpp … # 纯头/源文件免去系统安装 ├── models/ # 可视化 3D 模型 │ ├── car.dae # 车辆 mesh用于 RViz 显示 │ └── hummingbird.mesh # 无人机 mesh └── src/ # 核心源码 ├── Factors.h # ceres 残差项定义GPS/姿态/平滑因子等 ├── globalOpt.h / .cpp # GlobalOptimization 类 │ · 维护滑窗 关键帧 │ · 构建 ceres 问题融合 GPS 与 VIO ├── globalOptNode.cpp # ROS 节点入口 │ · 订阅 /vins_estimator/odometry 与 /gps │ · 调用 GlobalOptimization 更新 求解 │ · 发布 /global_odometry、/global_path │ · 在 RViz 中加载 car.dae 模型 └── tic_toc.h # 微型计时器宏统计运行耗时
loop_fusion
loop_fusion/ ―― 回环检测 位姿图优化 ├── CMakeLists.txt # 构建配置 ├── package.xml # ROS 依赖声明 ├── cmake/ # 辅助 CMake 脚本 └── src/ ├── parameters.h # 运行参数话题名、滑窗大小等 ├── keyframe.h / .cpp # 关键帧数据结构与管理 ├── pose_graph.h / .cpp # 回环位姿图、优化求解CERES ├── pose_graph_node.cpp # ROS 节点入口订阅 VIO、发布闭环后轨迹 ├── utility/ # 可视化、计时器、通用工具 │ ├── utility.h / .cpp │ ├── CameraPoseVisualization.* # RViz 相机位姿显示 │ └── tic_toc.h # 计时宏 └── ThirdParty/ # 第三方词袋库 DBoW2 ├── DBoW/ DUtils/ DVision/ # BoW 词向量、匹配/检索实现 └── VocabularyBinary.* # 词典二进制序列化 作用利用 ORB/BRIEF 描述子 DBoW2 做回环检测将回环约束加入位姿图后用 Ceres 进行全局优化显著降低累计漂移。
vins_estimator
vins_estimator/ ―― VINS-Fusion 核心 VIO 估计器 ├── CMakeLists.txt | package.xml | cmake/ | launch/ └── src/ ├── KITTIGPSTest.cpp / KITTIOdomTest.cpp # 跑 KITTI 的演示程序 ├── rosNodeTest.cpp # 在线 ROS 版本 demo │ ├── estimator/ # VIO 核心 │ ├── estimator.h / .cpp # Sliding-Window BA、滑动窗口管理 │ ├── feature_manager.* # 管理视差/三角化的特征 │ └── parameters.* # 全局参数读取 │ ├── factor/ # Ceres 残差块 │ ├── imu_factor.* # IMU 预积分误差 │ ├── projection_factor. # 重投影误差单目/双目/两帧 │ ├── marginalization_factor.* # 边缘化实现 │ └── integration_base.h # IMU 预积分基类 │ ├── featureTracker/ # 前端特征追踪 │ ├── feature_tracker.* # KLT 畸变校正 │ ├── initial/ # 系统初始化 │ ├── initial_sfm.* # SFM 结构初始化 │ ├── initial_alignment.* # 重力/尺度/速度对齐 │ ├── initial_ex_rotation.* # 外参旋转粗估 │ └── solve_5pts.* # 五点法求解 │ └── utility/ # 工具 可视化 ├── utility.* # 日志、转换函数 ├── tic_toc.h # 计时器 ├── visualization.* # RViz 可视化 └── CameraPoseVisualization.* # 相机姿态模型 作用读取相机/IMU 数据 → KLT 追踪 → 滑窗优化视觉重投影 IMU 预积分→ 输出高频无漂移里程计该模块也为 loop_fusion / global_fusion 提供里程计输入。
先从主函数开始
#include stdio.h
#include queue
#include map
#include thread
#include mutex
#include ros/ros.h
#include cv_bridge/cv_bridge.h
#include opencv2/opencv.hpp
#include estimator/estimator.h
#include estimator/parameters.h
#include utility/visualization.hEstimator estimator; // 全局 Estimator 对象负责 VIO 核心计算// 四个消息队列分别缓存 IMU、特征、左/右图像
queuesensor_msgs::ImuConstPtr imu_buf;
queuesensor_msgs::PointCloudConstPtr feature_buf;
queuesensor_msgs::ImageConstPtr img0_buf;
queuesensor_msgs::ImageConstPtr img1_buf;
std::mutex m_buf; // 互斥锁// – 将左相机图像sensor_msgs::ImageConstPtr推入 img0_buf 队列
void img0_callback(const sensor_msgs::ImageConstPtr img_msg)
{m_buf.lock();img0_buf.push(img_msg);m_buf.unlock();
}// – 将右相机图像如果启用双目推入 img1_buf 队列。
void img1_callback(const sensor_msgs::ImageConstPtr img_msg)
{m_buf.lock();img1_buf.push(img_msg);m_buf.unlock();
}// 将 ROS 图像消息转换为 OpenCV 的 cv::Mat若编码为 8UC1则转为 mono8cv_bridge 对图像编码有一定要求。如果 ROS 消息的 encoding 是 8UC1非标准命名cv_bridge::toCvCopy() 会报错因为它不识别 8UC1。
cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr img_msg)
{cv_bridge::CvImageConstPtr ptr;if (img_msg-encoding 8UC1){sensor_msgs::Image img;img.header img_msg-header;img.height img_msg-height;img.width img_msg-width;img.is_bigendian img_msg-is_bigendian;img.step img_msg-step;img.data img_msg-data;img.encoding mono8;ptr cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);}elseptr cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);cv::Mat img ptr-image.clone();return img;
}// extract images with same timestamp from two topics
// 从图像队列中同步提取图像帧
void sync_process()
{while(1){if(STEREO) // 双目{cv::Mat image0, image1;std_msgs::Header header;double time 0;m_buf.lock();if (!img0_buf.empty() !img1_buf.empty()){// 获取前端图像的时间戳double time0 img0_buf.front()-header.stamp.toSec();double time1 img1_buf.front()-header.stamp.toSec();// 0.003s sync tolerance// 表示左图像比右图像早太多差 3ms抛弃左图。if(time0 time1 - 0.003){img0_buf.pop();printf(throw img0\n);}// 表示右图比左图早太多抛弃右图。else if(time0 time1 0.003){img1_buf.pop();printf(throw img1\n);}// 时间戳差在容忍范围内认为是“匹配图像对”else{time img0_buf.front()-header.stamp.toSec();header img0_buf.front()-header;image0 getImageFromMsg(img0_buf.front());img0_buf.pop();image1 getImageFromMsg(img1_buf.front());img1_buf.pop();//printf(find img0 and img1\n);}}m_buf.unlock();if(!image0.empty())// 送入后端估计器处理estimator.inputImage(time, image0, image1);}// 单目else{cv::Mat image;std_msgs::Header header;double time 0;m_buf.lock();if(!img0_buf.empty()){time img0_buf.front()-header.stamp.toSec();header img0_buf.front()-header;image getImageFromMsg(img0_buf.front());img0_buf.pop();}m_buf.unlock();if(!image.empty())estimator.inputImage(time, image);}// 让当前线程休眠 2 毫秒避免高频空转 CPU。 std::chrono::milliseconds dura(2);std::this_thread::sleep_for(dura);}
}// 将接收到的 IMU 数据加速度 角速度提取出来并传递给前端 VIO 系统的 estimator 处理。
void imu_callback(const sensor_msgs::ImuConstPtr imu_msg)
{double t imu_msg-header.stamp.toSec();double dx imu_msg-linear_acceleration.x;double dy imu_msg-linear_acceleration.y;double dz imu_msg-linear_acceleration.z;double rx imu_msg-angular_velocity.x;double ry imu_msg-angular_velocity.y;double rz imu_msg-angular_velocity.z;Vector3d acc(dx, dy, dz);Vector3d gyr(rx, ry, rz);estimator.inputIMU(t, acc, gyr);return;
}// 用于接收视觉前端提取的特征点消息带ID 像素 速度 深度并将其封装后送入后端 estimator 做 VIO 优化。
void feature_callback(const sensor_msgs::PointCloudConstPtr feature_msg)
{
// 定义一个 featureFrame 容器
// key 是 feature_id
// value 是这个特征点在多个相机下的观测如双目每个观测是一个paircamera_id, [x, y, z, u, v, vx, vy]mapint, vectorpairint, Eigen::Matrixdouble, 7, 1 featureFrame;for (unsigned int i 0; i feature_msg-points.size(); i){int feature_id feature_msg-channels[0].values[i]; // 特征点idint camera_id feature_msg-channels[1].values[i]; // 相机id// 像素归一化坐标 z1是归一化保证double x feature_msg-points[i].x;double y feature_msg-points[i].y;double z feature_msg-points[i].z;// 像素坐标double p_u feature_msg-channels[2].values[i];double p_v feature_msg-channels[3].values[i];// 像素速度光流double velocity_x feature_msg-channels[4].values[i];double velocity_y feature_msg-channels[5].values[i];if(feature_msg-channels.size() 5){// 特征点真值坐标double gx feature_msg-channels[6].values[i];double gy feature_msg-channels[7].values[i];double gz feature_msg-channels[8].values[i];pts_gt[feature_id] Eigen::Vector3d(gx, gy, gz); // 存入全局真值点表//printf(receive pts gt %d %f %f %f\n, feature_id, gx, gy, gz);}// 用于确保该特征的深度是 归一化坐标也即从图像点 (u, v) 投影成 (x, y, 1)ROS_ASSERT(z 1);Eigen::Matrixdouble, 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);}double t feature_msg-header.stamp.toSec();// 把该帧所有特征点的信息featureFrame 时间戳 t送入后端优化模块 estimatorestimator.inputFeature(t, featureFrame);return;
}// 重启SLAM/VIO系统
void restart_callback(const std_msgs::BoolConstPtr restart_msg)
{if (restart_msg-data true){ROS_WARN(restart the estimator!);estimator.clearState();estimator.setParameter();}return;
}// 开启/关闭 IMU 使用
void imu_switch_callback(const std_msgs::BoolConstPtr switch_msg)
{if (switch_msg-data true){//ROS_WARN(use IMU!);estimator.changeSensorType(1, STEREO);}else{//ROS_WARN(disable IMU!);estimator.changeSensorType(0, STEREO);}return;
}// 切换相机模式单目/双目
void cam_switch_callback(const std_msgs::BoolConstPtr switch_msg)
{if (switch_msg-data true){//ROS_WARN(use stereo!);estimator.changeSensorType(USE_IMU, 1);}else{//ROS_WARN(use mono camera (left)!);estimator.changeSensorType(USE_IMU, 0);}return;
}int main(int argc, char **argv)
{ // 初始化 ROS 节点名为 vins_estimator
// 使用私有命名空间 NodeHandle~用于加载配置参数等。ros::init(argc, argv, vins_estimator);ros::NodeHandle n(~);// 设置日志级别为 Inforos::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);if(argc ! 2){printf(please intput: rosrun vins vins_node [config file] \nfor example: rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml \n);return 1;}// 将配置文件路径传入 readParameters()读取 YAML 中的传感器参数、话题名等。string config_file argv[1];printf(config_file: %s\n, argv[1]);readParameters(config_file);// 将 readParameters 中加载好的配置传入 Estimator 类设置内部的滑窗大小、预积分结构、相机参数等。estimator.setParameter();#ifdef EIGEN_DONT_PARALLELIZEROS_DEBUG(EIGEN_DONT_PARALLELIZE);
#endifROS_WARN(waiting for image and imu...);// 调用 registerPub(n) 注册轨迹发布器如 /vins/odometry, /vins/path, /vins/pose_graph。registerPub(n);ros::Subscriber sub_imu;if(USE_IMU){sub_imu n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());}// 订阅前端特征跟踪器的输出类型是 sensor_msgs/PointCloudros::Subscriber sub_feature n.subscribe(/feature_tracker/feature, 2000, feature_callback);// 图像左目或单目的订阅。ros::Subscriber sub_img0 n.subscribe(IMAGE0_TOPIC, 100, img0_callback);ros::Subscriber sub_img1;// 若为双目配置再订阅右目图像。if(STEREO){sub_img1 n.subscribe(IMAGE1_TOPIC, 100, img1_callback);}// 支持运行时通过布尔话题控制系统行为重启、切换 IMU、切换相机模式ros::Subscriber sub_restart n.subscribe(/vins_restart, 100, restart_callback);ros::Subscriber sub_imu_switch n.subscribe(/vins_imu_switch, 100, imu_switch_callback);ros::Subscriber sub_cam_switch n.subscribe(/vins_cam_switch, 100, cam_switch_callback);// 启动图像同步处理线程新开一个线程执行 sync_process()
// 从图像队列同步图像帧
// 送入 estimator.inputImage(...)
// 与 ROS 回调线程并行执行std::thread sync_thread{sync_process};// ROS 阻塞式事件循环
// 负责触发所有话题回调如 imu_callback, feature_callback 等
// 一直运行直到节点关闭ros::spin();return 0;
}opencv常见编码格式
编码类型含义MONO8单通道灰度图每像素 8 位BGR8彩色图OpenCV 默认顺序RGB8彩色图红绿蓝顺序MONO1616 位灰度图如深度图TYPE_32FC1浮点型图OpenCV 32位浮点
代码解析 ┌──────────────────────┐│ ros::spin() │ ← IMU / 图像 / 特征 回调线程└──────┬───────────────┘│
┌────────────▼────────────┐
│ callback 函数 │
│imu_callback/img_callback│
└────────────┬────────────┘│ push msg
┌────────────▼────────────┐
│ 消息缓冲区 │ ← 带锁访问
└────────────┬────────────┘│┌───────▼────────┐│ sync_process() │ ← 独立线程└───────┬────────┘│┌───────▼────────┐│ estimator │└────────────────┘void feature_callback(const sensor_msgs::PointCloudConstPtr feature_msg)是 ROS 中订阅 sensor_msgs::PointCloud 话题的回调函数。 feature_msg 存储了一个特征点集合一帧图像中的多个点每个点带有 点 ID、相机 ID 图像坐标 (u, v) 归一化坐标 (x, y, z)其中 z 1 是归一化保证 像素速度光流 (vx, vy) 可选特征点真值坐标 gx, gy, gz
数据来源存放位置特征点 IDchannels[0].values[i]相机 IDchannels[1].values[i]像素归一化坐标 (x, y, z1)points[i].x/y/z像素坐标 (u, v)channels[2/3].values[i]像素速度 (vx, vy)channels[4/5].values[i]可选GT 坐标channels[6/7/8].values[i]
模块功能feature_msg包含每个特征点的归一化坐标、像素坐标、ID、相机ID、速度等featureFrame按照 feature_id 组织的观测结构可支持多相机inputFeature()将所有特征数据传入后端优化器pts_gt存储特征点真值仿真/评估用
