VINS-Mono代码学习记录（四）---estimator_node

写在前面的话

终于把feature_tracker这一个node给整理好了，那些都是之前就已经看过的内容，所以整理起来比较快，接下来就慢慢边学边整理吧，这次先来看estimator_node.cpp里main()的内容。
补：在程序中会涉及到很多ROS里定义的数据类型和作者定义的复杂难理解的数据类型，比如sensor_msgs::ImageConstPtr sensor_msgs::PointCloudPtr feature_points 等等，在学习的时候我找到一个帮助理解的参考，如果你也因为这些数据类型而困扰，就点进去看一看吧。
参考连接： https://blog.csdn.net/qq_41839222/article/details/86030962

一、main()函数注释

int main(int argc, char **argv)
{
    //[1]初始化，设置句柄，设置logger级别
    ros::init(argc, argv, "vins_estimator");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

    //【2】从config_file中读取参数，imu话题，Ba,Bg,Na,Ng，最大迭代次数等
    readParameters(n);

    //【3】优化器设置参数，包括相机IMU外参tic和ric
    estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    //[4]注册visualization.cpp中创建的发布器registerPub(n)
    registerPub(n);

    //[5]订阅imu话题，订阅/feature_tracker/feature，/feature_tracker/restart，/pose_graph/match_points等话题,执行回调函数
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

    //[6]创建主线程函数process()，在process()中处理VIO后端，包括IMU预积分、松耦合初始化和local BA
    std::thread measurement_process{process};//创建线程measurement_process对象，一创建线程对象就启动线程 运行process
    ros::spin();

    return 0;
}

其中，步骤[2]的readParameters(n)的函数在paramaters.cpp中，如下：

void readParameters(ros::NodeHandle &n)
{
    //[1]读取参数文件euroc_config.yaml
    std::string config_file;
    config_file = readParam(n, "config_file");
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    
    //[2]文件是否正确打开判断
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }

    //[3]读取相应参数
    fsSettings["imu_topic"] >> IMU_TOPIC;//imu_topic: "/imu0"

    SOLVER_TIME = fsSettings["max_solver_time"];//solver的最大迭代时间0.04ms
    NUM_ITERATIONS = fsSettings["max_num_iterations"];//最大迭代次数8
    MIN_PARALLAX = fsSettings["keyframe_parallax"];//最小视差10pixel
    MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;//最小视差=最小视差/焦距=10.0/460.0

    //[4]设置输出路径
    std::string OUTPUT_PATH;
    fsSettings["output_path"] >> OUTPUT_PATH;
    VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.csv";
    std::cout << "result path " << VINS_RESULT_PATH << std::endl;
    
    FileSystemHelper::createDirectoryIfNotExists(OUTPUT_PATH.c_str());//如果文件不存在，就创建一个

    std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
    fout.close();
 
    //【5】继续读取参数
    ACC_N = fsSettings["acc_n"];//加速度计的noise
    ACC_W = fsSettings["acc_w"];//加速度计的bias
    GYR_N = fsSettings["gyr_n"];//陀螺仪的noise
    GYR_W = fsSettings["gyr_w"];//陀螺仪的bias
    G.z() = fsSettings["g_norm"];//重力
    ROW = fsSettings["image_height"];
    COL = fsSettings["image_width"];
    ROS_INFO("ROW: %f COL: %f ", ROW, COL);

    //【6】读取imu和camera的外参，根据读取的参数执行相应的操作
    ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"];
    if (ESTIMATE_EXTRINSIC == 2)//表示完全让算法标定imu和相机之间的外参
    {
        ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param");
        RIC.push_back(Eigen::Matrix3d::Identity());//给RIC赋值为单位阵
        TIC.push_back(Eigen::Vector3d::Zero());//给TIC赋值为另矩阵
        EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";

    }
    else 
    {
        if ( ESTIMATE_EXTRINSIC == 1)//表示我们提供一个初值，算法仅对初值进行优化
        {
            ROS_WARN(" Optimize extrinsic param around initial guess!");
            EX_CALIB_RESULT_PATH = OUTPUT_PATH + "/extrinsic_parameter.csv";
        }
        if (ESTIMATE_EXTRINSIC == 0)//表示算法信任我们提供的外参，不对其做任何修改
            ROS_WARN(" fix extrinsic param ");

        cv::Mat cv_R, cv_T;
        fsSettings["extrinsicRotation"] >> cv_R;//imu与camera的旋转外参
        fsSettings["extrinsicTranslation"] >> cv_T;//imu与camera的平移外参
        Eigen::Matrix3d eigen_R;
        Eigen::Vector3d eigen_T;
        cv::cv2eigen(cv_R, eigen_R);//opencv的数据结构转eigen
        cv::cv2eigen(cv_T, eigen_T);
        Eigen::Quaterniond Q(eigen_R);
        eigen_R = Q.normalized();//四元数归一化咋就转成Matrix3d啦？
        RIC.push_back(eigen_R);
        TIC.push_back(eigen_T);
        ROS_INFO_STREAM("Extrinsic_R : " << std::endl << RIC[0]);
        ROS_INFO_STREAM("Extrinsic_T : " << std::endl << TIC[0].transpose());
        
    } 

    //[7]给参数赋初始值
    INIT_DEPTH = 5.0;
    BIAS_ACC_THRESHOLD = 0.1;
    BIAS_GYR_THRESHOLD = 0.1;

    //[8]继续读取参数TD，图像和IMU数据在时间上的偏移量，这里配置文件中为0.0
    TD = fsSettings["td"];//0 initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
    ESTIMATE_TD = fsSettings["estimate_td"];//0 
    if (ESTIMATE_TD)
        ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD);
    else
        ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD);

    ROLLING_SHUTTER = fsSettings["rolling_shutter"];//0是全局快门，1是卷帘快门
    if (ROLLING_SHUTTER)
    {
        TR = fsSettings["rolling_shutter_tr"];//这个时间默认设置的是0
        ROS_INFO_STREAM("rolling shutter camera, read out time per line: " << TR);
    }
    else
    {
        TR = 0;
    }
    
    fsSettings.release();
}

