当前位置：首页 > java >正文

论文阅读-单目视觉惯性系统时间标定

java 2025/6/16 6:37:30

引言

已有研究在单目视觉方法上取得优异成果，能够估计出准确的相机运动与比例模糊的环境结构。为解决比例模糊问题，多传感器融合成为研究热点。尤其是将相机与IMU（惯性测量单元）结合，在实现6自由度SLAM（同时定位与建图）方面展现出卓越性能。
IMU可以提供俯仰角与比例信息，还能在视觉跟踪失败时进行补偿，从而增强系统稳定性。

IMU通常包含三轴陀螺仪和三轴加速度计，通过传感器融合（如卡尔曼滤波）可估算物体的三维姿态：

陀螺仪：测量角速度，通过积分可得到姿态变化（但存在漂移误差）。
加速度计：测量重力方向。当车辆静止或匀速运动时，重力分量在车体坐标系中的分布可直接推算出俯仰角（前后倾斜）和横滚角（左右倾斜）。
磁力计（如有）：提供航向角（偏航角）参考，辅助校准。

在视觉SLAM（如VIO视觉惯性里程计）中，单目摄像头无法直接感知环境的实际尺度（即距离的绝对值），而IMU通过加速度计提供的加速度信息（需二次积分得到位移）可以帮助恢复尺度信息。例如：
单目视觉SLAM的初始地图是“无尺度”的（比如只能确定物体A比物体B远2倍，但不知具体多少米），而IMU通过重力加速度和运动加速度的测量，可推断出实际位移的比例尺（Scale Factor），将地图从“相对比例”校正到“真实比例”。
为什么IMU能提供尺度？
IMU的加速度计测量的是实际物理加速度（单位：m/s²），通过对时间积分可得到实际位移（米），从而为视觉估计的相对运动提供尺度基准。

在进行多传感器融合时，不同传感器的时间戳必须精确同步。但在实际中，传感器时间戳常因触发延迟、传输延迟等问题而与采样时间不符，从而导致时间不同步（时间偏移）。这种时间偏移会对融合系统造成致命影响，尤其是在低成本或DIY的视觉-惯性系统中，由于缺乏硬件同步机制，时间偏移问题普遍存在。

为此，本文提出一种视觉-惯性系统中在线标定时间偏移的方法。我们将时间偏移建模为一个常数但未知的变量，并在SLAM系统中与相机与IMU状态、特征点位置一同进行在线估计。所提时间偏移标定方法是一个通用因子，可方便嵌入各种基于特征的优化框架。虽然本文使用单目视觉-惯性系统进行展示，但方法本身适用于多相机系统。主要贡献如下：

提出一种用于视觉-惯性系统中相机与IMU时间偏移的在线标定方法；
通过仿真与真实实验展示在线标定的重要性；
标定方法已集成至开源项目中。

算法

在这里插入图片描述

实验结果

在这里插入图片描述

数据集

EuRoC MAV 数据集是一个公开的、多场景、带地面真值的视觉-惯性导航评估数据集，主要用于评估 SLAM / VIO 系统的性能。它包括三个不同环境的飞行记录，每个环境下有多个子序列：

环境代号	含义	子序列名（示例）
MH	Machine Hall（工厂车间）	MH_01、MH_02、MH_03 等
V1	Vicon Room 1（室内实验）	V1_01、V1_02、V1_03 等
V2	Vicon Room 2	V2_01、V2_02

我们使用 EuRoC MAV 视觉-惯性数据集 [27] 评估所提时间标定方法对整体 VIO 性能的影响。该数据集由微型飞行器采集，包含：

立体图像（Aptina MT9V034 全局快门，WVGA，单通道，20 FPS），

IMU 数据（ADIS16448，200 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.h"Estimator estimator;  // 全局 Estimator 对象，负责 VIO 核心计算// 四个消息队列，分别缓存 IMU、特征、左/右图像
queue<sensor_msgs::ImuConstPtr> imu_buf;
queue<sensor_msgs::PointCloudConstPtr> feature_buf;
queue<sensor_msgs::ImageConstPtr> img0_buf;
queue<sensor_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，则转为 mono8）cv_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 是这个特征点在多个相机下的观测（如双目），每个观测是一个：pair<camera_id, [x, y, z, u, v, vx, vy]>map<int, vector<pair<int, Eigen::Matrix<double, 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// 像素归一化坐标 z==1是归一化保证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::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);}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] \n""for 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`	彩色图（红绿蓝顺序）
`MONO16`	16 位灰度图（如深度图）
`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

数据来源	存放位置
特征点 ID	`channels[0].values[i]`
相机 ID	`channels[1].values[i]`
像素归一化坐标 (x, y, z=1)	`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`	存储特征点真值（仿真/评估用）

查看全文

http://www.xdnf.cn/news/14169.html