Vins-mono 源码笔记 (2) estimator_node

总结

该节点：
1、接受并处理发送来的IMU数据。
2、处理前端节点发送的特征数据。
3、运行线程 measurement_process。

运行梳理：

1、数据的处理

1.1 IMU数据的处理

imu的数据在回调函数 imu_callback 进行处理 ，在imu_callback()函数中主要执行：
1、检查时间戳 看是否乱序。
2、将IMU数据 imu_msg 存入IMU数据缓存队列imu_buf。
3、(这里对源码有些改动)如果初始化完成，即进入优化模式，每当有IMU数据进来，都用该数据预测当前时刻的位姿PVQ并发布。

// IMU的数据回调 
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(); 

    // 将IMU数据存入IMU数据缓存队列imu_buf
    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();                // 唤醒getMeasurements()读取缓存imu_buf和feature_buf中的观测数据

    // 通过IMU测量值积分更新并发布里程计信息
    // last_imu_t = imu_msg->header.stamp.toSec();              // 这一行代码似乎重复了，上面有着一模一样的代码
    {
        std::lock_guard<std::mutex> lg(m_state);
        if (initialized){       // VINS初始化已完成，正处于滑动窗口非线性优化状态，如果VINS还在初始化，则不发布里程计信息
        // IMU航迹推算    更新 tmp_P、tmp_V、 tmp_Q   这里只是预测吧   
        predict(imu_msg);
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header); // 发布里程计信息，发布频率很高（与IMU数据同频），每次获取IMU数据都会及时进行更新，而且发布的是当前的里程计信息。
            // 还有一个pubOdometry()函数，似乎也是发布里程计信息，但是它是在estimator每次处理完一帧图像特征点数据后才发布的，有延迟，而且频率也不高（至多与图像同频）
        }
    }
}

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
采用中值积分对imu状态进行传播。

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();
    // 这里 latest_time在update()初始化 
    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};

    // 中值积分       第1个imu数据如何保存为acc_0   gyr_0 ？？？？                  
    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;                // 上一刻加速度
    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
    // 更新预测旋转Q 
    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);     // 中值   
    // 更新   预测速度V 和预测平移 P   
    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;
}

1.2 图像特征的处理

图像特征的处理在回调函数 feature_callback中完成，其中主要执行：
1、第一帧图像特征不处理。
2、将图像特征点数据存入图像特征点数据缓存队列feature_buf。

2、measurement_process

节点启动后，每当有IMU或者图像特征数据发送过来便立马回到回调函数中处理，将它们放到数组中，同时measurement_process线程也在不停的运行 process()。
在 estimator_node.cpp/main()函数中, 启动了 measurement_process 线程.

std::thread measurement_process{process}; 

2.1 process()

process() 流程：
1、调用getMeasurements()读取当前积累的IMU与图像数据到measurements。
注意，线程执行到这里时，会调用条件变量函数
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0;
});
其中con 是一个条件变量 , std::condition_variable con, 主要用于线程同步.
这里wait()函数的用法是, 如果里面的lamda表达式入法返回true的话，则会阻塞线程(此时lk解锁)，之后每当有imu数据或者图像特征数据到来，都会在回调函数中调用con.notify_one()从而唤醒wait()函数(此时lk上锁)，此时，继续判断lamda表达式，如果返回true，则线程向下运行，否则继续阻塞。
参考
2、 measurements 保存的是当前从IMU 图像队列中提取的图像特征数据和对应的IMU测量信息的pair，遍历每一组图像特征以及其匹配的IMU数据：
(1)、遍历该组全部IMU数据，执行IMU数据处理 processIMU()，求解预积分。
(2)、对图像特征进行处理 processImage()，进行紧耦合优化 。
(3)、重定位相关… 待学习 !!!
(4)、发布解算结果：

double whole_t = t_s.toc();
printStatistics(estimator, whole_t);
std_msgs::Header header = img_msg->header;
header.frame_id = "world";

// 每处理完一帧图像特征点数据，都要发布这些话题
pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
    pubRelocalization(estimator);

