VINS-Fusion源码逐行解析：程序入口rosNodeTest.cpp

时凯凯凯凯

已于 2024-06-09 17:12:27 修改

阅读量743

点赞数 5

分类专栏： VINS-Fusion源码解析文章标签：计算机视觉 c++ 算法

于 2024-06-09 17:10:40 首次发布

本文链接：https://blog.csdn.net/shikaiaixuexi/article/details/139562867

版权

VINS-Fusion源码解析专栏收录该内容

14 篇文章

订阅专栏

今天开始阅读VINS-Fusion，以专栏的形式记录一下，防止之后自己忘

首先我们看一下源码的文件夹组成：

camera_models：定义了抽象相机类。
config：存放了程序运行需要的yaml文件，需要什么相机，就用对应的配置文件就行。
docker：顾名思义，使用docker运行VINS-Fusion。
global_fusion：照VINS-Mono新增加的GPS等全局传感器。
loop_fusion：局部回环检测，若需要回环检测需要单独运行。
support_files：作者介绍自己代码的运行图片、论文等；
vins_estimator：主要看的地方；

接下来我们看整个vins-fusion的程序入口，rosNodeTest.cpp

首先是主函数：

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vins_estimator");// 初始化 ROS 节点，节点名为 "vins_estimator"
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

    // 检查命令行参数的数量是否为 2，如不是，打印提示信息，并返回
    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;
    }

    string config_file = argv[1];// 获取配置文件路径
    printf("config_file: %s\n", argv[1]);// 打印配置文件路径

    readParameters(config_file);// 读取配置文件中的参数
    estimator.setParameter();// 设置估计器的参数

#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");// 如果定义了 EIGEN_DONT_PARALLELIZE，打印调试信息
#endif

    ROS_WARN("waiting for image and imu...");// 打印警告信息，提示正在等待图像和 IMU 数据

    registerPub(n); // 注册发布者，在此节点下发布话题

    ros::Subscriber sub_imu;
    if(USE_IMU)// 如果使用 IMU
    {
        // 订阅 IMU 话题
        sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    }
    // 订阅特征点话题
    ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    // 订阅图像 0 话题
    ros::Subscriber sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);
    ros::Subscriber sub_img1;
    if(STEREO)// 如果使用双目相机
    {
        // 订阅图像 1 话题
        sub_img1 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);
    }
    // 订阅重新启动话题
    ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);
    // 订阅双目下 IMU 开关话题
    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);

    std::thread sync_thread{sync_process};// 创建并启动一个同步处理的线程
    ros::spin();// 进入 ROS 事件循环，处理回调函数

    return 0;
}

接着，我们看下该文件下的回调函数

// 图像0的回调函数，参数是图像消息的常量指针
void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    m_buf.lock();// 加锁，防止多个线程同时访问
    img0_buf.push(img_msg);// 将图像消息推入 img0 缓存队列
    m_buf.unlock();// 解锁
}

//右目同理
void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    m_buf.lock();
    img1_buf.push(img_msg);
    m_buf.unlock();
}

// IMU 数据的回调函数，参数是 IMU 消息的常量指针
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();// 获取 IMU 消息的时间戳（秒）
    double dx = imu_msg->linear_acceleration.x;// 获取 IMU 的线性加速度 x 轴分量
    double dy = imu_msg->linear_acceleration.y;// 获取 IMU 的线性加速度 y 轴分量
    double dz = imu_msg->linear_acceleration.z;// 获取 IMU 的线性加速度 z 轴分量
    double rx = imu_msg->angular_velocity.x;// 获取 IMU 的角速度 x 轴分量
    double ry = imu_msg->angular_velocity.y;// 获取 IMU 的角速度 y 轴分量
    double rz = imu_msg->angular_velocity.z;// 获取 IMU 的角速度 z 轴分量
    Vector3d acc(dx, dy, dz);// 创建一个 3 维向量表示线性加速度
    Vector3d gyr(rx, ry, rz);// 创建一个 3 维向量表示角速度
    estimator.inputIMU(t, acc, gyr);// 将时间戳、线性加速度和角速度输入到估计器中
    return;
}


