其他文章

1. VINS_MONO视觉导航算法【一】基础知识介绍
2. VINS_MONO视觉导航算法【二】论文讲解+GPU实现调研
3. VINS_MONO视觉导航算法【三】ROS基础知识介绍
4. VINS_MONO视觉导航算法【四】VINS_Mono代码解释

说明

这是第四篇，这一部分主要讲解VINS_Mono的代码，但是由于内容太多了，建议从参考文献中找到那几个逐行解释的github仓库，逐行学习，这里不一一讲解了。此外，本文还介绍了VINS_Mono中常见的配置文件、数据结构和OpenCV函数的解释。

代码解析

综述

总体布局

在正常运行过程中，初始化只进行一次。前端负责不断地提取特征点发给后端；后端负责IMU数据采集，预积分和优化/滑窗等操作。前端和后端在运行过程中不断地循环。

这个系统包括以下几个过程：特征提取与发布；IMU提取与预积分；初始化；滑窗与优化；回环检测。

代码主要包括3个node：feature_tracker，vins_estimator，pose_graph，其中feature_tracker仅负责特征点提取和发布，pose_graph关键帧的选择/位姿图建立/回环检测，而vins_estimator的内容最多了，包含了初始化，滑窗，优化等后端内容和IMU预积分等前端内容，并且在这个结点里分出了2个线程。

一般来讲，一个标准的SLAM系统是前后端分离的，如下图所示，

而VINS系统，他的前端仅包括光流计算；初始化等工作都放在vins_estimator里面了。

我的理解它之所以这么做，是因为IMU的数据获取如果放在前端，预积分完了之后还需要advertise到ROS系统里，让后端接收，与其这样，还不如让后端直接接收IMU数据就在后端处理了，减少通信过程。

配置文件

euroc.launch

1、设置局部变量"config_path"，表示euroc_config.yaml的具体地址；

<arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" >

	  <arg name="vins_path" default = "$(find feature_tracker)/../config/../" >

设置局部变量"vins_path"，在parameters.cpp中使用鱼眼相机mask中用到：

std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");

if (FISHEYE == 1)

        FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg";

2、启动"feature_tracker"节点，在该节点中需要读取参数文件，地址为config_file，即config_path；

<node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">

    <param name="config_file" type="string" value="$(arg config_path)" >

    <param name="vins_folder" type="string" value="$(arg vins_path)" >

</node>

3、启动"vins_estimator"节点，内容同上；

<node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">

   <param name="config_file" type="string" value="$(arg config_path)" >

   <param name="vins_folder" type="string" value="$(arg vins_path)" >

</node>

4、启动"pose_graph"节点，除了参数配置文件的地址外还设置了4个参数：

<node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">

    <param name="config_file" type="string" value="$(arg config_path)" >

    <param name="visualization_shift_x" type="int" value="0" >

    <param name="visualization_shift_y" type="int" value="0" >

    <param name="skip_cnt" type="int" value="0" >

    <param name="skip_dis" type="double" value="0" >

</node>

visualization_shift_x和visualization_shift_y

visualization_shift_x和visualization_shift_y表示在进行位姿图优化后，对得到的位姿在x坐标和y坐标的偏移量（一般都设为0）；

它们用于控制位姿图（pose graph）在可视化工具中的显示位置。具体来说，这两个参数允许你在 x 和 y 轴方向上对位姿图进行平移，以便更好地适应可视化窗口或与其他数据对齐。

// 这里声明了一个 geometry_msgs::PoseStamped 类型的对象 pose_stamped。PoseStamped 是一个包含时间戳和坐标系信息的位姿消息类型。

geometry_msgs::PoseStamped pose_stamped;

// 设置时间戳，ros::Time(cur_kf->time_stamp) 将当前关键帧的时间戳 cur_kf->time_stamp 转换为 ROS 时间戳格式。

pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);

// 设置坐标系

pose_stamped.header.frame_id = "world";

// 设置位姿的位置

// P.x() 是从某个位姿对象 P 中获取的 x 坐标。VISUALIZATION_SHIFT_X 是一个常量，表示在 x 方向上的偏移量。通常情况下，这个值设为 0，表示没有偏移。

pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;

pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;

pose_stamped.pose.position.z = P.z();

skip_cnt

skip_cnt在pose_graph_node的process()中，表示每隔skip_cnt个图像帧才进行一次处理；

用途：

减少处理频率：通过设置 skip_cnt，可以减少处理图像帧的频率。这对于计算资源有限的系统特别有用，可以减轻计算负担，提高系统的整体性能。

避免冗余数据：连续的图像帧之间变化不大，处理每一帧可能会引入冗余数据。通过跳过一些帧，可以减少冗余，使位姿图更加简洁和有效。

        if (skip_cnt < SKIP_CNT)

        {

            skip_cnt++;

            continue;

        }

        else

        {

            skip_cnt = 0;

        }

skip_dis也在pose_graph_node的process()中，目的是将距上一帧的时间间隔超过SKIP_DIS的图像创建为位姿图中的关键帧。

        if((T - last_t).norm() > SKIP_DIS)

        {

        }

用途：

选择关键帧：通过设置 skip_dis，可以控制哪些图像帧被选为关键帧。只有当当前帧与上一个关键帧的时间间隔超过 skip_dis 时，当前帧才会被创建为新的关键帧。

保持时间间隔：确保关键帧之间有一定的时间间隔，避免过于密集的关键帧，从而使位姿图更加合理和有效。

euroc_config.yaml

文件读取

从launch启动文件中可以看到，每个节点都会读取参数，例如在feature_traker_node.cpp中的main()通过以下函数实现参数读取：

void readParameters(ros::NodeHandle &n)

{

    std::string config_file;
    
    config_file = readParam<std::string>(n, "config_file");
    
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    
    if(!fsSettings.isOpened())
    
    {
    
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    
    }
    
    //…

std::string config_file;

config_file = readParam<std::string>(n, "config_file");

cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);

if(!fsSettings.isOpened())

{

    std::cerr << "ERROR: Wrong path to settings" << std::endl;

}

//…

}

下面将具体介绍euroc_config.yaml每个参数的具体意义。

通用参数

1）接收IMU和图像的topic，其中image_topic在节点/fature_tracker中被订阅，以进行角点的光流跟踪；imu_topic在节点/vins_estimator中被订阅，以进行IMU预积分。

2）output_path为输出文件的地址，输出以下内容：VINS的运行轨迹"/vins_result_no_loop.csv"与"/vins_result_loop.csv"，相机与IMU的外参估计 “/extrinsic_parameter.csv”（如果estimate_extrinsic不为0即需要对外参进行估计的话）。注意如果该文件夹不存在则不输出。

imu_topic: "/imu0"

image_topic: "/cam0/image_raw"

output_path: "/home/tony-ws1/output/"

相机的基础信息

包括：

相机模型：PINHOLE（针孔相机）、KANNALA_BRANDT（一种常用的鱼眼相机模型）等。

图像的宽和高、相机的畸变系数（如果是鱼眼相机还要求mu、mv、u0、v0）、相机的内参。

相机的内参数据建议进行标定以获得较准确结果，以降低之后调参的难度。

model_type: PINHOLE

camera_name: camera

image_width: 752

image_height: 480

distortion_parameters:

   k1: -2.917e-01

   k2: 8.228e-02

   p1: 5.333e-05

   p2: -1.578e-04

projection_parameters:

   fx: 4.616e+02

   fy: 4.603e+02

   cx: 3.630e+02

   cy: 2.481e+02

相机内参（Intrinsic Camera Parameters），用于描述相机的内部几何特性。它们在计算机视觉和机器人视觉中非常重要，特别是在进行图像校正、三维重建、视觉里程计（VO）和同时定位与地图构建（SLAM）等任务时。下面是这些参数的具体解释：

应用：

图像校正：

使用内参矩阵可以将图像从相机坐标系转换到像素坐标系，从而进行图像校正和去畸变处理。

三维重建：

在三维重建中，内参矩阵用于将二维图像特征点反投影到三维空间中，构建点云或三维模型。

视觉里程计（VO）：

内参矩阵用于将特征点的像素坐标转换为相机坐标系下的三维坐标，从而估计相机的运动。

同时定位与地图构建（SLAM）：

内参矩阵在 SLAM 系统中用于处理视觉特征点，构建地图和估计相机位姿。

imu和相机之间的外参

参数解释

这里需要注意的是其旋转矩阵ric、平移向量tic都是表示从camera坐标系到IMU坐标系的变换。

estimate_extrinsic：=0表示这外参已经是准确的了，之后不需要优化；=1表示外参只是一个估计值，后续还需要将其作为初始值放入非线性优化中；=2表示不知道外参，需要进行标定，在estimator.cpp的函数processImage()中调用了CalibrationExRotation()进行外参标定。

个人在实验时最直观的感受是：这个外参对VINS输出的精度和鲁棒性具有非常大的影响！尤其是初始化阶段，如果外参不准会对IMU的预积分和图像的SFM对齐以得到的尺度有很大偏差。强烈建议进行手眼标定，具体可以使用Kalibr工具进行IMU和相机的离线外参标定。