3、本轮求解完毕后，先上锁m_state、m_buf，锁定 imu_buf，让imu_callback()等待update()更新完状态， update()用优化后的结果更新 tmp_P、tmp_Q、tmp_V等。

   m_buf.lock();
   m_state.lock();   // 上锁   回调函数的预测部分等待update()完成   不上锁的话   运行update()时  会被imu_callback给打断  然后改变 latest_time acc_0、gyr_0等数据 
                     // 然后就乱了   
   if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
       // VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据
       update();
   m_state.unlock();
   m_buf.unlock();

update() 如下

// 当处理完measurements中的所有数据后，如果VINS系统正常完成滑动窗口优化，那么需要用优化后的结果更新以下数据，用于预测
void update()
{
	 TicToc t_predict;
	 latest_time = current_time;
	 // 首先获取滑动窗口中最新帧的P、V、Q
	 tmp_P = estimator.Ps[WINDOW_SIZE];
	 tmp_Q = estimator.Rs[WINDOW_SIZE];
	 tmp_V = estimator.Vs[WINDOW_SIZE];
	 tmp_Ba = estimator.Bas[WINDOW_SIZE];
	 tmp_Bg = estimator.Bgs[WINDOW_SIZE];
	 acc_0 = estimator.acc_0;
	 gyr_0 = estimator.gyr_0;
	 if(!initialized)    initialized = 1;
	 // 滑动窗口中最新帧并不是当前帧，中间隔着缓存队列的数据，所以还需要使用缓存队列中的IMU数据进行积分得到当前帧的里程计信息    这样使得精度更高  
	 queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
	 for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
	     predict(tmp_imu_buf.front());
}

这个过程需要再理清一下:
首先IMU的回调函数中, 对于每一个IMU数据都进行了 predict()并发布了里程计信息, 也就是发布了一个高频的,实时的位姿信息, 然后, 当IMU与图像特征积累足够多的时候, process()会取出部分数据进行紧耦合优化, 这个过程是比较耗时的, 比如需要20ms, 而IMU的频率以200hz算, 在这个过程当中, 将根据IMU输出4次位姿, 这个位姿都是相对于上一次优化的结果来的, 然后, 本次优化完成后, 得到了此前优化位置处的优化后位姿, 然后, 需要基于这个优化后的位姿用之后接收到的全部IMU数据重新进行运动学传播, 也就是predict(), 从而正确的更新预测值, 然后周而复始的运行这个过程.

2.2 getMeasurements()

getMeasurements() 流程：
注意要读取的数据是std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements
1、检查imu_buf,feature_buf 3个情况需要特殊处理：
(1)、imu_buf或者feature_buf有一个为空，则返回当前构建的measurements。
(2)、imu_buf队尾(最近获得的)元素的时间戳，早于或等于feature_buf队首元素的时间戳（时间偏移补偿后），则说明当前IMU数据不足够衡量图像帧间的运动，需要继续等待接收IMU数据，返回当前构建的measurements。如下图所示。

(3)、imu_buf队首(最早的数据)元素的时间戳，晚于或等于feature_buf队首元素的时间戳（时间偏移补偿后），则显然feature_buf队首的图像特征以无法和imu关联了 ，需要剔除feature_buf队首图像数据。如下图：

2、将feature_buf的队首图像特征，以及时间戳小于它的IMU数据，注意还要再加上后面一帧IMU数据用于插值出 img的加速度 角速度值 ，一起构成pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>放入measurements中。
可以想到，只有当imu_buf，feature_buf积累了足够多的数据时(imu_buf，feature_buf非空&imu_buf队首时间戳<feature_buf队首时间戳&imu_buf队尾时间戳>feature_buf队首时间戳)，才能进入 2 构建measurements。

2.3 IMU 数据处理

process()中处理IMU数据的部分如下：

 //遍历该组中的各帧imu数据，进行预积分
