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

迷南。 2023-05-30 09:27 6阅读 0赞

#### 写在前面的话 ####

终于把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<std::string>(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<nav_msgs::Odometry>("imu_propagate", 1000);//1000指队列长度
        pub_path = n.advertise<nav_msgs::Path>("path", 1000);
        pub_relo_path = n.advertise<nav_msgs::Path>("relocalization_path", 1000);
        pub_odometry = n.advertise<nav_msgs::Odometry>("odometry", 1000);
        pub_point_cloud = n.advertise<sensor_msgs::PointCloud>("point_cloud", 1000);
        pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>("history_cloud", 1000);
        pub_key_poses = n.advertise<visualization_msgs::Marker>("key_poses", 1000);
        pub_camera_pose = n.advertise<nav_msgs::Odometry>("camera_pose", 1000);
        pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
        pub_keyframe_pose = n.advertise<nav_msgs::Odometry>("keyframe_pose", 1000);
        pub_keyframe_point = n.advertise<sensor_msgs::PointCloud>("keyframe_point", 1000);
        pub_extrinsic = n.advertise<nav_msgs::Odometry>("extrinsic", 1000);
        pub_relo_relative_pose=  n.advertise<nav_msgs::Odometry>("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<std::mutex> 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( ),还有一些发布数据的函数，是一个比较大的事儿，写在下一节吧。。。朋友们，再见！