# Extrinsic parameter between IMU and Camera.

estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.

                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
    
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.        
                        
                        #If you choose 0 or 1, you should write down the following matrix.

#Rotation from camera frame to imu frame, imu^R_cam

extrinsicRotation: !!opencv-matrix

   rows: 3

   cols: 3

   dt: d

   data: [0.0148655429818, -0.999880929698, 0.00414029679422,

           0.999557249008, 0.0149672133247, 0.025715529948, 
    
           -0.0257744366974, 0.00375618835797, 0.999660727178]

#Translation from camera frame to imu frame, imu^T_cam

extrinsicTranslation: !!opencv-matrix

   rows: 3

   cols: 1

   dt: d

   data: [-0.0216401454975,-0.064676986768, 0.00981073058949]

IMU（惯性测量单元）和相机之间的外参（Extrinsic Parameters）用于描述这两个传感器之间的相对位置和姿态关系。这些参数在多传感器融合、视觉惯性里程计（VIO）、SLAM（同时定位与地图构建）等应用中非常重要。下面是对这些参数的详细解释及其用途：

外参定义

estimate_extrinsic：

含义：指示系统对外参的处理方式。

取值：

0：已经准确知道外参，信任提供的外参矩阵，不会对其进行优化。

1：有一个初始估计的外参，系统会在该初始估计的基础上进行优化。

2：完全不知道外参，系统会尝试从数据中自动校准外参。在这种情况下，不需要提供外参矩阵，但需要在开始时做一些旋转运动以帮助校准。

extrinsicRotation (imu^R_cam)：

含义：从相机坐标系到IMU坐标系的旋转矩阵。

格式：3x3 矩阵。

示例值：

extrinsicRotation: !!opencv-matrix

   rows: 3

   cols: 3

   dt: d

   data: [0.0148655429818, -0.999880929698, 0.00414029679422,

          0.999557249008, 0.0149672133247, 0.025715529948, 
    
          -0.0257744366974, 0.00375618835797, 0.999660727178]

extrinsicTranslation (imu^T_cam)：

含义：从相机坐标系到IMU坐标系的平移向量。

格式：3x1 矩阵。

示例值：

extrinsicTranslation: !!opencv-matrix

   rows: 3

   cols: 1

   dt: d

   data: [-0.0216401454975, -0.064676986768, 0.00981073058949]

###### 

外参的作用

坐标系转换：

旋转矩阵 imu^R_cam 和 平移向量 imu^T_cam 用于将相机坐标系中的点转换到IMU坐标系中。

公式：

多传感器融合：

在视觉惯性里程计（VIO）和SLAM中，IMU和相机的数据通常需要融合在一起。外参用于将视觉特征点和IMU数据对齐，从而实现更准确的定位和姿态估计。

例如：在VIO中，视觉特征点的位姿可以通过IMU的加速度和角速度数据进行修正，外参确保这两组数据在同一个坐标系下进行融合。

校准：

如果选择了 estimate_extrinsic 为 1 或 2，系统会根据提供的初始估计或从数据中自动校准外参，以提高系统的精度。

初始估计：提供一个接近真实的外参初始值，可以帮助系统更快地收敛到正确的外参。

自动校准：系统会通过优化算法从数据中自动估计外参，但这通常需要更多的数据和更长的时间。

示例

假设你有一个点Pc = [1 2 3]T在相机坐标系中，使用提供的外参将其转换到IMU坐标系中：

Eigen::Matrix3d R_cam_imu;

R_cam_imu << 0.0148655429818, -0.999880929698, 0.00414029679422,

             0.999557249008, 0.0149672133247, 0.025715529948, 
    
            -0.0257744366974, 0.00375618835797, 0.999660727178;

Eigen::Vector3d T_cam_imu(-0.0216401454975, -0.064676986768, 0.00981073058949);

Eigen::Vector3d P_c(1, 2, 3);

Eigen::Vector3d P_i = R_cam_imu * P_c + T_cam_imu;

std::cout << "Point in IMU frame: " << P_i.transpose() << std::endl;

###### 

总结

IMU和相机之间的外参用于描述这两个传感器之间的相对位置和姿态关系。这些参数在多传感器融合、视觉惯性里程计（VIO）、SLAM等应用中至关重要，确保不同传感器的数据能够在同一个坐标系下进行有效的处理和融合。通过合理设置 estimate_extrinsic 参数，可以选择合适的校准方法，提高系统的精度和鲁棒性。

在节点/feature_traker中需要用到的参数

#feature traker paprameters

max_cnt: 150            # max feature number in feature tracking

min_dist: 30            # min distance between two features 

freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 

F_threshold: 1.0        # ransac threshold (pixel)

show_track: 1           # publish tracking image as topic

equalize: 1             # if image is too dark or light, trun on equalize to find enough features

fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

###### 

max_cnt

max_cnt为进行特征光流跟踪时保持的特征点数量，具体在FeatureTracker::readImage()，通过当前帧成功跟踪的特征点数，计算是否需要提取新的特征点，如果需要则用过goodFeaturesToTrack()提取。

max_cnt值增大在一定条件下会提高跟踪的鲁棒性（但在环境特征本来就不丰富的地方反而会提取不够鲁棒的特征点），但是会增加之后所有算法的运算时间，例如在我调试时取250已经无法达到实时性。

int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());

if (n_max_cnt > 0)

        {
    
            //...
    
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
    
        }

min_dist

min_dist为两个相邻特征之间像素的最小间隔，目的是保证图像中均匀的特征分布。在FeatureTracker::setMask()中应用，该函数的作用是对跟踪点进行排序并去除密集点。

// 使用范围基的 for 循环遍历 cnt_pts_id 容器中的每一个元素。it 是一个引用，指向 cnt_pts_id 中的一个元素。

for (auto &it : cnt_pts_id)

{

// mask 是一个 OpenCV 的 Mat 对象，通常用于存储二值图像（掩码）。it.second.first 是一个 cv::Point 类型的点坐标。mask.at<uchar>(it.second.first) 获取 mask 图像中对应点的像素值。如果该点的像素值为 255（白色），则条件成立。

    if (mask.at<uchar>(it.second.first) == 255)

    {

        // forw_pts：存储符合条件的点坐标。

        forw_pts.push_back(it.second.first); 

        // ids：存储这些点的 ID。

        ids.push_back(it.second.second); 

        // track_cnt：存储这些点的跟踪计数（或索引）。

        track_cnt.push_back(it.first);

        // cv::circle(mask, it.second.first, MIN_DIST, 0, -1); 在 mask 图像中以 it.second.first 为中心，半径为 MIN_DIST 的圆区域内填充黑色（像素值为 0）。-1 表示填充整个圆区域，而不是画一个空心圆。

        cv::circle(mask, it.second.first, MIN_DIST, 0, -1);

    }

}

min_dist的实现原理是在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面便不再选取该区域内的点。

cv::circle(mask, it.second.first, MIN_DIST, 0, -1);

这个参数取大一些，比如30，可以保证图像的特征点分布非常均匀，有利于图像全局的光流跟踪；如果在特征明显的场合下可以适当取小一些，保证跟踪的特征都聚集在特征明显的地方。

freq

freq控制图像跟踪的发布频率。在回调函数img_callback()中应用，通过判断间隔时间内的发布次数控制发布频率。一般根据相机的运行速度以及其他参数调完后的实时运行情况进行调整。freq 参数决定了图像跟踪结果的发布频率。例如，如果 freq 设置为 10 Hz，那么系统每秒最多发布 10 次跟踪结果。如果 freq 设置为 0，发布频率将与原始图像的帧率相同，即每次接收到新的图像帧时都会发布一次跟踪结果。

// pub_count：已经发布的帧数。

// img_msg->header.stamp.toSec()：当前图像帧的时间戳（秒）。

// first_image_time：第一个图像帧的时间戳（秒）。

// 1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)：计算当前帧的平均发布频率。

// round(...)：将计算结果四舍五入到最接近的整数。

// 如果当前帧的平均发布频率小于或等于设定的 FREQ，则进入 if 语句块。

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{

    // 设置 PUB_THIS_FRAME 为 true，表示当前帧将被发布。
    
        PUB_THIS_FRAME = true;
    
        // 检查发布频率是否接近设定频率
    
        // 计算当前帧的平均发布频率与设定频率 FREQ 的差值。
    
        // 0.01 * FREQ：允许的最大误差，即设定频率的 1%。
    
        // 如果差值小于允许的最大误差，进入 if 语句块。
    
        if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
    
        {
    
            // 重置时间和计数器
    
            // 更新 first_image_time 为当前图像帧的时间戳。
    
            // 将 pub_count 重置为 0，重新开始计数。
    
            first_image_time = img_msg->header.stamp.toSec();
    
            pub_count = 0;
    
        }
    
    }

else

        PUB_THIS_FRAME = false; // 如果不满足条件，不发布当前帧

这段代码的主要功能是：

计算当前帧的平均发布频率：通过 pub_count 和当前帧与第一帧的时间差来计算。

比较当前帧的发布频率与设定频率：如果当前帧的平均发布频率小于或等于设定频率 FREQ，则决定发布当前帧。