for (auto &imu_msg : measurement.first)
{
    double t = imu_msg->header.stamp.toSec();                                                      // 最新IMU数据的时间戳
    double img_t = img_msg->header.stamp.toSec() + estimator.td; // 图像特征点数据的时间戳，补偿了通过优化得到的一个时间偏移
    if (t <= img_t)          // 补偿时间偏移后，图像特征点数据的时间戳晚于或等于IMU数据的时间戳
    { 
        if (current_time < 0)          // 第一次接收IMU数据时会出现这种情况     
            current_time = t;
        double dt = t - current_time;  // 对于每一次的第一个imu数据    这个dt是与上一帧图像之间的时间差    否则是和上一个imu的时间差  
        ROS_ASSERT(dt >= 0);
        current_time = t;             // 更新最近一次接收的IMU数据的时间戳
        // IMU数据测量值
        // 3轴加速度测量值
        dx = imu_msg->linear_acceleration.x;
        dy = imu_msg->linear_acceleration.y;
        dz = imu_msg->linear_acceleration.z;

        // 3轴角速度测量值
        rx = imu_msg->angular_velocity.x;
        ry = imu_msg->angular_velocity.y;
        rz = imu_msg->angular_velocity.z;
        // 预积分
        estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
        //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);
    }
    else // 时间戳晚于图像特征点数据（时间偏移补偿后）的第一帧IMU数据（也是一组measurement中的唯一一帧），对IMU数据进行插值，得到图像帧时间戳对应的IMU数据
    {
        // 时间戳的对应关系如下图所示：
        //                                            current_time         t
        // *               *               *               *               *     （IMU数据）
        //                                                          |            （图像特征点数据）
        //                                                        img_t
        double dt_1 = img_t - current_time;    
        double dt_2 = t - img_t;
        current_time = img_t;     // 对于最后这个在img后面的imu数据    current_time记录的是img的时间  
        ROS_ASSERT(dt_1 >= 0);
        ROS_ASSERT(dt_2 >= 0);
        ROS_ASSERT(dt_1 + dt_2 > 0);
// 计算插值系数       这么来算    dx + (cx - dx)*dt_1/(dt_1+dt_2)  
        double w1 = dt_2 / (dt_1 + dt_2);
        double w2 = dt_1 / (dt_1 + dt_2);
// 插值出img处imu的近似值
        dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
        dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
        dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
        rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
        ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
        rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
// 预积分
        estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
        //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
    }
}

对每一个IMU数据，按时间前于或落后与图像帧进行分别处理。
首先计算与上一个IMU数据current_time的时间差dt，
然后通过processIMU()更新预积分， processIMU() 详细总结在 IMU，
每一次处理，都会有且只有一个IMU数据落后于图像帧，对于这个IMU数据，将其和前一个IMU数据插值出图像帧位置的IMU数据，并将图像帧位置的时间戳赋值给current_time。

2.4 图像特征的处理

process()中处理图像特征的部分如下：

 // 将图像特征点数据存到一个map容器中，key是特征点id     这里还考虑了可能有多个相机    但是我们只考虑单目的清况  
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;  // < 特征点id, <相机id, 特征点 > >
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
    int v = img_msg->channels[0].values[i] + 0.5;     //  四舍五入        = p_id * NUM_OF_CAM + i 
    int feature_id = v / NUM_OF_CAM;        // v   = p_id   
    int camera_id = v % NUM_OF_CAM;         //  相机序号  NUM_OF_CAM = 1    这里 v = 0  
// 获得归一化平面的坐标     cur_un_pts  
    double x = img_msg->points[i].x;
    double y = img_msg->points[i].y;
    double z = img_msg->points[i].z;
// 像素坐标     cur_pts
    double p_u = img_msg->channels[1].values[i];
    double p_v = img_msg->channels[2].values[i];
// 像素在x,y上的速度     pts_velocity
    double velocity_x = img_msg->channels[3].values[i];
    double velocity_y = img_msg->channels[4].values[i];
    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;
    image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);           //  camera_id = 0
}
estimator.processImage(image, img_msg->header);

这部分简单说就是，将图像特征点数据构造为 map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image，即< 特征点id, <相机id, 特征点 >>， 这里相机id认为为0就行，然后调用
estimator.processImage(image, img_msg->header)。

