VINS-Fusion代码按执行顺序阅读（一）

参考博文链接：vins-fusion代码解读

程序入口

VINS-Fusion/vins-estimator/rosNodeTest.cpp

程序顺序解读
首先，定义了几个全局变量：

1.Estimator estimator;
自动执行该类的构造函数，其中两条重要的语句如下：
initThreadFlag = false;
clearState(); 本质上不仅清除了状态，而且为诸多变量赋初值。
（这里std::mutex似乎和C++11 的并发和多线程编程有关，留待后续了解）
2.queue imu_buf; queue feature_buf; queue img0_buf; queue img1_buf; std::mutex m_buf;

然后，进入了main()函数：
readParameters(config_file);
estimator.setParameter(); 读取参数并配置参数

1. 这部分的逻辑是这样的，配置参数一开始存放在.yaml文件中，通过parameters.h文件中的readParameters()函数（该文件中只有这一个函数），将该文件中声明的所有变量赋值（这些变量定义在parameters.cpp文件中！且变量名全部大写，然后使用extern修饰并放在parameters.h中，可以想象当该.h文件被其他文件#include则可以直接使用这些值了），而estimator.setParameter()是从parameters.h文件中获得相应的配置参数。
2. 除此之外，processThread = std::thread(&Estimator::processMeasurements, this);开启了一个新线程。

最后，订阅了几个topic，分别是IMU_TOPIC，两个相机图像IMAGE0_TOPIC, IMAGE1_TOPIC，和feature_tracker所提供的跟踪光流点？
同时开启了一个新线程sync_process。？？线程的作用：如果图像buffer里面有数据的话,读入数据并且添加到estimator中。为什么不在相机图像的回调函数就input，我的理解在于对于双目的话，能够检测同步问题，能够将同样时间戳的两帧图片同时放入estimator中。所以对于Imu以及feature直接在回调函数中进行添加。
这里有几句ros的指令需要进行了解！至此，main()函数就结束了，一直在此处循环处理。

    registerPub(n);

    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    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::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);
    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中的几行关键指令的含义：
roscpp是C++语言的ROS接口。直接调用它所提供的函数就可以实现topic、service等通信功能。（ROS为机器人开发者们提供了不同语言的编程接口，比如C++接口叫做roscpp，Python接口叫做rospy，Java接口叫做rosjava）
可以简单地把roscpp就当作一个C++的库，当我们创建一个工程，并在其中#include ，这样就可以使用ROS提供的函数了。

1. 初始化节点
   一个C++写的ROS程序，和普通的C++程序之间的区别：
   1.1. 调用了ros::init()函数，初始化节点的名称和其他信息（节点什么时候存在的？）
   1.2. 创建ros::NodeHandle对象，即节点的句柄；

2. 关闭节点
   直接在终端上ctrl+c，（等价于调用ros::shutdown()）

源代码里的节点初始化、关闭框架：

#include 
int main(int argc, char** argv){
	ros::init(argc, argv, "vins_estimator"); // node_name，初始化一个节点
	ros::NodeHandle n("~"); // 获取节点的句柄，此处似乎有个命名空间的问题
	// 节点功能
	//
	ros::spin(); // 用于触发topic, service的响应队列
	return 0;
}

NodeHandle是node的句柄，用来对当前节点进行各种操作。
在ROS中，NodeHandle是一个定义好的类，我们可以创建这个类，并使用它的成员函数。它的成员函数有（按照用到的介绍）roscpp官方文档：
ros::Subscriber subscribe (const std::string &topic, uint32_t queue_size, void(*fp)(M), const TransportHints &transport_hints=TransportHints())
第一个参数是订阅话题的名称；
第二个参数是订阅队列的长度；（如果收到的消息都没来得及处理，那么新消息入队，旧消息就会出队）；
第三个参数是回调函数的指针，指向回调函数来处理接收到的消息！
第四个参数：似乎与延迟有关系，暂时不关心。（该成员函数有13重载）
例如：
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
其中，std::string IMU_TOPIC;是在parameters.cpp中定义的一个参数。
ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);

这几个回调函数，分别定义在了该main()函数的开头位置。
简单看一下：

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);
    estimator.inputIMU(t, acc, gyr);
    return;
}

不难看出，传感器输出的的原始数据，经过该函数进入到了程序中。实际上此处可以看作为一个消息接收节点？？！

void Estimator::inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)
{
    mBuf.lock();
    accBuf.push(make_pair(t, linearAcceleration));
    gyrBuf.push(make_pair(t, angularVelocity));
    //printf("input imu with time %f \n", t);
    mBuf.unlock();

    fastPredictIMU(t, linearAcceleration, angularVelocity); // 计算最新的PVQ
    if (solver_flag == NON_LINEAR)
        pubLatestOdometry(latest_P, latest_Q, latest_V, t); //此处会发布最新的信息
}

此处，消息稍加处理，然后又发布出去。关于最后一句pubLatestOdometry()：

void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, double t)
{
    nav_msgs::Odometry odometry;  // 信息打包成约定格式
    odometry.header.stamp = ros::Time(t);
    odometry.header.frame_id = "world";
    odometry.pose.pose.position.x = P.x();
    odometry.pose.pose.position.y = P.y();
    odometry.pose.pose.position.z = P.z();
    odometry.pose.pose.orientation.x = Q.x();
    odometry.pose.pose.orientation.y = Q.y();
    odometry.pose.pose.orientation.z = Q.z();
    odometry.pose.pose.orientation.w = Q.w();
    odometry.twist.twist.linear.x = V.x();
    odometry.twist.twist.linear.y = V.y();
    odometry.twist.twist.linear.z = V.z();
    pub_latest_odometry.publish(odometry);  // 发布
}

其中，通常我们会把消息的收发的两端分成两个节点来写，一个节点就是一个完整的C++程序，此处是“发节点”
ros::Publisher pub_latest_odometry;
pub_latest_odometry = n.advertise("imu_propagate", 1000);
nav_msgs::Odometry odometry;
pub_latest_odometry.publish(odometry);
感叹，ROS的架构真的很清晰！！

图像的两个类似（很简单，这个回调函数仅仅接收保存数据而已）：

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

注意，这个收节点有些不同，前面三个收节点接收的都是原始传感器的数据，而此处的收节点接收的是已经经过处理的信息sensor_msgs::PointCloudConstPtr。那么是如何从原始数据到处理好的信息的呢？怎么继续执行下去呢？

void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    map>>> 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];
        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 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;
}