检查发布频率是否接近设定频率：如果当前帧的平均发布频率与设定频率的差值小于允许的最大误差（1%），则重置时间和计数器。

控制发布频率：通过上述逻辑，确保图像跟踪结果的发布频率接近于设定的 FREQ。

实时性：

较高的发布频率（如 10 Hz 或更高）可以提高系统的实时性，使跟踪结果更加及时和准确。

较低的发布频率可以降低系统的计算负担，适用于计算资源有限的环境。

性能平衡：

freq 参数需要根据相机的运行速度、系统的计算能力以及其他参数进行调整，以达到性能和实时性的平衡。

如果相机的帧率非常高，但系统的计算能力有限，可以适当降低 freq，以避免系统过载。

F_threshold

F_threshold为ransac算法的门限值，在FeatureTracker::rejectWithF()通过计算基本矩阵去除图像特征跟踪的外点时使用，一般不修改。

cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);

// un_cur_pts：当前帧的特征点。

// un_forw_pts：下一帧的特征点。

// cv::FM_RANSAC：使用 RANSAC 算法估计基本矩阵。

// F_THRESHOLD：RANSAC 算法的门限值，用于区分内点和外点。

// 0.99：置信度，表示算法找到最优解的概率。

// status：输出数组，用于存储每个点的状态（内点或外点）。

// un_cur_pts (Undistorted Current Points)

// un_forw_pts (Undistorted Forward Points)

show_track

show_track：将经过特征跟踪的图像进行发布，需要。

equalize

equalize: 如果图像整体太暗或者太亮则需要进行直方图均衡化，在FeatureTracker::readImage()中进行自适应直方图均衡化，需要。

下面这段代码用于在图像处理过程中根据需要进行直方图均衡化，特别是自适应直方图均衡化（CLAHE）。直方图均衡化是一种图像处理技术，用于增强图像的对比度，使得图像的亮度分布更加均匀。

if (EQUALIZE)

{

    // cv::createCLAHE 函数创建一个 CLAHE（Contrast Limited Adaptive Histogram Equalization）对象。
    
    // 3.0 是对比度限制（clip limit），用于限制直方图均衡化的强度。较高的值会导致更强的对比度增强。
    
    // cv::Size(8, 8) 是网格大小（tile grid size），表示将图像分成多少个小块进行局部直方图均衡化。
    
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
    
        // TicToc 是一个简单的计时器类，用于测量代码块的执行时间。t_c 是计时器对象。
    
        TicToc t_c;
    
        // 应用 CLAHE，clahe->apply(_img, img) 将 CLAHE 应用于输入图像 _img，并将结果存储在 img 中。
    
        clahe->apply(_img, img);
    
        // 记录时间
    
        // t_c.toc() 返回从计时器启动到当前的时间（以毫秒为单位）。
    
        // ROS_DEBUG 是 ROS 中的调试日志宏，用于输出调试信息。这里输出 CLAHE 处理所花费的时间。
    
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    
    }
    
    else
    
        img = _img;  // 不进行直方图均衡化

// cv::createCLAHE 函数创建一个 CLAHE（Contrast Limited Adaptive Histogram Equalization）对象。

// 3.0 是对比度限制（clip limit），用于限制直方图均衡化的强度。较高的值会导致更强的对比度增强。

// cv::Size(8, 8) 是网格大小（tile grid size），表示将图像分成多少个小块进行局部直方图均衡化。

    cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));

    // TicToc 是一个简单的计时器类，用于测量代码块的执行时间。t_c 是计时器对象。

    TicToc t_c;

    // 应用 CLAHE，clahe->apply(_img, img) 将 CLAHE 应用于输入图像 _img，并将结果存储在 img 中。

    clahe->apply(_img, img);

    // 记录时间

    // t_c.toc() 返回从计时器启动到当前的时间（以毫秒为单位）。

    // ROS_DEBUG 是 ROS 中的调试日志宏，用于输出调试信息。这里输出 CLAHE 处理所花费的时间。

    ROS_DEBUG("CLAHE costs: %fms", t_c.toc());

}

else

    img = _img;  // 不进行直方图均衡化

fisheye

fisheye：鱼眼相机一般需要圆形的mask，以去除外部噪声点。mask图在config文件夹中。

在节点/vins_estimator中需要用到的参数

#optimization parameters

max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time

max_num_iterations: 8   # max solver itrations, to guarantee real time

keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

max_solver_time和max_num_iterations分别定义了ceres优化器的最大迭代时间和最大迭代次数，以保证实时性。在Estimator::optimization()中使用，并根据不同的边缘化方案而有改变。

options.linear_solver_type = ceres::DENSE_SCHUR;

options.trust_region_strategy_type = ceres::DOGLEG;

options.max_num_iterations = NUM_ITERATIONS;

if (marginalization_flag == MARGIN_OLD)

	options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;

else

	options.max_solver_time_in_seconds = SOLVER_TIME;

keyframe_parallax 是一个用于选择关键帧的阈值参数。它定义了在选择关键帧时，特征点在不同帧之间的视差（parallax）必须达到的最小值。视差是指同一个特征点在不同帧之间的位置差异，通常用于衡量相机的运动。keyframe_parallax定义了关键帧的选择阈值。在FeatureManager::addFeatureCheckParallax()中通过计算每一个点跟踪次数和它在次新帧和次次新帧间的视差确定是否是关键帧。这个参数影响着算法中关键帧的个数。

其中keyframe_parallax为像素坐标系，MIN_PARALLAX为归一化相机坐标系。MIN_PARALLAX 是归一化相机坐标系中的视差阈值。归一化相机坐标系是指将像素坐标系中的点转换到相机坐标系中，消除了焦距的影响，使得视差值更具可比性。

// 参数初始化

//从配置文件中读取 keyframe_parallax 的值。

MIN_PARALLAX = fsSettings["keyframe_parallax"];

//将 keyframe_parallax 从像素坐标系转换到归一化相机坐标系。FOCAL_LENGTH 是相机的焦距。

MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;

// 遍历特征点

// 遍历所有特征点。feature 是一个存储特征点的容器，每个特征点包含其在不同帧中的位置信息。

    for (auto &it_per_id : feature)

{

    // 检查特征点的时间范围
    
    // it_per_id.start_frame：特征点首次出现的帧号。
    
    // frame_count：当前帧号。
    
    // it_per_id.feature_per_frame.size()：特征点在不同帧中出现的次数。
    
    //这个条件检查特征点是否在当前帧之前至少存在两帧，并且在次新帧（frame_count - 1）中仍然存在。
    
        if (it_per_id.start_frame <= frame_count - 2 &&
    
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
    
        {
    
            // 计算视差
    
            // compensatedParallax2(it_per_id, frame_count)：计算特征点在次新帧和次次新帧之间的视差。
    
            // parallax_sum：累加所有符合条件的特征点的视差。
    
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
    
            // parallax_num：统计符合条件的特征点数量。
    
            parallax_num++;
    
        }

}

// 检查是否有符合条件的特征点

    if (parallax_num == 0)

{

    // 如果没有任何符合条件的特征点，返回 true，表示当前帧应该被选为关键帧。
    
        return true;

}

// 计算平均视差并判断

    else

{

    // 计算所有符合条件的特征点的平均视差。
    
    // 如果平均视差大于或等于 MIN_PARALLAX，返回 true，表示当前帧应该被选为关键帧；否则返回 false。
    
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    
    }

这段代码的主要功能是：

遍历特征点：检查每个特征点是否在当前帧之前至少存在两帧，并且在次新帧中仍然存在。

计算视差：计算符合条件的特征点在次新帧和次次新帧之间的视差。

统计视差：累加所有符合条件的特征点的视差，并统计符合条件的特征点数量。

判断是否为关键帧：

如果没有任何符合条件的特征点，当前帧被选为关键帧。

如果有符合条件的特征点，计算平均视差，如果平均视差大于或等于 MIN_PARALLAX，当前帧被选为关键帧。

IMU参数

#imu parameters       The more accurate parameters you provide, the better performance

acc_n: 0.08          # accelerometer measurement noise standard deviation. #0.2   0.04

gyr_n: 0.004         # gyroscope measurement noise standard deviation.     #0.05  0.004

acc_w: 0.00004         # accelerometer bias random work noise standard deviation.  #0.02

gyr_w: 2.0e-6       # gyroscope bias random work noise standard deviation.     #4.0e-5

g_norm: 9.81007     # gravity magnitude

包括加速度计和陀螺仪测量值的噪声标准差和随机偏置的导数标准差，以及重力加速度大小。因为在进行IMU预积分时我们假设传感器的噪声noise服从高斯分布，偏置bias的导数服从高斯分布。

这部分的参数对结果的精度有一定影响，建议读取IMU的datasheet中具体参数或者进行IMU标定直接确定，以降低之后调参的难度。

闭环用到的参数

#loop closure parameters

loop_closure: 1                    # start loop closure

load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'

fast_relocalization: 0             # useful in real-time and large project

pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path

load_previous_pose_graph 为新功能：地图重用，通过载入先前的位姿图文件实现。

fast_relocalization常用于高实时性或者地图非常大、持续时间长的地方，得到的结果精度会降低。一般实验时先不打开。

在线时差校准

这是VINS的新功能：因为大多数相机和IMU不是时间同步的，所以将estimate_td设置为1可以在线估计相机和IMU之间的时间差。

参数td在Estimator::optimization()里放在视觉残差函数中进行非线性优化。

#unsynchronization parameters

estimate_td: 0                      # online estimate time offset between camera and imu

td: 0.0                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

##### 

支持卷帘相机。

参数说明

这也是VINS的新功能：将rolling_shutter设置为0表示全局曝光相机；设置为1表示卷帘曝光相机，此时需要从传感器的datasheet中得到rolling_shutter_tr的值。用于补偿卷帘曝光相机在不同行读取时间上的差异。不要使用网络摄像头webcam，不适合用。

#rolling shutter parameters

rolling_shutter: 0                  # 0: global shutter camera, 1: rolling shutter camera

rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet).

