VINS-FUSION代码解读【1】——程序入口

VINS-FUSION代码解读【1】——程序入口

本人基础不太扎实所以会把代码注释的比较详细，也会适当添加对应知识点的解读。
vins-fusion不像mono那样有三个node，它只有一个node，在rosNodeTest.cpp里。并且这个文件是整个vins-fusion的程序入口。所以我先从这个文件看起。
mian函数如下所示：

int main(int argc, char **argv)
{
	//初始化ros节点
    ros::init(argc, argv, "vins_estimator");
    ros::NodeHandle n("~");

    /*
    * 设置消息日志级别，默认有Debug,Info,Warn,Error,Fatal
    * Debug : 输出程序正常运行需要的信息
    * Info : 输出大量用户需要的信息
    * Warn : 输出警告，或许影响程序的应用，但系统仍处于可控的预期状态
    * Error : 输出严重错误（但错误可恢复）
    * Fatal : 输出不可恢复的崩溃式错误
    */
    ros::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;
    }
    //很明显就是在启动程序的时候将参数文件路径作为参数
    //启动程序 : rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
    string config_file = argv[1];
    printf("config_file: %s\n", argv[1]);

    // 读取YAML配置参数
    readParameters(config_file);
    estimator.setParameter();

#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif

    ROS_WARN("waiting for image and imu...");

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

    // 订阅IMU
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    // 订阅一帧跟踪的特征点，没有找到对应的发布者,实际上应该是没用到，实际上是使用featureBuf进行传递
    ros::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 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);
     //  这三个订阅可以通过外部的程序发布ROS消息进行调整
    // 订阅，重启节点，实际上应该是没用到
    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);

    // 从两个图像队列中取出最早的一帧，并从队列删除，双目相机的话要求两帧时差不得超过0.003s
    std::thread sync_thread{sync_process};
    //  等待回调
    ros::spin();

    return 0;
}

rosNodeTest.cpp的文件级变量

queue<sensor_msgs::ImuConstPtr> imu_buf;//imu缓存队列
queue<sensor_msgs::PointCloudConstPtr> feature_buf;//特征点缓存队列
queue<sensor_msgs::ImageConstPtr> img0_buf;//左目/单目缓存队列
queue<sensor_msgs::ImageConstPtr> img1_buf;//右目图像缓存队列
std::mutex m_buf;   // 操作缓存队列需要上锁，防止数据出现异常

接着，我们单独看一下每个订阅函数:

/*
 * 订阅左目图像，缓存
*/
void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    m_buf.lock();
    img0_buf.push(img_msg);
    m_buf.unlock();
}

/*
 * 订阅右目图像，缓存
*/
void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    m_buf.lock();
    img1_buf.push(img_msg);
    m_buf.unlock();
}

//*
 * 订阅IMU，交给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);
    /*
    *  inputIMU函数：
    * 1. 积分预测当前帧状态，latest_time，latest_Q，latest_P，latest_V，latest_acc_0，latest_gyr_0
    * 2. 发布里程计
    */
    estimator.inputIMU(t, acc, gyr);
    return;
}

/*
 * 订阅，重启节点
*/
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
    if (restart_msg->data == true)
    {
        ROS_WARN("restart the estimator!");
        estimator.clearState();
        estimator.setParameter();
    }
    return;
}

/*
 * 订阅一帧跟踪的特征点，包括3D坐标、像素坐标、速度，交给estimator处理
*/
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];
        int camera_id = feature_msg->channels[1].values[i];
        //  3D坐标
        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);
        }
        // 检查是不是归一化
        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();
    estimator.inputFeature(t, featureFrame);
    return;
}
/*
 * 订阅，重启节点
*/
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
    {
        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;
}

还有一个将ROS中的图像数据格式转换为OpenCV的数据格式的函数

/*
 * ROS图像转换成CV格式
*/
cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
{
    cv_bridge::CvImageConstPtr ptr;
    // 如果编码格式不是MONO8的话将编码格式进行转换
    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);
    }
    else
        ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);

    cv::Mat img = ptr->image.clone();
    return img;
}

接下来解析sync_process线程

/*
 * 从图像队列中取出最早的一帧，并从队列删除
 * 双目的话要求两帧时差不得超过0.003s
*/
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
                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();
                }
            }
            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);
        }
        //  线程休眠2ms
        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
}

以上就是对rosNodeTest.cpp代码的解读
下一篇：
VINS-FUSION代码解读【2】——参数读取和求解器参数设置