VINS-mono代码阅读 -- vins_estimator

ROS中节点之间通信机制是基于发布/订阅(publish/subscribe)模型的消息(message)。所以在读一个ROS节点的时候，先看这个节点发布和订阅的消息，以及对应的回调函数。

1. main函数

int main(int argc, char **argv)
{
	//初始化ros节点
    ros::init(argc, argv, "vins_estimator");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
	//读取参数，读顾及器的参数
    readParameters(n);
    estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");
	//发布用于RVIZ显示的Topic
    registerPub(n);
	//订阅话题及回调函数
    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);//重定位的匹配点
	//创建process线程
    std::thread measurement_process{process};
    ros::spin();

    return 0;
}

main函数中的主要步骤有以下几点：
（1）初始化ros节点
（2）读取参数
（3）订阅话题
（4）创建process线程

estimator.setParameter()

为什么要在这里写出这个函数呢，因为这里的两个参数让我头疼了好久。

void Estimator::setParameter()
{
	//相机IMU的外参tic和ric
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

视觉约束的噪声协方差与标定相机内参时的重投影误差，也就是偏离几个像素有关，对应的就是这里的ProjectionFactor::sqrt_info，这里取1.5个像素，对应到归一化相机平面上的协方差矩阵需要除以焦距f，信息矩阵等于协方差矩阵的逆
$\Omega = {{\sum}_{vis}}^{-1}= (\frac{1.5}{f}I_{2\times2})^{-1}=\frac{f}{1.5}I_{2\times2}$Ω=∑vis​−1=(f1.5​I2×2​)−1=1.5f​I2×2​

2. 回调函数

imu_callback

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
	//唤醒的是process线程
    con.notify_one();

    last_imu_t = imu_msg->header.stamp.toSec();

    {
        std::lock_guard<std::mutex> lg(m_state);
		//从IMU测量值imu_msg和上一个PVQ递推得出下一个的PVQ
        predict(imu_msg);
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
			//发布最新的由predict得到的 P V Q
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);//"imu_propagate"
    }
}

（1）首先对imu的时间戳进行检查，判断是否disorder
（2）把imu数据放到imu_buf中
（3）predict()从IMU测量值imu_msg和上一个PVQ递推得出下一个的PVQ
（4）发布消息 imu_propagate

//从IMU测量值imu_msg和上一个PVQ递推得出下一个的PVQ
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();
    if (init_imu)
    {
        latest_time = t;
        init_imu = 0;
        return;
    }
    double dt = t - latest_time;
    latest_time = t;

    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};

    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};

    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;//tmp_Q local->world

    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);

    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
	//中值积分
    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;

    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

上面这段代码对应的公式：
$\omega = \frac{1}{2}((\omega^{b_k}-b_k^g)+(\omega^{b_{k+1}}-b_k^g))$ω=21​((ωbk​−bkg​)+(ωbk+1​−bkg​))
$q_{b_i b_{k+1}}=q_{b_i b_k}\bigotimes\begin{bmatrix} 1 \\ \frac{1}{2}\omega\delta t\end{bmatrix}$qbi​bk+1​​=qbi​bk​​⨂[121​ωδt​]
$a=\frac{1}{2} (q_{b_i b_k}(a^{b_k}-b_k^a)+q_{b_i b_{k+1}}(a^{b_{k+1}}-b_k^a))$a=21​(qbi​bk​​(abk​−bka​)+qbi​bk+1​​(abk+1​−bka​))
剩下的就是速度，加速度和位置的关系，就不写了。

feature_callback

void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    if (!init_feature)
    {
        //skip the first detected feature, which doesn't contain optical flow speed
        init_feature = 1;
        return;
    }
    m_buf.lock();
    feature_buf.push(feature_msg);
    m_buf.unlock();
    con.notify_one();
}

整个代码就是将除了第一帧的特征点外push到buf中，relocalization_callback也是执行了相似的过程，在这里就不贴出来了。

restart_callback

void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
    if (restart_msg->data == true)
    {
        ROS_WARN("restart the estimator!");
        m_buf.lock();
        while(!feature_buf.empty())
            feature_buf.pop();
        while(!imu_buf.empty())
            imu_buf.pop();
        m_buf.unlock();
        m_estimator.lock();
        estimator.clearState();
        estimator.setParameter();
        m_estimator.unlock();
        current_time = -1;
        last_imu_t = 0;
    }
    return;
}

接收到restart的消息后，将所有的buf全部清空，时间置为-1。

至此，estimator_node中的主函数和回调函数已经分析完了。
接下来就是process线程了，在process线程中，包含了预积分、初始化、外参标定、非线性优化等部分。

3. 画个图吧

在开始下面的分析之前，我把estimator_node这部分代码的流程图画在这里，虽然不标准，但是对于理解程序执行的过程还是有帮助的。