以上两个功能具体在以下代码中实现：

pts_i_td表示处理时间同步误差和Rolling shutter时间后，角点在归一化平面的坐标。

TR / ROW * row_i就是相机 rolling 到这一行时所用的时间

pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;

pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;

变量说明

pts_i 和 pts_j：特征点在归一化平面上的初始坐标。

pts_i_td 和 pts_j_td：经过时间同步误差和卷帘曝光时间补偿后的特征点坐标。

td：当前帧的时间戳。

td_i 和 td_j：特征点 i 和 j 的时间戳。

TR：卷帘曝光相机的读出时间（rolling_shutter_tr）。

ROW：相机的行数。

row_i 和 row_j：特征点 i 和 j 所在的行号。

velocity_i 和 velocity_j：特征点 i 和 j 的运动速度。

代码逻辑

时间同步误差补偿：

(td - td_i)：当前帧的时间戳与特征点 i 的时间戳之差。

(td - td_j)：当前帧的时间戳与特征点 j 的时间戳之差。

卷帘曝光时间补偿：

TR / ROW * row_i：特征点 i 所在行的读取时间。卷帘曝光相机从顶部到底部逐行读取图像，每行的读取时间是 TR / ROW。

TR / ROW * row_j：特征点 j 所在行的读取时间。

总时间补偿：

(td - td_i + TR / ROW * row_i)：特征点 i 的总时间补偿。

(td - td_j + TR / ROW * row_j)：特征点 j 的总时间补偿。

坐标修正：

pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i：根据总时间补偿和特征点的速度，修正特征点 i 的坐标。

pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j：根据总时间补偿和特征点的速度，修正特征点 j 的坐标。

功能总结

这段代码的主要功能是：

时间同步误差补偿：通过当前帧的时间戳与特征点的时间戳之差，补偿时间同步误差。

卷帘曝光时间补偿：对于卷帘曝光相机，根据特征点所在行的读取时间，进一步补偿时间误差。

坐标修正：结合时间补偿和特征点的速度，修正特征点在归一化平面上的坐标。

应用场景

卷帘曝光相机：

卷帘曝光相机逐行读取图像，不同行的读取时间不同，这会导致运动物体在图像中的位置偏差。通过补偿卷帘曝光时间，可以减少这种偏差，提高特征点匹配的准确性。

时间同步：

在多传感器融合和视觉里程计（VO）、SLAM 等任务中，时间同步是非常重要的。通过补偿时间同步误差，可以确保不同传感器数据的一致性。

注意事项

不要使用网络摄像头（webcam）：网络摄像头通常不支持精确的时间戳和卷帘曝光时间的获取，因此不适合用于高精度的视觉处理任务。

从数据手册获取 rolling_shutter_tr：确保从相机传感器的数据手册中获取准确的卷帘曝光读出时间，以保证补偿的准确性。

RVIZ可视化参数

#visualization parameters

save_image: 1                   # 保存在位姿图中的图像，为0则关闭

visualize_imu_forward: 0        # 输出IMU前向递推的角度预测值，一般用于低延迟和高频率的应用要求下

visualize_camera_size: 0.4      # 在RVIZ显示中相机模型的大小

其中visualize_imu_forward在pose_graph_node中的imu_forward_callback()即imu前向递推的回调函数中用到，如果为1则接收IMU前向递推的角度预测值，经回环的偏移矫正并转换为世界坐标系下的相机姿态，发布；为0则什么都不做，不发布。

整个文件

%YAML:1.0

#common parameters

imu_topic: "/imu0"

image_topic: "/cam0/image_raw"

output_path: "/home/shaozu/output/"

#camera calibration 

model_type: PINHOLE

camera_name: camera

image_width: 752

image_height: 480

distortion_parameters:

   k1: -2.917e-01

   k2: 8.228e-02

   p1: 5.333e-05

   p2: -1.578e-04

projection_parameters:

   fx: 4.616e+02

   fy: 4.603e+02

   cx: 3.630e+02

   cy: 2.481e+02

# Extrinsic parameter between IMU and Camera.

estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.

                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
    
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        

#If you choose 0 or 1, you should write down the following matrix.

#Rotation from camera frame to imu frame, imu^R_cam

extrinsicRotation: !!opencv-matrix

   rows: 3

   cols: 3

   dt: d

   data: [0.0148655429818, -0.999880929698, 0.00414029679422,

           0.999557249008, 0.0149672133247, 0.025715529948, 
    
           -0.0257744366974, 0.00375618835797, 0.999660727178]

#Translation from camera frame to imu frame, imu^T_cam

extrinsicTranslation: !!opencv-matrix

   rows: 3

   cols: 1

   dt: d

   data: [-0.0216401454975,-0.064676986768, 0.00981073058949]

#feature traker paprameters

max_cnt: 150            # max feature number in feature tracking

min_dist: 30            # min distance between two features 

freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 

F_threshold: 1.0        # ransac threshold (pixel)

show_track: 1           # publish tracking image as topic

equalize: 1             # if image is too dark or light, trun on equalize to find enough features

fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#optimization parameters

max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time

max_num_iterations: 8   # max solver itrations, to guarantee real time

keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance

acc_n: 0.08          # accelerometer measurement noise standard deviation. #0.2   0.04

gyr_n: 0.004         # gyroscope measurement noise standard deviation.     #0.05  0.004

acc_w: 0.00004         # accelerometer bias random work noise standard deviation.  #0.02

gyr_w: 2.0e-6       # gyroscope bias random work noise standard deviation.     #4.0e-5

g_norm: 9.81007     # gravity magnitude

#loop closure parameters

loop_closure: 1                    # start loop closure

load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'

fast_relocalization: 0             # useful in real-time and large project

pose_graph_save_path: "/home/shaozu/output/pose_graph/" # save and load path

#unsynchronization parameters

estimate_td: 0                      # online estimate time offset between camera and imu

td: 0.0                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#rolling shutter parameters

rolling_shutter: 0                  # 0: global shutter camera, 1: rolling shutter camera

rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

#visualization parameters

save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results

visualize_camera_size: 0.4      # size of camera marker in RVIZ

feature_tracker

简介

feature_tracker是vins的前端，它的目录在/feature_tracker下，功能主要是获取摄像头的图像帧，并按照事先设定的频率，把cur帧上满足要求的特征点以sensor_msg::PointCloudPtr的格式发布出去，以便RVIZ和vins-estimator接收。

这个package下面主要包括4个功能：

feature_tracker——包含特征提取/光流追踪的所有算法函数；

feature_tracker_node——特征提取的主入口，负责一个特征处理结点的功能；

parameters——负责读取来自配置文件的参数；

tic_tok——计时器；

注意，还有package.xml和CmakeLists.txt上面定义了所有的外部依赖和可执行文件。流程图如下：

feature_trackers可被认为是一个单独的模块

输入：图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”

输出：

1、发布topic：“/feature_trackers/feature_img”

即跟踪的特征点图像，主要是之后给RVIZ用和调试用。

2、发布topic：“/feature_trackers/feature”

即跟踪的特征点信息，由/vins_estimator订阅并进行优化。

3、发布topic：“/feature_trackers/restart”

即判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅。

代码结构

VINS对于视觉跟踪相关的代码在feature_trackers文件夹中。该文件夹内src的文件有：

feature_trackers.cpp/feature_trackers.h：构建feature_trackers类，实现特征点跟踪的函数。

函数	功能
bool inBorder	判断跟踪的特征点是否在图像边界内
void reduceVector	去除无法跟踪的特征点
void FeatureTracker::setMask	对跟踪点进行排序并去除密集点
void FeatureTracker::addPoints	添将新检测到的特征点n_pts，ID初始化-1，跟踪次数1
void FeatureTracker::readImage	对图像使用光流法进行特征点跟踪
void FeatureTracker::rejectWithF	通过F矩阵去除outliers
bool FeatureTracker::updateID	更新特征点id
void FeatureTracker::readIntrinsicParameter	读取相机内参
void FeatureTracker::showUndistortion	显示去畸变矫正后的特征点
void FeatureTracker::undistortedPoints	对特征点的图像坐标去畸变矫正，并计算每个角点的速度

feature_trackers_node.cpp：main()函数 ROS的程序入口。