// 特征点数据的回调函数，参数是点云消息的常量指针
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    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];// 获取特征点 ID
        int camera_id = feature_msg->channels[1].values[i];// 获取相机 ID
        double x = feature_msg->points[i].x;// 获取点的 x 坐标
        double y = feature_msg->points[i].y;// 获取点的 y 坐标
        double z = feature_msg->points[i].z;// 获取点的 z 坐标
        double p_u = feature_msg->channels[2].values[i];// 获取点的 u 像素坐标
        double p_v = feature_msg->channels[3].values[i];// 获取点的 v 像素坐标
        double velocity_x = feature_msg->channels[4].values[i];// 获取点的 x 方向速度
        double velocity_y = feature_msg->channels[5].values[i];// 获取点的 y 方向速度
        if(feature_msg->channels.size() > 5)// 如果消息包含更多的通道
        {
            double gx = feature_msg->channels[6].values[i];// 获取点的真值 x 坐标
            double gy = feature_msg->channels[7].values[i];// 获取点的真值 y 坐标
            double gz = feature_msg->channels[8].values[i];// 获取点的真值 z 坐标
            pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz);// 将真值坐标存入地图
            //printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);
        }
        ROS_ASSERT(z == 1);// 断言 z 坐标等于 1（确保点是归一化平面上的点）
        Eigen::Matrix<double, 7, 1> xyz_uv_velocity;// 定义一个 7 维向量
        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();// 获取特征点消息的时间戳（秒）
    estimator.inputFeature(t, featureFrame);// 将时间戳和特征点信息输入到估计器中
    return;
}

// 重启的回调函数，参数是布尔消息的常量指针
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
    // 如果消息数据为 true
    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)
{
    // 如果消息数据为 true
    if (switch_msg->data == true)
    {
        //ROS_WARN("use IMU!");
        // 切换传感器类型，启用 IMU
        estimator.changeSensorType(1, STEREO);
    }
    else
    {
        //ROS_WARN("disable IMU!");
        // 切换传感器类型，禁用 IMU
        estimator.changeSensorType(0, STEREO);
    }
    return;
}

// 相机开关的回调函数，参数是布尔消息的常量指针
void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
{
    // 如果消息数据为 true
    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;
}

有一个从 ROS 图像消息获取 OpenCV 图像的函数

// 从 ROS 图像消息获取 OpenCV 图像
cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
{
    cv_bridge::CvImageConstPtr ptr;// 定义一个指向 CvImage 的常量指针
    if (img_msg->encoding == "8UC1")// 如果图像编码为 "8UC1"
    {
        sensor_msgs::Image img;// 创建一个新的 ROS 图像消息
        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";// 设置图像编码为 "mono8"（单通道8位灰度图）
        ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);// 将 ROS 图像消息转换为 OpenCV 图像
    }
    else
        ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); // 直接将 ROS 图像消息转换为 OpenCV 图像

    cv::Mat img = ptr->image.clone();// 克隆 OpenCV 图像，确保数据独立
    return img;// 返回 OpenCV 图像
}

和一个从两个话题中提取具有相同时间戳图像的线程

// 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
                if(time0 < time1 - 0.003)// 如果图像0的时间戳比图像1的时间戳早超过 0.003 秒
                {
                    img0_buf.pop();// 丢弃图像0
                    printf("throw img0\n");
                }
                else if(time0 > time1 + 0.003)// 如果图像0的时间戳比图像1的时间戳晚超过 0.003 秒
                {
                    img1_buf.pop();// 丢弃图像1
                    printf("throw img1\n");
                }
                else// 如果两个图像的时间戳在容差范围内
                {
                    time = img0_buf.front()->header.stamp.toSec();// 获取同步时间戳
                    header = img0_buf.front()->header;// 获取同步消息头
                    image0 = getImageFromMsg(img0_buf.front());// 从消息中提取图像0
                    img0_buf.pop();//在序列中删除
                    image1 = getImageFromMsg(img1_buf.front());// 从消息中提取图像1
                    img1_buf.pop();
                    //printf("find img0 and img1\n");
                }
            }
            m_buf.unlock();
            if(!image0.empty())// 如果图像0不为空
                estimator.inputImage(time, image0, image1);// 将同步的图像输入到估计器
        }
        else//单目
        {
            cv::Mat image;
            std_msgs::Header header;
            double time = 0;
            m_buf.lock();
            if(!img0_buf.empty())// 如果图像0缓存队列不为空
            {
                time = img0_buf.front()->header.stamp.toSec();// 获取图像0的时间戳
                header = img0_buf.front()->header;
                image = getImageFromMsg(img0_buf.front());// 从消息中提取图像0
                img0_buf.pop();//在序列中删除
            }
            m_buf.unlock();
            if(!image.empty())// 如果图像不为空
                estimator.inputImage(time, image);// 将图像输入到估计器
        }

        std::chrono::milliseconds dura(2);// 定义一个 2 毫秒的时间段
        std::this_thread::sleep_for(dura);// 线程休眠 2 毫秒
    }
}

至此，rosNodeTest.cpp结束。