上面看到有这么多的数据，难免会迷糊，所以赶紧去feature_tracker节点回顾下到底发布了啥信息。这部分对应代码在feature_tracker_node.cpp中的img_callback()中：

 vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{  
 auto &un_pts = trackerData[i].cur_un_pts;            // 去畸变特征点
 auto &cur_pts = trackerData[i].cur_pts;              // 原始特征点
 auto &ids = trackerData[i].ids;                                                          // ID
 auto &pts_velocity = trackerData[i].pts_velocity;   // 特征点速度
// 遍历全部特征点            
 for (unsigned int j = 0; j < ids.size(); j++)
 {  // 将跟踪的帧数大于1的     track_cnt   ids      un_pts一一对应  
     if (trackerData[i].track_cnt[j] > 1)
     {
         int p_id = ids[j];                        // 获取id 
// 插入到 set中    按id顺序     
         hash_ids[i].insert(p_id);
         geometry_msgs::Point32 p;
         p.x = un_pts[j].x;
         p.y = un_pts[j].y;
         p.z = 1;
         // 一下容器元素均是一一对应的  
         feature_points->points.push_back(p);                                            //  去畸变后的归一化坐标                
         id_of_point.values.push_back(p_id * NUM_OF_CAM + i);           // i = 0      NUM_OF_CAM = 1   这样   /NUM_OF_CAM = p_id      %NUM_OF_CAM = i
         u_of_point.values.push_back(cur_pts[j].x);
         v_of_point.values.push_back(cur_pts[j].y);
         velocity_x_of_point.values.push_back(pts_velocity[j].x);
         velocity_y_of_point.values.push_back(pts_velocity[j].y);
     }
 }
}
// 对sensor_msgs::PointCloudPtr的各个channels进行赋值 
feature_points->channels.push_back(id_of_point);            // channels[0]
feature_points->channels.push_back(u_of_point);             // channels[1]  .....
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);

2.4.1、processImage()

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)

processImage() 利用获得的图像特征数据与IMU数据执行紧耦合优化,
它的基本流程如下：

注意：
1、图像特征的处理： 首先将每一帧的图像特征数据添加到feature ，然后根据视差与跟踪数量选择是marg掉滑动窗口的最老的一帧还是第二最新帧。但是，这个marg一定是在滑动窗口滑动窗口填满之后才进行。 然后，Headers数组记录当前帧的时间戳，Headers数组只保存滑动窗口的所有帧的数据，构造图像帧ImageFrame imageframe ， 并构造make_pair(header.stamp.toSec(), imageframe)添加到all_image_frame，从第一帧开始所有的图像帧都会添加到all_image_frame, all_image_frame 中元素的删除在 SlideWindow()。

2、如果外参数为止的话则进行外参在线标定，外参标定从第二帧开始，先获取与前一帧的特征点匹配关系，然后进行标定计算，当一个滑动窗口所有帧(2-11)都进行了标定计算后，且第二小奇异值大于阈值，才认为标定完成，若一整个滑动窗口的数据都不能完成标定，则在后面进行滑动窗口marg掉一帧，新的帧来了之后再继续标定。

3、最开始需要完成初始化，初始化需要等滑动窗口填满后才进行，并且也要保证已经成功获取了外参，当外参标定完成后，会紧接着执行初始化，如果初始化失败了，那么就marg掉一帧， 下一帧来了后再进行初始化，直到初始化完成。

2.4.1.1. addFeatureCheckParallax

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
特征管理器类FeatureManager的成员函数，定义在feature_manager.cpp中。
processImage()中使用代码如下

if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    marginalization_flag = MARGIN_OLD;
else
    marginalization_flag = MARGIN_SECOND_NEW;

流程：

1、将image的全部特征点数据添加到feature list中。首先判断该特征点的id在滑动窗口特征点中是否存在，即查找 feature 中有无相同的id。如果不存在，则创建新的FeaturePerId添加到feature中，否则将该特征点在当前帧的信息添加到已有特征点的feature_per_frame容器中。
2、如果滑动窗口中已有特征点与当前图像特征点的匹配个数不足20，则 return true，即 将最老的一帧marg掉，第二新的帧作为关键帧。
3、计算第2最新帧和第3最新帧之间跟踪到的特征点的平均视差，如果平均视差大于等于可以接受的最小视差，说明移动有点大了，这样则将第2最新帧当作关键帧，marg掉最老的帧，否则marg掉第二新帧。

2.4.2 frame的定义

// 图像帧    
class ImageFrame
{
	 public:
	     ImageFrame(){};
	     ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>& _points, double _t):t{_t},is_key_frame{false}
	     {
	         points = _points;
	     };
	     map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>> > > points; // 特征点数据
	     double t;                         // 时间戳
	     Matrix3d R;                       // 旋转
	     Vector3d T;                       // 平移
	     IntegrationBase *pre_integration;           // 该帧的预积分   
	     bool is_key_frame;                          // 是否为关键帧    
};

一个图像帧保存所有特征点数据、时间戳、位姿、以及预积分量。