函数	功能
void img_callback()	ROS的回调函数，对新来的图像进行特征点追踪、处理和发布
int main()	程序入口

程序入口

parameters.cpp/parameters.h：实现参数的读取和设置。

feature_tracker_node

这段代码是一个 ROS 节点，用于处理图像流并提取特征点。它使用多个摄像头（由 NUM_OF_CAM 定义）进行特征点跟踪，并发布特征点的三维位置和相关属性。

图像回调函数：处理接收到的图像消息，进行特征点提取和跟踪。

频率控制：确保图像处理的频率符合设定的频率 FREQ。

特征点提取和跟踪：使用多个摄像头进行特征点提取和跟踪。

特征点发布：将提取的特征点及其属性发布为点云消息。

图像显示：显示特征点跟踪的结果图像。

vins_estimator

文件夹结构

• factor：
  • imu_factor.h：IMU残差、雅可比
  • intergration_base.h：IMU预积分
  • marginalization.cpp/.h：边缘化
  • pose_local_parameterization.cpp/.h：局部参数化
  • projection_factor.cpp/.h：视觉残差
• initial：
  • initial_alignment.cpp/.h：视觉和IMU校准(陀螺仪偏置、尺度、重力加速度和速度)
  • initial_ex_rotation.cpp/.h：相机和IMU外参标定
  • initial_sfm.cpp/.h：纯视觉SFM、三角化、PNP
  • solve_5pts.cpp/.h：5点法求基本矩阵得到Rt
• utility：
  • CameraPoseVisualization.cpp/.h：相机位姿可视化
  • tic_toc.h：记录时间
  • utility.cpp/.h：各种四元数、矩阵转换
• visualization.cpp/.h：各种数据发布
• estimator.cpp/.h：紧耦合的VIO状态估计器实现
• estimator_node.cpp：ROS 节点函数，回调函数
• feature_manager.cpp/.h：特征点管理，三角化，关键帧等
• parameters.cpp/.h：读取参数

流程图

其中论文中对于关键帧的选择（论文IV A部分）：

两个关键帧选择标准：

1、与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。

2、跟踪质量。如果跟踪的特征数量低于某一阈值，则将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。

具体在bool FeatureManager::addFeatureCheckParallax()中实现。

输入：

1、IMU的角速度和线加速度，即订阅了IMU发布的topic：IMU_TOPIC="/imu0"

2、图像追踪的特征点，即订阅了feature_trackers模块发布的topic：“/feature_tracker/feature"

3、复位信号，即订阅了feature_trackers模块发布的topic：“/feature_tracker/restart"

4、重定位的匹配点，即订阅了pose_graph模块发布的topic：“/pose_graph/match_points"

输出：

1、在线程void process()中给RVIZ发送里程计信息PQV、关键点三维坐标、相机位姿、点云信息、IMU到相机的外参、重定位位姿等。

pubOdometry(estimator, header);//"odometry"

pubKeyPoses(estimator, header);//"key_poses"

pubCameraPose(estimator, header);//"camera_pose"

pubPointCloud(estimator, header);//"history_cloud"

pubTF(estimator, header);//"extrinsic"

pubKeyframe(estimator);//"keyframe_point"、"keyframe_pose"

if (relo_msg != NULL)

	pubRelocalization(estimator);//"relo_relative_pose"

2、在回调函数void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)发布最新的由IMU直接递推得到的PQV。

if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)

            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);//"imu_propagate"

数据结构

std_msgs/Header

std_msgs/Header 是一个通用的消息头，通常包含在各种传感器数据结构中，用于提供关于消息的基本信息。

定义

uint32 seq       # 序列ID：连续增加的ID

time stamp       # 时间戳

                 # stamp.sec: 自纪元以来的秒数
    
                 # stamp.nsec: 自 stamp.sec 以来的纳秒数

string frame_id  # 坐标系ID

字段解释

seq (uint32)：

序列ID：一个连续增加的整数，用于标识消息的顺序。每个消息都有一个唯一的序列号，有助于在接收端进行消息排序和同步。

stamp (time)：

时间戳：表示消息生成的时间。

stamp.sec：自纪元（1970年1月1日00:00:00 UTC）以来的秒数。

stamp.nsec：自 stamp.sec 以来的纳秒数。这提供了更高的时间精度。

frame_id (string)：

坐标系ID：表示消息所属的坐标系。这对于多传感器系统尤其重要，因为它帮助确定数据的参考框架。

sensor_msgs/Image

sensor_msgs/Image 是用于表示图像数据的消息类型，通常用于传输相机捕获的图像。

定义

std_msgs/Header header      # 头信息

uint32 height              # 图像高度，行数

uint32 width               # 图像宽度，列数

string encoding            # 像素编码 —— 通道意义、顺序、大小

                          # 取自 include/sensor_msgs/image_encodings.h 中的字符串列表

uint8 is_bigendian        # 大端（big endian）或小端（little endian）

uint32 step                # 每行的总长度（以字节为单位）

uint8[] data               # 实际的图像数据，大小为 (step * rows)

字段解释

header (std_msgs/Header)：

头信息：包含消息的基本信息，如序列号、时间戳和坐标系ID。具体字段已在上面解释。

height (uint32)：

图像高度：图像的行数，即垂直方向上的像素数。

width (uint32)：

图像宽度：图像的列数，即水平方向上的像素数。

encoding (string)：

像素编码：表示图像数据的编码格式。常见的编码格式包括 mono8（单通道8位灰度图像）、bgr8（三通道8位BGR图像）、rgb8（三通道8位RGB图像）等。具体的编码格式定义在 include/sensor_msgs/image_encodings.h 文件中。

is_bigendian (uint8)：

字节序：表示图像数据的字节序。值为1表示大端（big endian），值为0表示小端（little endian）。大多数现代计算机系统使用小端字节序。

step (uint32)：

行步长：表示每行图像数据的总长度（以字节为单位）。这通常等于图像宽度乘以每个像素的字节数，但在某些情况下可能更大（例如，为了对齐内存）。

data (uint8[])：

图像数据：实际的图像数据，以一维数组的形式存储。数组的大小为 step * height 字节。每个元素是一个字节，表示一个像素的一个通道值。

示例代码

假设你在 feature_trackers_node.cpp 中有一个回调函数 img_callback，用于处理接收到的图像数据。以下是一个示例代码：

#include <ros/ros.h>

#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>

#include <opencv2/opencv.hpp>

void img_callback(const sensor_msgs::ImageConstPtr& msg)

{

    try
    
    {
    
        // 将ROS图像消息转换为OpenCV图像
    
        cv::Mat img = cv_bridge::toCvShare(msg, "bgr8")->image;
    
        // 打印图像的基本信息
    
        std::cout << "Image received: " << std::endl;
    
        std::cout << "Header: " << msg->header << std::endl;
    
        std::cout << "Height: " << msg->height << std::endl;
    
        std::cout << "Width: " << msg->width << std::endl;
    
        std::cout << "Encoding: " << msg->encoding << std::endl;
    
        std::cout << "Is bigendian: " << (int)msg->is_bigendian << std::endl;
    
        std::cout << "Step: " << msg->step << std::endl;
    
        // 显示图像
    
        cv::imshow("Received Image", img);
    
        cv::waitKey(1);
    
    }
    
    catch (cv_bridge::Exception& e)
    
    {
    
        ROS_ERROR("Could not convert from [%s] to [bgr8].", msg->encoding.c_str());
    
    }

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "image_listener");
    
    ros::NodeHandle nh;
    
    // 订阅图像话题
    
    ros::Subscriber sub = nh.subscribe("camera/image_raw", 1, img_callback);
    
    ros::spin();
    
    return 0;

}

详细解释

订阅图像话题：

使用 ros::Subscriber 订阅图像话题 camera/image_raw，并将回调函数设置为 img_callback。

回调函数 img_callback：

接收 sensor_msgs::ImageConstPtr 类型的消息。

使用 cv_bridge 将 ROS 图像消息转换为 OpenCV 图像。

打印图像的基本信息，包括头信息、高度、宽度、编码、字节序和行步长。

使用 cv::imshow 显示图像，并使用 cv::waitKey 等待用户按键。

sensor_msgs::PointCloud

sensor_msgs::PointCloud 是一个用于表示3D点云数据的消息类型，常用于存储和传输特征点信息。

定义

std_msgs/Header header      # 头信息

geometry_msgs/Point32[] points  # 3D点 (x, y, z)

sensor_msgs/ChannelFloat32[] channels  # 附加通道信息，如特征点的ID、像素坐标u,v、速度vx,vy等

字段解释

header (std_msgs/Header)：

头信息：包含消息的基本信息，如序列号、时间戳和坐标系ID。具体字段已在前面解释。

points (geometry_msgs/Point32[])：

3D点：表示每个特征点的3D坐标 (x, y, z)。geometry_msgs/Point32 结构定义如下：

float32 x

float32 y

float32 z

channels (sensor_msgs/ChannelFloat32[])：

附加通道信息：用于存储每个特征点的附加信息，如特征点的ID、像素坐标 (u, v)、速度 (vx, vy) 等。sensor_msgs/ChannelFloat32 结构定义如下：

string name # 通道名称