步骤【4】中的registerPub(ros::NodeHandle &n)函数在utility–>visualization.cpp中，注释如下：

void registerPub(ros::NodeHandle &n)
{
    //[1]发布相关topic
    pub_latest_odometry = n.advertise("imu_propagate", 1000);//1000指队列长度
    pub_path = n.advertise("path", 1000);
    pub_relo_path = n.advertise("relocalization_path", 1000);
    pub_odometry = n.advertise("odometry", 1000);
    pub_point_cloud = n.advertise("point_cloud", 1000);
    pub_margin_cloud = n.advertise("history_cloud", 1000);
    pub_key_poses = n.advertise("key_poses", 1000);
    pub_camera_pose = n.advertise("camera_pose", 1000);
    pub_camera_pose_visual = n.advertise("camera_pose_visual", 1000);
    pub_keyframe_pose = n.advertise("keyframe_pose", 1000);
    pub_keyframe_point = n.advertise("keyframe_point", 1000);
    pub_extrinsic = n.advertise("extrinsic", 1000);
    pub_relo_relative_pose=  n.advertise("relo_relative_pose", 1000);

    //[2]可视化的相关设置
    cameraposevisual.setScale(1);
    cameraposevisual.setLineWidth(0.05);
    keyframebasevisual.setScale(0.1);
    keyframebasevisual.setLineWidth(0.01);
}

接下来就是对四个回调函数的处理了。。。

二、 imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)回调函数处理

注释如下：

{

    //[1]当前imu采样时间小于上次imu采样时间，说明有bug,跳出此条件。
    if (imu_msg->header.stamp.toSec() <= last_imu_t)//初值last_imu_t=0
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

    //[2]在修改多个线程共享的变量的时候进行上锁，防止多个线程同时访问该变量
    m_buf.lock();
    imu_buf.push(imu_msg);//将IMU数据保存到imu_buf中
    m_buf.unlock();
    con.notify_one();//唤醒作用于process线程中的获取观测值数据的函数

    last_imu_t = imu_msg->header.stamp.toSec();
//这个括弧是啥写法？
    {
        //[3]预测tmp_P,tmp_V,tmp_Q
        std::lock_guard lg(m_state);//模板类std::lock_guard，在构造时就能提供已锁的互斥量，并在析构的时候进行解锁,在std::lock_guard对象构造时，传入的mutex对象(即它所管理的mutex对象)会被当前线程锁住
        predict(imu_msg);

        //[4]如果solver_flag为非线性优化，发布最新的里程计消息
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

在imu_callback( )函数中又调用了predict( )函数来获取临时的P,V,Q值，来看一下predict( )函数的注释：

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();
    //[1]判断是否是第一帧imu_msg
    if (init_imu) //if语句里的return，使程序跳出if所在的函数，返回到母函数中继续执行。
    {
        latest_time = t;
        init_imu = 0;
        return;
    }

   //[2]计算当前imu_msg距离上一个imu_msg的时间间隔
    double dt = t - latest_time;
    latest_time = t;

    //[3]取x,y,z三个方向上的线加速度
    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    Eigen::Vector3d linear_acceleration{dx, dy, dz};

    //【4】获取x,y,z三个方向上的角速度
    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Eigen::Vector3d angular_velocity{rx, ry, rz};

    //【5】tmp_P,tmp_V,tmp_Q更新,这一部分有公式推导 ，利用的是中值积分
    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;//a理想值=R(a测量值-Ba)-g  w理想值=w测量值-BW 此式为k时刻的加速度的模型

    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;//中值法：取k时刻和k+1时刻的角速度再除2
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);//旋转的更新

    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;//k+1时刻的加速度

    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

    tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
    tmp_V = tmp_V + dt * un_acc;

    //【6】迭代赋值
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

此外，还调用了pubLatestOdometry（）发布P、V、Q值，此函数在visualization.cpp文件中。

三、feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)注释

void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    //[1]判断是否是第一个特征，如果是第一个特征直接跳过
    if (!init_feature)//init_feature=0
    {
        //skip the first detected feature, which doesn't contain optical flow speed
        init_feature = 1;
        return;
    }

    //[2]把feature_msg加入feature_buf中
    m_buf.lock();
    feature_buf.push(feature_msg);
    m_buf.unlock();
    con.notify_one();//唤醒process线程
}

四、restart_callback(const std_msgs::BoolConstPtr &restart_msg)注释

void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{

    if (restart_msg->data == true)
    {
        ROS_WARN("restart the estimator!");
        //[1]当feature_buf和imu_buf非空时，循环删除队首元素
        m_buf.lock();
        while(!feature_buf.empty())
            feature_buf.pop();
        while(!imu_buf.empty())
            imu_buf.pop();
        m_buf.unlock();

        //[2]清除估计器的状态和参数，时间重置
        m_estimator.lock();
        estimator.clearState();
        estimator.setParameter();
        m_estimator.unlock();
        current_time = -1;
        last_imu_t = 0;
    }
    return;
}

重定位的回调函数就不贴过来了。。。因为就两三句，没啥好贴的。。。
下面还剩下process( )函数，这个函数里面又调用了getMeasurements( )、processIMU( )、setReloFrame( )、processImage( ),还有一些发布数据的函数，是一个比较大的事儿，写在下一节吧。。。朋友们，再见！