float32[] values # 通道值

示例代码

假设你在 feature_trackers.cpp 中有一个回调函数 img_callback，用于处理接收到的图像数据，并创建和封装 feature_points。然后在 main() 函数中发布这些特征点信息。

feature_trackers.cpp

#include <ros/ros.h>

#include <sensor_msgs/Image.h>

#include <sensor_msgs/PointCloud.h>

#include <sensor_msgs/ChannelFloat32.h>

#include <cv_bridge/cv_bridge.h>

#include <geometry_msgs/Point32.h>

#include <opencv2/opencv.hpp>

// 回调函数，处理接收到的图像数据

void img_callback(const sensor_msgs::ImageConstPtr& img_msg)

{

    try
    
    {
    
        // 将ROS图像消息转换为OpenCV图像
    
        cv::Mat img = cv_bridge::toCvShare(img_msg, "bgr8")->image;
    
        // 检测特征点
    
        std::vector<cv::Point2f> cur_pts;
    
        cv::goodFeaturesToTrack(img, cur_pts, 100, 0.01, 10);
    
        // 创建 PointCloud 消息
    
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
    
        feature_points->header = img_msg->header;
    
        feature_points->header.frame_id = "world";  // 设置坐标系ID
    
        // 添加3D点和通道信息
    
        for (const auto &pt : cur_pts)
    
        {
    
            geometry_msgs::Point32 point;
    
            point.x = pt.x;
    
            point.y = pt.y;
    
            point.z = 0.0;  // 假设z坐标为0
    
            feature_points->points.push_back(point);
    
            sensor_msgs::ChannelFloat32 id_channel;
    
            id_channel.name = "id";
    
            id_channel.values.push_back(static_cast<float>(feature_points->points.size() - 1));
    
            sensor_msgs::ChannelFloat32 u_channel;
    
            u_channel.name = "u";
    
            u_channel.values.push_back(pt.x);
    
            sensor_msgs::ChannelFloat32 v_channel;
    
            v_channel.name = "v";
    
            v_channel.values.push_back(pt.y);
    
            sensor_msgs::ChannelFloat32 vx_channel;
    
            vx_channel.name = "vx";
    
            vx_channel.values.push_back(0.0);  // 假设速度为0
    
            sensor_msgs::ChannelFloat32 vy_channel;
    
            vy_channel.name = "vy";
    
            vy_channel.values.push_back(0.0);  // 假设速度为0
    
            feature_points->channels.push_back(id_channel);
    
            feature_points->channels.push_back(u_channel);
    
            feature_points->channels.push_back(v_channel);
    
            feature_points->channels.push_back(vx_channel);
    
            feature_points->channels.push_back(vy_channel);
    
        }
    
        // 发布特征点信息
    
        pub_img.publish(feature_points);
    
    }
    
    catch (cv_bridge::Exception& e)
    
    {
    
        ROS_ERROR("Could not convert from [%s] to [bgr8].", img_msg->encoding.c_str());
    
    }

}

// 主函数

int main(int argc, char** argv)

{

    ros::init(argc, argv, "feature_tracker");
    
    ros::NodeHandle n;
    
    // 订阅图像话题
    
    ros::Subscriber sub = n.subscribe("camera/image_raw", 1, img_callback);
    
    // 发布特征点话题
    
    ros::Publisher pub_img = n.advertise<sensor_msgs::PointCloud>("/feature_tracker/feature", 1000);
    
    ros::spin();
    
    return 0;

}

详细解释

订阅图像话题：

使用 ros::Subscriber 订阅图像话题 camera/image_raw，并将回调函数设置为 img_callback。

回调函数 img_callback：

接收 sensor_msgs::ImageConstPtr 类型的消息。

使用 cv_bridge 将 ROS 图像消息转换为 OpenCV 图像。

使用 cv::goodFeaturesToTrack 函数检测图像中的特征点。

创建 sensor_msgs::PointCloudPtr 类型的 feature_points 消息。

设置 feature_points 的头信息，包括序列号、时间戳和坐标系ID。

遍历检测到的特征点，添加每个特征点的3D坐标和附加通道信息（如ID、像素坐标u,v、速度vx,vy等）。

使用 pub_img.publish(feature_points) 发布特征点信息。

主函数 main：

初始化 ROS 节点。

订阅图像话题 camera/image_raw。

发布特征点话题 /feature_tracker/feature。

使用 ros::spin 进入消息循环，处理回调函数。

sensor_msgs::PointCloudPtr

sensor_msgs::PointCloudPtr 是 sensor_msgs::PointCloud 的智能指针类型。它使用 boost::shared_ptr 来管理 sensor_msgs::PointCloud 对象的生命周期，确保资源的自动管理和释放。

定义

typedef boost::shared_ptr<sensor_msgs::PointCloud> sensor_msgs::PointCloudPtr;

使用场景

sensor_msgs::PointCloud

直接创建和使用对象：

当你需要创建一个 sensor_msgs::PointCloud 对象并直接使用它时，可以使用 sensor_msgs::PointCloud。

sensor_msgs::PointCloud msg_match_points;

msg_match_points.header.stamp = ros::Time::now();

msg_match_points.header.frame_id = “world”;

在回调函数中接收消息：

当你在回调函数中接收 sensor_msgs::PointCloud 消息时，通常会使用 sensor_msgs::PointCloudConstPtr，它是 sensor_msgs::PointCloudPtr 的常量版本。

示例：

void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)

{

    // 处理 points_msg

}

sensor_msgs::PointCloudPtr

动态创建对象：

当你需要动态创建一个 sensor_msgs::PointCloud 对象并传递它时，可以使用 sensor_msgs::PointCloudPtr。

示例：

sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);

feature_points->header = img_msg->header;

feature_points->header.frame_id = "world";

发布消息：

当你需要发布一个 sensor_msgs::PointCloud 消息时，通常会使用 sensor_msgs::PointCloudPtr。

示例：

ros::Publisher pub_img = n.advertise<sensor_msgs::PointCloud>(“feature”, 1000);

pub_img.publish(feature_points);

示例代码

feature_trackers.cpp 中的 img_callback()

#include <ros/ros.h>

#include <sensor_msgs/Image.h>

#include <sensor_msgs/PointCloud.h>

#include <sensor_msgs/ChannelFloat32.h>

#include <cv_bridge/cv_bridge.h>

#include <geometry_msgs/Point32.h>

#include <opencv2/opencv.hpp>

ros::Publisher pub_img;

void img_callback(const sensor_msgs::ImageConstPtr& img_msg)

{

    try
    
    {
    
        // 将ROS图像消息转换为OpenCV图像
    
        cv::Mat img = cv_bridge::toCvShare(img_msg, "bgr8")->image;
    
        // 检测特征点
    
        std::vector<cv::Point2f> cur_pts;
    
        cv::goodFeaturesToTrack(img, cur_pts, 100, 0.01, 10);
    
        // 创建 PointCloud 消息
    
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
    
        feature_points->header = img_msg->header;
    
        feature_points->header.frame_id = "world";  // 设置坐标系ID
    
        // 添加3D点和通道信息
    
        for (const auto &pt : cur_pts)
    
        {
    
            geometry_msgs::Point32 point;
    
            point.x = pt.x;
    
            point.y = pt.y;
    
            point.z = 0.0;  // 假设z坐标为0
    
            feature_points->points.push_back(point);
    
            sensor_msgs::ChannelFloat32 id_channel;
    
            id_channel.name = "id";
    
            id_channel.values.push_back(static_cast<float>(feature_points->points.size() - 1));
    
            sensor_msgs::ChannelFloat32 u_channel;
    
            u_channel.name = "u";
    
            u_channel.values.push_back(pt.x);
    
            sensor_msgs::ChannelFloat32 v_channel;
    
            v_channel.name = "v";
    
            v_channel.values.push_back(pt.y);
    
            sensor_msgs::ChannelFloat32 vx_channel;
    
            vx_channel.name = "vx";
    
            vx_channel.values.push_back(0.0);  // 假设速度为0
    
            sensor_msgs::ChannelFloat32 vy_channel;
    
            vy_channel.name = "vy";
    
            vy_channel.values.push_back(0.0);  // 假设速度为0
    
            feature_points->channels.push_back(id_channel);
    
            feature_points->channels.push_back(u_channel);
    
            feature_points->channels.push_back(v_channel);
    
            feature_points->channels.push_back(vx_channel);
    
            feature_points->channels.push_back(vy_channel);
    
        }
    
        // 发布特征点信息
    
        pub_img.publish(feature_points);
    
    }
    
    catch (cv_bridge::Exception& e)
    
    {
    
        ROS_ERROR("Could not convert from [%s] to [bgr8].", img_msg->encoding.c_str());
    
    }

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "feature_tracker");
    
    ros::NodeHandle n;
    
    // 订阅图像话题
    
    ros::Subscriber sub = n.subscribe("camera/image_raw", 1, img_callback);
    
    // 发布特征点话题
    
    pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
    
    ros::spin();
    
    return 0;

}

keyframe.cpp 中的 findConnection()

#include <ros/ros.h>

#include <sensor_msgs/PointCloud.h>

#include <geometry_msgs/Point32.h>

#include <sensor_msgs/ChannelFloat32.h>

#include <Eigen/Dense>

ros::Publisher pub_match_points;

void findConnection(const std::vector<cv::Point2f>& cur_pts, const std::vector<cv::Point2f>& ref_pts, Eigen::Matrix4f& T)

{

    // 假设已经计算好了匹配点和平移旋转矩阵 T
    
    // 创建 PointCloud 消息
    
    sensor_msgs::PointCloud msg_match_points;
    
    // 设置头信息
    
    msg_match_points.header.stamp = ros::Time::now();
    
    msg_match_points.header.frame_id = "world";  // 设置坐标系ID
    
    // 添加3D点和通道信息
    
    for (size_t i = 0; i < cur_pts.size(); ++i)
    
    {
    
        geometry_msgs::Point32 point;
    
        point.x = cur_pts[i].x;
    
        point.y = cur_pts[i].y;
    
        point.z = 0.0;  // 假设z坐标为0
    
        msg_match_points.points.push_back(point);
    
        sensor_msgs::ChannelFloat32 t_q_index;
    
        t_q_index.name = "t_q_index";
    
        // 添加平移向量 T 的 x, y, z
    
        t_q_index.values.push_back(T(0, 3));
    
        t_q_index.values.push_back(T(1, 3));
    
        t_q_index.values.push_back(T(2, 3));
    
        // 添加旋转四元数 w, x, y, z
    
        Eigen::Quaternionf q(T.block<3, 3>(0, 0));
    
        t_q_index.values.push_back(q.w());
    
        t_q_index.values.push_back(q.x());
    
        t_q_index.values.push_back(q.y());
    
        t_q_index.values.push_back(q.z());
    
        // 添加索引值
    
        t_q_index.values.push_back(static_cast<float>(i));
    
        msg_match_points.channels.push_back(t_q_index);
    
    }
    
    // 发布匹配点信息
    
    pub_match_points.publish(msg_match_points);

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "keyframe_node");
    
    ros::NodeHandle n;
    
    // 发布匹配点话题
    
    pub_match_points = n.advertise<sensor_msgs::PointCloud>("/pose_graph/match_points", 100);
    
    ros::Rate loop_rate(10);
    
    while (ros::ok())
    
    {
    
        // 假设 findConnection() 已经被调用并生成了 msg_match_points
    
        // findConnection(cur_pts, ref_pts, T);
    
        ros::spinOnce();
    
        loop_rate.sleep();
    
    }
    
    return 0;
    
    }

estimator_node.cpp 中的 relocalization_callback()

#include <ros/ros.h>

#include <sensor_msgs/PointCloud.h>

#include <sensor_msgs/ChannelFloat32.h>

#include <Eigen/Dense>

void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)

{

    // 解析匹配点信息
    
    for (size_t i = 0; i < points_msg->points.size(); ++i)
    
    {
    
        const geometry_msgs::Point32& point = points_msg->points[i];
    
        const sensor_msgs::ChannelFloat32& channel = points_msg->channels[i];
    
        // 获取3D点坐标
    
        float x = point.x;
    
        float y = point.y;
    
        float z = point.z;
    
        // 获取平移向量 T 的 x, y, z
    
        float tx = channel.values[0];
    
        float ty = channel.values[1];
    
        float tz = channel.values[2];
    
        // 获取旋转四元数 w, x, y, z
    
        float qw = channel.values[3];
    
        float qx = channel.values[4];
    
        float qy = channel.values[5];
    
        float qz = channel.values[6];
    
        // 获取索引值
    
        float index = channel.values[7];
    
        // 打印匹配点信息
    
        ROS_INFO("Match Point %f, %f, %f", x, y, z);
    
        ROS_INFO("Translation %f, %f, %f", tx, ty, tz);
    
        ROS_INFO("Rotation %f, %f, %f, %f", qw, qx, qy, qz);
    
        ROS_INFO("Index %f", index);
    
    }

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "estimator_node");
    
    ros::NodeHandle n;
    
    // 订阅匹配点话题
    
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
    
    ros::spin();
    
    return 0;

}

sensor_msgs::ImuConstPtr

sensor_msgs::ImuConstPtr 是 ROS (Robot Operating System) 中用于表示惯性测量单元 (IMU) 数据的标准消息类型的常量智能指针。IMU 数据通常包括四元数表示的姿态、角速度和线性加速度，以及相应的协方差矩阵。sensor_msgs::ImuConstPtr 使用 boost::shared_ptr 来管理 sensor_msgs::Imu 对象的生命周期，确保资源的自动管理和释放。

sensor_msgs::Imu 消息定义

首先，我们来看一下 sensor_msgs::Imu 消息的定义：

std_msgs/Header header

  uint32 seq

  time stamp

  string frame_id

geometry_msgs/Quaternion orientation

  float64 x

  float64 y

  float64 z

  float64 w

float64[9] orientation_covariance

geometry_msgs/Vector3 angular_velocity

  float64 x

  float64 y

  float64 z

float64[9] angular_velocity_covariance

geometry_msgs/Vector3 linear_acceleration

  float64 x

  float64 y

  float64 z

float64[9] linear_acceleration_covariance

字段解释

header (std_msgs/Header)：

头信息：包含消息的基本信息，如序列号、时间戳和坐标系ID。

uint32 seq       # 序列ID：连续增加的ID

time stamp       # 时间戳

                   # stamp.sec: 自纪元以来的秒数
    
                   # stamp.nsec: 自 stamp.sec 以来的纳秒数

string frame_id  # 坐标系ID

orientation (geometry_msgs/Quaternion)：

四元数：表示IMU的姿态，通常用于描述旋转。

float64 x

float64 y

float64 z

float64 w

orientation_covariance (float64[9])：

协方差矩阵：表示姿态估计的不确定性，是一个3x3的矩阵，按行主序存储。

[xx, xy, xz,

yx, yy, yz,

zx, zy, zz]

angular_velocity (geometry_msgs/Vector3)：

角速度：表示IMU绕x、y、z轴的旋转速度。

float64 x

float64 y

float64 z

angular_velocity_covariance (float64[9])：

协方差矩阵：表示角速度估计的不确定性，是一个3x3的矩阵，按行主序存储。

[xx, xy, xz,

yx, yy, yz,

zx, zy, zz]

linear_acceleration (geometry_msgs/Vector3)：

线性加速度：表示IMU沿x、y、z轴的线性加速度。

float64 x

float64 y

float64 z

linear_acceleration_covariance (float64[9])：

协方差矩阵：表示线性加速度估计的不确定性，是一个3x3的矩阵，按行主序存储。

[xx, xy, xz,

yx, yy, yz,

zx, zy, zz]

sensor_msgs::ImuConstPtr

sensor_msgs::ImuConstPtr 是 sensor_msgs::Imu 的常量智能指针类型，定义如下：

typedef boost::shared_ptr sensor_msgs::ImuConstPtr;

使用场景

在回调函数中接收 IMU 消息

当你在回调函数中接收 IMU 消息时，通常会使用 sensor_msgs::ImuConstPtr。这样可以确保消息对象在回调函数执行期间不会被意外修改或删除。

#include <ros/ros.h>

#include <sensor_msgs/Imu.h>

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)

{

    // 解析 IMU 消息
    
    const geometry_msgs::Quaternion& orientation = imu_msg->orientation;
    
    const geometry_msgs::Vector3& angular_velocity = imu_msg->angular_velocity;
    
    const geometry_msgs::Vector3& linear_acceleration = imu_msg->linear_acceleration;
    
    // 打印 IMU 数据
    
    ROS_INFO("Orientation: x=%f, y=%f, z=%f, w=%f",
    
             orientation.x, orientation.y, orientation.z, orientation.w);
    
    ROS_INFO("Angular Velocity: x=%f, y=%f, z=%f",
    
             angular_velocity.x, angular_velocity.y, angular_velocity.z);
    
    ROS_INFO("Linear Acceleration: x=%f, y=%f, z=%f",
    
             linear_acceleration.x, linear_acceleration.y, linear_acceleration.z);

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "imu_subscriber");
    
    ros::NodeHandle n;
    
    // 订阅 IMU 话题
    
    ros::Subscriber sub_imu = n.subscribe("imu/data", 1000, imu_callback);
    
    ros::spin();
    
    return 0;

}

示例代码

订阅 IMU 话题并在回调函数中处理消息

#include <ros/ros.h>

#include <sensor_msgs/Imu.h>

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)

{

    // 解析 IMU 消息
    
    const geometry_msgs::Quaternion& orientation = imu_msg->orientation;
    
    const geometry_msgs::Vector3& angular_velocity = imu_msg->angular_velocity;
    
    const geometry_msgs::Vector3& linear_acceleration = imu_msg->linear_acceleration;
    
    // 打印 IMU 数据
    
    ROS_INFO("Orientation: x=%f, y=%f, z=%f, w=%f",
    
             orientation.x, orientation.y, orientation.z, orientation.w);
    
    ROS_INFO("Angular Velocity: x=%f, y=%f, z=%f",
    
             angular_velocity.x, angular_velocity.y, angular_velocity.z);
    
    ROS_INFO("Linear Acceleration: x=%f, y=%f, z=%f",
    
             linear_acceleration.x, linear_acceleration.y, linear_acceleration.z);
    
    // 打印协方差矩阵
    
    ROS_INFO("Orientation Covariance:");
    
    for (int i = 0; i < 9; ++i)
    
    {
    
        if (i % 3 == 0) ROS_INFO(" ");
    
        ROS_INFO("%f ", imu_msg->orientation_covariance[i]);
    
    }
    
    ROS_INFO("Angular Velocity Covariance:");
    
    for (int i = 0; i < 9; ++i)
    
    {
    
        if (i % 3 == 0) ROS_INFO(" ");
    
        ROS_INFO("%f ", imu_msg->angular_velocity_covariance[i]);
    
    }
    
    ROS_INFO("Linear Acceleration Covariance:");
    
    for (int i = 0; i < 9; ++i)
    
    {
    
        if (i % 3 == 0) ROS_INFO(" ");
    
        ROS_INFO("%f ", imu_msg->linear_acceleration_covariance[i]);
    
    }

}

int main(int argc, char** argv)

{

    ros::init(argc, argv, "imu_subscriber");
    
    ros::NodeHandle n;
    
    // 订阅 IMU 话题
    
    ros::Subscriber sub_imu = n.subscribe("imu/data", 1000, imu_callback);
    
    ros::spin();
    
    return 0;

}

总结

sensor_msgs::Imu：表示 IMU 数据的标准消息类型，包含姿态、角速度和线性加速度及其协方差矩阵。

sensor_msgs::ImuConstPtr：是 sensor_msgs::Imu 的常量智能指针类型，用于在回调函数中接收和处理 IMU 消息，确保资源的自动管理和释放。

OpenCV函数

cv::findFundamentalMat

cv::findFundamentalMat 是 OpenCV 中用于计算两幅图像之间基础矩阵（Fundamental Matrix）的函数。这个函数主要用于计算机视觉中的立体视觉和多视图几何问题。下面是对 cv::findFundamentalMat 函数的详细解释：

函数定义

cv::findFundamentalMat(points1, points2, method, param1, param2, mask)

参数详解

points1 (InputArray):

第一幅图像中的特征点集，类型为 cv::Point2f 或 cv::Point2d。

points2 (InputArray):

第二幅图像中的特征点集，类型为 cv::Point2f 或 cv::Point2d。

method (int):

计算基础矩阵的方法。常见的方法包括：

cv::FM_7POINT：使用7点算法。

cv::FM_8POINT：使用8点算法。

cv::FM_LMEDS：使用最小中位数算法。

cv::FM_RANSAC：使用随机样本一致性（RANSAC）算法。

param1 (double):

根据不同的方法有不同的含义：

对于 cv::FM_RANSAC 和 cv::FM_LMEDS，这是距离阈值，用于确定内点和外点。

对于 cv::FM_7POINT 和 cv::FM_8POINT，这个参数被忽略。

param2 (double):

对于 cv::FM_RANSAC，这是置信度水平，表示希望算法达到的正确率。

对于其他方法，这个参数被忽略。

mask (OutputArray):

输出的状态向量，类型为 uchar。每个元素表示对应的点对是否被认为是内点（值为1）或外点（值为0）。

返回值

返回值 (cv::Mat):

计算得到的基础矩阵 F，是一个3x3的矩阵。

基础矩阵的作用

基础矩阵 F 描述了两个图像中对应点之间的极线约束。具体来说，对于一对对应点 x 和 x′，它们满足以下关系：

x’TFx = 0

其中：

x=(x,y,1)T是当前帧中的点的齐次坐标。x’=(x’,y’,1)T是前向帧中的对应点的齐次坐标。F 是 3×3 的基础矩阵。

计算极线

基础矩阵可以用来计算极线。给定当前帧中的一个点 x，可以通过以下方式计算前向帧中的极线I’: I’ = Fx

这里，I′是一个 3×1 的向量，表示前向帧中的极线。极线是前向帧中所有可能对应点的轨迹。

cv::goodFeaturesToTrack

cv::goodFeaturesToTrack 是 OpenCV 中用于检测图像中的角点（特征点）的函数。这个函数基于 Shi-Tomasi 角点检测算法（也称为 Harris 角点检测的改进版），能够找到图像中最显著的特征点。下面是对 cv::goodFeaturesToTrack 函数的详细解释：

函数定义

void cv::goodFeaturesToTrack(

    InputArray image,
    
    OutputArray corners,
    
    int maxCorners,
    
    double qualityLevel,
    
    double minDistance,
    
    InputArray mask = noArray(),
    
    int blockSize = 3,
    
    bool useHarrisDetector = false,
    
    double k = 0.04

);

参数详解

image (InputArray):

输入图像，必须是单通道的灰度图像。

corners (OutputArray):

输出的特征点集，类型为 std::vectorcv::Point2f 或 cv::Mat，每个点是一个二维浮点数坐标。

maxCorners (int):

最大特征点数量。如果检测到的特征点数量超过这个值，只返回最显著的特征点。

qualityLevel (double):

特征点的质量等级。范围是从0到1，表示最显著特征点的质量分数。例如，如果 qualityLevel 设为0.01，那么只有那些质量分数高于 qualityLevel * maxCorners 的特征点才会被保留。

minDistance (double):

特征点之间的最小距离。这个参数用于避免检测到过于密集的特征点。单位是像素。

mask (InputArray):

可选参数，用于指定感兴趣的区域。类型为 cv::Mat，值为非零的区域会被考虑，值为零的区域会被忽略。

blockSize (int):

邻域大小，用于计算每个像素的自相关矩阵。默认值为3。

useHarrisDetector (bool):

是否使用 Harris 角点检测器。默认值为 false，表示使用 Shi-Tomasi 角点检测器。

k (double):

Harris 角点检测器中的自由参数，用于计算响应值。默认值为0.04。

示例代码

下面是一个使用 cv::goodFeaturesToTrack 函数检测图像中特征点的示例代码：

#include <opencv2/opencv.hpp>

#include <iostream>

int main()

{

    // 读取图像
    
    cv::Mat img = cv::imread("image.jpg", cv::IMREAD_GRAYSCALE);
    
    if (img.empty())
    
    {
    
        std::cerr << "Error: Image not found!" << std::endl;
    
        return -1;
    
    }
    
    // 定义参数
    
    int maxCorners = 100;
    
    double qualityLevel = 0.01;
    
    double minDistance = 10;
    
    cv::Mat mask;
    
    int blockSize = 3;
    
    bool useHarrisDetector = false;
    
    double k = 0.04;
    
    // 检测特征点
    
    std::vector<cv::Point2f> corners;
    
    cv::goodFeaturesToTrack(img, corners, maxCorners, qualityLevel, minDistance, mask, blockSize, useHarrisDetector, k);
    
    // 绘制特征点
    
    cv::Mat img_with_corners;
    
    cv::cvtColor(img, img_with_corners, cv::COLOR_GRAY2BGR);
    
    for (const auto &corner : corners)
    
    {
    
        cv::circle(img_with_corners, corner, 3, cv::Scalar(0, 0, 255), -1);
    
    }
    
    // 显示结果
    
    cv::imshow("Good Features To Track", img_with_corners);
    
    cv::waitKey(0);
    
    return 0;

}

详细解释

读取图像：

使用 cv::imread 函数读取图像，并将其转换为灰度图像。

定义参数：

maxCorners：最多检测100个特征点。

qualityLevel：质量等级为0.01。

minDistance：特征点之间的最小距离为10像素。

mask：可选参数，这里未使用。

blockSize：邻域大小为3。

useHarrisDetector：不使用 Harris 角点检测器。

k：自由参数为0.04。

检测特征点：

调用 cv::goodFeaturesToTrack 函数检测图像中的特征点，结果存储在 corners 中。

绘制特征点：

将灰度图像转换为彩色图像。

使用 cv::circle 函数在图像上绘制检测到的特征点。

显示结果：

使用 cv::imshow 函数显示带有特征点的图像。

使用 cv::waitKey 函数等待用户按键。

总结

cv::goodFeaturesToTrack 函数用于检测图像中的显著特征点。通过设置适当的参数，可以控制检测到的特征点数量、质量等级、最小距离等。这个函数在计算机视觉中广泛应用于特征点检测、跟踪、匹配等任务。希望这些解释能帮助你更好地理解和使用 cv::goodFeaturesToTrack 函数。

参考文献

• 【SLAM】VINS-MONO解析——综述
• VINS论文解读与代码详解（集百家之精华）
• VINS-Mono代码解读——视觉跟踪 feature_trackers

参考文献

• VINS-Mono
• 超详细：VINS-Mono论文中文记录
• VINS-Mono论文详解
• vins-mono 理论 全篇通
• VINS-MONO概述
• 视觉、激光SLAM 源码详细解析
• VINS-Mono 论文解读（IMU预积分残差+Marg边缘化）
• 【SLAM】VINS-MONO解析——综述
• Vins-Mono-All-Noted
• VINS-Mono-Learning
• VINS论文解读与代码详解（集百家之精华）